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 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 reduceModel(self, jointsToRemove, config, len_prefix=0): jointIds = [ self.model.getJointId(jn[len_prefix:]) for jn in jointsToRemove ] self.model, self.gmodel = pinocchio.buildReducedModel( self.model, self.gmodel, jointIds, np.array(config)) self.data = pinocchio.Data(self.model) self.gdata = pinocchio.GeometryData(self.gmodel)
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 __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 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
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 __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
def __init__(self, controller=None): """ Description: 1. Initiates, Builds & Starts Robot Model components. 2. Initializes 'NYUFingerSimulator()' as Visualizer. Args: 1. connect_hardware = True (default). Connects to the robot hardware if enabled else, runs as simulation. Keyword Args: 1. controller = 'None': Does not initialize a joystick. 'keyboard': Enables joystick mode on the keyboard. """ # Load the .urdf files self.package_dir = os.path.abspath(os.getcwd()) self.urdf_path = self.package_dir + '/model/fingeredu.urdf' # Create Models from the .urdf files self.model, self.collision_model, self.visual_model = \ pin.buildModelsFromUrdf(self.urdf_path, self.package_dir) self.collision_model.addAllCollisionPairs() # Create datas from models self.model_data = self.model.createData() self.collision_model.addAllCollisionPairs() self.collision_data = pin.GeometryData(self.collision_model) # Display the models in viewer self.simulator = NYUFingerSimulator(self.package_dir, self.urdf_path) # Get the FrameID of the EOAT self.EOAT_ID = self.model.getFrameId('finger_tip_link') # Get the FrameID of the Base self.Base_ID = self.model.getFrameId('base_link') # Read Initial Joint Values init_q, init_v = self.simulator.get_state() # Initial Position memory.pose = self.ForwardKinematics(init_q, init_v)[0] # PD Controller self.control_freq = 0.001 self.PD = SimPD(self.control_freq, self.simulator, self.model, self.model_data, self.ForwardKinematics, self.Jacobian, self.TimeDerivativeJacobian, 9500, 35, 1.5, 18000, 100, 50) # Direct Joint Limits J1_limits = np.pi / 2 J2_limits = np.pi J3_limits = np.pi self.JointLimits = np.array([J1_limits, J2_limits, J3_limits]) # Init. Motion Planner self.Motion = MotionPlanner(self.model, self.model_data, self.PD, self.simulator, self.ForwardKinematics, self.Jacobian, self.JointLimits) # Feature Configuration Spaces self.StablePose = init_q self.parking_pose = np.array([0, -np.pi / 4, np.pi / 4]) # Use Keyboard as a controller if controller == 'keyboard': TeleKeyboard = NumStick(self.Motion.LinearPlanner) TeleKeyboard.run()
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)
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
# Find the absolute path to the srdf file srdf_path = find_paths("solo12")["srdf"] # Disable collision pairs specified in the srdf pin.removeCollisionPairs(robot.model, robot.collision_model, srdf_path) # 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]
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