Tutorials/drcsim/2.6/Atlas control over ROS topics with python

Under construction


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

This tutorial will demonstrate how to control the Atlas robot with a joint trajectory controller. In the course of this tutorial we're going to make the Atlas 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 topics demonstrated in the previous tutorial that used C++. This general-purpose controller can be used to make a set of joints follow a desired trajectory specified as joint angles. The controller is executed 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.

Important note: The approach to robot control described here is not the best or only way to control the Atlas 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

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

cd ~/ros roscreate-pkg tutorial_atlas_control atlas_msgs


 * 1) The Code ##

Move to the directory tutorial_atlas_control roscd tutorial_atlas_control

Add a file called traj_yaml.py

gedit traj_yaml.py

Copy the following code into it:

import roslib; roslib.load_manifest('tutorial_atlas_control') import rospy, yaml, sys from atlas_msgs.msg import AtlasCommand from sensor_msgs.msg import JointState from numpy import zeros, array, linspace from math import ceil
 * 1) !/usr/bin/env python

atlasJointNames = [ 'atlas::back_lbz', 'atlas::back_mby', 'atlas::back_ubx', 'atlas::neck_ay', 'atlas::l_leg_uhz', 'atlas::l_leg_mhx', 'atlas::l_leg_lhy', 'atlas::l_leg_kny', 'atlas::l_leg_uay', 'atlas::l_leg_lax', 'atlas::r_leg_uhz', 'atlas::r_leg_mhx', 'atlas::r_leg_lhy', 'atlas::r_leg_kny', 'atlas::r_leg_uay', 'atlas::r_leg_lax', 'atlas::l_arm_usy', 'atlas::l_arm_shx', 'atlas::l_arm_ely', 'atlas::l_arm_elx', 'atlas::l_arm_uwy', 'atlas::l_arm_mwx', 'atlas::r_arm_usy', 'atlas::r_arm_shx', 'atlas::r_arm_ely', 'atlas::r_arm_elx', 'atlas::r_arm_uwy', 'atlas::r_arm_mwx']

currentJointState = JointState def jointStatesCallback(msg): global currentJointState currentJointState = msg

if __name__ == '__main__': # first make sure the input arguments are correct 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])

# Setup subscriber to atlas states rospy.Subscriber("/atlas/joint_states", JointState, jointStatesCallback)

# initialize AtlasCommand message command = AtlasCommand n = len(atlasJointNames) command.position    = zeros(n) command.velocity    = zeros(n) command.effort      = zeros(n) command.kp_position = zeros(n) command.ki_position = zeros(n) command.kd_position = zeros(n) command.kp_velocity = zeros(n) command.i_effort_min = zeros(n) command.i_effort_max = zeros(n) # Set k_effort to 255 to indicate that we want PID control of each joint command.k_effort = [255] * n

# now get gains from parameter server rospy.init_node('tutorial_atlas_control') for i in xrange(len(atlasJointNames)): name = atlasJointNames[i] command.kp_position[i] = rospy.get_param('atlas_controller/gains/' + name[7::] + '/p') command.ki_position[i] = rospy.get_param('atlas_controller/gains/' + name[7::] + '/i') command.kd_position[i] = rospy.get_param('atlas_controller/gains/' + name[7::] + '/d') command.i_effort_max[i] = rospy.get_param('atlas_controller/gains/' + name[7::] + '/i_clamp') command.i_effort_min[i] = -command.i_effort_max[i]

# set up the publisher pub = rospy.Publisher('/atlas/atlas_command', AtlasCommand)

# for each trajectory for i in xrange(0, traj_len): # get initial joint positions initialPosition = array(currentJointState.position) # get joint commands from yaml y = traj_yaml[traj_name][i] # first value is time duration dt = float(y[0]) # subsequent values are desired joint positions commandPosition = array([ float(x) for x in y[1].split ]) # desired publish interval dtPublish = 0.02 n = ceil(dt / dtPublish) for ratio in linspace(0, 1, n): interpCommand = (1-ratio)*initialPosition + ratio * commandPosition command.position = [ float(x) for x in interpCommand ] pub.publish(command) rospy.sleep(dt / float(n))

