def frameClassicAcceleration(self, q, v, a, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, q, v, a)
f = self.model.frames[index]
af = f.placement.actInv(self.data.a[f.parent])
vf = f.placement.actInv(self.data.v[f.parent])
af.linear += np.cross(vf.angular.T, vf.linear.T).T
return af
def forwardKinematics(self, q, v=None, a=None):
if v is not None:
if a is not None:
se3.forwardKinematics(self.model, self.data, q, v, a)
else:
se3.forwardKinematics(self.model, self.data, q, v)
else:
se3.forwardKinematics(self.model, self.data, q)
def forwardKinematics(self, q, v=None, a=None):
if v is not None:
if a is not None:
se3.forwardKinematics(self.model, self.data, q, v, a)
else:
se3.forwardKinematics(self.model, self.data, q, v)
else:
se3.forwardKinematics(self.model, self.data, q)
def display(self, q):
if 'viewer' not in self.__dict__:
return
# Update the robot geometry.
se3.forwardKinematics(self.model, self.data, q)
# Iteratively place the moving robot bodies.
for i in range(1, self.model.nbody):
if self.model.hasVisual[i]:
M = self.data.oMi[i]
pinocchioConf = utils.se3ToXYZQUAT(M)
viewerConf = utils.XYZQUATToViewerConfiguration(pinocchioConf)
self.viewer.gui.applyConfiguration(self.viewerNodeNames(i), viewerConf)
# Iteratively place the fixed robot bodies.
for i in range(self.model.nFixBody):
if self.model.fix_hasVisual[i]:
index_last_movable = self.model.fix_lastMovingParent[i]
oMlmp = self.data.oMi[index_last_movable]
lmpMi = self.model.fix_lmpMi[i]
M = oMlmp * lmpMi
pinocchioConf = utils.se3ToXYZQUAT(M)
viewerConf = utils.XYZQUATToViewerConfiguration(pinocchioConf)
self.viewer.gui.applyConfiguration(self.viewerFixedNodeNames(i), viewerConf)
self.viewer.gui.refresh()
def frameAcceleration(self, q, v, a, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, q, v, a)
frame = self.model.frames[index]
parentJointAcc = self.data.a[frame.parent]
return frame.placement.actInv(parentJointAcc)
def acceleration(self, q, v, a, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, q, v, a)
return self.data.a[index]
def frameVelocity(self, q, v, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, q, v)
frame = self.model.frames[index]
parentJointVel = self.data.v[frame.parent]
return frame.placement.actInv(parentJointVel)
def framePosition(self, q, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, q)
frame = self.model.frames[index]
parentPos = self.data.oMi[frame.parent]
return parentPos.act(frame.placement)
def velocity(self, q, v, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, q, v)
return self.data.v[index]
def position(self, q, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, q)
return self.data.oMi[index]
def position(self, q, index, update_geometry=True):
if update_geometry:
se3.forwardKinematics(self.model, self.data, q)
return self.data.oMi[index]
def acceleration(self, q, v, a, index, update_acceleration=True):
if update_acceleration:
se3.forwardKinematics(self.model, self.data, q, v, a)
return self.data.a[index]
def velocity(self, q, v, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, q, v)
return self.data.v[index]
