def display(self, q, force): """Display the robot at configuration q in the viewer by placing all the bodies.""" gui = self.gui pin.framesForwardKinematics(self.model,self.data,q) if self.display_visuals: pin.updateGeometryPlacements(self.model, self.data, self.visual_model, self.visual_data) gui.applyConfigurations ( [ self.getViewerNodeName(visual,pin.GeometryType.VISUAL) for visual in self.visual_model.geometryObjects ], [ pin.SE3ToXYZQUATtuple(self.visual_data.oMg[self.visual_model.getGeometryId(visual.name)]) for visual in self.visual_model.geometryObjects ] ) for contactIndex, contactName in enumerate(self.contactNames): forceVector = np.zeros(3) if(self.type=="Biped"): for i in range(4): forceVector += force[:,4*contactIndex+i] elif(self.type=="Quadruped"): forceVector =force[:,contactIndex] forceNorm = np.linalg.norm(forceVector) if forceNorm<=1.e-3: forceVisiblity = "OFF" else: forceVisiblity = "ON" self.displayContact(contactName, forceVector, forceVisiblity) gui.refresh()
def collisionDistance(q): '''Return the minimal distance between robot and environment. ''' threshold = 1e-2 pin.updateGeometryPlacements(robot.model,robot.data,robot.collision_model,robot.collision_data,q) if pin.computeCollisions(robot.collision_model,robot.collision_data,False): return -threshold idx = pin.computeDistances(robot.collision_model,robot.collision_data) return pin.computeDistance(robot.collision_model,robot.collision_data,idx).min_distance - threshold
def display(self,q): pio.forwardKinematics(self.model,self.data,q) pio.updateGeometryPlacements(self.model,self.data,self.gmodel,self.gdata) if self.viewer is None: return for i,g in enumerate(self.gmodel.geometryObjects): self.viewer.applyConfiguration(g.name, pio.se3ToXYZQUATtuple(self.gdata.oMg[i])) self.viewer.refresh()
def collision(self, qw): if isinstance(qw, ConfigurationWrapper): q = qw.q elif isinstance(qw, np.ndarray): qw = ConfigurationWrapper(self, qw) q = qw.q else: raise ValueError( "qw should either be a ConfigurationWrapper or a numpy array.") if q.shape[0] != self._model.nq: raise ValueError( f"The given configuration vector is of shape {q.shape[0]} while \ the model requires a configuration vector of shape {self._model.nq}" ) model = self._model data = self._data geom_model = self._geom_model geom_data = self._geom_data pin.forwardKinematics(model, data, q) pin.updateGeometryPlacements(model, data, geom_model, geom_data) qw.oMi = data.oMi.tolist() qw.oMg = geom_data.oMg.tolist() # stop at the first collision collide = pin.computeCollisions(geom_model, geom_data, True) return collide
def compute_oMg(self, qw): q = qw.q model = self._model data = self._data geom_model = self._geom_model geom_data = self._geom_data pin.forwardKinematics(model, data, q) pin.updateGeometryPlacements(model, data, geom_model, geom_data, q) qw.oMg = geom_data.oMg.tolist()
def display(self, q): """Display the robot at configuration q in the viewer by placing all the bodies.""" pin.forwardKinematics(self.model, self.data, q) pin.updateGeometryPlacements(self.model, self.data, self.visual_model, self.visual_data) for visual in self.visual_model.geometryObjects: # Get mesh pose. M = self.visual_data.oMg[self.visual_model.getGeometryId(visual.name)] # Manage scaling S = np.diag(np.concatenate((visual.meshScale, np.array([1.0]))).flat) T = np.array(M.homogeneous).dot(S) # Update viewer configuration. self.viewer[self.getViewerNodeName( visual, pin.GeometryType.VISUAL)].set_transform(T)
def _updateGeometryPlacements(self, visual=False): """ @brief Update the generalized position of a geometry object. @remark This is a hidden function not intended to be called manually. @param[in] visual Wether it is a visual or collision update """ if visual: geom_model = self._rb.visual_model geom_data = self._rb.visual_data else: geom_model = self._rb.collision_model geom_data = self._rb.collision_data pin.updateGeometryPlacements(self.pinocchio_model, self.pinocchio_data, geom_model, geom_data)
def display(self, q: np.ndarray): """Display the robot at configuration q in the viewer by placing all the bodies.""" pin.forwardKinematics(self.model, self.data, q) if self.display_visuals: pin.updateGeometryPlacements(self.model, self.data, self.visual_model, self.visual_data) for i, visual in enumerate(self.visual_model.geometryObjects): # Get mesh pose M = self.visual_data.oMg[i] # Manage scaling S = np.diag( np.concatenate((visual.meshScale, np.array([1.0]))).flat) T = np.array(M.homogeneous).dot(S) # Update viewer configuration nodeName = self.getViewerNodeName(visual, pin.GeometryType.VISUAL) self.viewer[nodeName].set_transform(T) if self.display_collisions: pin.updateGeometryPlacements(self.model, self.data, self.collision_model, self.collision_data) for i, collision in enumerate( self.collision_model.geometryObjects): # Get mesh pose M = self.collision_data.oMg[i] # Manage scaling S = np.diag( np.concatenate( (collision.meshScale, np.array([1.0]))).flat) T = np.array(M.homogeneous).dot(S) # Update viewer configuration nodeName = self.getViewerNodeName(collision, pin.GeometryType.collision) self.viewer[nodeName].set_transform(T)
def reduceGeomModel(model, data, modelReduced, dataReduced, geomModel, geomData, q=None, qReduced=None): ''' Given a kinematic model and its reduced version, a geometry model corresponding to the unreduced model, the corresponding data and a reference configuration (in both unreduced and reduced version), modify the geometry model to make it corresponds to the reduced kinematic tree. ''' if q is not None: pio.forwardKinematics(model, data, q) pio.updateGeometryPlacements(model, data, geomModel, geomData) if qReduced is not None: pio.forwardKinematics(modelReduced, dataReduced, qReduced) for igeom, geom in enumerate(geomModel.geometryObjects): jname = model.names[geom.parentJoint] if (modelReduced.existJointName(jname)): # Case 1: the parent joint just change ID, not name geom.parentJoint = modelReduced.getJointId(jname) else: # Case 2: Geom is now orphan' # Get joint ID from the parent frame frame = modelReduced.frames[modelReduced.getFrameId( model.frames[geom.parentFrame].name)] geom.parentJoint = frame.parent # Change geom placement wrt new joint oMg = geomData.oMg[igeom] oMj = dataReduced.oMi[geom.parentJoint] jMg = oMj.inverse() * oMg geom.placement = jMg
def update_quantities(robot: jiminy.Model, position: np.ndarray, velocity: Optional[np.ndarray] = None, acceleration: Optional[np.ndarray] = None, update_physics: bool = True, update_com: bool = True, update_energy: bool = True, update_jacobian: bool = False, update_collisions: bool = True, use_theoretical_model: bool = True) -> None: """Compute all quantities using position, velocity and acceleration configurations. Run multiple algorithms to compute all quantities which can be known with the model position, velocity and acceleration. This includes: - body spatial transforms, - body spatial velocities, - body spatial drifts, - body transform acceleration, - body transform jacobians, - center-of-mass position, - center-of-mass velocity, - center-of-mass drift, - center-of-mass acceleration, - center-of-mass jacobian, - articular inertia matrix, - non-linear effects (Coriolis + gravity) - collisions and distances .. note:: Computation results are stored internally in the robot, and can be retrieved with associated getters. .. warning:: This function modifies the internal robot data. .. warning:: It does not called overloaded pinocchio methods provided by `jiminy_py.core` but the original pinocchio methods instead. As a result, it does not take into account the rotor inertias / armatures. One is responsible of calling the appropriate methods manually instead of this one if necessary. This behavior is expected to change in the future. :param robot: Jiminy robot. :param position: Robot position vector. :param velocity: Robot velocity vector. :param acceleration: Robot acceleration vector. :param update_physics: Whether or not to compute the non-linear effects and internal/external forces. Optional: True by default. :param update_com: Whether or not to compute the COM of the robot AND each link individually. The global COM is the first index. Optional: False by default. :param update_energy: Whether or not to compute the energy of the robot. Optional: False by default :param update_jacobian: Whether or not to compute the jacobians. Optional: False by default. :param use_theoretical_model: Whether the state corresponds to the theoretical model when updating and fetching the robot's state. Optional: True by default. """ if use_theoretical_model: pnc_model = robot.pinocchio_model_th pnc_data = robot.pinocchio_data_th else: pnc_model = robot.pinocchio_model pnc_data = robot.pinocchio_data if (update_physics and update_com and update_energy and update_jacobian and velocity is not None and acceleration is None): pin.computeAllTerms(pnc_model, pnc_data, position, velocity) else: if velocity is None: pin.forwardKinematics(pnc_model, pnc_data, position) elif acceleration is None: pin.forwardKinematics(pnc_model, pnc_data, position, velocity) else: pin.forwardKinematics( pnc_model, pnc_data, position, velocity, acceleration) if update_com: if velocity is None: pin.centerOfMass(pnc_model, pnc_data, position) elif acceleration is None: pin.centerOfMass(pnc_model, pnc_data, position, velocity) else: pin.centerOfMass( pnc_model, pnc_data, position, velocity, acceleration) if update_jacobian: if update_com: pin.jacobianCenterOfMass(pnc_model, pnc_data) pin.computeJointJacobians(pnc_model, pnc_data) if update_physics: if velocity is not None: pin.nonLinearEffects(pnc_model, pnc_data, position, velocity) pin.crba(pnc_model, pnc_data, position) if update_energy: if velocity is not None: pin.computeKineticEnergy(pnc_model, pnc_data) pin.computePotentialEnergy(pnc_model, pnc_data) pin.updateFramePlacements(pnc_model, pnc_data) if update_collisions: pin.updateGeometryPlacements( pnc_model, pnc_data, robot.collision_model, robot.collision_data) pin.computeCollisions( robot.collision_model, robot.collision_data, stop_at_first_collision=False) pin.computeDistances(robot.collision_model, robot.collision_data) for dist_req in robot.collision_data.distanceResults: if np.linalg.norm(dist_req.normal) < 1e-6: pin.computeDistances( robot.collision_model, robot.collision_data) break
urdf_model_path, mesh_dir) print('model name: ' + model.name) # Create data required by the algorithms data, collision_data, visual_data = pinocchio.createDatas( model, collision_model, visual_model) # Sample a random configuration q = pinocchio.randomConfiguration(model) print('q: %s' % q.T) # Perform the forward kinematics over the kinematic tree pinocchio.forwardKinematics(model, data, q) # Update Geometry models pinocchio.updateGeometryPlacements(model, data, collision_model, collision_data) pinocchio.updateGeometryPlacements(model, data, visual_model, visual_data) # Print out the placement of each joint of the kinematic tree print("\nJoint placements:") for name, oMi in zip(model.names, data.oMi): print(("{:<24} : {: .2f} {: .2f} {: .2f}".format(name, *oMi.translation.T.flat))) # Print out the placement of each collision geometry object print("\nCollision object placements:") for k, oMg in enumerate(collision_data.oMg): print(("{:d} : {: .2f} {: .2f} {: .2f}".format(k, *oMg.translation.T.flat))) # Print out the placement of each visual geometry object
geom_model = pin.buildGeomFromUrdf(model,urdf_model_path,model_path,pin.GeometryType.COLLISION) # Add collisition pairs geom_model.addAllCollisionPairs() print("num collision pairs - initial:",len(geom_model.collisionPairs)) # Remove collision pairs listed in the SRDF file srdf_filename = "romeo.srdf" srdf_model_path = model_path + "/romeo_description/srdf/" + srdf_filename pin.removeCollisionPairs(model,geom_model,srdf_model_path) print("num collision pairs - final:",len(geom_model.collisionPairs)) # Load reference configuration pin.loadReferenceConfigurations(model,srdf_model_path) # Retrieve the half sitting position from the SRDF file q = model.referenceConfigurations["half_sitting"] # Create data structures data = model.createData() geom_data = pin.GeometryData(geom_model) # Compute all the collisions pin.computeCollisions(model,data,geom_model,geom_data,q,False) # Compute for a single pair of collision pin.updateGeometryPlacements(model,data,geom_model,geom_data,q) pin.computeCollision(geom_model,geom_data,0)
def coll(q): '''Return true if in collision, false otherwise.''' pin.updateGeometryPlacements(robot.model,robot.data,robot.collision_model,robot.collision_data,q) return pin.computeCollisions(robot.collision_model,robot.collision_data,False)
def updateGeometryPlacements(self, q): pinocchio.framesForwardKinematics(self.model, self.data, np.array(q)) pinocchio.updateGeometryPlacements(self.model, self.data, self.gmodel, self.gdata)
np.matrix(xyzrpy[:3]).T ) # Set object placement wrt parent robot.collision_model.addGeometryObject(obs,robot.model,False) # Add object to collision model robot.visual_model .addGeometryObject(obs,robot.model,False) # Add object to visual model # Also create a geometric object in gepetto viewer, with according name. try: robot.viewer.gui.addCapsule( "world/pinocchio/"+obs.name, rad,length, [ 1.0, 0.2, 0.2, 1.0 ] ) except: pass # Add all collision pairs robot.collision_model.addAllCollisionPairs() # Remove collision pairs that can not be relevant (forearm with upper arm, forearm with wrist, etc). for idx in [ 56,35,23 ]: #print "Remove pair",robot.collision_model.collisionPairs[idx] robot.collision_model.removeCollisionPair(robot.collision_model.collisionPairs[idx]) # Collision/visual models have been modified => re-generate corresponding data. robot.collision_data = se3.GeometryData(robot.collision_model) robot.visual_data = se3.GeometryData(robot.visual_model ) # Display the robot will also update capsule placements. for iloop in range(100): q = rand(6)*2*np.pi-np.pi se3.updateGeometryPlacements(robot.model,robot.data,robot.collision_model,robot.collision_data,q) if se3.computeCollisions(robot.collision_model,robot.collision_data,False): for i,p in enumerate(robot.collision_model.collisionPairs): if se3.computeCollision(robot.collision_model,robot.collision_data,i): print i,p, robot.collision_model.geometryObjects[p.first].name, \ robot.collision_model.geometryObjects[p.second].name robot.display(q) break
# Load collision geometries geom_model = pin.buildGeomFromUrdf(model,urdf_model_path,[model_path],pin.GeometryType.COLLISION) # Add collisition pairs geom_model.addAllCollisionPairs() # Remove collision pairs listed in the SRDF file srdf_filename = "romeo_small.srdf" srdf_model_path = model_path + "/romeo_description/srdf/" + srdf_filename pin.removeCollisionPairs(model,geom_model,srdf_model_path) # Load reference configuration pin.loadReferenceConfigurations(model,srdf_model_path) # Retrieve the half sitting position from the SRDF file q = model.referenceConfigurations["half_sitting"] # Create data structures data = model.createData() geom_data = pin.GeometryData(geom_model) # Compute all the collisions pin.computeCollisions(model,data,geom_model,geom_data,q,False) # Compute for a single pair of collision pin.updateGeometryPlacements(model,data,geom_model,geom_data,q) pin.computeCollision(geom_model,geom_data,0)