def __init__(self,
filename=MODELPATH[0] +
'baxter_description/urdf/baxter.urdf'):
RobotWrapper.initFromURDF(self, filename, MODELPATH)
self.Q_INIT = np.zeros(self.nq - NQ_OFFSET)
self.q_def = np.zeros(self.nq)
self.v_def = np.zeros(self.nv)
# 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 = list(range(7))
self.head = list(range(7, 7))
self.l_arm = list(range(8, 14))
self.r_arm = list(range(15, 21))
self.opCorrespondances = {
"lh": "left_w2",
"rh": "right_w2",
}
for op, name in list(self.opCorrespondances.items()):
idx = self.__dict__[op] = self.index(name)
def __init__(self, name, initialConfig, device=None, tracer=None):
self.OperationalPointsMap = {
'left-wrist': 'arm_left_7_joint',
'right-wrist': 'arm_right_7_joint',
'left-ankle': 'leg_left_6_joint',
'right-ankle': 'leg_right_6_joint',
'gaze': 'head_2_joint',
'waist': 'root_joint',
'chest': 'torso_2_joint'
}
from rospkg import RosPack
rospack = RosPack()
urdfPath = rospack.get_path('talos_data') + "/urdf/talos_reduced.urdf"
urdfDir = [rospack.get_path('talos_data') + "/../"]
# Create a wrapper to access the dynamic model provided through an urdf file.
from pinocchio.robot_wrapper import RobotWrapper
import pinocchio as se3
from dynamic_graph.sot.dynamics_pinocchio import fromSotToPinocchio
pinocchioRobot = RobotWrapper()
pinocchioRobot.initFromURDF(urdfPath, urdfDir,
se3.JointModelFreeFlyer())
self.pinocchioModel = pinocchioRobot.model
self.pinocchioData = pinocchioRobot.data
AbstractHumanoidRobot.__init__(self, name, tracer)
self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest')
# Create rigid body dynamics model and data (pinocchio)
self.dynamic = DynamicPinocchio(self.name + "_dynamic")
self.dynamic.setModel(self.pinocchioModel)
self.dynamic.setData(self.pinocchioData)
self.dimension = self.dynamic.getDimension()
# Initialize device
self.device = device
self.timeStep = self.device.getTimeStep()
self.device.resize(self.dynamic.getDimension())
# TODO For position limit, we remove the first value to get
# a vector of the good size because SoT use euler angles and not
# quaternions...
self.device.setPositionBounds(
self.pinocchioModel.lowerPositionLimit.T.tolist()[0][1:],
self.pinocchioModel.upperPositionLimit.T.tolist()[0][1:])
self.device.setVelocityBounds(
(-self.pinocchioModel.velocityLimit).T.tolist()[0],
self.pinocchioModel.velocityLimit.T.tolist()[0])
self.device.setTorqueBounds(
(-self.pinocchioModel.effortLimit).T.tolist()[0],
self.pinocchioModel.effortLimit.T.tolist()[0])
self.halfSitting = initialConfig
self.device.set(self.halfSitting)
plug(self.device.state, self.dynamic.position)
self.AdditionalFrames.append(
("leftFootForceSensor", self.forceSensorInLeftAnkle,
self.OperationalPointsMap["left-ankle"]))
self.AdditionalFrames.append(
("rightFootForceSensor", self.forceSensorInRightAnkle,
self.OperationalPointsMap["right-ankle"]))
self.dimension = self.dynamic.getDimension()
self.plugVelocityFromDevice = True
self.dynamic.displayModel()
# Initialize velocity derivator if chosen
if self.enableVelocityDerivator:
self.velocityDerivator = Derivator_of_Vector('velocityDerivator')
self.velocityDerivator.dt.value = self.timeStep
plug(self.device.state, self.velocityDerivator.sin)
plug(self.velocityDerivator.sout, self.dynamic.velocity)
else:
self.dynamic.velocity.value = self.dimension * (0., )
# Initialize acceleration derivator if chosen
if self.enableAccelerationDerivator:
self.accelerationDerivator = \
Derivator_of_Vector('accelerationDerivator')
self.accelerationDerivator.dt.value = self.timeStep
plug(self.velocityDerivator.sout, self.accelerationDerivator.sin)
plug(self.accelerationDerivator.sout, self.dynamic.acceleration)
else:
self.dynamic.acceleration.value = self.dimension * (0., )
# Create operational points based on operational points map (if provided)
if self.OperationalPointsMap is not None:
self.initializeOpPoints()
