Rosanswers logo


Plan from A to B, not colliding with obstacles, using all planners in OMPL and CHOMP. Compare the trajectories of those planners.


I am using Moveit to plan for a 6 DOF articulated robot arm. The workspace contains obstacles. I use C++ move_group interface to compute the plan with different OMPL Library planners.


I am interested in joint_6 movement. When I plot the 'position' attribute of the trajectory points, I make two observations:

  1. The planners start at a common point (the start pose), but end at completely different points. Since the manipulator is 6 DOF, I can only imagine this to be some kind of misconfiguration issue.
  2. The trajectories are not equal in length (length = number of trajectory points).


My goal is to compare the trajectories from each Algorithm. For an accurate comparison, the two observations are problematic.


Is there a way to

a) have every planner output the exact same number of waypoints (I am thinking here of a "fine" grid ~ 1000 for example ~ something that is way above the default 40 - 150 number that I observed)?

b) fix the position generation? Since the robot arm is 6 DOF, the end pose should be uniquely defined through a set of joint positions. It is unclear to me how the different planners can have a varying end position for joint_6.

Edit: First of all, thank you for your answer.

A trajectory point position attribute equals the position member of the trajectory message. As my robot has six revolute joints, that equals one float64[6] array for each point on the trajectory. Since only joint_6 is of interest for me for now, I plot joint_6 RAD value on the y-Axis and the corresponding trajectory point number on the x-Axis.

I may mix up something here, please correct me if I am wrong. A robot with a serial kinematic chain and DOF > 6 is considered redundant, i.e. the same end effector pose can be reached with multiple joint configurations. Since my manipulator is 6 DOF and I specify x,y,z,r,p,y for my end pose, I assume that this pose directly transfers to a unique set of joint values.

Question b) of my original question was (likely) solved due to a bug in my program where I export the trajectory in json format.

I tried a different aproach where instead of exporting the trajectory, I recorded the movement of the /joint_states topic during excecution of the trajectory. There seems to be a lot of smoothing and interpolating going on, but coupled with the bug fix I get a farely stable fix to question b).

Regarding question a) nothing has changed here. But since I am now recording the excecution of the trajectory, I can be sure that the intervall between my measuring points is defined by the timestamp of the /joint_states topic's message. Before I relied on the trajectory generation, which as you too mentioned is a rather unstable approach.

A downside of the new approach is, that my measurement depends on the fake excecution manager trajectory processing. I am using the ros - industrial - fanuc packages.

Originally posted by cpetersmeier on ROS Answers with karma: 35 on 2019-08-14

Post score: 0


1 Answer 1


Rosanswers logo

Just to clarify something when you're talking about the position attribute of trajectory points, are you referring to the position member of the trajectory message here? This is not the Cartesian position of the last joint of the robot but the final joint position, an angle in your case.

All the 6 DOF revolute arms I can think of often have several different joint solutions for a given end effector pose, so it's perfectly possible that different planners will end up with a different joint angle solution while giving the end effector exactly the same pose.

However you can use Moveit to define a goal in terms of joint angles instead of end effector pose, this means that the planners will all end up with the same (to within a tolerance) joint angles. The method you need is moveit::planning_interface::MoveGroup::setJointValueTarget

Regarding trajectory resolution, I don't think there is anyway to do this. The number of points on a trajectory is defined by the movement tolerance specified, and is calculated on the fly. There is nothing to stop you interpolating the different trajectories you generate to the same sample spacing though, It could get a bit strange if trajectories are of significantly different durations.

Finally, can you explain exactly how you're comparing the trajectories?

Originally posted by PeteBlackerThe3rd with karma: 9529 on 2019-08-15

This answer was ACCEPTED on the original site

Post score: 1

Original comments

Comment by PeteBlackerThe3rd on 2019-08-16:
Firstly I'm glad you've managed to find a solution so you can compare the trajectories. I do need to correct you regarding joint solution of 6 DOF revolute arms though. A revolute arm with > 6 joints has an infinite number of solutions for any end effector pose, however a 6 DOF arm can have a discrete number of joint solutions. Often there are 2 or 4 different joint configurations that will put the end effector in exactly the same location. These are often referred to as elbow up/down and shoulder left/right, this however will depend on the exact configuration of your arm. Arms will greater than 360 degree of movement joints can have many more.

Comment by cpetersmeier on 2019-08-16:
Thanks for the quick answer!

That explains why the trajectories were converging to multiple end points when not planning in joint-goals! Glad I have learned something! :)

Comment by gvdhoorn on 2019-08-17:
@cpetersmeier: please don't post updates or follow-up questions as answers, unless you are answering your own question. Either update your original question text (use the edit link/button for that) or post a comment.

I've merged your non-answer into your OP this time and re-arranged the comments, but please keep this in mind for the future.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.