def computeAllTerms(self, q, v):
''' pin.computeAllTerms is equivalent to calling:
pinocchio::forwardKinematics
pinocchio::crba
pinocchio::nonLinearEffects
pinocchio::computeJointJacobians
pinocchio::centerOfMass
pinocchio::jacobianCenterOfMass
pinocchio::kineticEnergy
pinocchio::potentialEnergy
This is too much for our needs, so we call only the functions
we need, including those for the frame kinematics
'''
# pin.computeAllTerms(self.model, self.data, q, v);
pin.forwardKinematics(self.model, self.data, q, v,
np.zeros(self.model.nv))
pin.computeJointJacobians(self.model, self.data)
pin.updateFramePlacements(self.model, self.data)
pin.crba(self.model, self.data, q)
pin.nonLinearEffects(self.model, self.data, q, v)
def frameVelocity(self,
q,
v,
index,
update_kinematics=True,
ref_frame=pin.ReferenceFrame.LOCAL_WORLD_ALIGNED):
if update_kinematics:
pin.forwardKinematics(self.model, self.data, q, v)
v_local = pin.getFrameVelocity(self.model, self.data, index)
if ref_frame == pin.ReferenceFrame.LOCAL:
return v_local
H = self.data.oMf[index]
if ref_frame == pin.ReferenceFrame.WORLD:
v_world = H.act(v_local)
return v_world
Hr = pin.SE3(H.rotation, np.zeros(3))
v = Hr.act(v_local)
return v
def forwardKinematics(self, q, v=None, a=None):
if v is not None:
if a is not None:
pin.forwardKinematics(self.model, self.data, q, v, a)
else:
pin.forwardKinematics(self.model, self.data, q, v)
else:
pin.forwardKinematics(self.model, self.data, q)
def frameClassicAcceleration(
self,
q,
v,
a,
index,
update_kinematics=True,
ref_frame=pin.ReferenceFrame.LOCAL_WORLD_ALIGNED):
if update_kinematics:
pin.forwardKinematics(self.model, self.data, q, v, a)
v = pin.getFrameVelocity(self.model, self.data, index)
a_local = pin.getFrameAcceleration(self.model, self.data, index)
a_local.linear += np.cross(v.angular, v.linear, axis=0)
if ref_frame == pin.ReferenceFrame.LOCAL:
return a_local
H = self.data.oMf[index]
if ref_frame == pin.ReferenceFrame.WORLD:
a_world = H.act(a_local)
return a_world
Hr = pin.SE3(H.rotation, np.zeros(3))
a = Hr.act(a_local)
return a
def forwardKinematics(self, q, v=None, a=None):
q_mat = np.asmatrix(q).reshape((self.model.nq,1))
if v is not None:
v_mat = np.asmatrix(v).reshape((self.model.nv,1))
if a is not None:
a_mat = np.asmatrix(a).reshape((self.model.nv,1))
se3.forwardKinematics(self.model, self.data, q_mat, v_mat, a_mat)
else:
se3.forwardKinematics(self.model, self.data, q_mat, v_mat)
else:
se3.forwardKinematics(self.model, self.data, q_mat)
def framePlacement(self, q, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, np.asmatrix(q).reshape((self.model.nq,1)))
return se3.updateFramePlacement(self.model, self.data, index)