Tutorials/drcsim/2.2/sending joint controller commands over ros


 * 1) DRC Tutorial: Sending Joint Position Commands to Atlas Joint Controller over ROS Topic

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.

The DRCSim User Guide provides specifications of basic controller API over ROS topics.

In particular, we will make use of two ROS topics in this tutorial:

- `/atlas/joint_states` published by the robot, and - `/atlas/joint_commands` subscribed by the robot plugin.


 * 1) Setup

We assume that you've already done the installation step.

If you haven't done so, make sure to source the environment setup.sh files with every new terminal you open:

source /usr/share/drcsim/setup.sh

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.

echo 'source /usr/share/drcsim/setup.sh' >> ~/.bashrc source ~/.bashrc

But remember to remove them from your `.bashrc` file when they are not needed any more.


 * 1) Create a ROS Package Workspace

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 export ROS_PACKAGE_PATH=${HOME}/ros:${ROS_PACKAGE_PATH}

Use roscreate-pkg to create a ROS package for this tutorial, depending on `roscpp`, `sensor_msgs` and `osrf_msgs`:

cd ~/ros roscreate-pkg drcsim_joint_commands_tutorial roscpp trajectory_msgs osrf_msgs

Copy and paste the following code as file `~/ros/drcsim_joint_commands_tutorial/src/publish_joint_commands.cpp` with any text editor (e.g. gedit, vi, emac):
 * 1) Create a ROS Node

