ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Constant Velocity for moving along the path

asked 2022-06-23 01:50:14 -0600

Monzon gravatar image

updated 2022-06-27 11:18:24 -0600


I have a Yaskawa Motoman GP50 working with ROS (noetic) and ubuntu 20.04, using packages Moveit and Descartes.

When I create trajectories made by several waypoints, I can use time parameter for fixing the movement speed, but the problem is that doing so the robot almost stop in everypoint (making aceleration and deceleration ramps). Is there any way around for setting constant speed and make the robot move fly the points instead of move from one point to the next one?

Thank you!



// Core ros functionality like ros::init and spin
#include <ros/ros.h>
// ROS Trajectory Action server definition
#include <control_msgs/FollowJointTrajectoryAction.h>
// Means by which we communicate with above action-server
#include <actionlib/client/simple_action_client.h>

// Includes the descartes robot model we will be using
#include <descartes_moveit/ikfast_moveit_state_adapter.h>

// Includes the descartes trajectory type we will be using
#include <descartes_trajectory/axial_symmetric_pt.h>
#include <descartes_trajectory/cart_trajectory_pt.h>

// Includes the planner we will be using
#include <descartes_planner/dense_planner.h>

// Includes the utility function for converting to trajectory_msgs::JointTrajectory's
#include <descartes_utilities/ros_conversions.h>
#include <eigen_conversions/eigen_msg.h>

// Includes Moveit Planning Interface
#include <moveit/move_group_interface/move_group_interface.h>

#include <iostream>
#include <fstream>
#include <vector>
#include <cstring>
#include <string>
#include <algorithm>
#include <iterator>
#include <cassert>
#include <sstream>

// Initialize 
std::vector<descartes_core::TrajectoryPtPtr> makePath(const std::vector<std::vector<float>>& trajectoryPath);
std::vector<std::vector<float>>  readPath();

// Create subscriber for first point
std::vector<double> getCurrentJointState(const std::string& topic)
  sensor_msgs::JointStateConstPtr state = ros::topic::waitForMessage<sensor_msgs::JointState>(topic, ros::Duration(0.0));
  if (!state) throw std::runtime_error("Joint state message capture failed");
  return state->position;

// Sends a ROS trajectory to the robot controller
bool executeTrajectory(const trajectory_msgs::JointTrajectory& trajectory);

int main(int argc, char** argv)
  // Initialize ROS
  ros::init(argc, argv, "descartes_tutorials");
  ros::NodeHandle nh;

  ros::AsyncSpinner spinner (1);

  descartes_core::RobotModelPtr model (new descartes_moveit::IkFastMoveitStateAdapter());

  // Name of description on parameter server
  const std::string robot_description = "robot_description";

  // kinematic group
  const std::string group_name = "acroba_robot";

  // Base frame
  const std::string world_frame = "base_link";

  // tool center point frame 
  const std::string tcp_frame = "link_6_t";

  // model initializes
  if (!model->initialize(robot_description, group_name, world_frame, tcp_frame))
    ROS_INFO("Could not initialize robot model");
    return -1;

  model->setCheckCollisions(true); // turn on collision checking.

  // read path from test.pto
  std::vector<std::vector<float>>  points_test = readPath();

  // convert path to TrajectoryPtPtr
  std::vector<descartes_core::TrajectoryPtPtr> points = makePath(points_test);

  // create a planner
  descartes_planner::DensePlanner planner;

  // initialize planner
  if (!planner.initialize(model))
    ROS_ERROR("Failed to initialize planner");
    return -2;

  if (!planner.planPath(points))
    ROS_ERROR("Could not solve for a valid path");
    return -3;

  std::vector<descartes_core::TrajectoryPtPtr> result;
  if (!planner.getPath(result))
    ROS_ERROR("Could not retrieve path");
    return -4;

  // get joint names
  std::vector<std::string> names;
  nh.getParam("controller_joint_names", names);

  // Create a JointTrajectory
  trajectory_msgs::JointTrajectory joint_solution;
  joint_solution.joint_names = names;

  // Define a default velocity
  const static double default_joint_vel = 0.3; // rad/s
  if (!descartes_utilities::toRosJointPoints(*model, result, default_joint_vel, joint_solution.points))
    ROS_ERROR("Unable to convert Descartes trajectory to joint points");
    return -5;

  // Send the ROS trajectory to the robot for execution
  if (!executeTrajectory(joint_solution))
    ROS_ERROR("Could not execute trajectory!");
    return -6;

  // Wait till user kills ...
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2022-06-26 09:59:14 -0600

Mike Scheutzow gravatar image

I am not familiar with this particular robot arm, but it sounds like you are using a position_controller plugin in the ros_control subsystem. There exist more sophisticated trajectory controller plugins which generate smoother movement, but they require that your arm support velocity or effort input. Start here:

edit flag offensive delete link more


Hi Mike, No, I am using JointTrajectoryController, but anyway I guess that JointTrajectoryController is driven the path that Moveit generate between two points. My problem comes with path generated with several points (more than two), then the robot slow down to the point and speed up again for going to the next one.

Monzon gravatar image Monzon  ( 2022-06-27 01:20:43 -0600 )edit

The majority of JointTrajectory have more than 2 points, and (in general) an arm can move smoothly if you provide the necessary information. Have you set the velocities[] field in the message?

Mike Scheutzow gravatar image Mike Scheutzow  ( 2022-06-27 07:09:01 -0600 )edit

I have added the code I am using to the Question. Right now, I am adding the velocity parameter through the "timing constraint" in the AxialSymmetricPt funcion. The only velocity msg that I can found is the one in the /joint_states topic, but I guess that it is a only reading field. How could I set the velocities[] field you comment?

Monzon gravatar image Monzon  ( 2022-06-27 11:23:18 -0600 )edit

The velocity arrays are inside the trajectory_msgs/JointTrajectory message. The value specifies a joint's target velocity (radians/second) just as it reaches the waypoint position.

Mike Scheutzow gravatar image Mike Scheutzow  ( 2022-06-28 06:53:11 -0600 )edit

But that is a problem Mike, I need a cartesian constant speed. If I command the speed in joints the velocity will depend on the position. Well, maybe I could use the inverse kinematic for calculate the joint speed from the cartesian speed if there is no easier way.

Monzon gravatar image Monzon  ( 2022-06-29 02:35:50 -0600 )edit

Question Tools



Asked: 2022-06-23 01:50:14 -0600

Seen: 199 times

Last updated: Jun 27 '22