Skip to content

Commit a31789a

Browse files
caguerocaguero
committed
Merged in drcsim_ros_python (pull request #52)
Adding controlling atlas through ROS and Python.
2 parents 484ce7f + 5327784 commit a31789a

File tree

4 files changed

+225
-0
lines changed

4 files changed

+225
-0
lines changed

‎drcsim_ros_python/files/Traj_data2.yaml

+18
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
# you probably want to get a really wide window to edit this file. sorry 80 char windows.
2+
# the first column is the number of seconds to take to reach this point, relative to the previous point
3+
# spaces separate the following sub-vectors:
4+
# [ neck/body left_leg right_leg left_arm right_arm ]
5+
step_and_fall:
6+
- [1.0, "0 0 0 0.2 0 -0.1 0 0 0 0 0 -0.7 -1.2 1.4 -1.4 0 0 0 0 0 0 0 0 0 0 0 0 0 " ]
7+
- [0.5, "0 0 0 0 0 0.4 1.2 0.8 -1.4 0.2 0 -0.7 -1.2 1.4 -1.4 0 0 0 0 0 0 0 0 0 0 0 0 0 " ]
8+
- [1.5, "0 0 1 1 0 0 0 0 0.1 0 0 0 -0.2 0.4 -0.4 0 0 0 0 0 0 0 0 0 0 0 0 0 " ]
9+
- [1.0, "0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 " ]
10+
touchdown:
11+
- [2.0, "0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.5 0 2 0 0 0 0.5 0 -2 0 0 " ]
12+
- [3.0, "0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1.2 0 0.5 0 0 0 -1.2 0 -0.5 0 0 " ]
13+
- [1.0, "0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -1.0 0 0 0 0 0 1.0 0 0 0 0 " ]
14+
touchdown_exhausted:
15+
- [2.0, "0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.5 0 2 0 0 0 0.5 0 -2 0 0 " ]
16+
- [3.0, "0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1.2 0 0.5 0 0 0 -1.2 0 -0.5 0 0 " ]
17+
- [1.0, "0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -1.0 -1.0 0 0 0 0 -1.0 1.0 0 0 0 0 " ]
18+
- [1.0, "0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 " ]

‎drcsim_ros_python/files/traj_yaml.py

+81
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,81 @@
1+
#!/usr/bin/env python
2+
import roslib; roslib.load_manifest('tutorial_atlas_control')
3+
import rospy, yaml, sys
4+
from osrf_msgs.msg import JointCommands
5+
from sensor_msgs.msg import JointState
6+
from numpy import zeros, array, linspace
7+
from math import ceil
8+
9+
atlasJointNames = [
10+
'atlas::back_lbz', 'atlas::back_mby', 'atlas::back_ubx', 'atlas::neck_ay',
11+
'atlas::l_leg_uhz', 'atlas::l_leg_mhx', 'atlas::l_leg_lhy', 'atlas::l_leg_kny', 'atlas::l_leg_uay', 'atlas::l_leg_lax',
12+
'atlas::r_leg_uhz', 'atlas::r_leg_mhx', 'atlas::r_leg_lhy', 'atlas::r_leg_kny', 'atlas::r_leg_uay', 'atlas::r_leg_lax',
13+
'atlas::l_arm_usy', 'atlas::l_arm_shx', 'atlas::l_arm_ely', 'atlas::l_arm_elx', 'atlas::l_arm_uwy', 'atlas::l_arm_mwx',
14+
'atlas::r_arm_usy', 'atlas::r_arm_shx', 'atlas::r_arm_ely', 'atlas::r_arm_elx', 'atlas::r_arm_uwy', 'atlas::r_arm_mwx']
15+
16+
currentJointState = JointState()
17+
def jointStatesCallback(msg):
18+
global currentJointState
19+
currentJointState = msg
20+
21+
if __name__ == '__main__':
22+
# first make sure the input arguments are correct
23+
if len(sys.argv) != 3:
24+
print "usage: traj_yaml.py YAML_FILE TRAJECTORY_NAME"
25+
print " where TRAJECTORY is a dictionary defined in YAML_FILE"
26+
sys.exit(1)
27+
traj_yaml = yaml.load(file(sys.argv[1], 'r'))
28+
traj_name = sys.argv[2]
29+
if not traj_name in traj_yaml:
30+
print "unable to find trajectory %s in %s" % (traj_name, sys.argv[1])
31+
sys.exit(1)
32+
traj_len = len(traj_yaml[traj_name])
33+
34+
# Setup subscriber to atlas states
35+
rospy.Subscriber("/atlas/joint_states", JointState, jointStatesCallback)
36+
37+
# initialize JointCommands message
38+
command = JointCommands()
39+
command.name = list(atlasJointNames)
40+
n = len(command.name)
41+
command.position = zeros(n)
42+
command.velocity = zeros(n)
43+
command.effort = zeros(n)
44+
command.kp_position = zeros(n)
45+
command.ki_position = zeros(n)
46+
command.kd_position = zeros(n)
47+
command.kp_velocity = zeros(n)
48+
command.i_effort_min = zeros(n)
49+
command.i_effort_max = zeros(n)
50+
51+
# now get gains from parameter server
52+
rospy.init_node('tutorial_atlas_control')
53+
for i in xrange(len(command.name)):
54+
name = command.name[i]
55+
command.kp_position[i] = rospy.get_param('atlas_controller/gains/' + name[7::] + '/p')
56+
command.ki_position[i] = rospy.get_param('atlas_controller/gains/' + name[7::] + '/i')
57+
command.kd_position[i] = rospy.get_param('atlas_controller/gains/' + name[7::] + '/d')
58+
command.i_effort_max[i] = rospy.get_param('atlas_controller/gains/' + name[7::] + '/i_clamp')
59+
command.i_effort_min[i] = -command.i_effort_max[i]
60+
61+
# set up the publisher
62+
pub = rospy.Publisher('/atlas/joint_commands', JointCommands)
63+
64+
# for each trajectory
65+
for i in xrange(0, traj_len):
66+
# get initial joint positions
67+
initialPosition = array(currentJointState.position)
68+
# get joint commands from yaml
69+
y = traj_yaml[traj_name][i]
70+
# first value is time duration
71+
dt = float(y[0])
72+
# subsequent values are desired joint positions
73+
commandPosition = array([ float(x) for x in y[1].split() ])
74+
# desired publish interval
75+
dtPublish = 0.02
76+
n = ceil(dt / dtPublish)
77+
for ratio in linspace(0, 1, n):
78+
interpCommand = (1-ratio)*initialPosition + ratio * commandPosition
79+
command.position = [ float(x) for x in interpCommand ]
80+
pub.publish(command)
81+
rospy.sleep(dt / float(n))

‎drcsim_ros_python/tutorial.md

+119
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,119 @@
1+
# Overview
2+
3+
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.
4+
5+
We're using the ROS topics demonstrated in the [previous tutorial that used C++](http://gazebosim.org/tutorials/?tut=drcsim_ros_cmds). 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.
6+
7+
**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.
8+
9+
## Install DRC Simulator
10+
11+
[Click to see the instructions](http://gazebosim.org/tutorials/?tut=drcsim_install) for installing the DRC simulator and associated utilities.
12+
13+
## Create a new ROS package
14+
15+
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
16+
17+
~~~
18+
mkdir ~/ros
19+
echo "export ROS_PACKAGE_PATH=~/ros:\$ROS_PACKAGE_PATH" >> ~/.bashrc
20+
source ~/.bashrc
21+
~~~
22+
23+
Use [roscreate-pkg](http://ros.org/wiki/roscreate) to create a ROS package for this tutorial, depending on a ROS package called `osrf_msgs`:
24+
25+
~~~
26+
cd ~/ros
27+
roscreate-pkg tutorial_atlas_control osrf_msgs
28+
~~~
29+
30+
## The Code
31+
32+
Move to the directory `tutorial_atlas_control`
33+
34+
~~~
35+
roscd tutorial_atlas_control
36+
~~~
37+
38+
Copy the file [`traj_yaml.py`](http://bitbucket.org/osrf/gazebo_tutorials/raw/default/drcsim_ros_python/files/traj_yaml.py) into it:
39+
40+
<include from='/#include/' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/drcsim_ros_python/files/traj_yaml.py' />
41+
42+
Then download the [Traj_data2.yaml](http://bitbucket.org/osrf/gazebo_tutorials/raw/default/drcsim_ros_python/files/Traj_data2.yaml) YAML file of a few trajectories, and place it in the same directory (`~/ros/tutorial_atlas_control`).
43+
44+
## Trying it out
45+
46+
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
47+
48+
~~~
49+
roslaunch drcsim_gazebo atlas.launch
50+
~~~
51+
52+
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.
53+
54+
In a separate terminal, put Atlas in User mode:
55+
56+
~~~
57+
rostopic pub /atlas/control_mode std_msgs/String -- "User"
58+
~~~
59+
60+
In a separate terminal, run the node that you just built:
61+
62+
~~~
63+
roscd tutorial_atlas_control
64+
python traj_yaml.py Traj_data2.yaml step_and_fall
65+
~~~
66+
67+
You should see the robot try to take a step with its right leg and then fall down.
68+
69+
Here is another trajectory, since it's football season:
70+
71+
~~~
72+
python traj_yaml.py Traj_data2.yaml touchdown
73+
~~~
74+
75+
Then, just for fun:
76+
77+
~~~
78+
python traj_yaml.py Traj_data2.yaml touchdown_exhausted
79+
~~~
80+
81+
### Restarting
82+
83+
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.
84+
85+
## The Code Explained
86+
87+
<include to='/import ceil/' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/drcsim_ros_python/files/traj_yaml.py' />
88+
89+
Specify the names of the joints in the correct order.
90+
91+
<include from='/atlasJointNames/' to='/'atlas::r_arm_mwx'\]/' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/drcsim_ros_python/files/traj_yaml.py' />
92+
93+
Create a ROS callback for reading the JointState message published on /atlas/joint_states
94+
95+
<include from='/currentJointState/' to='/currentJointState = msg/' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/drcsim_ros_python/files/traj_yaml.py' />
96+
97+
Import the files that we need.
98+
99+
<include from='/if __name/' to='/traj_name\]\)/' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/drcsim_ros_python/files/traj_yaml.py' />
100+
101+
Set up the joint states subscriber.
102+
103+
<include from='/ # Setup subscriber/' to='/, jointStatesCallback\)/' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/drcsim_ros_python/files/traj_yaml.py' />
104+
105+
Initialize the joint command message.
106+
107+
<include from='/ # initialize JointCommands/' to='/command.i_effort_max = zeros\(n\)/' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/drcsim_ros_python/files/traj_yaml.py' />
108+
109+
Get the current controller gains from the parameter server.
110+
111+
<include from='/ # now get gains/' to='/-command.i_effort_max\[i\]/' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/drcsim_ros_python/files/traj_yaml.py' />
112+
113+
Set up the joint command publisher.
114+
115+
<include from='/ # set up the publisher/' to='/, JointCommands\)/' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/drcsim_ros_python/files/traj_yaml.py' />
116+
117+
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.
118+
119+
<include from='/ # for each trajectory/' to='/dt \/ float\(n\)\)/' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/drcsim_ros_python/files/traj_yaml.py' />

‎manifest.xml

+7
Original file line numberDiff line numberDiff line change
@@ -379,6 +379,12 @@
379379
<description>How to setup synchronized controller updates over ROS topics by using the built-in synchronization mechanism within Atlas simulation interface AtlasPlugin</description>
380380
<skill>advance</skill>
381381
</tutorial>
382+
383+
<tutorial title="Atlas control over ROS with python" ref='drcsim_ros_python'>
384+
<markdown version="1.9+">drcsim_ros_python/tutorial.md</markdown>
385+
<description>How to control the Atlas robot with a python joint trajectory controller.</description>
386+
<skill>intermediate</skill>
387+
</tutorial>
382388
</tutorials>
383389

384390
<categories>
@@ -527,6 +533,7 @@
527533
<tutorial>drcsim_switch_modes</tutorial>
528534
<tutorial>drcsim_atlas_siminterface</tutorial>
529535
<tutorial>drcsim_control_sync</tutorial>
536+
<tutorial>drcsim_ros_python</tutorial>
530537
</tutorials>
531538
</category>
532539

0 commit comments

Comments
 (0)