def display(self, q):
if 'viewer' not in self.__dict__:
return
# Update the robot geometry.
se3.geometry(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 display(self,q):
if 'viewer' not in self.__dict__: return
# Update the robot geometry.
se3.geometry(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(0,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 position(self,q,index, update_geometry = True):
if update_geometry:
se3.geometry(self.model,self.data,q)
return self.data.oMi[index]
def geometry(self,q):
se3.geometry(self.model, self.data, q)
