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 __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 dict_to_geom_obj(props): geom = dict_to_geom(props["geom"]) placement = pin.SE3(props["placement"]) geom_obj = pin.GeometryObject( name=props["name"], parent_joint=props["parentJoint"], parent_frame=props["parentFrame"], collision_geometry=geom, placement=placement, mesh_path=props["meshPath"], mesh_scale=props["meshScale"], override_material=props["overrideMaterial"], mesh_color=props["meshColor"], ) geom_obj.meshScale = props["meshScale"] geom_obj.meshColor = props["meshColor"] # geom_obj.overrideMaterial = props["overrideMaterial"] # geom_obj.meshTexturePath = props["meshTexturePath"] return geom_obj
def addCapsule(name, length, radius, linkFrameId, jointId, fMcaps, rmodel, gmodel, gui=None): caps = gmodel.addGeometryObject( pio.GeometryObject( name, linkFrameId, jointId, hppfcl.Capsule( radius, length), # WARNING : FCL takses the capsule halfLength ! fMcaps), rmodel) if (gui != None): gui.addCapsule("world/pinocchio/collisions/" + name, radius, length, [1, 0, 0, .1]) return caps
def show_robot_aabb(self): def hex_to_rgb(value): value = value.lstrip("#") lv = len(value) return tuple( int(value[i:i + lv // 3], 16) for i in range(0, lv, lv // 3)) # geom_objs = self.geom_model.geometryObjects[1:7] geom_objs = self.geom_model.geometryObjects geometries = [geom_obj.geometry for geom_obj in geom_objs] parents = [geom_obj.parentJoint for geom_obj in geom_objs] i = 0 colors = cycle([ "#377eb8", "#e41a1c", "#4daf4a", "#984ea3", "#ff7f00", "#ffff33", "#a65628", "#f781bf", ]) for geom, parent_id, color in zip(geometries, parents, colors): aabb = geom.aabb_local w, h, d = aabb.width(), aabb.height(), aabb.depth() box = hppfcl.Box(w, h, d) placement = pin.SE3(np.eye(3), aabb.center()) geom_obj = pin.GeometryObject(f"aabb{i}", 0, parent_id, box, placement) color = np.array(hex_to_rgb(color)) / 255 geom_obj.meshColor = np.array((color[0], color[1], color[2], 0.5)) self.viz_model.addGeometryObject(geom_obj) i += 1 print("show aabb") self._create_data() self._create_viz()
pin.switchToNumpyArray() model = pin.Model() geom_model = pin.GeometryModel() geometries = [ hppfcl.Capsule(0.1, 0.8), hppfcl.Sphere(0.5), hppfcl.Box(1, 1, 1), hppfcl.Cylinder(0.1, 1.0), hppfcl.Cone(0.5, 1.0), ] for i, geom in enumerate(geometries): placement = pin.SE3(np.eye(3), np.array([i, 0, 0])) geom_obj = pin.GeometryObject("obj{}".format(i), 0, 0, geom, placement) color = np.random.uniform(0, 1, 4) color[3] = 1 geom_obj.meshColor = color geom_model.addGeometryObject(geom_obj) viz = GepettoVisualizer( model=model, collision_model=geom_model, visual_model=geom_model, ) # Initialize the viewer. try: viz.initViewer() except ImportError as error:
import numpy as np import math import time import sys N = 10 # number of pendulums model = pin.Model() geom_model = pin.GeometryModel() parent_id = 0 joint_placement = pin.SE3.Identity() body_mass = 1. body_radius = 0.1 shape0 = fcl.Sphere(body_radius) geom0_obj = pin.GeometryObject("base", 0, shape0, pin.SE3.Identity()) geom0_obj.meshColor = np.array([1., 0.1, 0.1, 1.]) geom_model.addGeometryObject(geom0_obj) for k in range(N): joint_name = "joint_" + str(k + 1) joint_id = model.addJoint(parent_id, pin.JointModelRY(), joint_placement, joint_name) body_inertia = pin.Inertia.FromSphere(body_mass, body_radius) body_placement = joint_placement.copy() body_placement.translation[2] = 1. model.appendBodyToJoint(joint_id, body_inertia, body_placement) geom1_name = "ball_" + str(k + 1) shape1 = fcl.Sphere(body_radius)
y_dim = y_grid[-1] - y_grid[0] point_bins = y_bins * nx + x_bins heights = np.zeros((ny, nx)) np.maximum.at(heights.ravel(), point_bins, Z) point_cloud = fcl.BVHModelOBBRSS() point_cloud.beginModel(0, num_points) point_cloud.addVertices(points.T) height_field = fcl.HeightFieldOBBRSS(x_dim, y_dim, heights, min(Z)) height_field_placement = point_cloud_placement * pin.SE3( np.eye(3), 0.5 * np.array([x_grid[0] + x_grid[-1], y_grid[0] + y_grid[-1], 0.])) go_point_cloud = pin.GeometryObject("point_cloud", 0, point_cloud, point_cloud_placement) go_point_cloud.meshColor = np.ones((4)) 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
print(err) sys.exit(0) # Load the robot in the viewer. viz.loadViewerModel() # Display a robot configuration. q0 = pin.neutral(model) viz.display(q0) viz.displayCollisions(True) viz.displayVisuals(False) mesh = visual_model.geometryObjects[0].geometry mesh.buildConvexRepresentation(True) convex = mesh.convex if convex is not None: placement = pin.SE3.Identity() placement.translation[0] = 2. geometry = pin.GeometryObject("convex", 0, convex, placement) geometry.meshColor = np.ones((4)) visual_model.addGeometryObject(geometry) # Display another robot. viz2 = MeshcatVisualizer(model, collision_model, visual_model) viz2.initViewer(viz.viewer) viz2.loadViewerModel(rootNodeName="pinocchio2") q = q0.copy() q[1] = 1.0 viz2.display(q)
geometry_placement.rotation = pin.Quaternion(np.array( [0., 0., 1.]), np.array([0., 1., 0.])).toRotationMatrix() joint_id = model.addJoint(parent_id, pin.JointModelPY(), pin.SE3.Identity(), joint_name) body_inertia = pin.Inertia.FromCylinder(cart_mass, cart_radius, cart_length) body_placement = geometry_placement model.appendBodyToJoint( joint_id, body_inertia, body_placement ) # We need to rotate the inertia as it is expressed in the LOCAL frame of the geometry shape_cart = fcl.Cylinder(cart_radius, cart_length) geom_cart = pin.GeometryObject("shape_cart", joint_id, shape_cart, geometry_placement) geom_cart.meshColor = np.array([1., 0.1, 0.1, 1.]) geom_model.addGeometryObject(geom_cart) parent_id = joint_id else: base_radius = 0.2 shape_base = fcl.Sphere(base_radius) geom_base = pin.GeometryObject("base", 0, shape_base, pin.SE3.Identity()) geom_base.meshColor = np.array([1., 0.1, 0.1, 1.]) geom_model.addGeometryObject(geom_base) joint_placement = pin.SE3.Identity() body_mass = 1. body_radius = 0.1