Tutorials/drcsim/whole body trajectory


 * 1) DRC Tutorial: DRC Robot whole-body joint trajectory control

This tutorial will demonstrate how to control the DRC robot with a joint trajectory controller. In the course of this tutorial we're going to make the DRC robot try to take a step. It will fall down, and that's OK, because our trajectory is incredibly simple and not at all reactive. You're welcome to extend the example here to make the robot walk or go through other motions.

We're using the ROS Joint Trajectory Action Controller. This general-purpose controller can be used to make a set of joints follow a desired trajectory specified as joint angles. The controller is loaded by the pr2_controller_manager, which is in turn loaded as part of a Gazebo plugin. This arrangement allows the controller to run in-line with the simulation, approximating the on-robot situation in which the controller runs in a real-time environment. The Joint Trajectory Action Controller, like the rest of the ROS robot mechanism controllers, has been used to control a variety of robots, in both hardware and simulation.

The controller presents an action interface in the `/drc_controller/follow_joint_trajectory/` topic namespace. We will command the controller by sending desired joint trajectories as goals to the action server in that namespace.

Important note: The approach to robot control described here is not the best or only way to control the DRC robot. It is provided for demonstration purposes. DRC participants should be aware that we expect the control system in simulation to change substantially as more sophisticated controller become available.


 * 1) Install DRC Simulator ##

Click to see the instructions for installing the DRC simulator and associated utilities.


 * 1) Create a new ROS package ##

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

If you haven't already, install `rviz` (this is a temporary requirement and will go away):

sudo apt-get install ros-fuerte-visualization

Use roscreate-pkg to create a ROS package for this tutorial, depending on a ROS package called `drc_robot_utils`:

cd ~/ros roscreate-pkg trajectory_control_tutorial drc_robot_utils

Create a `src` directory in this package:

roscd trajectory_control_tutorial


 * 1) The Code ##

Move to the directory trajectory_control_tutorial roscd trajectory_control_tutorial

Add a file called traj_yaml.py

gedit traj_yaml.py

Copy the following code into it:

#!/usr/bin/env python import roslib; roslib.load_manifest('trajectory_control_tutorial') import rospy, actionlib, yaml, sys from control_msgs.msg import * if __name__ == '__main__': if len(sys.argv) != 3: print "usage: traj_yaml.py YAML_FILE TRAJECTORY_NAME" print " where TRAJECTORY is a dictionary defined in YAML_FILE" sys.exit(1) traj_yaml = yaml.load(file(sys.argv[1], 'r')) traj_name = sys.argv[2] if not traj_name in traj_yaml: print "unable to find trajectory %s in %s" % (traj_name, sys.argv[1]) sys.exit(1) traj_len = len(traj_yaml[traj_name]) rospy.init_node('traj') traj_client = actionlib.SimpleActionClient( \                                    '/drc_controller/follow_joint_trajectory', \                                     FollowJointTrajectoryAction) traj_client.wait_for_server # now, build the trajectory-action goal message from the YAML snippet goal = FollowJointTrajectoryGoal goal.trajectory.joint_names = ['l_leg_uhz', 'l_leg_mhx', 'l_leg_lhy', 'l_leg_kny', 'l_leg_uay', 'l_leg_lax', 'r_leg_uhz', 'r_leg_mhx', 'r_leg_lhy', 'r_leg_kny', 'r_leg_uay', 'r_leg_lax', 'l_arm_usy', 'l_arm_shx', 'l_arm_ely', 'l_arm_elx', 'l_arm_uwy', 'l_arm_mwx', 'r_arm_usy', 'r_arm_shx', 'r_arm_ely', 'r_arm_elx', 'r_arm_uwy', 'r_arm_mwx', 'neck_ay', 'back_lbz', 'back_mby', 'back_ubx'] goal.trajectory.points = [ trajectory_msgs.msg.JointTrajectoryPoint \ for x in xrange(0, traj_len) ] t = 0.0 for i in xrange(0, traj_len): y = traj_yaml[traj_name][i] goal_pt = goal.trajectory.points[i] t += float(y[0]) goal_pt.time_from_start = rospy.Duration.from_sec(t) goal_pt.velocities = [0] * 28 goal_pt.positions = [ float(x) for x in y[1].split ] traj_client.send_goal(goal) traj_client.wait_for_result(rospy.Duration.from_sec(t + 3))

