stack-of-tasks/tsid

Expose SE3ToVector

Closed this issue · 7 comments

EtienneAr commented

Hello,

Would it be possible to expose tsid::math::SE3ToVector(const pinocchio::SE3 & M, RefVector vec) (and maybe tsid::math::vectorToSE3(RefVector vec, pinocchio::SE3 & M)) in Python ?

In fact, using SE3 equality constraint is not very user-friendly in Python as it's not clearly specified in the doc if the rotation matrix should be transformed to vector in column-major or row-major, and the default numpy.flatten() does not do it the right way (numpy.flatten('F') must be used).

andreadelprete commented

Sure that would be possible. But could I ask you in which scenario you need such method?
If you create a TrajectorySE3Constant, you can directly specify an SE3 object in the constructor, and then use the method TrajectorySE3Constant::computeNext to convert it to a TrajectorySample, which contains the corresponding vectorized position, velocity and acceleration.

jcarpent commented

There are these features already exposed in Pinocchio: SETToXYZQuat

andreadelprete commented

There are these features already exposed in Pinocchio: SETToXYZQuat

Actually SE3ToVector does something different, see here: https://github.com/stack-of-tasks/tsid/blob/master/src/math/utils.cpp#L32

EtienneAr commented

Sure that would be possible. But could I ask you in which scenario you need such method?

It is in the scenario where I got a temporal plan for one of my end effector, and I want it to follow it. So at each step I want to manually set the TaskSE3Equality value, derivative and second derivative with the values (SE3 and vectors) coming from my plan.

If you create a TrajectorySE3Constant, you can directly specify an SE3 object in the constructor, and then use the method TrajectorySE3Constant::computeNext to convert it to a TrajectorySample, which contains the corresponding vectorized position, velocity and acceleration.

I could use this method, but it seemed strange to me to use a TrajectorySE3Constant, set it to a value, then use computeNext and finally modify the result sample to set it to my desired velocity and acceleration.

andreadelprete commented

Sorry @EtienneAr for the long delay, I dropped the ball on this one. Do you still need SE3ToVector in Python? If so, I am in favor of adding this feature.

EtienneAr commented

I found a work-around by doing :
ee_pos_vec = np.concatenate((ee_pos.translation, ee_pos.rotation.flatten('F')))
so I don't "need" it anymore.

But I think that it would be nice to add it anyway, to have a consistent API, because :

[..] using SE3 equality constraint is not very user-friendly in Python as it's not clearly specified in the doc if the rotation matrix should be transformed to vector in column-major or row-major, and the default numpy.flatten() does not do it the right way (numpy.flatten('F') must be used).

(I am sorry that I cannot expose this by myself and do a PR as I don't have enough practice with Boost.Python to expose a new class from scratch.)

jcarpent commented

Solved by #178.