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 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 computeCollisions(self, q, vq=None): res = pio.computeCollisions(self.rmodel, self.rdata, self.gmodel, self.gdata, q, False) pio.computeDistances(self.rmodel, self.rdata, self.gmodel, self.gdata, q) pio.computeJointJacobians(self.rmodel, self.rdata, q) if not (vq is None): pio.forwardKinematics(self.rmodel, self.rdata, q, vq, 0 * vq) return res
def testGeomConfig(x_rot, y_rot, collisionPair): test_config = robot_config.copy() test_config[7] = x_rot test_config[8] = y_rot pio.computeDistances(rmodel,rdata,gmodel,gdata, test_config) pio.computeCollisions(rmodel, rdata, gmodel, gdata, test_config, True) # Get distance and collision results from collision data collisions_dist = gdata.distanceResults collisions = gdata.collisionResults is_col = collisions[collisionPair].isCollision() # pair 0 is between the leg and the body dist = collisions_dist[collisionPair].min_distance p1 = collisions_dist[collisionPair].getNearestPoint1() p2 = collisions_dist[collisionPair].getNearestPoint2() # get nearest points on the segments return is_col, dist, [p1,p2]
def is_auto_colliding(self, q): """ Check if the configuration is autocolliding """ model = self.wrapper.model data = self.dummy_data collision_model = self.wrapper.collision_model collision_data = self.wrapper.collision_data is_colliding = True is_colliding = pin.computeCollisions(model, data, collision_model, collision_data, q, True) return is_colliding
def searchContactPosture(self, numberOfContacts, qguess=None, ntrial=100): """ Search (randomly) a contact configuration with a given number of contacts. - number of contacts: between 1 and len(self.contactCandidates). - qguess is a configuration, of dim rmodel.nq. If None, a random configuration is first sampled. - ntrial: number of random trial. If successfull, set self.success to True, and store the contact posture found in self.qcontact. If not sucessfull, set self.success to False. Returns self.success. """ assert 0 < numberOfContacts < len(self.contactCandidates) if qguess is None: qguess = pin.randomConfiguration(self.rmodel) for itrial in range(ntrial): limbs = random.sample(list(self.contactCandidates.values()), numberOfContacts) stones = random.sample(self.terrain, numberOfContacts) constraints = [Constraint(self.rmodel, limb.frameIndex, stone, limb.contactType) for limb, stone in zip(limbs, stones)] self.postureGenerator.run(qguess, constraints) if self.postureGenerator.sucess: # We found a candidate posture, let' s check that it is acceptable. qcandidate = self.postureGenerator.qopt.copy() # computeCollisions check collision among the collision pairs. collision = pin.computeCollisions(self.rmodel, self.rdata, self.collision_model, self.collision_data, qcandidate, True) if not collision: self.qcontact = qcandidate self.success = True return True self.success = False return False
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
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 robotClogsFieldOfView(self): """whether the camera is clogged by the selected robot bodies. It assumes that updateGeometryPlacements has been called """ return pinocchio.computeCollisions(self.gmodel, self.gdata, True)
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
# Read the reference configurations defined in the srdf pin.loadReferenceConfigurations(robot.model, srdf_path) q = robot.model.referenceConfigurations["straight_standing"] # Display the configuration in the viewer. robot.display(q) # Initialize the collision data robot.collision_data = pin.GeometryData(robot.collision_model) # Compute all the collisions pin.computeCollisions( robot.model, robot.data, robot.collision_model, robot.collision_data, q, False, ) # Print the status of collision for all collision pairs valid = True for k in range(len(robot.collision_model.collisionPairs)): cr = robot.collision_data.collisionResults[k] cp = robot.collision_model.collisionPairs[k] # print("collision pair:",cp.first,",",cp.second,"- collision:","Yes" if cr.isCollision() else "No") # Optionnal display of all the collision pairs if cr.isCollision(): valid = False print("## First configuration valid: ", valid) # Move to a configuration in self collision
# # Option open=True pens the visualizer. # Note: the visualizer can also be opened seperately by visiting the provided URL. try: viz.initViewer(open=True) except ImportError as err: print( "Error while initializing the viewer. It seems you should install Python meshcat" ) print(err) sys.exit(0) # Load the robot in the viewer. viz.loadViewerModel() # Display a robot configuration. q0 = pin.neutral(model) viz.display(q0) is_collision = False data = model.createData() collision_data = collision_model.createData() while not is_collision: q = pin.randomConfiguration(model) is_collision = pin.computeCollisions(model, data, collision_model, collision_data, q, True) print("Found a configuration in collision:", q) viz.display(q)
# 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)