def __init__(self,*args):
filename = args[0]
if len(args) >= 2:
mesh_dir = args[1]
RobotWrapper.__init__(self,filename,mesh_dir,se3.JointModelFreeFlyer())
else:
RobotWrapper.__init__(self,filename,se3.JointModelFreeFlyer())
# RobotWrapper.__init__(self,filename, root_joint='ff')
self.q0 = np.matrix( [
0.0, 0.0, 0.648702, 0.0, 0.0 , 0.0, 1.0, # Free flyer 0-6
0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10
0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.174532925, # LARM 11-17
0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.174532925, # RARM 18-24
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # LLEG 25-30
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36
] ).T
self.ff = range(7)
self.chest = range(7,9)
self.head = range(9,11)
self.l_arm = range(11,18)
self.r_arm = range(18,25)
self.l_leg = range(25,31)
self.r_leg = range(31,37)
self.opCorrespondances = { "lh": "LARM_JOINT5",
"rh": "RARM_JOINT5",
"lf": "LLEG_JOINT5",
"rf": "RLEG_JOINT5",
}
for op,name in self.opCorrespondances.items():
idx = self.__dict__[op] = self.index(name)
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,filename):
RobotWrapper.__init__(self,filename)
a=-0.001616164386097708
self.q0= np.matrix( [
0, 0, 0.817, 0, 0, 0, 1, # Free flyer
0, 0.05, -0.40+a, 0.75, -0.35-a, -0.05, # left leg
0, -0.05, -0.40+a, 0.75, -0.35-a, 0.05, # right leg
0,0, # chest
0, 0.15, 0, 0.3, 0, 0.5, 0, # left arm
0, 0.15, 0, 0.3, 0, 0.5, 0, # right arm
0.0, 0.0 # head
] ).T
self.opCorrespondances = { "lh": "arm_left_7_joint",
"rh": "arm_right_7_joint",
"rf": "leg_right_6_joint",
"lf": "leg_left_6_joint",
}
for op,name in self.opCorrespondances.items():
idx = self.__dict__[op] = self.index(name)
def __init__(self, *args):
filename = args[0]
if len(args) >= 2:
mesh_dir = args[1]
RobotWrapper.__init__(self, filename, mesh_dir,
se3.JointModelFreeFlyer())
else:
RobotWrapper.__init__(self, filename, se3.JointModelFreeFlyer())
# RobotWrapper.__init__(self,filename, root_joint='ff')
self.q0 = np.matrix([
0.0,
0.0,
0.648702,
0.0,
0.0,
0.0,
1.0, # Free flyer 0-6
0.0,
0.0,
0.0,
0.0, # CHEST HEAD 7-10
0.261799388,
0.174532925,
0.0,
-0.523598776,
0.0,
0.0,
0.174532925, # LARM 11-17
0.261799388,
-0.174532925,
0.0,
-0.523598776,
0.0,
0.0,
0.174532925, # RARM 18-24
0.0,
0.0,
-0.453785606,
0.872664626,
-0.41887902,
0.0, # LLEG 25-30
0.0,
0.0,
-0.453785606,
0.872664626,
-0.41887902,
0.0, # RLEG 31-36
]).T
self.ff = range(7)
self.chest = range(7, 9)
self.head = range(9, 11)
self.l_arm = range(11, 18)
self.r_arm = range(18, 25)
self.l_leg = range(25, 31)
self.r_leg = range(31, 37)
self.opCorrespondances = {
"lh": "LARM_JOINT5",
"rh": "RARM_JOINT5",
"lf": "LLEG_JOINT5",
"rf": "RLEG_JOINT5",
}
for op, name in self.opCorrespondances.items():
idx = self.__dict__[op] = self.index(name)
