Skip to content

Commit ccfbf93

Browse files
caguerocaguero
committed
Merged in drcsim_ros_cmds (pull request #51)
Adding controlling atlas through ROS
2 parents a31789a + c4b2e19 commit ccfbf93

File tree

4 files changed

+383
-0
lines changed

4 files changed

+383
-0
lines changed

‎drcsim_ros_cmds/files/CMakeLists.txt

+35
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
cmake_minimum_required(VERSION 2.4.6)
2+
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
3+
4+
# Set the build type. Options are:
5+
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
6+
# Debug : w/ debug symbols, w/o optimization
7+
# Release : w/o debug symbols, w/ optimization
8+
# RelWithDebInfo : w/ debug symbols, w/ optimization
9+
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
10+
#set(ROS_BUILD_TYPE RelWithDebInfo)
11+
12+
rosbuild_init()
13+
14+
#set the default path for built executables to the "bin" directory
15+
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
16+
#set the default path for built libraries to the "lib" directory
17+
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
18+
19+
#find gazebo include (FindPkgConfig)
20+
include (FindPkgConfig)
21+
if (PKG_CONFIG_FOUND)
22+
pkg_check_modules(GAZEBO gazebo)
23+
else()
24+
message(FATAL_ERROR "pkg-config is required; please install it")
25+
endif()
26+
27+
# depends on DRCVehiclePlugin
28+
include_directories(
29+
${GAZEBO_INCLUDE_DIRS}
30+
)
31+
link_directories(
32+
${GAZEBO_LIBRARY_DIRS}
33+
)
34+
35+
rosbuild_add_executable(publish_joint_commands src/publish_joint_commands.cpp)

‎drcsim_ros_cmds/files/publish_joint_commands.cpp

+152
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,152 @@
1+
/*
2+
* Copyright 2012 Open Source Robotics Foundation
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*
16+
*/
17+
#include <math.h>
18+
#include <ros/ros.h>
19+
#include <ros/subscribe_options.h>
20+
#include <boost/thread.hpp>
21+
#include <boost/algorithm/string.hpp>
22+
#include <sensor_msgs/JointState.h>
23+
#include <osrf_msgs/JointCommands.h>
24+
25+
ros::Publisher pub_joint_commands_;
26+
osrf_msgs::JointCommands jointcommands;
27+
28+
void SetJointStates(const sensor_msgs::JointState::ConstPtr &_js)
29+
{
30+
static ros::Time startTime = ros::Time::now();
31+
{
32+
// for testing round trip time
33+
jointcommands.header.stamp = _js->header.stamp;
34+
35+
// assign sinusoidal joint angle targets
36+
for (unsigned int i = 0; i < jointcommands.name.size(); i++)
37+
jointcommands.position[i] =
38+
3.2* sin((ros::Time::now() - startTime).toSec());
39+
40+
pub_joint_commands_.publish(jointcommands);
41+
}
42+
}
43+
44+
int main(int argc, char** argv)
45+
{
46+
ros::init(argc, argv, "pub_joint_command_test");
47+
48+
ros::NodeHandle* rosnode = new ros::NodeHandle();
49+
50+
// Waits for simulation time update.
51+
ros::Time last_ros_time_;
52+
bool wait = true;
53+
while (wait)
54+
{
55+
last_ros_time_ = ros::Time::now();
56+
if (last_ros_time_.toSec() > 0)
57+
wait = false;
58+
}
59+
60+
// must match those inside AtlasPlugin
61+
jointcommands.name.push_back("atlas::back_lbz");
62+
jointcommands.name.push_back("atlas::back_mby");
63+
jointcommands.name.push_back("atlas::back_ubx");
64+
jointcommands.name.push_back("atlas::neck_ay");
65+
jointcommands.name.push_back("atlas::l_leg_uhz");
66+
jointcommands.name.push_back("atlas::l_leg_mhx");
67+
jointcommands.name.push_back("atlas::l_leg_lhy");
68+
jointcommands.name.push_back("atlas::l_leg_kny");
69+
jointcommands.name.push_back("atlas::l_leg_uay");
70+
jointcommands.name.push_back("atlas::l_leg_lax");
71+
jointcommands.name.push_back("atlas::r_leg_uhz");
72+
jointcommands.name.push_back("atlas::r_leg_mhx");
73+
jointcommands.name.push_back("atlas::r_leg_lhy");
74+
jointcommands.name.push_back("atlas::r_leg_kny");
75+
jointcommands.name.push_back("atlas::r_leg_uay");
76+
jointcommands.name.push_back("atlas::r_leg_lax");
77+
jointcommands.name.push_back("atlas::l_arm_usy");
78+
jointcommands.name.push_back("atlas::l_arm_shx");
79+
jointcommands.name.push_back("atlas::l_arm_ely");
80+
jointcommands.name.push_back("atlas::l_arm_elx");
81+
jointcommands.name.push_back("atlas::l_arm_uwy");
82+
jointcommands.name.push_back("atlas::l_arm_mwx");
83+
jointcommands.name.push_back("atlas::r_arm_usy");
84+
jointcommands.name.push_back("atlas::r_arm_shx");
85+
jointcommands.name.push_back("atlas::r_arm_ely");
86+
jointcommands.name.push_back("atlas::r_arm_elx");
87+
jointcommands.name.push_back("atlas::r_arm_uwy");
88+
jointcommands.name.push_back("atlas::r_arm_mwx");
89+
90+
unsigned int n = jointcommands.name.size();
91+
jointcommands.position.resize(n);
92+
jointcommands.velocity.resize(n);
93+
jointcommands.effort.resize(n);
94+
jointcommands.kp_position.resize(n);
95+
jointcommands.ki_position.resize(n);
96+
jointcommands.kd_position.resize(n);
97+
jointcommands.kp_velocity.resize(n);
98+
jointcommands.i_effort_min.resize(n);
99+
jointcommands.i_effort_max.resize(n);
100+
101+
for (unsigned int i = 0; i < n; i++)
102+
{
103+
std::vector<std::string> pieces;
104+
boost::split(pieces, jointcommands.name[i], boost::is_any_of(":"));
105+
106+
rosnode->getParam("atlas_controller/gains/" + pieces[2] + "/p",
107+
jointcommands.kp_position[i]);
108+
109+
rosnode->getParam("atlas_controller/gains/" + pieces[2] + "/i",
110+
jointcommands.ki_position[i]);
111+
112+
rosnode->getParam("atlas_controller/gains/" + pieces[2] + "/d",
113+
jointcommands.kd_position[i]);
114+
115+
rosnode->getParam("atlas_controller/gains/" + pieces[2] + "/i_clamp",
116+
jointcommands.i_effort_min[i]);
117+
jointcommands.i_effort_min[i] = -jointcommands.i_effort_min[i];
118+
119+
rosnode->getParam("atlas_controller/gains/" + pieces[2] + "/i_clamp",
120+
jointcommands.i_effort_max[i]);
121+
122+
jointcommands.velocity[i] = 0;
123+
jointcommands.effort[i] = 0;
124+
jointcommands.kp_velocity[i] = 0;
125+
}
126+
127+
// ros topic subscriptions
128+
ros::SubscribeOptions jointStatesSo =
129+
ros::SubscribeOptions::create<sensor_msgs::JointState>(
130+
"/atlas/joint_states", 1, SetJointStates,
131+
ros::VoidPtr(), rosnode->getCallbackQueue());
132+
133+
// Because TCP causes bursty communication with high jitter,
134+
// declare a preference on UDP connections for receiving
135+
// joint states, which we want to get at a high rate.
136+
// Note that we'll still accept TCP connections for this topic
137+
// (e.g., from rospy nodes, which don't support UDP);
138+
// we just prefer UDP.
139+
jointStatesSo.transport_hints = ros::TransportHints().unreliable();
140+
141+
ros::Subscriber subJointStates = rosnode->subscribe(jointStatesSo);
142+
// ros::Subscriber subJointStates =
143+
// rosnode->subscribe("/atlas/joint_states", 1000, SetJointStates);
144+
145+
pub_joint_commands_ =
146+
rosnode->advertise<osrf_msgs::JointCommands>(
147+
"/atlas/joint_commands", 1, true);
148+
149+
ros::spin();
150+
151+
return 0;
152+
}

‎drcsim_ros_cmds/tutorial.md

+189
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,189 @@
1+
# Overview
2+
3+
In this tutorial, we'll send target joint positions to a robot in simulation through the use of a simple joint position command ROS topic publisher.
4+
5+
The [DRCSim User Guide](https://bitbucket.org/osrf/drcsim/wiki/DRC/UserGuide) provides specifications of basic controller API over [ROS topics](http://www.ros.org/wiki/Topics).
6+
7+
In particular, we will make use of two ROS topics in this tutorial:
8+
9+
- `/atlas/joint_states` published by the robot, and
10+
- `/atlas/joint_commands` subscribed by the robot plugin.
11+
12+
## Setup
13+
14+
We assume that you've already done the [installation step](http://gazebosim.org/tutorials/?tut=drcsim_install).
15+
16+
If you haven't done so, make sure to source the environment setup.sh files with every new terminal you open:
17+
18+
~~~
19+
source /usr/share/drcsim/setup.sh
20+
~~~
21+
22+
To save on typing, you can add this script to your `.bashrc` files, so it's automatically sourced every time you start a new terminal.
23+
24+
~~~
25+
echo 'source /usr/share/drcsim/setup.sh' >> ~/.bashrc
26+
source ~/.bashrc
27+
~~~
28+
29+
But remember to remove them from your `.bashrc` file when they are not needed any more.
30+
31+
### Create a ROS Package Workspace
32+
33+
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
34+
35+
~~~
36+
mkdir ~/ros
37+
export ROS_PACKAGE_PATH=${HOME}/ros:${ROS_PACKAGE_PATH}
38+
~~~
39+
40+
Use [roscreate-pkg](http://ros.org/wiki/roscreate) to create a ROS package for this tutorial, depending on `roscpp`, `sensor_msgs` and `osrf_msgs`:
41+
42+
~~~
43+
cd ~/ros
44+
roscreate-pkg drcsim_joint_commands_tutorial roscpp trajectory_msgs osrf_msgs
45+
~~~
46+
47+
### Create a ROS Node
48+
Copy and paste the following code as file [`~/ros/drcsim_joint_commands_tutorial/src/publish_joint_commands.cpp`](http://bitbucket.org/osrf/gazebo_tutorials/raw/default/drcsim_ros_cmds/files/publish_joint_commands.cc) with any text editor (e.g. gedit, vi, emacs):
49+
50+
<include src='http://bitbucket.org/osrf/gazebo_tutorials/raw/drcsim_ros_cmds/files/publish_joint_commands.cpp' />
51+
52+
### Compiling the ROS Node
53+
54+
Edit [`~/ros/drcsim_joint_commands_tutorial/CMakeLists.txt`](http://bitbucket.org/osrf/gazebo_tutorials/raw/default/drcsim_ros_cmds/files/CMakeLists.txt) so that it looks like below:
55+
56+
<include src='http://bitbucket.org/osrf/gazebo_tutorials/raw/drcsim_ros_cmds/files/CMakeLists.txt' />
57+
58+
59+
To compile, type below in a terminal:
60+
61+
~~~
62+
roscd drcsim_joint_commands_tutorial
63+
make
64+
~~~
65+
66+
If you have gazebo installed in a non-standard location (such as a local install), you must set the PKG_CONFIG_PATH appropriately. For example, use the following if you have installed into ~/local:
67+
68+
~~~
69+
roscd drcsim_joint_commands_tutorial
70+
PKG_CONFIG_PATH=~/local/lib/pkgconfig make
71+
~~~
72+
73+
74+
## The code explained
75+
76+
### Headers and Global Variable Declarations
77+
Below contains file license, various system and library dependency includes and
78+
a couple of global variables.
79+
80+
<include to='/JointCommands jointcommands;/' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/drcsim_ros_cmds/files/publish_joint_commands.cpp' />
81+
82+
83+
### ROS Topic Callback Function
84+
85+
`SetJointStates` is a callback function for ROS topic `/atlas/joint_states`.
86+
When a `JointState` message is received, following code section copies the
87+
header time stamp from the `JointState` message into `JointCommands` message
88+
for the purpose of measuring the age of the `JointCommands` message.
89+
This callback then populates target joint positions with some arbitrarily chosen values for purpose of demo, and publishes them over ROS topic `/atlas/joint_commands`.
90+
91+
<include from='/void SetJointStates/' to='/publish\(jointcommands\);\n \}\n}/' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/drcsim_ros_cmds/files/publish_joint_commands.cpp' />
92+
93+
94+
### Main Subroutine
95+
Initialize ros and creates a `ros::NodeHandle`.
96+
97+
<include from='/int main/' to='/= new ros::NodeHandle\(\);/' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/drcsim_ros_cmds/files/publish_joint_commands.cpp' />
98+
99+
100+
### Waits for Simulation Time Update
101+
Make sure simulation time has propagated to this node before running the rest of this demo code:
102+
103+
<include from='/ // Waits/' to='\false;\n }\' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/drcsim_ros_cmds/files/publish_joint_commands.cpp' />
104+
105+
106+
### Hardcoded List of Joint Names
107+
108+
List of joint names in the Atlas robot. Note the order must not change for this function to work correctly.
109+
110+
<include from='/ // must/' to='/r_arm_mwx"\);/' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/drcsim_ros_cmds/files/publish_joint_commands.cpp' />
111+
112+
113+
### Size JointCommands Variables
114+
115+
Resize `JointCommands` based on the size of joint names list.
116+
117+
<include from='/ unsigned int n/' to='i_effort_max.resize\(n\);/' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/drcsim_ros_cmds/files/publish_joint_commands.cpp' />
118+
119+
120+
### Fill in JointCommands Gains and Target Values
121+
122+
Retrieve JointCommands gains from ROS parameter server.
123+
Set target velocities and efforts to zero.
124+
125+
<include from='/for \(unsigned int i = 0; i < n/' to='/jointcommands.kp_velocity\[i\] = 0;\n }/' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/drcsim_ros_cmds/files/publish_joint_commands.cpp' />
126+
127+
### Subscribe to `/atlas/joint_states` Message
128+
129+
Subscribe to the JointState message, but also set the subscriber option to use
130+
unreliable transport (i.e. UDP connection).
131+
132+
<include from='/ // ros topic subscriptions/' to='/1000, SetJointStates\);/' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/drcsim_ros_cmds/files/publish_joint_commands.cpp' />
133+
134+
### Setup Publisher
135+
136+
Initialize JointCommands publisher.
137+
138+
<include from='/pub_joint_commands_ =/' to='/, 1, true\);/' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/drcsim_ros_cmds/files/publish_joint_commands.cpp' />
139+
140+
### Call `ros::spin()`
141+
142+
to process messages in the ROS callback queue
143+
144+
<include from='/ros::spin\(\);/' to='/return 0;\n }/' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/drcsim_ros_cmds/files/publish_joint_commands.cpp' />
145+
146+
## Running the Simulation
147+
148+
1. In terminal, source the DRC simulator setup script and start the DRC robot simulation:
149+
150+
~~~
151+
roslaunch drcsim_gazebo atlas_sandia_hands.launch
152+
~~~
153+
154+
1. In a separate terminal, put Atlas in User mode:
155+
156+
~~~
157+
rostopic pub /atlas/control_mode std_msgs/String -- "User"
158+
~~~
159+
160+
1. In a separate terminal, run the node constructed above:
161+
162+
~~~
163+
export ROS_PACKAGE_PATH=${HOME}/ros:${ROS_PACKAGE_PATH}
164+
rosrun drcsim_joint_commands_tutorial publish_joint_commands
165+
~~~
166+
167+
## Altering the code
168+
169+
The reference trajectories for each joint position is a set of sinusoids. Since we know the exact function for the positions, we can compute the desired velocities by differentiation and supply this to the controller. This will improve the smoothness of the controller.
170+
171+
To add a reference velocity, alter the for loop in the SetJointStates function to match the following **(Note this only works if kp_velocity is non-zero)**:
172+
173+
~~~
174+
// assign sinusoidal joint angle and velocity targets
175+
for (unsigned int i = 0; i < jointcommands.name.size(); i++)
176+
{
177+
jointcommands.position[i] =
178+
3.2* sin((ros::Time::now() - startTime).toSec());
179+
jointcommands.velocity[i] =
180+
3.2* cos((ros::Time::now() - startTime).toSec());
181+
}
182+
~~~
183+
184+
Then rebuild and re-run the node:
185+
186+
~~~
187+
make
188+
rosrun drcsim_joint_commands_tutorial publish_joint_commands
189+
~~~

‎manifest.xml

+7
Original file line numberDiff line numberDiff line change
@@ -384,6 +384,12 @@
384384
<markdown version="1.9+">drcsim_ros_python/tutorial.md</markdown>
385385
<description>How to control the Atlas robot with a python joint trajectory controller.</description>
386386
<skill>intermediate</skill>
387+
</tutorial>
388+
389+
<tutorial title="Sending joint commands with ROS" ref='drcsim_ros_cmds'>
390+
<markdown version="1.9+">drcsim_ros_cmds/tutorial.md</markdown>
391+
<description>How to send joint positions to a robot in simulation through the use of a simple joint position command ROS topic publisher.</description>
392+
<skill>intermediate</skill>
387393
</tutorial>
388394
</tutorials>
389395

@@ -534,6 +540,7 @@
534540
<tutorial>drcsim_atlas_siminterface</tutorial>
535541
<tutorial>drcsim_control_sync</tutorial>
536542
<tutorial>drcsim_ros_python</tutorial>
543+
<tutorial>drcsim_ros_cmds</tutorial>
537544
</tutorials>
538545
</category>
539546

0 commit comments

Comments
 (0)