def test_self_load(self): hint_list = [self.model_dir] model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer()) collision_model_ref = pin.buildGeomFromUrdf(model, self.model_path, pin.GeometryType.COLLISION, hint_list) collision_model_self = pin.GeometryModel() pin.buildGeomFromUrdf(model, self.model_path, pin.GeometryType.COLLISION, collision_model_self, hint_list) self.assertTrue(checkGeom(collision_model_ref, collision_model_self)) collision_model_self = pin.GeometryModel() pin.buildGeomFromUrdf(model, self.model_path, pin.GeometryType.COLLISION, collision_model_self, self.model_dir) self.assertTrue(checkGeom(collision_model_ref, collision_model_self)) hint_vec = pin.StdVec_StdString() hint_vec.append(self.model_dir) collision_model_self = pin.GeometryModel() pin.buildGeomFromUrdf(model, self.model_path, pin.GeometryType.COLLISION, collision_model_self, hint_vec) self.assertTrue(checkGeom(collision_model_ref, collision_model_self))
def loadSoloLeg(solo8=True): if solo8: URDF_FILENAME = "solo.urdf" legMaxId = 4 else: URDF_FILENAME = "solo12.urdf" legMaxId = 5 SRDF_FILENAME = "solo.srdf" SRDF_SUBPATH = "/solo_description/srdf/" + SRDF_FILENAME URDF_SUBPATH = "/solo_description/robots/" + URDF_FILENAME modelPath = example_robot_data.getModelPath(URDF_SUBPATH) robot = example_robot_data.loadSolo(solo8) m1 = robot.model m2 = pinocchio.Model() for index, [j, M, name, parent, Y] in enumerate( zip(m1.joints, m1.jointPlacements, m1.names, m1.parents, m1.inertias)): if j.id < legMaxId: jointType = j.shortname() if jointType == 'JointModelFreeFlyer': # for the freeflyer we just take reduced mass and zero inertia jointType = 'JointModelPZ' Y.mass = Y.mass / 4 Y.inertia = np.diag(1e-6 * np.ones(3)) M = pinocchio.SE3.Identity() if index == 2: # start with the prismatic joint on the sliding guide M.translation = np.zeros(3) # 2D model, flatten y axis vector2d = M.translation vector2d[1] = 0.0 M.translation = vector2d jid = m2.addJoint(parent, getattr(pinocchio, jointType)(), M, name) assert (jid == j.id) m2.appendBodyToJoint(jid, Y, pinocchio.SE3.Identity()) for f in m1.frames: if f.parent < legMaxId: m2.addFrame(f) g2 = pinocchio.GeometryModel() for g in robot.visual_model.geometryObjects: if g.parentJoint < legMaxId: g2.addGeometryObject(g) robot.model = m2 robot.data = m2.createData() robot.visual_model = g2 robot.visual_data = pinocchio.GeometryData(g2) # Load SRDF file #q0 = example_robot_data.readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, 'standing') robot.q0 = np.zeros(m2.nq + 1) assert ((m2.rotorInertia[:m2.joints[1].nq] == 0.).all()) return robot
def __init__(self): super(TalosLegsLoader, self).__init__() legMaxId = 14 m1 = self.robot.model m2 = pin.Model() for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements, m1.names, m1.parents, m1.inertias): if j.id < legMaxId: jid = m2.addJoint(parent, getattr(pin, j.shortname())(), M, name) upperPos = m2.upperPositionLimit lowerPos = m2.lowerPositionLimit effort = m2.effortLimit upperPos[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.upperPositionLimit[j.idx_q:j.idx_q + j.nq] lowerPos[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.lowerPositionLimit[j.idx_q:j.idx_q + j.nq] effort[m2.joints[jid].idx_v:m2.joints[jid].idx_v + j.nv] = m1.effortLimit[j.idx_v:j.idx_v + j.nv] m2.upperPositionLimit = upperPos m2.lowerPositionLimit = lowerPos m2.effortLimit = effort assert jid == j.id m2.appendBodyToJoint(jid, Y, pin.SE3.Identity()) upperPos = m2.upperPositionLimit upperPos[:7] = 1 m2.upperPositionLimit = upperPos lowerPos = m2.lowerPositionLimit lowerPos[:7] = -1 m2.lowerPositionLimit = lowerPos effort = m2.effortLimit effort[:6] = np.inf m2.effortLimit = effort # q2 = self.robot.q0[:19] for f in m1.frames: if f.parent < legMaxId: m2.addFrame(f) g2 = pin.GeometryModel() for g in self.robot.visual_model.geometryObjects: if g.parentJoint < 14: g2.addGeometryObject(g) self.robot.model = m2 self.robot.data = m2.createData() self.robot.visual_model = g2 # self.robot.q0=q2 self.robot.visual_data = pin.GeometryData(g2) # Load SRDF file self.q0 = readParamsFromSrdf(self.robot.model, self.srdf_path, self.verbose, self.has_rotor_parameters, self.ref_posture) assert (m2.armature[:6] == 0.).all() # Add the free-flyer joint limits to the new model self.addFreeFlyerJointLimits()
def loadTalosLegs(): robot = loadTalos() URDF_FILENAME = "talos_reduced.urdf" SRDF_FILENAME = "talos.srdf" SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME modelPath = getModelPath(URDF_SUBPATH) legMaxId = 14 m1 = robot.model m2 = pinocchio.Model() for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements, m1.names, m1.parents, m1.inertias): if j.id < legMaxId: jid = m2.addJoint(parent, getattr(pinocchio, j.shortname())(), M, name) up = m2.upperPositionLimit down = m2.lowerPositionLimit up[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.upperPositionLimit[j.idx_q:j.idx_q + j.nq] down[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.lowerPositionLimit[j.idx_q:j.idx_q + j.nq] m2.upperPositionLimit = up m2.lowerPositionLimit = down assert (jid == j.id) m2.appendBodyToJoint(jid, Y, pinocchio.SE3.Identity()) u = m2.upperPositionLimit u[:7] = 1 m2.upperPositionLimit = u limit = m2.lowerPositionLimit limit[:7] = -1 m2.lowerPositionLimit = limit # q2 = robot.q0[:19] for f in m1.frames: if f.parent < legMaxId: m2.addFrame(f) g2 = pinocchio.GeometryModel() for g in robot.visual_model.geometryObjects: if g.parentJoint < 14: g2.addGeometryObject(g) robot.model = m2 robot.data = m2.createData() robot.visual_model = g2 # robot.q0=q2 robot.visual_data = pinocchio.GeometryData(g2) # Load SRDF file robot.q0 = np.array(np.resize(robot.q0, robot.model.nq)).T readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False) assert ((m2.armature[:6] == 0.).all()) # Add the free-flyer joint limits addFreeFlyerJointLimits(robot) return robot
def createPendulum(nbJoint, length=1.0, mass=1.0, viewer=None): ''' Creates the Pinocchio kinematic <rmodel> and visuals <gmodel> models for a N-pendulum. @param nbJoint: number of joints <N> of the N-pendulum. @param length: length of each arm of the pendulum. @param mass: mass of each arm of the pendulum. @param viewer: gepetto-viewer CORBA client. If not None, then creates the geometries in the viewer. ''' rmodel = pio.Model() gmodel = pio.GeometryModel() color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0] colorred = [1.0, 0.0, 0.0, 1.0] radius = 0.1 * length prefix = '' jointId = 0 jointPlacement = pio.SE3.Identity() inertia = pio.Inertia(mass, np.matrix([0.0, 0.0, length / 2]).T, mass / 5 * np.diagflat([1e-2, length**2, 1e-2])) viewerPrefix = getViewerPrefix(viewer, ["world", "pinocchio", "visuals"]) for i in range(nbJoint): istr = str(i) name = prefix + "joint" + istr jointName, bodyName = [name + "_joint", name + "_body"] jointId = rmodel.addJoint(jointId, pio.JointModelRY(), jointPlacement, jointName) rmodel.appendBodyToJoint(jointId, inertia, pio.SE3.Identity()) if viewer is not None: viewer.addSphere(viewerPrefix + jointName, 1.5 * radius, colorred) viewer.addCapsule(viewerPrefix + bodyName, radius, .8 * length, color) gmodel.addGeometryObject( Capsule(jointName, jointId, pio.SE3.Identity(), 1.5 * radius, 0.0)) gmodel.addGeometryObject( Capsule(bodyName, jointId, pio.SE3(eye(3), np.matrix([0., 0., length / 2]).T), radius, 0.8 * length)) jointPlacement = pio.SE3(eye(3), np.matrix([0.0, 0.0, length]).T) rmodel.addFrame( pio.Frame('tip', jointId, 0, jointPlacement, pio.FrameType.OP_FRAME)) rmodel.upperPositionLimit = np.zeros(nbJoint) + 2 * np.pi rmodel.lowerPositionLimit = np.zeros(nbJoint) - 2 * np.pi rmodel.velocityLimit = np.zeros(nbJoint) + 5.0 return rmodel, gmodel
def __init__(self, model=None, geom_model=None): if model is None: model = pin.Model() if geom_model is None: geom_model = pin.GeometryModel() self._model = model self._geom_model = geom_model self._data = None self._geom_data = None self._clip_bounds = (None, None)
def __init__(self): self.viewer = GepettoViewerServer() #self.visuals = [] self.model = pio.Model() self.gmodel = pio.GeometryModel() self.createHand() self.addCollisionPairs() self.data = self.model.createData() self.gdata = pio.GeometryData(self.gmodel) self.gdata.collisionRequest.enable_contact=True self.q0 = zero(self.model.nq) self.q0[0] = np.pi self.q0[-2] = -np.pi/3 self.q0[2:-4] = -np.pi/6 self.v0 = zero(self.model.nv) self.collisionPairs = []
def loadTalosLegs(modelPath='/opt/openrobots/share/example-robot-data'): from pinocchio import JointModelFreeFlyer, JointModelRX, JointModelRY, JointModelRZ robot = loadTalos(modelPath=modelPath) SRDF_FILENAME = "talos.srdf" SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME legMaxId = 14 m1 = robot.model m2 = pinocchio.Model() for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements, m1.names, m1.parents, m1.inertias): if j.id < legMaxId: jid = m2.addJoint(parent, locals()[j.shortname()](), M, name) assert (jid == j.id) m2.appendBodyToJoint(jid, Y, pinocchio.SE3.Identity()) m2.upperPositionLimit = np.matrix([1.] * 19).T m2.lowerPositionLimit = np.matrix([-1.] * 19).T #q2 = robot.q0[:19] for f in m1.frames: if f.parent < legMaxId: m2.addFrame(f) g2 = pinocchio.GeometryModel() for g in robot.visual_model.geometryObjects: if g.parentJoint < 14: g2.addGeometryObject(g) robot.model = m2 robot.data = m2.createData() robot.visual_model = g2 # robot.q0=q2 robot.visual_data = pinocchio.GeometryData(g2) # Load SRDF file pinocchio.getNeutralConfiguration(m2, modelPath + SRDF_SUBPATH, False) pinocchio.loadRotorParameters(m2, modelPath + SRDF_SUBPATH, False) m2.armature = \ np.multiply(m2.rotorInertia.flat, np.square(m2.rotorGearRatio.flat)) assert ((m2.armature[:6] == 0.).all()) robot.q0 = m2.neutralConfiguration.copy() return robot
import sys import numpy as np import pinocchio as pin try: import hppfcl except ImportError: print("This example requires hppfcl") sys.exit(0) from pinocchio.visualize import GepettoVisualizer 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(
def loadTalos(legs=False, arm=False): URDF_FILENAME = "talos_left_arm.urdf" if arm else "talos_reduced.urdf" SRDF_FILENAME = "talos.srdf" robot = robot_loader('talos_data', URDF_FILENAME, SRDF_FILENAME, free_flyer=not arm) assert (robot.model.armature[:6] == 0.).all() if legs: legMaxId = 14 m1 = robot.model m2 = pin.Model() for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements, m1.names, m1.parents, m1.inertias): if j.id < legMaxId: jid = m2.addJoint(parent, getattr(pin, j.shortname())(), M, name) upperPos = m2.upperPositionLimit lowerPos = m2.lowerPositionLimit effort = m2.effortLimit upperPos[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.upperPositionLimit[j.idx_q:j.idx_q + j.nq] lowerPos[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.lowerPositionLimit[j.idx_q:j.idx_q + j.nq] effort[m2.joints[jid].idx_v:m2.joints[jid].idx_v + j.nv] = m1.effortLimit[j.idx_v:j.idx_v + j.nv] m2.upperPositionLimit = upperPos m2.lowerPositionLimit = lowerPos m2.effortLimit = effort assert jid == j.id m2.appendBodyToJoint(jid, Y, pin.SE3.Identity()) upperPos = m2.upperPositionLimit upperPos[:7] = 1 m2.upperPositionLimit = upperPos lowerPos = m2.lowerPositionLimit lowerPos[:7] = -1 m2.lowerPositionLimit = lowerPos effort = m2.effortLimit effort[:6] = np.inf m2.effortLimit = effort # q2 = robot.q0[:19] for f in m1.frames: if f.parent < legMaxId: m2.addFrame(f) g2 = pin.GeometryModel() for g in robot.visual_model.geometryObjects: if g.parentJoint < 14: g2.addGeometryObject(g) robot.model = m2 robot.data = m2.createData() robot.visual_model = g2 # robot.q0=q2 robot.visual_data = pin.GeometryData(g2) # Load SRDF file robot.q0 = robot.q0[:robot.model.nq] model_path = getModelPath(join('talos_data/robots', URDF_FILENAME)) robot.q0 = readParamsFromSrdf(robot.model, join(model_path, 'talos_data/srdf', SRDF_FILENAME), False) assert (m2.armature[:6] == 0.).all() # Add the free-flyer joint limits to the new model addFreeFlyerJointLimits(robot.model) return robot