def createMonoped(nbJoint, linkLength=1.0, floatingMass=1.0, linkMass=1.0): rmodel = pinocchio.Model() prefix = '' baseInertia = pinocchio.Inertia(floatingMass, np.matrix([0.0, 0.0, 0.0]).T, np.diagflat([1e-6, 1e-6, 1e-6])) linkInertia = pinocchio.Inertia(linkMass, np.matrix([0.0, 0.0, linkLength/2]).T, linkMass/5*np.diagflat([1e-2, linkLength**2, 1e-2])) # PRISMATIC JOINT jointId = 0 jointPlacement = pinocchio.SE3.Identity() jointName,bodyName = ["prismatic_joint", "mass"] jointId = rmodel.addJoint(jointId, pinocchio.JointModelPZ(), jointPlacement, jointName) rmodel.addFrame(pinocchio.Frame('base', jointId, 0, jointPlacement, pinocchio.FrameType.OP_FRAME)) rmodel.appendBodyToJoint(jointId, baseInertia, pinocchio.SE3.Identity()) # REVOLUTE JOINTS for i in range(1, nbJoint + 1): jointName,bodyName = ["revolute_joint_" + str(i), "link_" + str(i)] jointId = rmodel.addJoint(jointId,pinocchio.JointModelRY(),jointPlacement,jointName) rmodel.appendBodyToJoint(jointId,linkInertia, pinocchio.SE3.Identity()) rmodel.addFrame(pinocchio.Frame('revolute_' + str(i), jointId, i-1, pinocchio.SE3.Identity(), pinocchio.FrameType.JOINT)) jointPlacement = pinocchio.SE3(eye(3), np.matrix([0.0, 0.0, linkLength]).T) rmodel.addFrame( pinocchio.Frame('foot', jointId, 0, jointPlacement, pinocchio.FrameType.OP_FRAME)) rmodel.upperPositionLimit = np.concatenate((np.array([100]), 2 * np.pi * np.ones(nbJoint)), axis=0) rmodel.lowerPositionLimit = np.concatenate((np.array([0.0]), -2 * np.pi * np.ones(nbJoint)), axis=0) rmodel.velocityLimit = np.concatenate((np.array([100]), 5 * np.ones(nbJoint)), axis=0) return rmodel
def loadTiago(initViewer=True): '''Load the Tiago robot, and add two operation frames: - at the front of the basis, named framebasis. - at the end effector, name frametool. Take care: Tiago as 3 continuous joints (one for the basis, two for the wheels), which makes nq == nv+3. ''' URDF_FILENAME = "tiago_no_hand.urdf" URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME modelPath = getModelPath(URDF_SUBPATH) robot = pio.RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pio.JointModelPlanar()) robot.model.addFrame( pio.Frame('framebasis', 1, 1, pio.SE3(numpy.eye(3), numpy.matrix([0.28, 0, 0.1]).T), pio.FrameType.OP_FRAME)) robot.model.addFrame( pio.Frame( 'frametool', 9, 51, pio.SE3(numpy.matrix([[0, 1., 0], [0, 0, 1], [1, 0, 0]]), numpy.matrix([0., 0, 0.06]).T), pio.FrameType.OP_FRAME)) robot.model.addFrame( pio.Frame('framegaze', 11, 57, pio.SE3(numpy.eye(3), numpy.matrix([0.4, 0, 0]).T), pio.FrameType.OP_FRAME)) robot.data = robot.model.createData() jllow = robot.model.lowerPositionLimit jllow[:2] = -2 robot.model.lowerPositionLimit = jllow jlupp = robot.model.upperPositionLimit jlupp[:2] = 2 robot.model.upperPositionLimit = jlupp def display(robot, q): robot.realdisplay(q) pio.updateFramePlacements(robot.model, robot.viz.data) robot.viewer.gui.applyConfiguration( 'world/' + robot.model.frames[-2].name, list(pio.se3ToXYZQUAT(robot.viz.data.oMf[-2]).flat)) robot.viewer.gui.applyConfiguration( 'world/' + robot.model.frames[-1].name, list(pio.se3ToXYZQUAT(robot.viz.data.oMf[-1]).flat)) robot.viewer.gui.refresh() if initViewer: robot.initViewer(loadModel=True) gv = robot.viewer.gui gv.addFloor('world/floor') gv.addXYZaxis('world/framebasis', [1., 0., 0., 1.], .03, .1) gv.addXYZaxis('world/frametool', [1., 0., 0., 1.], .03, .1) #gv.addXYZaxis('world/framegaze', [1., 0., 0., 1.], .03, .1) robot.realdisplay = robot.display import types robot.display = types.MethodType(display, robot) return robot
def test_frame_equality(self): M = pin.SE3.Random() frame1 = pin.Frame("name", 1, 2, M, pin.OP_FRAME) frame2 = pin.Frame("name", 1, 2, M, pin.OP_FRAME) frame3 = pin.Frame("othername", 3, 4, pin.SE3.Random(), pin.BODY) self.assertTrue(frame1 == frame2) self.assertFalse(frame1 != frame2) self.assertTrue(frame1 != frame3) self.assertFalse(frame1 == frame3)
def createPendulum(self,nbJoint,rootId=0,prefix='',jointPlacement=None): color = [red,green,blue,transparency] = [1,1,0.78,1.0] colorred = [1.0,0.0,0.0,1.0] jointId = rootId jointPlacement = jointPlacement if jointPlacement!=None else se3.SE3.Identity() length = self.length mass = self.mass # inertia = se3.Inertia(mass, # np.matrix([0.0,0.0,length/2]).T, # mass/5*np.diagflat([ 1e-2,length**2, 1e-2 ]) ) inertia = se3.Inertia(mass, np.matrix([0.0,0.0,length/2]).T, 0*mass/5*np.diagflat([ 1e-2,length**2, 1e-2 ]) ) for i in range(nbJoint): istr = str(i) name = prefix+"joint"+istr jointName,bodyName = [name+"_joint",name+"_body"] jointId = self.model.addJoint(jointId,se3.JointModelRY(),jointPlacement,jointName) self.model.appendBodyToJoint(jointId,inertia,se3.SE3.Identity()) if self.viewer is not None: try:self.viewer.viewer.gui.addSphere('world/'+prefix+'sphere'+istr, length*0.15,colorred) except: pass self.visuals.append( Visual('world/'+prefix+'sphere'+istr,jointId,se3.SE3.Identity()) ) try:self.viewer.viewer.gui.addCapsule('world/'+prefix+'arm'+istr, length*.1,.8*length,color) except:pass self.visuals.append( Visual('world/'+prefix+'arm'+istr,jointId, se3.SE3(eye(3),np.matrix([0.,0.,length/2])))) jointPlacement = se3.SE3(eye(3),np.matrix([0.0,0.0,length]).T) self.model.addFrame( se3.Frame('tip',jointId,0,jointPlacement,se3.FrameType.OP_FRAME) )
def createSoloTB(): rmodel = pinocchio.Model() prefix = '' nbJoint = 2 floatingMass = 1.48538/4 # one quater of total base mass linkLength = 0.16 firstLinkMass = 0.148538 secondLinkMass = 0.0376361 firstLinkCOMLength = 0.078707 secondLinkCOMLength = 0.102249 baseInertia = pinocchio.Inertia(floatingMass, np.matrix([0.0, 0.0, 0.0]).T, np.diagflat([1e-6, 1e-6, 1e-6])) firstLinkInertia = pinocchio.Inertia(firstLinkMass, np.matrix([0.0, 0.0, firstLinkCOMLength]).T, firstLinkMass/3*np.diagflat([1e-6, firstLinkCOMLength**2, 1e-6])) secondLinkInertia = pinocchio.Inertia(secondLinkMass, np.matrix([0.0, 0.0, secondLinkCOMLength]).T, secondLinkMass/3*np.diagflat([1e-6, secondLinkCOMLength**2, 1e-6])) # PRISMATIC JOINT jointId = 0 jointPlacement = pinocchio.SE3.Identity() jointName,bodyName = ["prismatic_joint", "mass"] jointId = rmodel.addJoint(jointId, pinocchio.JointModelPZ(), jointPlacement, jointName) rmodel.addFrame(pinocchio.Frame('base', jointId, 0, jointPlacement, pinocchio.FrameType.OP_FRAME)) rmodel.appendBodyToJoint(jointId, baseInertia, pinocchio.SE3.Identity()) # REVOLUTE JOINTS for i in range(1, nbJoint + 1): jointName,bodyName = ["revolute_joint_" + str(i), "link_" + str(i)] jointId = rmodel.addJoint(jointId,pinocchio.JointModelRY(),jointPlacement,jointName) if i != nbJoint: rmodel.appendBodyToJoint(jointId,firstLinkInertia, pinocchio.SE3.Identity()) else: rmodel.appendBodyToJoint(jointId,secondLinkInertia, pinocchio.SE3.Identity()) rmodel.addFrame(pinocchio.Frame('revolute_' + str(i), jointId, i-1, pinocchio.SE3.Identity(), pinocchio.FrameType.JOINT)) jointPlacement = pinocchio.SE3(eye(3), np.matrix([0.0, 0.0, linkLength]).T) rmodel.addFrame( pinocchio.Frame('foot', jointId, 0, jointPlacement, pinocchio.FrameType.OP_FRAME)) rmodel.upperPositionLimit = np.concatenate((np.array([100]), 2 * np.pi * np.ones(nbJoint)), axis=0) rmodel.lowerPositionLimit = np.concatenate((np.array([0.0]), -2 * np.pi * np.ones(nbJoint)), axis=0) rmodel.velocityLimit = np.concatenate((np.array([100]), 5 * np.ones(nbJoint)), axis=0) return rmodel
def __init__(self, urdf, pkgs): RobotWrapper.__init__(self, urdf, pkgs, pinocchio.JointModelPlanar()) M0 = pinocchio.SE3(eye(3), np.matrix([0., 0., .6]).T) self.model.jointPlacements[2] = M0 * self.model.jointPlacements[2] self.visual_model.geometryObjects[0].placement = M0 * self.visual_model.geometryObjects[0].placement self.visual_data.oMg[0] = M0 * self.visual_data.oMg[0] # Placement of the "mobile" frame wrt basis center. basisMop = pinocchio.SE3(eye(3), np.matrix([.3, .0, .1]).T) self.model.addFrame(pinocchio.Frame('mobile', 1, 1, basisMop, pinocchio.FrameType.OP_FRAME)) # Placement of the tool frame wrt end effector frame (located at the center of the wrist) effMop = pinocchio.SE3(eye(3), np.matrix([.0, .08, .095]).T) self.model.addFrame(pinocchio.Frame('tool', 6, 6, effMop, pinocchio.FrameType.OP_FRAME)) # Create data again after setting frames self.data = self.model.createData()
def __init__(self, name, filename, package_dirs, root_joint, consimVisualizer, visualOptions, conf): self.name = name self.contactNames= visualOptions["contact_names"] self.robotColor = visualOptions["robot_color"] self.forceColor = visualOptions["force_color"] self.coneColor = visualOptions["cone_color"] self.force_radius = visualOptions["force_radius"] self.force_length = visualOptions["force_length"] self.cone_length = visualOptions["cone_length"] self.friction_coeff = visualOptions["friction_coeff"] self.wireframeMode = visualOptions["wireframe_mode"]# "FILL", "WIREFRAME" or "FILL_AND_WIREFRAME". self.cone_radius = self.cone_length * self.friction_coeff # self.urdfDir = filename self.meshDir = package_dirs self.gui = consimVisualizer.gui self.sceneName = consimVisualizer.sceneName self.model, self.collision_model, self.visual_model = buildModelsFromUrdf(filename, package_dirs, root_joint) self.display_collisions = False self.display_visuals = True self.display_forces = True self.display_cones = True self.data, self.collision_data, self.visual_data = createDatas(self.model,self.collision_model,self.visual_model) self.rootNodeName="pinocchio" self.RootNodeName = self.sceneName + "/" + self.rootNodeName+ "_" + self.name self.forceGroup = self.RootNodeName + "/contact_forces" self.frictionGroup = self.RootNodeName + "/friction_cone" self.x_axis = np.array([1., 0., 0.]) self.z_axis = np.array([0., 0., 1.]) self.totalWeight = sum(m.mass for m in self.model.inertias) * np.linalg.norm(self.model.gravity.linear) self.parentIndex = conf.parent_frame_index self.type=conf.TYPE if (self.type=="Biped"): contact_point = np.ones((3,4)) * conf.lz contact_point[0, :] = [-conf.lxn, -conf.lxn, conf.lxp, conf.lxp] contact_point[1, :] = [-conf.lyn, conf.lyp, -conf.lyn, conf.lyp] for j,cf in enumerate(conf.parent_frames): parentFrameId = self.model.getFrameId(cf) parentJointId = self.model.frames[parentFrameId].parent for i in range(4): frame_name = conf.contact_frames[4*j + i] placement = pin.XYZQUATToSE3(list(contact_point[:,i])+[0, 0, 0, 1.]) placement = self.model.frames[parentFrameId].placement * placement fr = pin.Frame(frame_name, parentJointId, parentFrameId, placement, pin.FrameType.OP_FRAME) self.model.addFrame(fr) self.data = self.model.createData() elif(self.type=="Quadruped"): pass else: pass
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 setUp(self): self.model = pin.buildSampleModelHumanoidRandom() self.parent_idx = self.model.getJointId( "rarm2_joint") if self.model.existJointName("rarm2_joint") else ( self.model.njoints - 1) self.frame_name = self.model.names[self.parent_idx] + "_frame" self.frame_placement = pin.SE3.Random() self.frame_type = pin.FrameType.OP_FRAME self.model.addFrame( pin.Frame(self.frame_name, self.parent_idx, 0, self.frame_placement, self.frame_type)) self.frame_idx = self.model.getFrameId(self.frame_name)
def test_pickle(self): import pickle frame = pin.Frame("name", 1, 2, pin.SE3.Random(), pin.OP_FRAME) filename = "frame.pickle" with open(filename, 'wb') as f: pickle.dump(frame, f) with open(filename, 'rb') as f: frame_copy = pickle.load(f) self.assertEqual(frame, frame_copy)
def setUp(self): self.model = pin.buildSampleModelHumanoidRandom() self.parent_idx = self.model.getJointId( "rarm2_joint") if self.model.existJointName("rarm2_joint") else ( self.model.njoints - 1) self.frame_name = self.model.names[self.parent_idx] + "_frame" self.frame_placement = pin.SE3.Random() self.frame_type = pin.FrameType.OP_FRAME self.model.addFrame( pin.Frame(self.frame_name, self.parent_idx, 0, self.frame_placement, self.frame_type)) self.frame_idx = self.model.getFrameId(self.frame_name) self.data = self.model.createData() self.q = pin.randomConfiguration(self.model) self.v = np.random.rand((self.model.nv)) self.a = np.random.rand((self.model.nv))
robot = RobotWrapper.BuildFromURDF(conf.urdf, [conf.modelPath], pin.JointModelFreeFlyer()) contact_point = np.ones((3, 4)) * conf.lz contact_point[0, :] = [-conf.lxn, -conf.lxn, conf.lxp, conf.lxp] contact_point[1, :] = [-conf.lyn, conf.lyp, -conf.lyn, conf.lyp] contact_frames = [] for cf in conf.contact_frames: parentFrameId = robot.model.getFrameId(cf) parentJointId = robot.model.frames[parentFrameId].parent for i in range(4): frame_name = cf + "_" + str(i) placement = pin.XYZQUATToSE3( list(contact_point[:, i]) + [0, 0, 0, 1.]) placement = robot.model.frames[parentFrameId].placement * placement fr = pin.Frame(frame_name, parentJointId, parentFrameId, placement, pin.FrameType.OP_FRAME) robot.model.addFrame(fr) contact_frames += [frame_name] conf.contact_frames = contact_frames robot.data = robot.model.createData() #conf.unilateral_contacts = 1 PRINT_N = int(conf.PRINT_T / dt) ground_truth_file_name = robot_name + "_" + motionName + str(dt) + "_cpp.p" nq, nv = robot.nq, robot.nv if conf.use_viewer: import subprocess, os launched = subprocess.getstatusoutput( "ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
def buildModel(self): '''Internal:build a 3+2+2 DOF model representing a simplified version of HRP-2 legs.''' robot = self.hrpfull se3.crba(robot.model, robot.data, robot.q0) se3.forwardKinematics(robot.model, robot.data, robot.q0) WAIST_ID = 1 CHEST_ID = 2 RHIP_ID = 26 LHIP_ID = 20 RTHIGH_ID = 28 LTHIGH_ID = 22 RTIBIA_ID = 29 LTIBIA_ID = 23 RKNEE_ID = 29 LKNEE_ID = 23 RANKLE_ID = 31 LANKLE_ID = 25 self.viewer = viewer = robot.viewer.gui self.model = modelred = se3.Model.BuildEmptyModel() self.visuals = visuals = [] colorwhite = [red, green, blue, transparency] = [1, 1, 0.78, 1.0] colorred = [1.0, 0.0, 0.0, 1.0] colorblue = [0.0, 0.0, 0.8, 1.0] jointId = 0 # Add first (free-floating) joint name = "waist" JointModel = se3.JointModelPlanar inertia = (robot.data.oMi[WAIST_ID].inverse()*robot.data.oMi[CHEST_ID])\ .act(robot.data.Ycrb[CHEST_ID])+robot.model.inertias[WAIST_ID] placement = se3.SE3(rotate('x', pi / 2) * rotate('y', pi / 2), zero(3)) t = placement.translation t[0] = 0 placement.translation = t jointName, bodyName = [name + "_joint", name + "_body"] jointId = modelred.addJoint(jointId, JointModel(), placement, jointName) modelred.appendBodyToJoint(jointId, inertia, se3.SE3.Identity()) viewer.addSphere('world/red' + bodyName, 0.05, colorwhite) visuals.append( Visual('world/red' + bodyName, jointId, se3.SE3.Identity())) # Add right hip joint name = "rhip" JointModel = se3.JointModelRX inertia = robot.model.inertias[RTHIGH_ID] placement = robot.data.oMi[WAIST_ID].inverse( ) * robot.data.oMi[RHIP_ID] t = placement.translation t[0] = 0 t[1] = -0.095 placement.translation = t placement = se3.SE3( rotate('x', -pi / 2) * rotate('z', -pi / 2), zero(3)) * placement jointName, bodyName = [name + "_joint", name + "_body"] jointId = modelred.addJoint(jointId, JointModel(), placement, jointName) modelred.appendBodyToJoint(jointId, inertia, se3.SE3.Identity()) viewer.addSphere('world/red' + bodyName, 0.05, colorred) visuals.append( Visual('world/red' + bodyName, jointId, se3.SE3.Identity())) # Add right knee joint name = "rknee" JointModel = se3.JointModelPZ inertia = robot.model.inertias[RTIBIA_ID] placement = robot.data.oMi[RHIP_ID].inverse( ) * robot.data.oMi[RKNEE_ID] t = placement.translation t[0] = 0 placement.translation = t placement.rotation = rotate('y', 0) jointName, bodyName = [name + "_joint", name + "_body"] jointId = modelred.addJoint(jointId, JointModel(), placement, jointName) modelred.appendBodyToJoint(jointId, inertia, se3.SE3.Identity()) #viewer.addSphere('world/red'+bodyName, 0.05,colorred) viewer.addBox('world/red' + bodyName, 0.05, 0.05, 0.05, colorred) visuals.append( Visual('world/red' + bodyName, jointId, se3.SE3.Identity())) # Add right ankle spot name = "rankle" placement = robot.data.oMi[RKNEE_ID].inverse( ) * robot.data.oMi[RANKLE_ID] t = placement.translation t[0] = 0 t[1] = 0 placement.translation = t placement.rotation = rotate('y', 0) modelred.addFrame( se3.Frame(name, jointId, 0, placement, se3.FrameType.OP_FRAME)) viewer.addSphere('world/red' + name, 0.05, colorred) visuals.append( Visual('world/red' + name, jointId, placement * se3.SE3(eye(3), np.matrix([0, 0, .05]).T))) viewer.addBox('world/red' + name + 'sole', 0.15, .06, .01, colorred) visuals.append(Visual('world/red' + name + 'sole', jointId, placement)) # Add left hip name = "lhip" JointModel = se3.JointModelRX inertia = robot.model.inertias[LTHIGH_ID] placement = robot.data.oMi[WAIST_ID].inverse( ) * robot.data.oMi[LHIP_ID] t = placement.translation t[0] = 0 t[1] = 0.095 placement.translation = t placement = se3.SE3( rotate('x', -pi / 2) * rotate('z', -pi / 2), zero(3)) * placement jointName, bodyName = [name + "_joint", name + "_body"] jointId = modelred.addJoint(1, JointModel(), placement, jointName) modelred.appendBodyToJoint(jointId, inertia, se3.SE3.Identity()) viewer.addSphere('world/red' + bodyName, 0.05, colorblue) visuals.append( Visual('world/red' + bodyName, jointId, se3.SE3.Identity())) # Add left knee name = "lknee" JointModel = se3.JointModelPZ inertia = robot.model.inertias[LTIBIA_ID] placement = robot.data.oMi[LHIP_ID].inverse( ) * robot.data.oMi[LKNEE_ID] t = placement.translation t[0] = 0 placement.translation = t placement.rotation = rotate('y', 0) jointName, bodyName = [name + "_joint", name + "_body"] jointId = modelred.addJoint(jointId, JointModel(), placement, jointName) modelred.appendBodyToJoint(jointId, inertia, se3.SE3.Identity()) #viewer.addSphere('world/red'+bodyName, 0.05,colorblue) viewer.addBox('world/red' + bodyName, 0.05, 0.05, 0.05, colorblue) visuals.append( Visual('world/red' + bodyName, jointId, se3.SE3.Identity())) # Add left ankle spot name = "lankle" placement = robot.data.oMi[LKNEE_ID].inverse( ) * robot.data.oMi[LANKLE_ID] t = placement.translation t[0] = 0 t[1] = 0 placement.translation = t placement.rotation = rotate('y', 0) modelred.addFrame( se3.Frame(name, jointId, 0, placement, se3.FrameType.OP_FRAME)) viewer.addSphere('world/red' + name, 0.05, colorblue) visuals.append( Visual('world/red' + name, jointId, placement * se3.SE3(eye(3), np.matrix([0, 0, .05]).T))) viewer.addBox('world/red' + name + 'sole', 0.15, .06, .01, colorblue) visuals.append(Visual('world/red' + name + 'sole', jointId, placement)) self.data = modelred.createData() self.q0 = np.matrix([0, 0.569638, 1., 0, 0, 0, 0, 0]).T
d = m.createData() # Set the joint configuration (x,y,z,qx,qy,qz,qw) q = pin.utils.zero(m.nq) q[0] = 1. q[1] = 2. q[2] = 0. q[3] = 0. q[4] = 0. q[5] = 0.259 q[6] = 0.966 theta = math.pi / 6 print(q) # Create contact frame w.r.t. the object's CoM object_contactId = m.addFrame( pin.Frame("object_contactPoint", m.getJointId("box_root_joint"), 0, pin.SE3.Identity(), pin.FrameType.OP_FRAME)) # Set the displacement of the contact frame w.r.t. the object's CoM lx = -0.025 ly = 0.01 lz = 0. m.frames[object_contactId].placement.translation = np.array([lx, ly, lz]) # Perform forward kinematics pin.computeJointJacobians(m, d, q) pin.framesForwardKinematics(m, d, q) # Get and print the Jacobians w.r.t. local, local-world-aligned, and world frames Jlocal = pin.getFrameJacobian(m, d, object_contactId, pin.ReferenceFrame.LOCAL) print(Jlocal) Jworld = pin.getFrameJacobian(m, d, object_contactId, pin.ReferenceFrame.WORLD) print(Jworld) Jlocalw = pin.getFrameJacobian(m, d, object_contactId, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)