/* * Copyright 2012 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * *    http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. *
 * 1) include 
 * 2) include 
 * 3) include 
 * 4) include 
 * 5) include 
 * 6) include 
 * 7) include 
 * 1) include 

ros::Publisher pub_joint_commands_; osrf_msgs::JointCommands jointcommands;

void SetJointStates(const sensor_msgs::JointState::ConstPtr &_js) { static ros::Time startTime = ros::Time::now; {   // for testing round trip time jointcommands.header.stamp = _js->header.stamp;

// assign sinusoidal joint angle targets for (unsigned int i = 0; i < jointcommands.name.size; i++) jointcommands.position[i] = 3.2* sin((ros::Time::now - startTime).toSec);

pub_joint_commands_.publish(jointcommands); } }

int main(int argc, char** argv) { ros::init(argc, argv, "pub_joint_command_test");

ros::NodeHandle* rosnode = new ros::NodeHandle;

ros::Time last_ros_time_; bool wait = true; while (wait) {   last_ros_time_ = ros::Time::now; if (last_ros_time_.toSec > 0) wait = false; }

// must match those inside AtlasPlugin jointcommands.name.push_back("atlas::back_lbz"); jointcommands.name.push_back("atlas::back_mby"); jointcommands.name.push_back("atlas::back_ubx"); jointcommands.name.push_back("atlas::neck_ay"); jointcommands.name.push_back("atlas::l_leg_uhz"); jointcommands.name.push_back("atlas::l_leg_mhx"); jointcommands.name.push_back("atlas::l_leg_lhy"); jointcommands.name.push_back("atlas::l_leg_kny"); jointcommands.name.push_back("atlas::l_leg_uay"); jointcommands.name.push_back("atlas::l_leg_lax"); jointcommands.name.push_back("atlas::r_leg_uhz"); jointcommands.name.push_back("atlas::r_leg_mhx"); jointcommands.name.push_back("atlas::r_leg_lhy"); jointcommands.name.push_back("atlas::r_leg_kny"); jointcommands.name.push_back("atlas::r_leg_uay"); jointcommands.name.push_back("atlas::r_leg_lax"); jointcommands.name.push_back("atlas::l_arm_usy"); jointcommands.name.push_back("atlas::l_arm_shx"); jointcommands.name.push_back("atlas::l_arm_ely"); jointcommands.name.push_back("atlas::l_arm_elx"); jointcommands.name.push_back("atlas::l_arm_uwy"); jointcommands.name.push_back("atlas::l_arm_mwx"); jointcommands.name.push_back("atlas::r_arm_usy"); jointcommands.name.push_back("atlas::r_arm_shx"); jointcommands.name.push_back("atlas::r_arm_ely"); jointcommands.name.push_back("atlas::r_arm_elx"); jointcommands.name.push_back("atlas::r_arm_uwy"); jointcommands.name.push_back("atlas::r_arm_mwx");

unsigned int n = jointcommands.name.size; jointcommands.position.resize(n); jointcommands.velocity.resize(n); jointcommands.effort.resize(n); jointcommands.kp_position.resize(n); jointcommands.ki_position.resize(n); jointcommands.kd_position.resize(n); jointcommands.kp_velocity.resize(n); jointcommands.i_effort_min.resize(n); jointcommands.i_effort_max.resize(n);

for (unsigned int i = 0; i < n; i++) {   std::vector pieces; boost::split(pieces, jointcommands.name[i], boost::is_any_of(":"));

rosnode->getParam("atlas_controller/gains/" + pieces[2] + "/p",     jointcommands.kp_position[i]);

rosnode->getParam("atlas_controller/gains/" + pieces[2] + "/i",     jointcommands.ki_position[i]);

rosnode->getParam("atlas_controller/gains/" + pieces[2] + "/d",     jointcommands.kd_position[i]);

rosnode->getParam("atlas_controller/gains/" + pieces[2] + "/i_clamp",     jointcommands.i_effort_min[i]); jointcommands.i_effort_min[i] = -jointcommands.i_effort_min[i];

rosnode->getParam("atlas_controller/gains/" + pieces[2] + "/i_clamp",     jointcommands.i_effort_max[i]); jointcommands.velocity[i]    = 0; jointcommands.effort[i]      = 0; jointcommands.kp_velocity[i] = 0; }

// ros topic subscribtions ros::SubscribeOptions jointStatesSo = ros::SubscribeOptions::create(   "/atlas/joint_states", 1, SetJointStates,    ros::VoidPtr, rosnode->getCallbackQueue);

// Because TCP causes bursty communication with high jitter, // declare a preference on UDP connections for receiving // joint states, which we want to get at a high rate. // Note that we'll still accept TCP connections for this topic // (e.g., from rospy nodes, which don't support UDP); // we just prefer UDP. jointStatesSo.transport_hints = ros::TransportHints.unreliable;

ros::Subscriber subJointStates = rosnode->subscribe(jointStatesSo); // ros::Subscriber subJointStates = //  rosnode->subscribe("/atlas/joint_states", 1000, SetJointStates);

pub_joint_commands_ = rosnode->advertise(   "/atlas/joint_commands", 1, true);

ros::spin;

return 0; }


 * 1) Compiling the ROS Node

Edit `~/ros/drcsim_joint_commands_tutorial/CMakeLists.txt` so that it looks like below:

cmake_minimum_required(VERSION 2.4.6) include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)


 * 1) Set the build type.  Options are:
 * 2)  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
 * 3)  Debug          : w/ debug symbols, w/o optimization
 * 4)  Release        : w/o debug symbols, w/ optimization
 * 5)  RelWithDebInfo : w/ debug symbols, w/ optimization
 * 6)  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
 * 7) set(ROS_BUILD_TYPE RelWithDebInfo)

rosbuild_init

set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 * 1) set the default path for built executables to the "bin" directory
 * 1) set the default path for built libraries to the "lib" directory

include (FindPkgConfig) if (PKG_CONFIG_FOUND) pkg_check_modules(GAZEBO gazebo) else message(FATAL_ERROR "pkg-config is required; please install it") endif
 * 1) find gazebo include (FindPkgConfig)

include_directories( ${GAZEBO_INCLUDE_DIRS} ) link_directories( ${GAZEBO_LIBRARY_DIRS} )
 * 1) depends on DRCVehiclePlugin

rosbuild_add_executable(publish_joint_commands src/publish_joint_commands.cpp)

To compile, type below in a terminal:

roscd drcsim_joint_commands_tutorial make

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:

roscd drcsim_joint_commands_tutorial PKG_CONFIG_PATH=~/local/lib/pkgconfig make


 * 1) The code explained

Below contains file license, various system and library dependency includes and a couple of global variables.
 * 1) Headers and Global Variable Declarations

/* * Copyright 2012 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * *    http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. *
 * 1) include 
 * 2) include 
 * 3) include 
 * 4) include 
 * 5) include 
 * 6) include <gazebo/common/Time.hh>
 * 7) include <sensor_msgs/JointState.h>
 * 8) include <osrf_msgs/JointCommands.h>
 * 1) include <osrf_msgs/JointCommands.h>

ros::Publisher pub_joint_commands_; osrf_msgs::JointCommands jointcommands;

`SetJointStates` is a callback function for ROS topic `/atlas/joint_states`. When a `JointState` message is received, following code section copies the header time stamp from the `JointState` message into `JointCommands` message for the purpose of measuring the age of the `JointCommands` message. 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`.
 * 1) ROS Topic Callback Function

void SetJointStates(const sensor_msgs::JointState::ConstPtr &_js) { static ros::Time startTime = ros::Time::now; {   // for testing round trip time jointcommands.header.stamp = _js->header.stamp;

// assign sinusoidal joint angle targets for (unsigned int i = 0; i < jointcommands.name.size; i++) jointcommands.position[i] = 3.2* sin((ros::Time::now - startTime).toSec);

pub_joint_commands_.publish(jointcommands); } }

Initialize ros and creates a `ros::NodeHandle`.
 * 1) Main Subroutine

int main(int argc, char** argv) { ros::init(argc, argv, "pub_joint_command_test");

ros::NodeHandle* rosnode = new ros::NodeHandle;

Make sure simulation time has propagated to this node before running the rest of this demo code:
 * 1) Waits for Simulation Time Update

ros::Time last_ros_time_; bool wait = true; while (wait) {   last_ros_time_ = ros::Time::now; if (last_ros_time_.toSec > 0) wait = false; }

List of joint names in the Atlas robot. Note the order must not change for this function to work correctly.
 * 1) Hardcoded List of Joint Names

// must match those inside AtlasPlugin jointcommands.name.push_back("atlas::back_lbz"); jointcommands.name.push_back("atlas::back_mby"); jointcommands.name.push_back("atlas::back_ubx"); jointcommands.name.push_back("atlas::neck_ay"); jointcommands.name.push_back("atlas::l_leg_uhz"); jointcommands.name.push_back("atlas::l_leg_mhx"); jointcommands.name.push_back("atlas::l_leg_lhy"); jointcommands.name.push_back("atlas::l_leg_kny"); jointcommands.name.push_back("atlas::l_leg_uay"); jointcommands.name.push_back("atlas::l_leg_lax"); jointcommands.name.push_back("atlas::r_leg_uhz"); jointcommands.name.push_back("atlas::r_leg_mhx"); jointcommands.name.push_back("atlas::r_leg_lhy"); jointcommands.name.push_back("atlas::r_leg_kny"); jointcommands.name.push_back("atlas::r_leg_uay"); jointcommands.name.push_back("atlas::r_leg_lax"); jointcommands.name.push_back("atlas::l_arm_usy"); jointcommands.name.push_back("atlas::l_arm_shx"); jointcommands.name.push_back("atlas::l_arm_ely"); jointcommands.name.push_back("atlas::l_arm_elx"); jointcommands.name.push_back("atlas::l_arm_uwy"); jointcommands.name.push_back("atlas::l_arm_mwx"); jointcommands.name.push_back("atlas::r_arm_usy"); jointcommands.name.push_back("atlas::r_arm_shx"); jointcommands.name.push_back("atlas::r_arm_ely"); jointcommands.name.push_back("atlas::r_arm_elx"); jointcommands.name.push_back("atlas::r_arm_uwy"); jointcommands.name.push_back("atlas::r_arm_mwx");

Resize `JointCommands` based on the size of joint names list.
 * 1) Size JointCommands Variables

unsigned int n = jointcommands.name.size; jointcommands.position.resize(n); jointcommands.velocity.resize(n); jointcommands.effort.resize(n); jointcommands.kp_position.resize(n); jointcommands.ki_position.resize(n); jointcommands.kd_position.resize(n); jointcommands.kp_velocity.resize(n); jointcommands.i_effort_min.resize(n); jointcommands.i_effort_max.resize(n);

Retrieve JointCommands gains from ROS parameter server. Set target velocities and efforts to zero.
 * 1) Fill in JointCommands Gains and Target Values

for (unsigned int i = 0; i < n; i++) {   std::vector<std::string> pieces; boost::split(pieces, jointcommands.name[i], boost::is_any_of(":"));

rosnode->getParam("atlas_controller/gains/" + pieces[2] + "/p",     jointcommands.kp_position[i]);

rosnode->getParam("atlas_controller/gains/" + pieces[2] + "/i",     jointcommands.ki_position[i]);

rosnode->getParam("atlas_controller/gains/" + pieces[2] + "/d",     jointcommands.kd_position[i]);

rosnode->getParam("atlas_controller/gains/" + pieces[2] + "/i_clamp",     jointcommands.i_effort_min[i]); jointcommands.i_effort_min[i] = -jointcommands.i_effort_min[i];

rosnode->getParam("atlas_controller/gains/" + pieces[2] + "/i_clamp",     jointcommands.i_effort_max[i]); jointcommands.velocity[i]    = 0; jointcommands.effort[i]      = 0; jointcommands.kp_velocity[i] = 0; }

Subscribe to the JointState message, but also set the subscriber option to use unreliable transport (i.e. UDP connection).
 * 1) Subscribe to `/atlas/joint\_states` Message

// ros topic subscribtions ros::SubscribeOptions jointStatesSo = ros::SubscribeOptions::create<sensor_msgs::JointState>(   "/atlas/joint_states", 1, SetJointStates,    ros::VoidPtr, rosnode->getCallbackQueue);

// Because TCP causes bursty communication with high jitter, // declare a preference on UDP connections for receiving // joint states, which we want to get at a high rate. // Note that we'll still accept TCP connections for this topic // (e.g., from rospy nodes, which don't support UDP); // we just prefer UDP. jointStatesSo.transport_hints = ros::TransportHints.unreliable;

ros::Subscriber subJointStates = rosnode->subscribe(jointStatesSo); // ros::Subscriber subJointStates = //  rosnode->subscribe("/atlas/joint_states", 1000, SetJointStates);

Initialize JointCommands publisher.
 * 1) Setup Publisher

pub_joint_commands_ = rosnode->advertise<osrf_msgs::JointCommands>(   "/atlas/joint_commands", 1, true);

to process messages in the ROS callback queue
 * 1) Call `ros::spin`

ros::spin;

return 0; }


 * 1) Running the Simulation

1. In terminal, source the DRC simulator setup script and start the DRC robot simulation:

roslaunch atlas_utils atlas_sandia_hands.launch

1. In a separate terminal, run the node constructed above:

export ROS_PACKAGE_PATH=${HOME}/ros:${ROS_PACKAGE_PATH} rosrun drcsim_joint_commands_tutorial publish_joint_commands


 * 1) Altering the code

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.

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

// assign sinusoidal joint angle and velocity targets for (unsigned int i = 0; i < jointcommands.name.size; i++) {     jointcommands.position[i] = 3.2* sin((ros::Time::now - startTime).toSec); jointcommands.velocity[i] = 3.2* cos((ros::Time::now - startTime).toSec); }

Then rebuild and re-run the node:

make rosrun drcsim_joint_commands_tutorial publish_joint_commands


 * 1) Next ##

Next: Atlas control over ROS topics using python