Then download this YAML file of a few trajectories, and place it in the same directory as the previous Python program: [[Media:Traj_data.yaml]]


 * 1) Trying it out ##

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

You should see the DRC robot standing in an empty world. It will likely sway back and forth; that's an artifact of the controllers holding position on the joints.

In a separate terminal, run the node that you just built: roscd trajectory_control_tutorial python traj_yaml.py Traj_data.yaml step_and_fall

You should see the robot try to take a step with its right leg and then fall down.

Here is another trajectory, since it's football season:

python traj_yaml.py Traj_data.yaml touchdown

Then, just for fun:

python traj_yaml.py Traj_data.yaml touchdown_exhausted


 * 1) Restarting ###

To try it again, go to the Gazebo "Edit" menu and click on "Reset Model Poses". That will teleport the robot back to its initial pose, from where you can run a trajectory again. In this way, you can iterate, making changes to the program sending the trajectory and checking the effect in simulation, without shutting everything down.


 * 1) The Code Explained ##

#!/usr/bin/env python import roslib; roslib.load_manifest('trajectory_control_tutorial') import rospy, actionlib, yaml, sys from control_msgs.msg import * Import the files we need.

if __name__ == '__main__': if len(sys.argv) != 3: print "usage: traj_yaml.py YAML_FILE TRAJECTORY_NAME" print " where TRAJECTORY is a dictionary defined in YAML_FILE" sys.exit(1) traj_yaml = yaml.load(file(sys.argv[1], 'r')) traj_name = sys.argv[2] if not traj_name in traj_yaml: print "unable to find %s in %s" % (traj_name, sys.argv[1]) traj_len = len(traj_yaml[traj_name])

Load in the YAML file containing a few hard-coded trajectories.

rospy.init_node('traj') traj_client = actionlib.SimpleActionClient( \                                    '/drc_controller/follow_joint_trajectory', \                                     FollowJointTrajectoryAction) traj_client.wait_for_server

Create an ActionClient for the drc_controller/follow_joint_trajectory action server.

goal = FollowJointTrajectoryGoal goal.trajectory.joint_names = ['l_leg_uhz', 'l_leg_mhx', 'l_leg_lhy', 'l_leg_kny', 'l_leg_uay', 'l_leg_lax', 'r_leg_uhz', 'r_leg_mhx', 'r_leg_lhy', 'r_leg_kny', 'r_leg_uay', 'r_leg_lax', 'l_arm_usy', 'l_arm_shx', 'l_arm_ely', 'l_arm_elx', 'l_arm_uwy', 'l_arm_mwx', 'r_arm_usy', 'r_arm_shx', 'r_arm_ely', 'r_arm_elx', 'r_arm_uwy', 'r_arm_mwx', 'neck_ay', 'back_lbz', 'back_mby', 'back_ubx'] goal.trajectory.points = [ trajectory_msgs.msg.JointTrajectoryPoint \ for x in xrange(0, traj_len) ]

Start to populate the FollowJointTrajectory action goal.

t = 0.0 for i in xrange(0, traj_len): y = traj_yaml[traj_name][i] goal_pt = goal.trajectory.points[i] t += float(y[0]) goal_pt.time_from_start = rospy.Duration.from_sec(t) goal_pt.velocities = [0] * 28 goal_pt.positions = [ float(x) for x in y[1].split ]

Read the trajectory points out of the YAML file and stuff them into the action goal message.

traj_client.send_goal(goal) traj_client.wait_for_result(rospy.Duration.from_sec(t + 3))

Send the goal message and wait for it to complete.