def display(self, q): if 'viewer' not in self.__dict__: return # Update the robot geometry. se3.kinematics(self.model, self.data, q, self.v0) # Iteratively place the robot bodies. for i in range(1, self.model.nbody): if self.model.hasVisual[i]: M = self.data.oMi[i] self.viewer.gui.applyConfiguration(self.viewerNodeNames(i), utils.se3ToXYZQUAT(M)) self.viewer.gui.refresh()
def display(self,q): if 'viewer' not in self.__dict__: return # Update the robot geometry. se3.kinematics(self.model,self.data,q,self.v0) # Iteratively place the robot bodies. for i in range(1,self.model.nbody): if self.model.hasVisual[i]: M = self.data.oMi[i] self.viewer.gui.applyConfiguration(self.viewerNodeNames(i), utils.se3ToXYZQUAT(M)) self.viewer.gui.refresh()
def display(self,q): if 'viewer' not in self.__dict__: return # Update the robot geometry. se3.kinematics(self.model,self.data,q,self.v0) # Iteratively place the moving robot bodies. for i in range(1,self.model.nbody): if self.model.hasVisual[i]: M = self.data.oMi[i] self.viewer.gui.applyConfiguration(self.viewerNodeNames(i), utils.se3ToXYZQUAT(M)) # 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 self.viewer.gui.applyConfiguration(self.viewerFixedNodeNames(i),utils.se3ToXYZQUAT(M)) self.viewer.gui.refresh()
def velocity(self,q,v,index, update_kinematics = True): if update_kinematics: se3.kinematics(self.model,self.data,q,v) return self.data.v[index]
def kinematics(self,q,v): se3.kinematics(self.model, self.data, q, v)
def velocity(self,q,v,index): se3.kinematics(self.model,self.data,q,v) return self.data.v[index]
def position(self,q,index): se3.kinematics(self.model,self.data,q,self.v0) return self.data.oMi[index]