Then download the following YAML file (Right-click + Save As) of a few trajectories, and place it in the same directory (~/ros/tutorial_atlas_control): [[Media:Traj_data2.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 atlas_utils atlas.launch

You should see the Atlas 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 tutorial_atlas_control python traj_yaml.py Traj_data2.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_data2.yaml touchdown

Then, just for fun:

python traj_yaml.py Traj_data2.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('tutorial_atlas_control') import rospy, yaml, sys from atlas_msgs.msg import AtlasCommand from sensor_msgs.msg import JointState from numpy import zeros, array, linspace from math import ceil

Specify the names of the joints in the correct order.

atlasJointNames = [ 'atlas::back_lbz', 'atlas::back_mby', 'atlas::back_ubx', 'atlas::neck_ay', 'atlas::l_leg_uhz', 'atlas::l_leg_mhx', 'atlas::l_leg_lhy', 'atlas::l_leg_kny', 'atlas::l_leg_uay', 'atlas::l_leg_lax', 'atlas::r_leg_uhz', 'atlas::r_leg_mhx', 'atlas::r_leg_lhy', 'atlas::r_leg_kny', 'atlas::r_leg_uay', 'atlas::r_leg_lax', 'atlas::l_arm_usy', 'atlas::l_arm_shx', 'atlas::l_arm_ely', 'atlas::l_arm_elx', 'atlas::l_arm_uwy', 'atlas::l_arm_mwx', 'atlas::r_arm_usy', 'atlas::r_arm_shx', 'atlas::r_arm_ely', 'atlas::r_arm_elx', 'atlas::r_arm_uwy', 'atlas::r_arm_mwx']

Create a ROS callback for reading the JointState message published on /atlas/joint_states

currentJointState = JointState def jointStatesCallback(msg): global currentJointState currentJointState = msg

Import the files that we need.

if __name__ == '__main__': # first make sure the input arguments are correct 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])

Set up the joint states subscriber.

# Setup subscriber to atlas states rospy.Subscriber("/atlas/joint_states", JointState, jointStatesCallback)

Initialize the joint command message.

command = AtlasCommand n = len(atlasJointNames) command.position    = zeros(n) command.velocity    = zeros(n) command.effort      = zeros(n) command.kp_position = zeros(n) command.ki_position = zeros(n) command.kd_position = zeros(n) command.kp_velocity = zeros(n) command.i_effort_min = zeros(n) command.i_effort_max = zeros(n) command.k_effort = [255] * n
 * 1) initialize AtlasCommand message
 * 1) Set k_effort to 255 to indicate that we want PID control of each joint

Get the current controller gains from the parameter server.

# now get gains from parameter server rospy.init_node('tutorial_atlas_control') for i in xrange(len(atlasJointNames)): name = atlasJointNames[i] command.kp_position[i] = rospy.get_param('atlas_controller/gains/' + name[7::] + '/p') command.ki_position[i] = rospy.get_param('atlas_controller/gains/' + name[7::] + '/i') command.kd_position[i] = rospy.get_param('atlas_controller/gains/' + name[7::] + '/d') command.i_effort_max[i] = rospy.get_param('atlas_controller/gains/' + name[7::] + '/i_clamp') command.i_effort_min[i] = -command.i_effort_max[i]

Set up the joint command publisher.

# set up the publisher pub = rospy.Publisher('/atlas/atlas_command', AtlasCommand)

Read in each line from the yaml file as a trajectory command, and the current joint states from the ROS topic. Publish trajectory commands that interpolate between the initial state and the desired position to generate a smooth motion.

# for each trajectory for i in xrange(0, traj_len): # get initial joint positions initialPosition = array(currentJointState.position) # get joint commands from yaml y = traj_yaml[traj_name][i] # first value is time duration dt = float(y[0]) # subsequent values are desired joint positions commandPosition = array([ float(x) for x in y[1].split ]) # desired publish interval dtPublish = 0.02 n = ceil(dt / dtPublish) for ratio in linspace(0, 1, n): interpCommand = (1-ratio)*initialPosition + ratio * commandPosition command.position = [ float(x) for x in interpCommand ] pub.publish(command) rospy.sleep(dt / float(n))