Moveit2 joint velocity control for Panda robot arm

Prerequisites

You have installed moveit2.

NOTE: The code is based on Humble and NOT apply to Jazzy.

Create ROS package

cd ~/ws_moveit2/src
ros2 pkg create --build-type ament_cmake moveit_joint_velocity --dependencies rclcpp control_msgs geometry_msgs moveit_servo moveit_core moveit_msgs planning_scene_monitor tf2_ros --maintainer-email [e-mail]

Add the Control Node

In the moveit_joint_velocity/src directory inside the package, create a file named joint_velocity.cpp and add following code:

/**
    Robotlabs Newzealand
    https://www.youtube.com/@robotlabs
**/
#include <rclcpp/rclcpp.hpp>
#include <moveit_servo/servo_parameters.h>
#include <moveit_servo/servo.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>

using namespace std::chrono_literals;

static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_joint_velocity.joint_velocity_node");

rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr joint_cmd_pub_;
std::string joint_name = "panda_joint1";
double velocity = 0.0;

void publishCommands()
{
    auto msg = std::make_unique<control_msgs::msg::JointJog>();
    msg->header.stamp = node_->now();
    msg->joint_names.push_back(joint_name);
    msg->velocities.push_back(velocity);
    joint_cmd_pub_->publish(std::move(msg));
}

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions node_options;

  node_options.use_intra_process_comms(false);
  node_ = std::make_shared<rclcpp::Node>("joint_velocity_node", node_options);

  node_->declare_parameter("velocity", velocity);
  node_->declare_parameter("joint_name", joint_name);
  node_->get_parameter("velocity", velocity);
  node_->get_parameter("joint_name", joint_name);

  auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
  auto planning_scene_monitor = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
      node_, "robot_description", tf_buffer, "planning_scene_monitor");

  if (planning_scene_monitor->getPlanningScene())
  {
    planning_scene_monitor->startStateMonitor("/joint_states");
    planning_scene_monitor->setPlanningScenePublishingFrequency(25);
    planning_scene_monitor->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
                                                         "/moveit_servo/publish_planning_scene");
    planning_scene_monitor->startSceneMonitor();
    planning_scene_monitor->providePlanningSceneService();
  }
  else
  {
    RCLCPP_ERROR(LOGGER, "Planning scene not configured");
    return EXIT_FAILURE;
  }

  joint_cmd_pub_ = node_->create_publisher<control_msgs::msg::JointJog>("joint_velocity_node/delta_joint_cmds", 10);
  auto servo_parameters = moveit_servo::ServoParameters::makeServoParameters(node_);
  if (!servo_parameters)
  {
    RCLCPP_FATAL(LOGGER, "Failed to load the servo parameters");
    return EXIT_FAILURE;
  }
  auto servo = std::make_unique<moveit_servo::Servo>(node_, servo_parameters, planning_scene_monitor);
  servo->start();

  rclcpp::TimerBase::SharedPtr timer = node_->create_wall_timer(50ms, publishCommands);

  auto executor = std::make_unique<rclcpp::executors::MultiThreadedExecutor>();
  executor->add_node(node_);
  executor->spin();

  rclcpp::shutdown();
  return 0;
}

Edit CmakeList.txt

cmake_minimum_required(VERSION 3.8)
project(moveit_joint_velocity)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(control_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(moveit_servo REQUIRED)
find_package(moveit_core REQUIRED)
find_package(moveit_msgs REQUIRED)
find_package(tf2_ros REQUIRED)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

add_executable(joint_velocity_node src/joint_velocity.cpp)
ament_target_dependencies(joint_velocity_node
  rclcpp
  control_msgs
  geometry_msgs
  moveit_servo
  moveit_core
  moveit_msgs
  tf2_ros
)

install(TARGETS
  joint_velocity_node
  DESTINATION lib/${PROJECT_NAME}
)

ament_package()

Build the package

cd ~/ws_moveit2
colcon build --packages-select moveit_joint_velocity

Test and Run

Launch the Panda robot demo in Rviz first:

ros2 launch moveit2_tutorials demo.launch.py

Run moveit_velocity_node and set the joint_name and velocity:

source install/setup.bash
ros2 run moveit_joint_velocity joint_velocity_node --ros-args -p joint_name:="panda_joint7" -p velocity:=1.0

The joint should be moving in given velocity now. If the robot doesn't move, it may reach the joint limit.