Commit 2f5d2c6b authored by Joseph Mirabel's avatar Joseph Mirabel Committed by olivier stasse

Follow changes in sot-dynamic-pinocchio.

parent a30f2ccb
......@@ -9,49 +9,47 @@ class Robot(Talos):
This class instantiates LAAS TALOS Robot
"""
halfSitting = (
0.0,
0.0,
1.018213,
0.00,
0.0,
0.0, # Free flyer
0.0,
0.0,
-0.411354,
0.859395,
-0.448041,
-0.001708, # Left Leg
0.0,
0.0,
-0.411354,
0.859395,
-0.448041,
-0.001708, # Right Leg
0.0,
0.006761, # Chest
0.25847,
0.173046,
-0.0002,
-0.525366,
0.0,
-0.0,
0.1,
-0.005, # Left Arm
-0.25847,
-0.173046,
0.0002,
-0.525366,
0.0,
0.0,
0.1,
-0.005, # Right Arm
0.,
0. # Head
)
def defineHalfSitting (self, q):
"""
q is the configuration to fill.
When this function is called, the attribute pinocchioModel has been filled.
"""
model = self.pinocchioModel
# Free flyer
q[2] = 1.018213
# left leg
self.setJointValueInConfig(q,
[ "leg_left_{}_joint".format(i+1) for i in range(6) ],
[ 0., 0., -0.411354, 0.859395, -0.448041, -0.001708,])
# right leg
self.setJointValueInConfig(q,
[ "leg_right_{}_joint".format(i+1) for i in range(6) ],
[ 0., 0., -0.411354, 0.859395, -0.448041, -0.001708,])
# torso
self.setJointValueInConfig(q,
[ "torso_{}_joint".format(i+1) for i in range(2) ],
[ 0., 0.006761])
# left arm
self.setJointValueInConfig(q,
[ "arm_left_{}_joint".format(i+1) for i in range(7) ],
[ 0.25847, 0.173046, -0.0002, -0.525366, 0., -0., 0.1 ])
# right arm
self.setJointValueInConfig(q,
[ "arm_right_{}_joint".format(i+1) for i in range(7) ],
[ -0.25847, -0.173046, 0.0002, -0.525366, 0., 0., 0.1 ])
# grippers
self.setJointValueInConfig(q,
[ "gripper_left_joint", "gripper_right_joint" ],
[ -0.005, -0.005 ])
# torso
self.setJointValueInConfig(q,
[ "head_{}_joint".format(i+1) for i in range(2) ],
[ 0., 0.])
def __init__(self, name, device=None, tracer=None, fromRosParam=None):
Talos.__init__(self, name, self.halfSitting, device, tracer)
Talos.__init__(self, name, device, tracer,
fromRosParam=fromRosParam)
"""
TODO:Confirm these values
# Define camera positions w.r.t gaze.
......
......@@ -5,8 +5,8 @@ from __future__ import print_function
from dynamic_graph import plug
from dynamic_graph.sot.core.math_small_entities import Derivator_of_Vector
from dynamic_graph.sot.dynamics_pinocchio import DynamicPinocchio
from dynamic_graph.sot.dynamics_pinocchio.humanoid_robot import AbstractHumanoidRobot
from dynamic_graph.sot.dynamic_pinocchio import DynamicPinocchio
from dynamic_graph.sot.dynamic_pinocchio.humanoid_robot import AbstractHumanoidRobot
import pinocchio
from rospkg import RosPack
......@@ -89,53 +89,16 @@ class Talos(AbstractHumanoidRobot):
self.dynamic = DynamicPinocchio(self.name + "_dynamic")
self.dynamic.setModel(self.pinocchioModel)
self.dynamic.setData(self.pinocchioData)
self.dynamic.displayModel()
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.tolist()[1:],
self.pinocchioModel.upperPositionLimit.tolist()[1:])
self.device.setVelocityBounds((-self.pinocchioModel.velocityLimit).tolist(),
self.pinocchioModel.velocityLimit.tolist())
self.device.setTorqueBounds((-self.pinocchioModel.effortLimit).tolist(),
self.pinocchioModel.effortLimit.tolist())
self.halfSitting = initialConfig
self.device.set(self.halfSitting)
plug(self.device.state, self.dynamic.position)
self.initializeRobot()
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()
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment