Sending joint commands with ROS

개요

target joint positions을 로봇에게 보냅니다. 이 DRCSim User Guide 가이드는 기본 콘트롤러 api를 제공합니다. 이 지침서안의 2개의 ros 토픽을 사용할겁니다.

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

Setup

시물레시션 환경을 설치하세요, 이미 되어있으면 재설치 필요없음. 단신이 그렇게 하지않으면, 환경 셋업화일의 소스를 이용합니다. 터미널 창을 2개 사용합니다.

source /usr/share/drcsim/setup.sh

타이핑을 저장하기 위해, 당신은 이 스크립트를 당신의 .bashrc files에 추가할수 있습니다, 그래서 그것이 자동으로 매번 당신이 터미널을 실행할때마다 적용되게 할수있습니다.

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

그러나 당신은 추가한 내용을 지우기 위해서 기억해야 합니다. 당신이 더이상 필요치 않을때를 위해서.

ROS Package Workspace 생성하기

만일 당신이 이미 ros directory를 당신의 홈 directory에 저장하고 있다면, 당신은 이것을 당신의 ROSPACKAGEPATH에 추가를 하세요 아래 명령으로 하면 됩니다.

mkdir ~/ros
export ROS_PACKAGE_PATH=${HOME}/ros:${ROS_PACKAGE_PATH}

당신은 roscreate-pkg을 당신의 패키지를 만들기 위해 사용하시면 됩니다.

cd ~/ros
roscreate-pkg drcsim_joint_commands_tutorial roscpp trajectory_msgs osrf_msgs

ROS Node 생성하기

아틀라스 4,5버전은 다른 조인트 네임을 필요로 합니다. 아틀라스 4,5 지침서를 보세요

아래 코드를 ~/ros/drcsim_joint_commands_tutorial/src/publish_joint_commands.cpp 에 텍스트 툴로 복사해서 추가하세요 아니면 여기에 다운로드해도 됩니다.

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

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

the ROS Node 컴파일링하기

~/ros/drcsim_joint_commands_tutorial/CMakeLists.txt 화일을 편집하세요. 아래와 같이. 또는 여기에 다운로드해도 됩니다.

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)

아래 터리널 밑에 타이핑해서 컴파일 하기위해서 아래 명령어 실행하세요

roscd drcsim_joint_commands_tutorial
make

만일 당신이 가제보를 다른 비표준위치에 서리를 했다면, 당신은 PKG_CONFIG_PATH를 사용해야 합니다. 예를 들면 , 당신이 ~/local에 설치를 했다면 아래 명령어를 실행하세요

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

코드설명입니다.

헤더와 글로벌 변수 선언 아래는 화일의 라이선스,다양한 시스템,라이브러리,글로벌 변수를 포함하고 있습니다.

/*
 * 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 함수

---

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::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 /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"
©```