ROS Resources: Documentation | Support | Discussion Forum | Service Status | Q&A

Moveit! motion planning issue




I am trying to manage to Pick and Place using UR5 and 2F-Robotiq85, simulating it in Gazebo.
Is there any way to combine Moveit motions of different groups together in a row, but not separately.
Here is a link to a video to understand the question.
My assumption, I’m not subscribing to simulated robot Current joint trajectory?
Another question, what is this strange motion planning solution of the manipulator group.
Feel free to ask for extra information.
Please, give me any references, thank you in advance!

Best Regards,


Hi Artemii,

I struggled myself on this one a few years ago. It’s actually not a motion planning issue !
If I remember well, the UR5 joints can each rotate from -2pi to 2pi, so there is several joint solution to reach the same position.
The problem you have here is that the IK solver does not give you the closest solution to your current joint position. To solve that issue I recommend using trac_ik with the option solve_type: Distance
You can see an example here :

Jimmy Da Silva


My personal solution, which worked pretty well, was to limit the range of the angles, in particular the elbow (3rd axis).


Thank you, Jimmy, for your response!
I am definitely trying this IK solver later on, sounds like a good solution.
And let you know about the progress.



Thank you, facontidavide

It solved the second part of the problem.
Simply added joint limits to urdf/ur5.urdf.xacro and updated limits to ur5_moveit_config/config/joint_limits.yaml.
But the first part problem didn’t get solved, it still collides without any reason, although pose_goal is in the workspace. but not under.


If I understand your question correctly, you want to append a robotiq gripper trajectory at the end of your ur5 robot trajectory. This shouldn’t be a problem since the moveit trajectory execution manager takes in a moveit_msgs/RobotTrajectory which simply maps the trajectory for each joint in joint_names to an appropriate controller for that joint.

In the video that you provide, it is unclear what you are displaying, please include the rviz displays side panel. If you are able to see the robot moving with only the planning_scene robot visualized, then you are on the right track. If you are instead just displaying the published joint trajectory, then you need to look into how you are connecting to gazebo.


Also, I would recommend using the ur_kinematics analytic solution for the UR5 since it is much much faster than any iterative solver (kdl or trac_ik)