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