Joint state plan and constraints

[bmdyrdal]

Hi,

I'm trying to force my end effector keep level, that is it can yaw, but not roll or pitch.

Using plan_single() with pose cost metric with hold_vec_weight of [1, 1, 0, 0, 0, 0] the end effector is mostly kept level. The issue with plan_single is that it mostly generates "bad" paths. Often it generates a large motion for a relative short path.

Switching to plan_single_js with the same pose cost metric generates a much better plan, but it doesn't keep the end effector level. I have the same results with and without having pose cost metrics..

I already have the pose and the joint state for the goal position, and it is not a big factor for me if I'm using plan_single or plan_single_js, but I have to keep the end effector level.

Any suggestions?

Best regards