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

MoveIt velocity scaling for Cartesian Path

asked 2018-04-18 03:17:27 -0500

callum91 gravatar image

Hi Everyone,

I am really happy with the cartesian path planning built into move it as the result seems very accurate compared to a real industrial robot. I am just wondering if anyone has been able to implement a method so that the velocity scaling factor will work using this method.

Currently whenever I change the velocity scaling factor the time to complete a given path does not change.

Any help with how this works or how I could go about implementing it myself would be much appreciated.

Thanks, Callum

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
4

answered 2018-04-19 03:30:03 -0500

v4hn gravatar image

updated 2018-04-25 03:17:46 -0500

The computeCartesianPath interface of MoveIt is sadly not implemented as a Planner (in moveIt's terminology) and thus you can't use it with the MoveGroup action server, which uses these scaling factors.

Thoughts to change this or expose the factors to the cartesian interface too have been around for a while, but nobody implemented it yet - As always, users are strongly encouraged to provide patches.

As noted in the comments below, the easiest way to get a different time scaling is to create a new time parameterization for your trajectory. In C++ you can use trajectory_processing::IterativeParabolicTimeParameterization::computeTimeStamps. In Python, there exists a retimeTrajectory method in the MoveGroupInterface that does the same. Keep in mind though, that lately there have been alternative time parameterization methods proposed, one is merged upstream but not yet released at the time of writing. Both interfaces currently only make use of the "old" method and you might consider using a different one.

Another way to go is to rescale the timing of the trajectory after you computed the plan (and the system already added a parameterization for you). So increase the time_from_start, reduce velocity, adjust acceleration to match (squared velocity). We will hopefully soon have function interfaces for this around too, but at this point in time you probably have to do it by hand.

edit flag offensive delete link more

Comments

Would feeding the result of computeCartesianPath(..) to the IPTP be an option? With suitable values of course.

gvdhoorn gravatar image gvdhoorn  ( 2018-04-19 03:40:47 -0500 )edit

Yes, pretty sure I've done that before with success.

Stefan Kohlbrecher gravatar image Stefan Kohlbrecher  ( 2018-04-19 15:26:31 -0500 )edit

Yes, you can do that of course, this will generate completely new time parameters. Note (1) this only is available in C++ (2) IPTP is not the only time parameterization anymore, so you might want to play with the alternative(s)

v4hn gravatar image v4hn  ( 2018-04-20 04:06:31 -0500 )edit

(3) If you use C++ anyway, depending on the method you use you might not get any time parameterization(!) in your result RobotState::computeCartesianPath does not provide that afair. So you need to do this yourself afterwards. My reply assumes the OP wanted to use python.

v4hn gravatar image v4hn  ( 2018-04-20 04:08:20 -0500 )edit

Doesn't the commander have a retimeTrajectory() method that can do this in Python?

gvdhoorn gravatar image gvdhoorn  ( 2018-04-20 06:28:55 -0500 )edit

You are correct, I did not know about this wrapper for IPTP. This probably also works for many use cases, but (2) still remains and additionally it does not support acceleration changes until now.

I updated my answer to reflect the comments.

v4hn gravatar image v4hn  ( 2018-04-25 03:10:30 -0500 )edit

I tried to use trajectory_processing::IterativeParabolicTimeParameterization::computeTimeStamps to control my real robot .but it seemed didnt workd.

     moveit::planning_interface::MoveGroupInterface::Plan plan;
    robot_trajectory::RobotTrajectory rt(arm.getCurrentState()->getRobotModel(),"manipulator");
    rt.setRobotTrajectoryMsg(*arm.getCurrentState(),trajectory);
    trajectory_processing::IterativeParabolicTimeParameterization iptp;
    bool ItSuccess = iptp.computeTimeStamps(rt);
    ROS_INFO("Computed time stamp %s", ItSuccess ? "SUCCEDED" : "FAILED");
    rt.getRobotTrajectoryMsg(trajectory);
   plan.trajectory_=trajectory;

how should i do to control the speed of realrobot

Ataraxy gravatar image Ataraxy  ( 2022-04-24 03:59:14 -0500 )edit

Please don't post questions in comments. Post a new question instead and link to this one.

fvd gravatar image fvd  ( 2022-04-25 08:59:42 -0500 )edit

Question Tools

5 followers

Stats

Asked: 2018-04-18 03:17:27 -0500

Seen: 3,733 times

Last updated: Apr 25 '18