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)
