Atlas control over ROS with python

Overview

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.

Install DRC Simulator

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

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=${HOME}/ros:${ROS_PACKAGE_PATH}" >> ~/.bashrc
source ~/.bashrc

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

cd ~/ros
roscreate-pkg tutorial_atlas_control osrf_msgs

The Code

Move to the directory tutorial_atlas_control

roscd tutorial_atlas_control

Copy the file traj_yaml.py into it:

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

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 JointCommands message
  command = JointCommands()
  command.name = list(atlasJointNames)
  n = len(command.name)
  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)

  # now get gains from parameter server
  rospy.init_node('tutorial_atlas_control')
  for i in xrange(len(command.name)):
    name = command.name[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/joint_commands', JointCommands, queue_size=1)

  # 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 Traj_data2.yaml YAML file of a few trajectories, and place it in the same directory (~/ros/tutorial_atlas_control).

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 drcsim_gazebo 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, put Atlas in User mode:

rostopic pub /atlas/control_mode std_msgs/String -- "User"

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

Restarting

To try it again, go to the Gazebo "Edit" menu and click on "Reset Model Poses" (you might need to do this several times, use Shift+Ctrl+R for convenience). 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.

The Code Explained

#!/usr/bin/env python
import roslib; roslib.load_manifest('tutorial_atlas_control')
import rospy, yaml, sys
from osrf_msgs.msg import JointCommands
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.

  # initialize JointCommands message
  command = JointCommands()
  command.name = list(atlasJointNames)
  n = len(command.name)
  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)

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(command.name)):
    name = command.name[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/joint_commands', JointCommands, queue_size=1)

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))