Planning to custom pose fails

[mazaaa]

Hi everyone,

motion planning with the youbot works fine within the rviz demo . However, when I try to plan a path with the c++ interface and set a custom target pose all planners fail. If I set the goal tolerance to lets say 0.1 then it works. But in this case the target pose is too far away from my desired pose.
I isolated the IK solver and can ensure that my target poses are valid and joint values can be found.

Any ideas how to precisely move to a custom target pose?

Thank you for your time!

[ludusrusso]

@mazaaa I have exactly the same problem. Did you solved it?
I'm trying it on python too... Same issues

[svenschneider]

Hi everyone,

first note, that the inverse position kinematics (IPK) solver in this package calculates "best-effort" results. The youBot has only five degrees of freedom (DOF) in its arm, i.e. one joint/DOF is missing to reach arbitrary goals in Cartesian space. Consequently, there will most probably be a difference between the requested pose and the pose calculated by applying the forward position kinematics (FPK) to the result (i.e. FPK(IPK(x)) != x). Now my assumption is that the C++ interface checks this constraint and fails, while the interface used by the RViz demo does not perform this check. As I said, this is an assumption and you would have to validate it either in the code of the MoveIt! interface or by asking the MoveIt! developers. In case you any new insights I would be interested in them, as well.

Best regards
Sven

[ludusrusso]

@svenschneider yes it is! I just checked that RVIZ computes the IK Itself and then sends directly a Joint position goal! I'm aware about the 5DoF of Youbot, however, I would like to control it directly without computing the IK in my code. This because I would like to control it using cartesian path, and I'm not sure if moveit allows a "cartesian path like" control in the joint space.
I will investigate it.

[svenschneider]

Ok, that's good to know. Thanks for looking into the issue.

If you want to move arm's end-effector along a Cartesian (linear) path, I see the following two major approaches:

1. Rely on MoveIt! with the computeCartesianPath function. To my knowledge it uses the IPK internally to solve for each provided waypoint.
2. Maybe your problem is better solved by an inverse velocity kinematics solver (IVK). Then you command instantaneous Cartesian velocities and map them to joint space via the manipulator's Jacobian. Note, that this is not a supported use case of MoveIt! or my IPK plugin and consequently you have to get rid of both ;) .

[shivangg]

I'm experiencing the same problem of the custom pose to be failing, except that it fails even if I set significant goal tolerances. The C++ interface works successfully only with getRandomPose() function in MoveIt! only for trac_ik . Any pose I set using setPoseTarget and any position I set using setPositionTarget fails.
File with C++ code using MoveIt!
youbotxyz.cpp.zip
Output of the nodes

[willnil]

Hello guys i am facing the same problem here :( but using the python interface
Did anyone one of you @mazaaa @ludusrusso @shivangg come out with a solution to this IK problem when controlling with just Cartesian coordinates?
Great if someone can solve this issue or give me some tips on how to handle this. Thank you!