def test_pair_equals(self): c1 = pin.CollisionPair(1, 2) c2 = pin.CollisionPair(1, 2) c3 = pin.CollisionPair(3, 4) self.assertEqual(c1, c2) self.assertTrue(c1 == c2) self.assertFalse(c1 != c2) self.assertNotEqual(c1, c3) self.assertTrue(c1 != c3) self.assertFalse(c1 == c3)
def addTerrainsToGeomModel(gmodel, terrain, obstacles): """ Add a list of stepstones and obstacles to the robot geometry object. Each step stone is defined by its SE3 placement. It is added as a red disk of 20cm radius. Each obstacles is defined by its 3d position. It is added as a white sphere of radius 20cm. - gmodel is a pinocchio geometry model - terrain is a list of SE3 placement of the step stones - obstacles is a list of 3d position of the sphere centers. """ # Create pinocchio 3d objects for each step stone for i, d in enumerate(terrain): # The step stones have name "debris0X" and are attached to the world (jointId=0). g2 = pin.GeometryObject("debris%02d" % i, 0, hppfcl.Cylinder(.2, .01), d) g2.meshColor = np.array([1, 0, 0, 1.]) gmodel.addGeometryObject(g2) # Create Pinocchio 3d objects for the obstacles. for i, obs in enumerate(obstacles): # The obstacles have name "obs0X" and are attached to the world (jointId=0). g2 = pin.GeometryObject("obs%02d" % i, 0, hppfcl.Sphere(.2), pin.SE3(np.eye(3), obs)) g2.meshColor = np.array([1, 1, 1, 1.]) gmodel.addGeometryObject(g2) # Add the collision pair to check the robot collision against the step stones and the obstacles. # For simplicity, does not activate the self-collision pairs. ngeomRobot = len(gmodel.geometryObjects) - len(terrain) - len(obstacles) for irobot in range(ngeomRobot): for ienv in range(len(terrain) + len(obstacles)): gmodel.addCollisionPair(pin.CollisionPair(irobot, ngeomRobot + ienv))
def appendUrdfModel(self, urdfFilename, frame_name, fMm, prefix=None): if not isinstance(fMm, pinocchio.SE3): fMm = pinocchio.XYZQUATToSE3(fMm) id_parent_frame = self.model.getFrameId(frame_name) model, gmodel = pinocchio.buildModelsFromUrdf( hpp.rostools.retrieve_resource(urdfFilename), geometry_types=pinocchio.GeometryType.COLLISION) if prefix is not None: for i in range(1, len(model.names)): model.names[i] = prefix + model.names[i] for f in model.frames: f.name = prefix + f.name for go in gmodel.geometryObjects: go.name = prefix + go.name igeom = self.gmodel.ngeoms nmodel, ngmodel = pinocchio.appendModel(self.model, model, self.gmodel, gmodel, id_parent_frame, fMm) self.gid_field_of_view = ngmodel.getGeometryId("field_of_view") for go in gmodel.geometryObjects: ngmodel.addCollisionPair( pinocchio.CollisionPair(self.gid_field_of_view, ngmodel.getGeometryId(go.name))) self.model, self.gmodel = nmodel, ngmodel self.data = pinocchio.Data(self.model) self.gdata = pinocchio.GeometryData(self.gmodel)
def test_pair_copy(self): c1 = pin.CollisionPair(1, 2) c2 = c1.copy() self.assertEqual(c1, c2) c2.second = 3 self.assertNotEqual(c1, c2)
def __init__( self, urdfString=None, urdfFilename=None, fov=np.radians((60., 90.)), # fov = np.radians((49.5, 60)), geoms=["arm_3_link_0"], ): if isinstance(fov, str): self.fov = fov fov_fcl = hppfcl.MeshLoader().load( hpp.rostools.retrieve_resource(fov)) else: # build FOV tetahedron dx, dy = np.sin(fov) pts = hppfcl.StdVec_Vec3f() self.fov = [ (0, 0, 0), (dx, dy, 1), (-dx, dy, 1), (-dx, -dy, 1), (dx, -dy, 1), ] pts.extend([np.array(e) for e in self.fov]) self.fov.append(self.fov[1]) fov_fcl = hppfcl.BVHModelOBBRSS() fov_fcl.beginModel(4, 5) fov_fcl.addSubModel(pts, _tetahedron_tris) fov_fcl.endModel() self.cos_angle_thr = np.cos(np.radians(70)) if urdfFilename is None: assert urdfString is not None # Pinocchio does not allow to build a GeometryModel from a XML string. urdfFilename = '/tmp/tmp.urdf' with open(urdfFilename, 'w') as f: f.write(urdfString) self.model, self.gmodel = pinocchio.buildModelsFromUrdf( hpp.rostools.retrieve_resource(urdfFilename), root_joint=pinocchio.JointModelPlanar(), geometry_types=pinocchio.GeometryType.COLLISION) id_parent_frame = self.model.getFrameId("xtion_rgb_optical_frame") parent_frame = self.model.frames[id_parent_frame] go = pinocchio.GeometryObject("field_of_view", id_parent_frame, parent_frame.parent, fov_fcl, parent_frame.placement) self.gid_field_of_view = self.gmodel.addGeometryObject(go, self.model) for n in geoms: assert self.gmodel.existGeometryName(n) self.gmodel.addCollisionPair( pinocchio.CollisionPair(self.gmodel.getGeometryId(n), self.gid_field_of_view)) self.data = pinocchio.Data(self.model) self.gdata = pinocchio.GeometryData(self.gmodel)
def addDriller(self, mesh, frame_name, fMm): if not isinstance(fMm, pinocchio.SE3): fMm = pinocchio.XYZQUATToSE3(fMm) id_parent_frame = self.model.getFrameId(frame_name) parent_frame = self.model.frames[id_parent_frame] go = pinocchio.GeometryObject("driller", id_parent_frame, parent_frame.parent, hppfcl.MeshLoader().load(hpp.rostools.retrieve_resource(mesh)), parent_frame.placement * fMm) self.gmodel.addGeometryObject(go, self.model) self.gmodel.addCollisionPair(pinocchio.CollisionPair(self.gid_field_of_view, self.gmodel.ngeoms-1)) self.gdata = pinocchio.GeometryData(self.gmodel)
def add_obstacle(self, geom_obj, static): if static: geom_model = self.model_wrapper.geom_model geom_model.addGeometryObject(geom_obj) else: raise NotImplementedError # check collisions between the robot and obstacles robot_n_joints = self.robot_n_joints check_collision = range(robot_n_joints) n_geom_model = len(geom_model.geometryObjects) for collision_id in check_collision: geom_model.addCollisionPair( pin.CollisionPair(collision_id, n_geom_model - 1))
def initSolo(): robot = loadSolo(solo=False) # Get robot model, data, and collision model rmodel = robot.model rdata = rmodel.createData() gmodel = robot.collision_model # Get the base_link and FL_UPPER_LEG meshes base_link_geom = gmodel.getGeometryId("base_link_0") leg_geom = gmodel.getGeometryId("FL_UPPER_LEG_0") # Add the collision pair to the geometric model gmodel.addCollisionPair(pio.CollisionPair(base_link_geom, leg_geom)) gdata = gmodel.createData() return robot, rmodel, rdata, gmodel, gdata
def createRobotWithObstacles(robotname='ur5'): ### Robot # Load the robot robot = robex.load(robotname) ### Obstacle map # Capsule obstacles will be placed at these XYZ-RPY parameters oMobs = [[0.40, 0., 0.30, np.pi / 2, 0, 0], [-0.08, -0., 0.69, np.pi / 2, 0, 0], [0.23, -0., 0.04, np.pi / 2, 0, 0], [-0.32, 0., -0.08, np.pi / 2, 0, 0]] # Load visual objects and add them in collision/visual models color = [1.0, 0.2, 0.2, 1.0] # color of the capsules rad, length = .1, 0.4 # radius and length of capsules for i, xyzrpy in enumerate(oMobs): obs = pin.GeometryObject.CreateCapsule( rad, length) # Pinocchio obstacle object obs.meshColor = np.array( [1.0, 0.2, 0.2, 1.0]) # Don't forget me, otherwise I am transparent ... obs.name = "obs%d" % i # Set object name obs.parentJoint = 0 # Set object parent = 0 = universe obs.placement = XYZRPYtoSE3(xyzrpy) # Set object placement wrt parent robot.collision_model.addGeometryObject( obs) # Add object to collision model robot.visual_model.addGeometryObject(obs) # Add object to visual model ### Collision pairs nobs = len(oMobs) nbodies = robot.collision_model.ngeoms - nobs robotBodies = range(nbodies) envBodies = range(nbodies, nbodies + nobs) for a, b in itertools.product(robotBodies, envBodies): robot.collision_model.addCollisionPair(pin.CollisionPair(a, b)) ### Geom data # Collision/visual models have been modified => re-generate corresponding data. robot.collision_data = pin.GeometryData(robot.collision_model) robot.visual_data = pin.GeometryData(robot.visual_model) return robot
# Get the robot frames we're interested in fl_upper_leg = rmodel.getFrameId("FL_UPPER_LEG") fl_lower_leg = rmodel.getFrameId("FL_LOWER_LEG") fr_upper_leg = rmodel.getFrameId("FR_UPPER_LEG") fr_lower_leg = rmodel.getFrameId("FR_LOWER_LEG") hl_upper_leg = rmodel.getFrameId("HL_UPPER_LEG") hl_lower_leg = rmodel.getFrameId("HL_LOWER_LEG") hr_upper_leg = rmodel.getFrameId("HR_UPPER_LEG") hr_lower_leg = rmodel.getFrameId("HR_LOWER_LEG") base_link = rmodel.getFrameId("base_link") # Get the base_link and FL_UPPER_LEG meshes base_link_geom = gmodel.getGeometryId("base_link_0") fl_upper_geom = gmodel.getGeometryId("FL_UPPER_LEG_0") # Add the collision pair to the geometric model gmodel.addCollisionPair(pio.CollisionPair(base_link_geom, fl_upper_geom)) if(enableGUI): # Display the robot robot.rebuildData() robot.displayCollisions(True) robot.displayVisuals(False) robot.display(robot_config) for n in gv.getNodeList(): if 'collision' in n and 'simple' not in n and len(n)>27: gv.setVisibility(n,'ON') gv.setColor(n, [1,1,1,0.2]) if 'collision' in n and 'base_link' in n and len(n)>27: gv.setColor(n, [1,0.5,0.2,1]) if 'collision' in n and 'FL_UPPER_LEG' in n and len(n)>27: gv.setColor(n, [1,0.5,0.2,1])
collision_model.addGeometryObject(go_point_cloud) visual_model.addGeometryObject(go_point_cloud) go_height_field = pin.GeometryObject("height_field", 0, height_field, height_field_placement) go_height_field.meshColor = np.ones((4)) height_field_collision_id = collision_model.addGeometryObject(go_height_field) visual_model.addGeometryObject(go_height_field) # Add colllision pair between the height field and the panda_hand geometry panda_hand_collision_id = collision_model.getGeometryId("panda_hand_0") go_panda_hand = collision_model.geometryObjects[panda_hand_collision_id] go_panda_hand.geometry.buildConvexRepresentation(False) go_panda_hand.geometry = go_panda_hand.geometry.convex # We need to work with the convex hull of the real mesh collision_pair = pin.CollisionPair(height_field_collision_id, panda_hand_collision_id) collision_model.addCollisionPair(collision_pair) viz = MeshcatVisualizer(model, collision_model, visual_model) # Start a new MeshCat server and client. # Note: the server can also be started separately using the "meshcat-server" command in a terminal: # this enables the server to remain active after the current script ends. # # 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"
def addCollisionPairs(self): # # Collision between finger 1 and right palm # for nph in range(1,4): # n= 'world/finger%d%d' % (1,nph) # self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId(n), # self.gmodel.getGeometryId('world/wpalmr'))) # # Collision between finger 2 and wirst # for nph in range(1,4): # n= 'world/finger%d%d' % (2,nph) # self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId(n), # self.gmodel.getGeometryId('world/wrist'))) # # Collision between finger 3 and left palm # for nph in range(1,4): # n= 'world/finger%d%d' % (3,nph) # self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId(n), # self.gmodel.getGeometryId('world/wpalml'))) # # Collision between phallange 2 and 3 and second palm # for nf in range(1,4): # for nph in range(2,4): # n= 'world/finger%d%d' % (nf,nph) # self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId(n), # self.gmodel.getGeometryId('world/palm2'))) # self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId(n), # self.gmodel.getGeometryId('world/wpalmfr'))) # # Collision between phallange 1 and phallange 3 # for nf in range(1,4): # n1= 'world/finger%d%d' % (nf,1) # n2= 'world/finger%d%d' % (nf,3) # self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId(n1), # self.gmodel.getGeometryId(n2))) # self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId('world/wpalml'), # self.gmodel.getGeometryId('world/thumb2'))) # self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId('world/wpalml'), # self.gmodel.getGeometryId('world/thumb2'))) # for it in range(1,3): # nt= 'world/thumb%d' % it # self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId('world/wpalmfr'), # self.gmodel.getGeometryId(nt))) # self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId('world/palm2'), # self.gmodel.getGeometryId(nt))) # for iph in range(1,4): # nph= 'world/finger%d%d' % (3,iph) # self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId(nt), # self.gmodel.getGeometryId(nph))) pairs = [ ['finger11','wpalmr'], ['finger12','wpalmr'], ['finger13','wpalmr'], ['finger21','wrist'], ['finger22','wrist'], ['finger23','wrist'], ['finger31','wpalml'], ['finger32','wpalml'], ['finger33','wpalml'], ['finger12','palm2'], ['finger12','wpalmfr'], ['finger13','palm2'], ['finger13','wpalmfr'], ['finger22','palm2'], ['finger22','wpalmfr'], ['finger23','palm2'], ['finger23','wpalmfr'], ['finger32','palm2'], ['finger32','wpalmfr'], ['finger33','palm2'], ['finger33','wpalmfr'], ['finger11','finger13'], ['finger21','finger23'], ['finger31','finger33'], ['wpalml','thumb2'], ['wpalmfr','thumb1'], ['palm2','thumb1'], ['thumb1','finger31'], ['thumb1','finger31'], ['thumb1','finger33'], ['wpalmfr','thumb2'], ['palm2','thumb2'], ['thumb2','finger31'], ['thumb2','finger32'], ['thumb2','finger33'], ] for (n1,n2) in pairs: self.gmodel.addCollisionPair(pio.CollisionPair(self.gmodel.getGeometryId('world/'+n1), self.gmodel.getGeometryId('world/'+n2)))