Computation of "Classic" Cartesian velocity of a frame

EnricoMingo commented 4 years ago

Hi all, I hope to fin you all good and thank you very much for your work on this very nice library.

We have a question regarding the computation of "Classical" (not Spatial) velocity of a frame, in particular we want to compute the velocity of a certain frame $l$ wrt the world frame $w$ and express this velocity in the world frame $w$ , I'll call this velocity ${{^w}\mathbf{v}_{w,l}}$ . As we understood from the documentation, this can be done multiplying joint space velocities with the (frame) Jacobian ${{^w}\mathbf{J}_{w,l}}$ computed using the option LOCAL_WORLD_ALIGNED.

We compared the linear velocities obtained by ${{^w}\mathbf{J}_{w,l}}\mathbf{\dot{q}} = {{^w}\mathbf{v}_{w,l}}$ with the one obtained by numerically differentiating (using Matlab's diff) the position of the frame ${^w}\mathbf{p}_l$ obtaining quite different results, as you can see from the following plot: where the dashed lines are obtained using diff.

Notice that comparing some of the $\mathbf{\dot{q}}$ with the differentiated $\mathbf{q}$ leads to the expected result:

The Jacobian is computed in the following way:

pinocchio::computeJointJacobians(model, data, q);
pinocchio::updateFramePlacements(model, data);
pinocchio::getFrameJacobian(model, data, frame_idx, pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, J);

Are we missing something? Thank you for your help.

jcarpent commented 4 years ago

@EnricoMingo Thanks a lot for using Pinocchio.

At first glance, I would say you're doing the things correctly. Are you sure that J (output argument of getFrameJacobian) is set to Zero? Could you share the Matlab code for testing to be sure of what happens? Are you sure that is expressed in the world frame coordinate system?

In the meantime, I can share with you some codes that show that the two expected velocities are marching correctly:

import pinocchio as pin 
import numpy as np

model = pin.buildSampleModelHumanoid() 
data = model.createData()

q = pin.randomConfiguration(model,-np.ones(model.nq),np.ones(model.nq))
v = np.random.rand(model.nv)

pin.forwardKinematics(model,data,q,v)
pin.computeJointJacobians(model,data,q)

frame_name = "larm_effector_body"
frame_id = model.getFrameId(frame_name)
frame = model.frames[frame_id]
frame_placement = frame.placement
parent_joint = frame.parent

pin.updateFramePlacements(model,data)
frame_J = pin.getFrameJacobian(model,data,frame_id,pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)

local_to_world_transform = pin.SE3.Identity()
local_to_world_transform.rotation = data.oMf[frame_id].rotation
frame_v = local_to_world_transform.act(frame_placement.actInv(data.v[parent_joint]))

J_dot_v = pin.Motion(frame_J.dot(v))

print("Frame velocity:\n",frame_v)
print("J_dot_v:\n",J_dot_v)

jcarpent commented 4 years ago

And if you want to check against finite differences:

eps = 1e-8
q_plus = pin.integrate(model,q,v*eps)
data_plus = model.createData()

pin.forwardKinematics(model,data_plus,q_plus)
pin.updateFramePlacements(model,data_plus)

p = data.oMf[frame_id].translation
p_plus = data_plus.oMf[frame_id].translation

v_est = (p_plus - p)/eps
print("Frame velocity FD:\n",v_est)

cmastalli commented 4 years ago

@EnricoMingo I suspect your error might be in the diff routine. You need to note that the system lies in a geometrical manifold, therefore you need to integrate it as shown by @jcarpent's code.

jcarpent commented 4 years ago

@EnricoMingo Do you have any feedback on my answer?

EnricoMingo commented 4 years ago

Dear @jcarpent , @cmastalli, sorry if I have not given any feedback but I am running several tests, still without any result. We are missing something: we are running experiments with a floating-base systems and the first thing that we can not figure out is why the floating_base Jacobian, expressed in the world frame (so with LOCAL_WORLD_ALIGNED), is not something like: $\left\[\mathbf{I}_{6\times6} \quad \mathbf{0}_{6\times n} \right\]$ , shouldn't be?

Thank you!

jcarpent commented 4 years ago

No, because the free floating Jacobian is expressed in the local frame of the joint. So its columns are then aligned with the world coordinates frame.

jcarpent commented 4 years ago

From my previous answer, this also means that the joint velocity of the free flyer is also expressed in the local frame of the free flyer too. Do you have this quantity rightly expressed in your setup?

mfocchi commented 4 years ago

@jcarpent

I would like to ask you about this lines that are a bit obscure to me

local_to_world_transform = pin.SE3.Identity() local_to_world_transform.rotation = data.oMf[frame_id].rotation frame_v = local_to_world_transform.act(frame_placement.actInv(data.v[parent_joint]))

we have frame_J already expressed in the LOCAL_WORLD_ALIGNED now you need the spatial twist at frame_id expressed in LOCAL_WORLD_ALIGNED why don't you just do this?:

v_local = pin.getFrameVelocity(model, data, frame_id) frame_v = local_to_world_transform.act(v_local)

moreover, what pin.Motion( ..) has to do?

is it possible to have a documentation with extensive examples on the use of these functions?

Thanks!

cmastalli commented 4 years ago

@mfocchi Your suggested code is equivalent, i.e.

v1 = frame_placement.actInv(data.v[parent_joint])
v2 = pin.getFrameVelocity(model, data, frame_id)
# v1 == v2 --> True

Maybe what you are missing it is the fact that data.v[parent_joint] is the spatial velocity at the joint level (i.e. $\mathbf{v}_j$ ). Furthermore, frame_placement defines a relative SE3 transformation between the joint and the frame. Therefore, actInv applies the inverse of the Motion transform (i.e. $\mathbf{v}_f={}^j\mathbf{X}_f^{-1} \mathbf{v}_j$ )

pin.Motion defines any "motion" spatial vector (velocity and accelerations).

jcarpent commented 4 years ago

why don't you just do this?:

v_local = pin.getFrameVelocity(model, data, frame_id) frame_v = local_to_world_transform.act(v_local)

I wrote a mistake in my previous answers, so I corrected it. In fact, pin.getFrameVelocity retrieves the velocity of the point coinciding with the origin of the WORLD but attached to the Frame given by frame_id. It is equivalent to:

data.oMi[joint_id].act(data.v[joint_id])

where joint_id is the joint index supporting the frame given by frame_id. On the contrary, local_to_world_transform.act(v_local) is nothing more than the change of coordinates to express the frame velocity originally expressed in the local coordinate system of the system in the coordinate system aligned with the WORLD axes. Is it clear now?

moreover, what pin.Motion( ..) has to do?

It is just for casting a numpy 6d array to a Motion for printing purposes.

is it possible to have a documentation with extensive examples on the use of these functions?

I do agree. The documentation is not exhaustive enough. We would really appreciate contributions on this point coming from all users if they are willing to of course.

jcarpent commented 4 years ago

@mfocchi I've just updated my answer as I wrote something wrong before ;)

EnricoMingo commented 4 years ago

Dear @jcarpent, I think I solved the issue. Our basic error was due to the fact that we were not considering the floating-base velocity in local coordinates but in global. Our quaternion propagation routine as well as the integrator were working with everything in global therefore solutions where not compatible with the Jacobians provided by Pinocchio. Now I got the expected results I was putting in the first comment of the issue. I have though another small question: when using the rnea() function, should I use a particular Jacobian for the generalized contact forces?

jcarpent commented 4 years ago

The contact forces are currently expressed in the local frame of the joints. So you need to provide an std::vector<Force> with the same dimension as model.njoints. Is it answering your question?

jcarpent commented 4 years ago

According to @EnricoMingo, this issue seems to be solved. I will close it. @EnricoMingo Do not hesitate to ask further questions if needed.

stack-of-tasks / pinocchio

Computation of "Classic" Cartesian velocity of a frame #1140