dual arm closed kinematic chain planning in Moveit
I would like to perform a dual-arm robot pick-and-place task, using Moveit to plan trajectories for both arms. e.g. dual arm carrying a large box from a start configuration to target configuration, while avoiding possible collisions.
I found a possible solution here, where dual-arm robot could perform synchronized movements and keep the distance of their end effector poses constant. However, the code uses compute_cartesian_path to plan both arms, which means it probably cannot give a good solution when there are obstacles in the workspace.
So, is there a existing function or plugin in Moveit for dual arm closed kinematic chain planning, while considering possible obstacles in the workspace? or do I have to write my own code to complete the task?
It is poorly documented, but
compute_cartesian_path()does much less than you might think. All it does is linearly interpolate between eef waypoints that you provide to it. Determining good waypoints is the difficult task. If the function encounters an obstacle in its path, it simply aborts. This is discussed in #q411915.OMPL supports constrained planning, in which constraints are given in the form of a constraint function $f(q)=0$. Projection (by Newton-Raphson method or some other ways) is used during sampling and local planning to ensure the planned path is within the constraint manifold. That is a common way to solve the closed chain planning problem. In Moveit, however, constraints are given in the form of
moveit_msgs/Constraints, which does not support constraint function as far as I know. I'm still trying to learn how to use constraint function and projection method in Moveit...In addition: There is a Moveit2 tutorial on how to use OMPL’s Constrained planning capabilities. However it has nothing to do with constraint function, and still doesn't support closed chains.