Tutorials/drcsim/joint control


 * 1) DRC Tutorial: DRC Robot basic joint control

This tutorial will explain how to control the DRC robot through a rospy node.


 * 1) Setup ##

We assume that you've already done the installation step.

If you haven't done so, add the environment setup.sh files to your .bashrc. Important note: Fill in drcsim-x.y to refer to the correct location of the setup files, for example, if you have drcsim-1.0.x installed, type:

echo 'source /usr/share/drcsim-1.0/setup.sh' >> ~/.bashrc source ~/.bashrc

or, if you have drcsim-1.2 or later, type:

echo 'source /usr/share/drcsim/setup.sh' >> ~/.bashrc source ~/.bashrc

Install the ROS visualization tools if you do not yet have them. From the command line:

sudo apt-get install ros-fuerte-visualization

If you haven't already, create a ros directory in your home directory and add it to your `$ROS_PACKAGE_PATH`. From the command line

mkdir ~/ros echo "export ROS_PACKAGE_PATH=~/ros:\$ROS_PACKAGE_PATH" >> ~/.bashrc source ~/.bashrc

Use roscreate-pkg to create a ROS package for this tutorial, depending on a ROS package called `drc_robot_utils` for drcsim-1.2 and earlier, and `atlas_utils` for drcsim-1.3 and later:

cd ~/ros # for drcsim-1.2 and earlier roscreate-pkg joint_control_tutorial drc_robot_utils # for drcsim-1.3 and later roscreate-pkg joint_control_tutorial atlas_utils

Create a `scripts` directory in this package:

roscd joint_control_tutorial mkdir scripts

If roscd fails, then make sure `$ROS_PACKAGE_PATH` contains `[full path to your home directory]/ros`. You can check by echoing the environment variable:

echo $ROS_PACKAGE_PATH


 * 1) The Code ##

Move to the directory joint_control_tutorial/scripts roscd joint_control_tutorial/scripts

Add a file called joint_control.py

gedit joint_control.py

Copy the following code into it:

#!/usr/bin/env python import roslib; roslib.load_manifest('joint_control_tutorial') import rospy, math from std_msgs.msg import Float64 def jointStateCommand: # Setup the publishers for each joint r_arm_elx = rospy.Publisher('/r_arm_elx_position_controller/command', Float64) r_arm_ely = rospy.Publisher('/r_arm_ely_position_controller/command', Float64) # Initialize the node rospy.init_node('joint_control') #Initial pose r_arm_elx.publish(-1.17) r_arm_ely.publish(0.4) # Sleep for 1 second to wait for the home position rospy.sleep(1) #This while loop will continue until ROS tells it to shutdown while not rospy.is_shutdown: t = 6 * rospy.get_time elbow_x = -1.57 + 0.4 * math.cos(t) elbow_y = 0.4 + 0.4 * math.sin(t) r_arm_elx.publish(elbow_x) r_arm_ely.publish(elbow_y) # Wait 0.01 second rospy.sleep(0.01) if __name__ == '__main__': try: jointStateCommand except rospy.ROSInterruptException: pass

In a terminal, make the script file executable

chmod +x joint_control.py

If you haven't brought down your previous instance of the DRC simulator, kill the process by pressing Control + C in it's terminal. Now launch the robot

roslaunch drc_robot_utils drc_robot.launch roslaunch atlas_utils atlas.launch
 * 1) For drcsim-1.2 and earlier:
 * 1) For drcsim-1.3 and later:

In a separate terminal, run the node

rosrun joint_control_tutorial joint_control.py


 * 1) The Code Explained ##

#!/usr/bin/env python import roslib; roslib.load_manifest('drc_robot_utils') This is standard for each ROS node. This imports roslib and runs load_manifest, which will load the dependencies from the package manifest so that all those items will be importable. For example, std_msgs is loaded from the manifest and now, in the code below, we can import from it.

import rospy, math from std_msgs.msg import Float64 Importing the necessary modules. For each message type we want to publish, we need to import that into our script. In this case, the Float64 message.

def jointStateCommand: # Setup the publishers for each joint r_arm_elx = rospy.Publisher('/r_arm_elx_position_controller/command', Float64) r_arm_ely = rospy.Publisher('/r_arm_ely_position_controller/command', Float64) This is how you setup a publisher in rospy. It takes two arguments, the topic and the message type. # Initialize the node rospy.init_node('joint_control') This initializes the node with the ROS Master. This needs to happen in every ROS node.

#Initial pose r_arm_elx.publish(-1.17) # Elbow flex r_arm_ely.publish(0.4) # Shoulder roll # Sleep for 1 second to wait for the home position rospy.sleep(1) We publish the first joint states to the robot. The command takes a goal location, and if it can't get there, it will get as close as possible. These lines bend the elbow to 1.17 radians and rotate the shoulder forward to 0.4 radians.

#This while loop will continue until ROS tells it to shut down while not rospy.is_shutdown: t = 6 * rospy.get_time elbow_x = -1.57 + 0.4 * math.cos(t) elbow_y = 0.4 + 0.4 * math.sin(t) r_arm_elx.publish(elbow_x) r_arm_ely.publish(elbow_y) # Wait 0.01 second rospy.sleep(0.01) This continuously updates the command goal to a new position. By using rospy.get_time we can create smooth goal commands that are time-dependent.

if __name__ == '__main__': try: jointStateCommand except rospy.ROSInterruptException: pass This is more typical rospy node code to prevent the node from executing when it's not supposed to.


 * 1) What it should look like ##

If it all went smoothly then you should see the arm moving in a lasso motion. Pictures don't really capture the awesomeness.




 * 1) Next ##

Next: DRC Robot Teleop