Sending joint commands with ROS

Overview

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.

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.

Create a ROS Package Workspace

If you haven't already, create a ros directory in your home directory and add it to your $ROSPACKAGEPATH. 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

Create a ROS Node

Note: Atlas versions 4 and 5 require different joint names. See Atlas v4 and v5 instructions

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, emacs) or download it here:

/*

• 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.

*/ #include <math.h> #include <ros/ros.h> #include <ros/subscribe_options.h> #include <boost/thread.hpp> #include <boost/algorithm/string.hpp> #include <sensor_msgs/JointState.h> #include <osrf_msgs/JointCommands.h>

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

// 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; }

// 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::vectorstd::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;

}

// ros topic subscriptions 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);

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

ros::spin();

return 0; }

Compiling the ROS Node

Edit ~/ros/drcsim_joint_commands_tutorial/CMakeLists.txt so that it looks like below (or download it here):

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

# Set the build type.  Options are:
# Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug          : w/ debug symbols, w/o optimization
# Release        : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
# set(ROS_BUILD_TYPE RelWithDebInfo)

rosbuild_init()

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

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

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

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

The code explained

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

/*

• 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.

*/ #include <math.h> #include <ros/ros.h> #include <ros/subscribe_options.h> #include <boost/thread.hpp> #include <boost/algorithm/string.hpp> #include <sensor_msgs/JointState.h> #include <osrf_msgs/JointCommands.h>

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

ROS Topic Callback Function

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.

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

} }

Main Subroutine

Initialize ros and creates a ros::NodeHandle.

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

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

Waits for Simulation Time Update

Make sure simulation time has propagated to this node before running the rest of this demo code:

// 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; }

Hardcoded List of Joint Names

List of joint names in the Atlas robot. Note the order must not change for this function to work correctly.

// 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");

Size JointCommands Variables

Resize JointCommands based on the size of joint names list.

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

Fill in JointCommands Gains and Target Values

Retrieve JointCommands gains from ROS parameter server. Set target velocities and efforts to zero.

for (unsigned int i = 0; i < n; i++) { std::vectorstd::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 /atlas/joint_states Message

Subscribe to the JointState message, but also set the subscriber option to use unreliable transport (i.e. UDP connection).

// ros topic subscriptions 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);

Setup Publisher

Initialize JointCommands publisher.

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

Call ros::spin()

to process messages in the ROS callback queue

ros::spin();

return 0; }

Running the Simulation

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

roslaunch drcsim_gazebo atlas_sandia_hands.launch 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 constructed above, which will cause Atlas to writhe around on the ground with all joints cycling through their full range of motion (see this video for a variant of this behavior):

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

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

Atlas v4 and v5

The sample code given above will not work for Atlas v4 and v5 because these later models have different joint names and more joints.

To control Atlas v4/v5, change lines 60-87 of publish_joint_commands.cpp to the following (or download a modified version of the code):

jointcommands.name.push_back("atlas::l_leg_hpz"); jointcommands.name.push_back("atlas::l_leg_hpx"); jointcommands.name.push_back("atlas::l_leg_hpy"); jointcommands.name.push_back("atlas::l_leg_kny"); jointcommands.name.push_back("atlas::l_leg_aky"); jointcommands.name.push_back("atlas::l_leg_akx"); jointcommands.name.push_back("atlas::r_leg_hpz"); jointcommands.name.push_back("atlas::r_leg_hpx"); jointcommands.name.push_back("atlas::r_leg_hpy"); jointcommands.name.push_back("atlas::r_leg_kny"); jointcommands.name.push_back("atlas::r_leg_aky"); jointcommands.name.push_back("atlas::r_leg_akx"); jointcommands.name.push_back("atlas::l_arm_shz"); 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_wry"); jointcommands.name.push_back("atlas::l_arm_wrx"); jointcommands.name.push_back("atlas::l_arm_wry2"); jointcommands.name.push_back("atlas::r_arm_shz"); 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_wry"); jointcommands.name.push_back("atlas::r_arm_wrx"); jointcommands.name.push_back("atlas::r_arm_wry2"); jointcommands.name.push_back("atlas::neck_ry"); jointcommands.name.push_back("atlas::back_bkz"); jointcommands.name.push_back("atlas::back_bky"); jointcommands.name.push_back("atlas::back_bkx"); Next, make the ROS package as above. Note that if you downloaded the source code, you will have to either rename the file from publish_joint_commands_v4v5.cpp to publish_joint_commands.cpp. Alternatively, you can build a new executable from publish_joint_commands_v4v5.cpp by modifying CMakeLists.txt.

To run the new joint publisher, follow the steps above, but start DRCSim with the following command to launch Atlas v4:

roslaunch drcsim_gazebo atlas_sandia_hands.launch model_args:="_v4" Or you can launch Atlas v5:

roslaunch drcsim_gazebo atlas_sandia_hands.launch model_args:="_v5" ©