def __init__(self,
name,
pinocchio_model,
pinocchio_data,
initialConfig,
OperationalPointsMap=None,
tracer=None):
AbstractHumanoidRobot.__init__(self, name, tracer)
self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest')
self.OperationalPointsMap = OperationalPointsMap
self.dynamic = DynamicPinocchio(self.name + "_dynamic")
self.dynamic.setModel(pinocchio_model)
self.dynamic.setData(pinocchio_data)
self.dimension = self.dynamic.getDimension()
self.device = RobotSimu(self.name + "_device")
self.device.resize(self.dynamic.getDimension())
self.halfSitting = initialConfig
self.device.set(self.halfSitting)
plug(self.device.state, self.dynamic.position)
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., )
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., )
if self.OperationalPointsMap is not None:
self.initializeOpPoints()
def __init__(self, name, pinocchio_model, pinocchio_data, initialConfig, OperationalPointsMap = None, tracer = None):
AbstractHumanoidRobot.__init__(self, name, tracer)
self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest')
self.OperationalPointsMap = OperationalPointsMap
self.dynamic = DynamicPinocchio(self.name + "_dynamic")
self.dynamic.setModel(pinocchio_model)
self.dynamic.setData(pinocchio_data)
self.dimension = self.dynamic.getDimension()
self.device = RobotSimu(self.name + "_device")
self.device.resize(self.dynamic.getDimension())
self.halfSitting = initialConfig
self.device.set(self.halfSitting)
plug(self.device.state, self.dynamic.position)
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.,)
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.,)
if self.OperationalPointsMap is not None:
self.initializeOpPoints()
robot.m2qLF = MatrixHomoToPoseQuaternion('m2qLF')
plug(robot.dynamic.LF, robot.m2qLF.sin)
plug(robot.m2qLF.sout, robot.base_estimator.lf_ref_xyzquat)
robot.m2qRF = MatrixHomoToPoseQuaternion('m2qRF')
plug(robot.dynamic.RF, robot.m2qRF.sin)
plug(robot.m2qRF.sout, robot.base_estimator.rf_ref_xyzquat)
# robot.be_filters = create_be_filters(robot, dt)
# --- Conversion
e2q = EulerToQuat('e2q')
plug(robot.base_estimator.q, e2q.euler)
robot.e2q = e2q
# --- Kinematic computations
robot.rdynamic = DynamicPinocchio("real_dynamics")
robot.rdynamic.setModel(robot.dynamic.model)
robot.rdynamic.setData(robot.rdynamic.model.createData())
plug(robot.base_estimator.q, robot.rdynamic.position)
robot.rdynamic.velocity.value = [0.0] * robotDim
robot.rdynamic.acceleration.value = [0.0] * robotDim
# --- CoM Estimation
cdc_estimator = DcmEstimator('cdc_estimator')
cdc_estimator.init(dt, robot_name)
plug(robot.e2q.quaternion, cdc_estimator.q)
plug(robot.base_estimator.v, cdc_estimator.v)
robot.cdc_estimator = cdc_estimator
# --- DCM Estimation
estimator = DummyDcmEstimator("dummy")
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()
def __init__(self, name):
DynamicPinocchio.__init__(self, name)
self.namespace = "sot_controller"
self.jointsParameterName_ = "jrl_map"
def __init__(self, name):
DynamicPinocchio.__init__(self, name)
self.namespace = "sot_controller"
self.jointsParameterName_ = "jrl_map"
