Trajectory Length Problem

edit

MISSION:

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

SETUP:

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.

OBSERVATIONS:

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).

PROBLEM STATEMENT:

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

QUESTION:

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 ... (more)