Ask Your Question
2

How to control cartesian velocity?

asked 2017-10-06 11:17:59 -0500

AndresCampos gravatar image

I'm using a Fanuc M10iA robot from the ROS industrial package and I would like to generate a trajectory for this robot from a cartesian path (i.e. a list of cartesian positions and velocities).

Currently I use moveIt! and move_group->ComputeCartesianPath() to generate the robot trajectory. The robot moves but I don't know how to specify the velocities for the waypoints.

I want to move the robot with a given cartesian velocity and change this velocity in any point in the trajectory. How can I do this with ROS?

This is the code that i use to move the robot with MoveIt: (example package: https://gitlab.com/InstitutMaupertuis... )

#include <vector>
#include <ros/ros.h>
#include <eigen_conversions/eigen_msg.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_msgs/ExecuteKnownTrajectory.h>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "robot_trajectory");
  ros::NodeHandle node;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  moveit::planning_interface::MoveGroupInterface group("manipulator");
  group.setPoseReferenceFrame("/base");
  group.setPlanningTime(1);

  // Create a trajectory (vector of Eigen poses)
  std::vector<Eigen::Affine3d, Eigen::aligned_allocator<Eigen::Affine3d> > way_points_vector;
  Eigen::Affine3d pose (Eigen::Affine3d::Identity());

  // Square belonging to XY plane
  pose.linear() <<
      1, 0, 0,
      0,-1, 0,
      0, 0, -1;
  pose.translation() << 0.8, 0.1, -0.1;
  way_points_vector.push_back(pose);
  pose.translation() << 1.0, 0.1, -0.1;
  way_points_vector.push_back(pose);
  pose.translation() << 1.0, 0.3, -0.1;
  way_points_vector.push_back(pose);
  pose.translation() << 0.8, 0.3, -0.1;
  way_points_vector.push_back(pose);
  way_points_vector.push_back(way_points_vector[0]);

  // Copy the vector of Eigen poses into a vector of ROS poses
  std::vector<geometry_msgs::Pose> way_points_msg;
  way_points_msg.resize(way_points_vector.size());
  for (size_t i = 0; i < way_points_msg.size(); i++)
    tf::poseEigenToMsg(way_points_vector[i], way_points_msg[i]);

  moveit_msgs::RobotTrajectory robot_trajectory;
  group.computeCartesianPath(way_points_msg, 0.05, 15, robot_trajectory);

  // Execute trajectory
  ros::ServiceClient executeKnownTrajectoryServiceClient = node.serviceClient<moveit_msgs::ExecuteKnownTrajectory>(
      "/execute_kinematic_path");
  moveit_msgs::ExecuteKnownTrajectory srv;

  srv.request.wait_for_execution = true;
  srv.request.trajectory = robot_trajectory;
  executeKnownTrajectoryServiceClient.call(srv);

  ros::waitForShutdown();
  spinner.stop();
   return 0;
}
edit retag flag offensive close merge delete

Comments

Hi, I was looking for something similar to what you have asked for controlling the cartesian velocity of the waypoints. I searched but it seems its not yet done. I want to know if you were able to solve it?

sharath gravatar imagesharath ( 2018-05-22 01:10:49 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2017-10-10 10:25:18 -0500

v4hn gravatar image

MoveIt currently does not provide a method to request specific end-effector velocities.

We would welcome such an addition and it is clearly feasible. You can generate a valid time-parameterization for your cartesian trajectory using the moveit_core/trajectory_processing module. These methods generate velocities according to the maxima specified for each joint. If you want to attain a constant end-effector velocity you can rescale the timing relative to the slowest segment.

edit flag offensive delete link more

Comments

Is there still interest in adding this to the MoveIt source code? If so, how you would like this functionality to be added to the overall MoveIt architecture? I've added a function to moveit_core/trajectory_processing/iterative_time_parameterization.cpp that rescales the timing based on a desired cartesian velocity for a specified end effector link. I was planning on using the MoveGroupInterface to pass the desired velocity and end effector link to the trajectory. I am not a MoveIt expert by any means, so if there is an alternate way you would like this implemented please let me know.

Alternatively, I could implement it separately from any time parameterization after the trajectory has been generated from the move_group.plan function call. This way, no existing data structures would need to be changed.

kwh5484 gravatar imagekwh5484 ( 2019-07-11 09:54:03 -0500 )edit
1

Speaking for myself: yes, there would be interest.

but a comment on an already answered ROS Answers question is not necessarily the best place to discuss/announce this.

I would suggest to post an issue on the MoveIt issue tracker on github.

gvdhoorn gravatar imagegvdhoorn ( 2019-07-11 14:42:41 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

4 followers

Stats

Asked: 2017-10-06 11:17:59 -0500

Seen: 1,010 times

Last updated: Oct 10 '17