Closed dsami2 closed 1 year ago
I have a modified version of the same test where I add 1 revolute dof:
<?xml version="1.0" encoding="utf-8"?>
<robot name="1DofMass">
<link name="root_link"/>
<link name="point_mass">
<inertial>
<mass value="1.0"/>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="joint" type="revolute">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
<axis xyz="1.0 0.0 0.0" />
<parent link="root_link" />
<child link="point_mass" />
<limit effort="0.0" lower="0.0" upper="0.0" velocity="0.0" />
</joint>
</robot>
And modified script to change the floating body angular velocity to be a velocity in the revolute joint instead:
import pinocchio as pin
from pinocchio.utils import *
import numpy as np
model_1dof = pin.buildModelFromUrdf("1dof_mass.urdf", pin.JointModelFreeFlyer())
data_1dof = pin.Data(model_1dof)
q = np.array([0,0,0,0,0,0,0,0])
qdd = np.array([0,0,0,0,0,0,0])
qd = np.array([0,0,0,0,0,0,0])
pin.computeAllTerms(model_1dof, data_1dof, q, qd)
print(pin.rnea(model_1dof, data_1dof, q, qd, qdd))
# prints [0. 0. 9.81 0. 0. 0. ]
qd = np.array([0,0,1,0,0,0,0])
pin.computeAllTerms(model_1dof, data_1dof, q, qd)
print(pin.rnea(model_1dof, data_1dof, q, qd, qdd))
# prints [0. 0. 9.81 0. 0. 0. ]
qd = np.array([0,0,1,0,0,0,1])
pin.computeAllTerms(model_1dof, data_1dof, q, qd)
print(pin.rnea(model_1dof, data_1dof, q, qd, qdd))
# prints [0. 0. 9.81 0. 0. 0. ]
In my mind, this is basically the same model as the point mass above, except that I added a massless link in between the floating base and the point mass. But now, that cross term has gone away.
To shortly answer your question, the [0, -1, 0]
is coming from the Coriolis/Centrifugal effects that you can compute via:
pin.computeCoriolisMatrix(model,data,q,qd) @ qd
Also, don't forget to use pin.neutral(model)
to compute a configuration vector, otherwise, it will not be properly normalized.
import pinocchio as pin
from pinocchio.utils import *
import numpy as np
model = pin.buildModelFromUrdf("point_mass.urdf", pin.JointModelFreeFlyer())
data = pin.Data(model)
q = pin.neutral(model)
qdd = np.array([0,0,0,0,0,0])
qd = np.array([0,0,0,0,0,0])
pin.computeAllTerms(model, data, q, qd)
print(pin.rnea(model, data, q, qd, qdd))
# prints [0. 0. 9.81 0. 0. 0. ]
qd = np.array([0,0,1,0,0,0])
pin.computeAllTerms(model, data, q, qd)
print(pin.rnea(model, data, q, qd, qdd))
# prints [0. 0. 9.81 0. 0. 0. ]
qd = np.array([0,0,1,1,0,0])
pin.computeAllTerms(model, data, q, qd)
print(pin.rnea(model, data, q, qd, qdd))
# prints [ 0. -1. 9.81 0. 0. 0. ]
I have a question that probably comes down to some frame convention that I'm missing. I have a point mass URDF:
And I have a minimal test script calling RNEA here:
So, the first two show the gravity force in Z. The third one also shows a negative force in Y, which looks to me to be like a cross product term:
omega x v = [1 0 0] x [0 0 1] = [0 -1 0]
I don't understand where that term comes from, could somebody explain?