Closed longwoo closed 6 years ago
Hi @longwoo. It appears that you have several mistakes in your code:
In the line Eigen::VectorXd v = randomConfiguration(model,lb,ub);
, randomConfiguration returns a configuration vector. What you want is v a velocity vector, not a configuration vector. In other words, v belongs to the tangent space of the configuration manifold at q. Which means that q and v might not have the same dimension.
In this case, you should use directly your own linear random function between two bounds to sample v adequately.
Also, ddq is only filled via a call to aba
or forwardDynamics
, but you never call such a function in your test code.
Then data.ddq
appears to be filled with random values when data is built.
@jcarpent Ha ha ha. It is a stupid question for me. It has already been checked in pinocchio/unittest/aba.cpp
.
What interest me is the meaning of velocity vector
you mentioned. According to your description, the input v
of computeAllTerms
or nonLinearEffects
is a velocity vector, not a configuration vector. The point is , for a floating base model , velocity vector is w.r.t. World Frame or Other Frame?
Second, the order of velocity is the same as Featherstone's Book(p12) (w_x,w_y,w_z, v_x,v_y,v_z)
or (v_x,v_y,v_z, w_x,w_y,w_z)
?
We are currently following the convention of Roy Featherstone who represents the velocity of each inside its local frame to retrieve a sparsity pattern in the computations. So for the free flyer, the velocity are expressed in the local frame of the joint. On the other side, we have chose to use the linear then the angular velocity inside their vectorial representation. If you look into the spatial classes like Motion, you have a enum structure with LINEAR and ANGULAR info. LINEAR is set to 0 and ANGULAR to 3.
@longwoo Do not hesitate to reopen this issue if needed.
The source code
pinocchio/multibody/model.hpp
line 499 explain that residual torque is Temporary corresponding to the residual torquetau - b
.According to the unconstrained rigid-body system equation
M*ddq + b = tau
. So I computeM*ddq
alone, but it is not equal to residual torque.M*ddq + b
is not equal totau
Test code is like this
In mujoco, equation is almost zero, is that because I missed the constrained force
JTF
in pinocchio?