def initializeRobot(self):
"""
If the robot model is correctly loaded, this method will then
initialize the operational points, set the position to
half-sitting with null velocity/acceleration.
To finish, different tasks are initialized:
- the center of mass task used to keep the robot stability
- one task per operational point to ease robot control
"""
if not self.dynamic:
raise RunTimeError("robots models have to be initialized first")
if not self.device:
self.device = RobotSimu(self.name + '_device')
"""
Robot timestep
"""
self.timeStep = self.device.getTimeStep()
# Freeflyer reference frame should be the same as global
# frame so that operational point positions correspond to
# position in freeflyer frame.
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.,)
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, robot, name='flextimator', useMocap=True, dt=0.005):
DGIMUModelBaseFlexEstimation.__init__(self,name)
self.setSamplingPeriod(dt)
self.robot = robot
initDevice(self.robot)
computeDynamic(self.robot,0)
# Covariances
self.setProcessNoiseCovariance(matrixToTuple(np.diag((1e-8,)*12+(1e-4,)*3+(1e-4,)*3+(1e-4,)*3+(1e-4,)*3+(1.e-2,)*6+(1e-15,)*2+(1.e-8,)*3)))
self.setMeasurementNoiseCovariance(matrixToTuple(np.diag((1e-3,)*3+(1e-6,)*3)))
self.setUnmodeledForceVariance(1e-13)
self.setForceVariance(1e-4)
self.setAbsolutePosVariance(1e-4)
# Contact model definition
# self.setKfe(matrixToTuple(np.diag((40000,40000,40000))))
self.setKfe(matrixToTuple(np.diag((150000,150000,150000))))
self.setKfv(matrixToTuple(np.diag((600,600,600))))
self.setKte(matrixToTuple(np.diag((600,600,600))))
self.setKtv(matrixToTuple(np.diag((10,10,10))))
self.setKfeCordes(matrixToTuple(np.diag((10000,10000,10000))))
self.setKfvCordes(matrixToTuple(np.diag((300,300,800))))
self.setKteCordes(matrixToTuple(np.diag((600,600,600))))
self.setKtvCordes(matrixToTuple(np.diag((60,60,60))))
# Estimator interface
self.interface=EstimatorInterface(name+"EstimatorInterface")
self.interface.setSamplingPeriod(dt)
self.interface.setLeftHandSensorTransformation((0.,0.,1.57))
self.interface.setRightHandSensorTransformation((0.,0.,1.57))
# State and measurement definition
self.interface.setWithUnmodeledMeasurements(False)
self.interface.setWithModeledForces(True)
self.interface.setWithAbsolutePose(False)
self.setWithComBias(False)
# Contacts velocities
self.leftFootVelocity = Multiply_matrix_vector ('leftFootVelocity')
plug(self.robot.frames['leftFootForceSensor'].jacobian,self.leftFootVelocity.sin1)
plug(self.robot.device.velocity,self.leftFootVelocity.sin2)
self.rightFootVelocity = Multiply_matrix_vector ('rightFootVelocity')
plug(self.robot.frames['rightFootForceSensor'].jacobian,self.rightFootVelocity.sin1)
plug(self.robot.device.velocity,self.rightFootVelocity.sin2)
self.interface.setFDInertiaDot(True)
# Contacts forces, positions and velocities
# Feet
plug (self.robot.device.forceLLEG,self.interface.force_lf)
plug (self.robot.device.forceRLEG,self.interface.force_rf)
plug (self.robot.frames['leftFootForceSensor'].position,self.interface.position_lf)
plug (self.robot.frames['rightFootForceSensor'].position,self.interface.position_rf)
plug (self.leftFootVelocity.sout,self.interface.velocity_lf)
plug (self.rightFootVelocity.sout,self.interface.velocity_rf)
# Hands
plug (self.robot.device.forceLARM,self.interface.force_lh)
plug (self.robot.device.forceRARM,self.interface.force_rh)
plug (self.robot.dynamic.signal('right-wrist'),self.interface.position_lh)
plug (self.robot.dynamic.signal('left-wrist'),self.interface.position_rh)
# Strings
self.Peg = (0,0,4.60) # Position of the anchorage in the global frame
self.setPe(self.Peg)
self.Prl1 = np.matrix([[1,0,0,0-3.19997004e-02],[0,1,0,0.15-0],[0,0,1,1.28-1],[0,0,0,1]]) # Positions of the contacts on the robot (in the local frame) with respect to the chest
self.Prl2 = np.matrix([[1,0,0,0-3.19997004e-02],[0,1,0,-0.15-0],[0,0,1,1.28-1],[0,0,0,1]])
(self.contact1OpPoint,self.contact1Pos,self.contact1)=self.createContact('contact1', self.Prl1)
(self.contact2OpPoint,self.contact2Pos,self.contact2)=self.createContact('contact2', self.Prl2)
plug(self.contact1.sout,self.interface.position_ls)
plug(self.contact2.sout,self.interface.position_rs)
# Contacts model and config
plug(self.interface.contactsModel,self.contactsModel)
self.setWithConfigSignal(True)
plug(self.interface.config,self.config)
if(useMocap):
# Mocap signal
self.ros = RosExport('rosExportMocap')
self.ros.add('matrixHomoStamped', "chest", "/evart/hrp2_14_head/hrp2_14_head")
# Filtering
from dynamic_graph.sot.tools import MocapDataFilter
self.mocapFilter = MocapDataFilter('MocapDataFilter')
plug(self.ros.signal('chest'),self.mocapFilter.sin)
self.mocapSignal = self.mocapFilter.sout
# Drift
self.drift = DriftFromMocap(name+'Drift')
plug(self.mocapSignal,self.drift.limbGlobal)
plug(self.robot.dynamic.chest,self.drift.limbLocal)
self.drift.init()
plug(self.drift.driftInvVector,self.interface.drift)
# Measurement reconstruction
plug(self.robot.device.accelerometer,self.interface.accelerometer)
plug(self.robot.device.gyrometer,self.interface.gyrometer)
plug(self.interface.measurement,self.measurement)
# Input reconstruction
# IMU Vector
self.inputPos = MatrixHomoToPoseUTheta(name+'InputPosition')
plug(robot.frames['accelerometer'].position,self.inputPos.sin)
self.robot.dynamic.createJacobian(name+'ChestJ_OpPoint','chest')
self.imuOpPoint = OpPointModifier(name+'IMU_oppoint')
self.imuOpPoint.setEndEffector(False)
self.imuOpPoint.setTransformation(matrixToTuple(np.linalg.inv(np.matrix(self.robot.dynamic.chest.value))*np.matrix(self.robot.frames['accelerometer'].position.value)))
plug (self.robot.dynamic.chest,self.imuOpPoint.positionIN)
plug (self.robot.dynamic.signal(name+'ChestJ_OpPoint'),self.imuOpPoint.jacobianIN)
self.inputVel = Multiply_matrix_vector(name+'InputVelocity')
plug(self.imuOpPoint.jacobian,self.inputVel.sin1)
plug(self.robot.device.velocity,self.inputVel.sin2)
self.inputPosVel = Stack_of_vector (name+'InputPosVel')
plug(self.inputPos.sout,self.inputPosVel.sin1)
plug(self.inputVel.sout,self.inputPosVel.sin2)
self.inputPosVel.selec1 (0, 6)
self.inputPosVel.selec2 (0, 6)
self.IMUVector = PositionStateReconstructor (name+'EstimatorInput')
self.IMUVector.setSamplingPeriod(dt)
plug(self.inputPosVel.sout,self.IMUVector.sin)
self.IMUVector.inputFormat.value = '001111'
self.IMUVector.outputFormat.value = '011111'
self.IMUVector.setFiniteDifferencesInterval(1)
self.inputPosVel.sout.recompute(0)
self.IMUVector.setLastVector(self.inputPosVel.sout.value+(0.,)*6)
# CoM and derivatives
self.com=self.robot.dynamic.com
self.DCom = Multiply_matrix_vector(name+'DCom')
plug(self.robot.dynamic.Jcom,self.DCom.sin1)
plug(self.robot.device.velocity,self.DCom.sin2)
self.comVectorIn = Stack_of_vector (name+'ComVectorIn')
plug(self.com,self.comVectorIn.sin1)
plug(self.DCom.sout,self.comVectorIn.sin2)
self.comVectorIn.selec1 (0, 3)
self.comVectorIn.selec2 (0, 3)
self.comVector = PositionStateReconstructor (name+'ComVector')
self.comVector.setSamplingPeriod(dt)
plug(self.comVectorIn.sout,self.comVector.sin)
self.comVector.inputFormat.value = '000101'
self.comVector.outputFormat.value = '010101'
self.comVector.setFiniteDifferencesInterval(1)
self.DCom.sout.recompute(0)
self.comVector.setLastVector(self.com.value+(0.,)*15)#(0.,)*3+self.DCom.sout.value+(0.,)*9)
# Compute derivative of Angular Momentum
self.angMomDerivator = Derivator_of_Vector(name+'angMomDerivator')
plug(self.robot.dynamic.angularmomentum,self.angMomDerivator.sin)
self.angMomDerivator.dt.value = dt
# self.angMomDerivator = PositionStateReconstructor (name+'angMomDerivator')
# self.angMomDerivator.setSamplingPeriod(dt)
# plug(self.robot.dynamic.angularmomentum,self.angMomDerivator.sin)
# self.angMomDerivator.inputFormat.value = '000001'
# self.angMomDerivator.outputFormat.value = '000100'
# self.angMomDerivator.setFiniteDifferencesInterval(2)
# self.robot.dynamic.angularmomentum.recompute(0)
# self.angMomDerivator.setLastVector(self.robot.dynamic.angularmomentum.value+(0.,)*15)
# Concatenate with interface estimator
plug(self.comVector.sout,self.interface.comVector)
plug(self.robot.dynamic.inertia,self.interface.inertia)
self.interface.dinertia.value=(0,0,0,0,0,0)
plug(self.robot.dynamic.angularmomentum,self.interface.angMomentum)
plug(self.angMomDerivator.sout,self.interface.dangMomentum)
plug(self.robot.dynamic.waist,self.interface.positionWaist)
plug(self.IMUVector.sout,self.interface.imuVector)
plug(self.interface.input,self.input)
plug (self.interface.modeledContactsNbr,self.contactNbr)
self.robot.flextimator = self
def initializeRobot(self):
"""
If the robot model is correctly loaded, this method will then
initialize the operational points, set the position to
half-sitting with null velocity/acceleration.
To finish, different tasks are initialized:
- the center of mass task used to keep the robot stability
- one task per operational point to ease robot control
"""
if not self.dynamic:
raise RunTimeError("robots models have to be initialized first")
if not self.device:
self.device = RobotSimu(self.name + '_device')
# Freeflyer reference frame should be the same as global
# frame so that operational point positions correspond to
# position in freeflyer frame.
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.,)
self.initializeOpPoints(self.dynamic)
# --- additional frames ---
self.frames = dict()
frameName = 'rightHand'
hp = self.dynamic.getHandParameter (True)
transformation = list (map (list, I4))
for i in range (3): transformation [i][3] = hp [i][3]
transformation = tuple (map (tuple, transformation))
self.frames [frameName] = self.createFrame (
"{0}_{1}".format (self.name, frameName),
transformation, "right-wrist")
frameName = 'leftHand'
hp = self.dynamic.getHandParameter (False)
transformation = list (map (list, I4))
for i in range (3): transformation [i][3] = hp [i][3]
transformation = tuple (map (tuple, transformation))
self.frames [frameName] = self.createFrame (
"{0}_{1}".format (self.name, frameName),
self.dynamic.getHandParameter (False), "left-wrist")
for (frameName, transformation, signalName) in self.AdditionalFrames:
self.frames[frameName] = self.createFrame(
"{0}_{1}".format(self.name, frameName),
transformation,
signalName)
def __init__(self, robot, name='flextimator2'):
DGIMUModelBaseFlexEstimation.__init__(self,name)
self.setSamplingPeriod(0.005)
self.robot = robot
# Definition of IMU vector
self.sensorStack = Stack_of_vector (name+'Sensors')
plug(self.robot.device.accelerometer,self.sensorStack.sin1)
plug(self.robot.device.gyrometer,self.sensorStack.sin2)
self.sensorStack.selec1 (0, 3)
self.sensorStack.selec2 (0, 3)
# Calibration
self.calibration= Calibrate('calibration')
plug(self.sensorStack.sout,self.calibration.imuIn)
plug(self.robot.dynamic.com,self.calibration.comIn)
plug(self.calibration.imuOut,self.measurement)
self.inputPos = MatrixHomoToPoseUTheta(name+'InputPosition')
plug(robot.frames['accelerometer'].position,self.inputPos.sin)
self.robot.dynamic.createJacobian('ChestJ_OpPoint','chest')
self.imuOpPoint = OpPointModifier('IMU_oppoint')
self.imuOpPoint.setEndEffector(False)
self.imuOpPoint.setTransformation(matrixToTuple(np.linalg.inv(np.matrix(self.robot.dynamic.chest.value))*np.matrix(self.robot.frames['accelerometer'].position.value)))
plug (self.robot.dynamic.chest,self.imuOpPoint.positionIN)
plug (self.robot.dynamic.signal('ChestJ_OpPoint'),self.imuOpPoint.jacobianIN)
self.inputVel = Multiply_matrix_vector(name+'InputVelocity')
plug(self.imuOpPoint.jacobian,self.inputVel.sin1)
plug(self.robot.device.velocity,self.inputVel.sin2)
self.inputPosVel = Stack_of_vector (name+'InputPosVel')
plug(self.inputPos.sout,self.inputPosVel.sin1)
plug(self.inputVel.sout,self.inputPosVel.sin2)
self.inputPosVel.selec1 (0, 6)
self.inputPosVel.selec2 (0, 6)
self.IMUVector = PositionStateReconstructor (name+'EstimatorInput')
plug(self.inputPosVel.sout,self.IMUVector.sin)
self.IMUVector.inputFormat.value = '001111'
self.IMUVector.outputFormat.value = '011111'
self.IMUVector.setFiniteDifferencesInterval(2)
# Definition of inertia, angular momentum and derivatives
self.robot.dynamic.inertia.recompute(0)
self.inertia=self.robot.dynamic.inertia #(48.2378,48.2378,2.87339,0,0,0) #
self.dotInertia=(0,0,0,0,0,0)
self.zeroMomentum=(0,0,0,0,0,0)
# Waist position
self.robot.dynamic.waist.recompute(0)
#self.robot.dynamic.chest.recompute(1)
#self.robot.dynamic.com.recompute(1)
self.positionWaist=self.robot.dynamic.waist
# Definition of com and derivatives
self.com=self.calibration.comOut #self.robot.dynamic.com
self.DCom = Multiply_matrix_vector(name+'DCom')
self.robot.dynamic.Jcom.recompute(0)
plug(self.robot.dynamic.Jcom,self.DCom.sin1)
plug(self.robot.device.velocity,self.DCom.sin2)
self.comVectorIn = Stack_of_vector (name+'ComVectorIn')
plug(self.calibration.comOut,self.comVectorIn.sin1)
plug(self.DCom.sout,self.comVectorIn.sin2)
self.comVectorIn.selec1 (0, 3)
self.comVectorIn.selec2 (0, 3)
self.comVector = PositionStateReconstructor (name+'ComVector')
plug(self.comVectorIn.sout,self.comVector.sin)
self.comVector.inputFormat.value = '000101'
self.comVector.outputFormat.value = '010101'
# Concatenate with InputReconstructor entity
self.inputVector=InputReconstructor(name+'inputVector')
plug(self.comVector.sout,self.inputVector.comVector)
plug(self.inertia,self.inputVector.inertia)
self.inputVector.dinertia.value=self.dotInertia
plug(self.positionWaist,self.inputVector.positionWaist)
self.inputVector.setSamplingPeriod(robot.timeStep)
self.inputVector.setFDInertiaDot(True)
plug(self.robot.dynamic.angularmomentum,self.inputVector.angMomentum)
self.angMomDerivator = Derivator_of_Vector('angMomDerivator')
plug(self.robot.dynamic.angularmomentum,self.angMomDerivator.sin)
self.angMomDerivator.dt.value = self.robot.timeStep
plug(self.angMomDerivator.sout,self.inputVector.dangMomentum)
#self.inputVector.dangMomentum.value = self.zeroMomentum
plug(self.IMUVector.sout,self.inputVector.imuVector)
plug(self.contactNbr,self.inputVector.nbContacts)
# plug(self.contacts,self.inputVector.contactsPosition)
plug(self.inputVector.input,self.input)
self.robot.flextimator = self
kfe=40000
kfv=600
kte=600
ktv=60
self.setKfe(matrixToTuple(np.diag((kfe,kfe,kfe))))
self.setKfv(matrixToTuple(np.diag((kfv,kfv,kfv))))
self.setKte(matrixToTuple(np.diag((kte,kte,kte))))
self.setKtv(matrixToTuple(np.diag((ktv,ktv,ktv))))
def __init__(self, robot, name='flextimatorEncoders'):
DGIMUModelBaseFlexEstimation.__init__(self, name)
self.setSamplingPeriod(0.005)
self.robot = robot
# Covariances
self.setProcessNoiseCovariance(
matrixToTuple(
np.diag((1e-8, ) * 12 + (1e-4, ) * 3 + (1e-4, ) * 3 +
(1e-4, ) * 3 + (1e-4, ) * 3 + (1.e-2, ) * 6 +
(1e-15, ) * 2 + (1.e-8, ) * 3)))
self.setMeasurementNoiseCovariance(
matrixToTuple(np.diag((1e-3, ) * 3 + (1e-6, ) * 3)))
self.setUnmodeledForceVariance(1e-13)
self.setForceVariance(1e-4)
self.setAbsolutePosVariance(1e-4)
# Contact model definition
self.setContactModel(1)
self.setKfe(matrixToTuple(np.diag((40000, 40000, 40000))))
self.setKfv(matrixToTuple(np.diag((600, 600, 600))))
self.setKte(matrixToTuple(np.diag((600, 600, 600))))
self.setKtv(matrixToTuple(np.diag((60, 60, 60))))
#Estimator interface
self.interface = EstimatorInterface(name + "EstimatorInterface")
self.interface.setLeftHandSensorTransformation((0., 0., 1.57))
self.interface.setRightHandSensorTransformation((0., 0., 1.57))
self.interface.setFDInertiaDot(True)
# State and measurement definition
self.interface.setWithUnmodeledMeasurements(False)
self.interface.setWithModeledForces(True)
self.interface.setWithAbsolutePose(False)
self.setWithComBias(False)
# Contacts forces
plug(self.robot.device.forceLLEG, self.interface.force_lf)
plug(self.robot.device.forceRLEG, self.interface.force_rf)
plug(self.robot.device.forceLARM, self.interface.force_lh)
plug(self.robot.device.forceRARM, self.interface.force_rh)
# Selecting robotState
self.robot.device.robotState.value = 46 * (0., )
self.robotState = Selec_of_vector('robotState')
plug(self.robot.device.robotState, self.robotState.sin)
self.robotState.selec(0, 36)
# Reconstruction of the position of the free flyer from encoders
# Create dynamic with the free flyer at the origin of the control frame
self.robot.dynamicOdo = self.createDynamic(self.robotState.sout,
'_dynamicOdo')
self.robot.dynamicOdo.inertia.recompute(1)
self.robot.dynamicOdo.waist.recompute(1)
# Reconstruction of the position of the contacts in dynamicOdo
self.leftFootPosOdo = Multiply_of_matrixHomo(name + "leftFootPosOdo")
plug(self.robot.dynamicOdo.signal('left-ankle'),
self.leftFootPosOdo.sin1)
self.leftFootPosOdo.sin2.value = self.robot.forceSensorInLeftAnkle
self.rightFootPosOdo = Multiply_of_matrixHomo(name + "rightFootPosOdo")
plug(self.robot.dynamicOdo.signal('right-ankle'),
self.rightFootPosOdo.sin1)
self.rightFootPosOdo.sin2.value = self.robot.forceSensorInRightAnkle
# Odometry
self.odometry = Odometry(name + 'odometry')
plug(self.robot.frames['leftFootForceSensor'].position,
self.odometry.leftFootPositionRef)
plug(self.robot.frames['rightFootForceSensor'].position,
self.odometry.rightFootPositionRef)
plug(self.rightFootPosOdo.sout, self.odometry.rightFootPositionIn)
plug(self.leftFootPosOdo.sout, self.odometry.leftFootPositionIn)
plug(self.robot.device.forceLLEG, self.odometry.force_lf)
plug(self.robot.device.forceRLEG, self.odometry.force_rf)
self.odometry.setLeftFootPosition(
self.robot.frames['leftFootForceSensor'].position.value)
self.odometry.setRightFootPosition(
self.robot.frames['rightFootForceSensor'].position.value)
plug(self.interface.stackOfSupportContacts,
self.odometry.stackOfSupportContacts)
# Create dynamicEncoders
self.robotStateWoFF = Selec_of_vector('robotStateWoFF')
plug(self.robot.device.robotState, self.robotStateWoFF.sin)
self.robotStateWoFF.selec(6, 36)
self.stateEncoders = Stack_of_vector(name + 'stateEncoders')
plug(self.odometry.freeFlyer, self.stateEncoders.sin1)
plug(self.robotStateWoFF.sout, self.stateEncoders.sin2)
self.stateEncoders.selec1(0, 6)
self.stateEncoders.selec2(0, 30)
self.robot.dynamicEncoders = self.createDynamic(
self.stateEncoders.sout, '_dynamicEncoders')
# self.robot.dynamicEncoders=self.createDynamic(self.robotState.sout,'_dynamicEncoders')
# plug(self.odometry.freeFlyer,self.robot.dynamicEncoders.ffposition)
# self.robot.dynamicEncoders=self.createDynamic(self.robot.device.state,'_dynamicEncoders')
# Reconstruction of the position of the contacts in dynamicEncoders
self.leftFootPos = Multiply_of_matrixHomo("leftFootPos")
plug(self.robot.dynamicEncoders.signal('left-ankle'),
self.leftFootPos.sin1)
self.leftFootPos.sin2.value = self.robot.forceSensorInLeftAnkle
self.rightFootPos = Multiply_of_matrixHomo("rightFootPos")
plug(self.robot.dynamicEncoders.signal('right-ankle'),
self.rightFootPos.sin1)
self.rightFootPos.sin2.value = self.robot.forceSensorInRightAnkle
# Contacts velocities
self.leftFootVelocity = Multiply_matrix_vector('leftFootVelocity')
plug(self.robot.frames['leftFootForceSensor'].jacobian,
self.leftFootVelocity.sin1)
plug(self.robot.dynamicEncoders.velocity, self.leftFootVelocity.sin2)
self.rightFootVelocity = Multiply_matrix_vector('rightFootVelocity')
plug(self.robot.frames['rightFootForceSensor'].jacobian,
self.rightFootVelocity.sin1)
plug(self.robot.dynamicEncoders.velocity, self.rightFootVelocity.sin2)
# Contacts positions and velocities
plug(self.leftFootPos.sout, self.interface.position_lf)
plug(self.rightFootPos.sout, self.interface.position_rf)
plug(self.leftFootVelocity.sout, self.interface.velocity_lf)
plug(self.rightFootVelocity.sout, self.interface.velocity_rf)
plug(self.robot.dynamicEncoders.signal('right-wrist'),
self.interface.position_lh)
plug(self.robot.dynamicEncoders.signal('left-wrist'),
self.interface.position_rh)
# Compute contacts number
plug(self.interface.supportContactsNbr, self.contactNbr)
# Contacts model and config
plug(self.interface.contactsModel, self.contactsModel)
self.setWithConfigSignal(True)
plug(self.interface.config, self.config)
# Drift
self.drift = DriftFromMocap(name + 'Drift')
# Compute measurement vector
plug(self.robot.device.accelerometer, self.interface.accelerometer)
plug(self.robot.device.gyrometer, self.interface.gyrometer)
plug(self.drift.driftVector, self.interface.drift)
plug(self.interface.measurement, self.measurement)
# Input reconstruction
# IMU Vector
# Creating an operational point for the IMU
self.robot.dynamicEncoders.createJacobian(name + 'ChestJ_OpPoint',
'chest')
self.imuOpPoint = OpPointModifier(name + 'IMU_oppoint')
self.imuOpPoint.setTransformation(
matrixToTuple(
np.linalg.inv(np.matrix(
self.robot.dynamicEncoders.chest.value)) *
np.matrix(self.robot.frames['accelerometer'].position.value)))
self.imuOpPoint.setEndEffector(False)
plug(self.robot.dynamicEncoders.chest, self.imuOpPoint.positionIN)
plug(self.robot.dynamicEncoders.signal(name + 'ChestJ_OpPoint'),
self.imuOpPoint.jacobianIN)
# IMU position
self.PosAccelerometer = Multiply_of_matrixHomo(name +
"PosAccelerometer")
plug(self.robot.dynamicEncoders.chest, self.PosAccelerometer.sin1)
self.PosAccelerometer.sin2.value = matrixToTuple(
self.robot.accelerometerPosition)
self.inputPos = MatrixHomoToPoseUTheta(name + 'InputPosition')
plug(self.PosAccelerometer.sout, self.inputPos.sin)
# IMU velocity
self.inputVel = Multiply_matrix_vector(name + 'InputVelocity')
plug(self.imuOpPoint.jacobian, self.inputVel.sin1)
plug(self.robot.dynamicEncoders.velocity, self.inputVel.sin2)
# Concatenate
self.inputPosVel = Stack_of_vector(name + 'InputPosVel')
plug(self.inputPos.sout, self.inputPosVel.sin1)
plug(self.inputVel.sout, self.inputPosVel.sin2)
self.inputPosVel.selec1(0, 6)
self.inputPosVel.selec2(0, 6)
# IMU Vector
self.IMUVector = PositionStateReconstructor(name + 'EstimatorInput')
plug(self.inputPosVel.sout, self.IMUVector.sin)
self.IMUVector.inputFormat.value = '001111'
self.IMUVector.outputFormat.value = '011111'
self.IMUVector.setFiniteDifferencesInterval(2)
# CoM and derivatives
self.comIn = self.robot.dynamicEncoders.com
self.comVector = PositionStateReconstructor(name + 'ComVector')
plug(self.comIn, self.comVector.sin)
self.comVector.inputFormat.value = '000001'
self.comVector.outputFormat.value = '010101'
self.comVector.setFiniteDifferencesInterval(20)
# Compute derivative of Angular Momentum
self.angMomDerivator = Derivator_of_Vector(name + 'angMomDerivator')
plug(self.robot.dynamicEncoders.angularmomentum,
self.angMomDerivator.sin)
self.angMomDerivator.dt.value = self.robot.timeStep
# Concatenate with interace estimator
plug(self.comVector.sout, self.interface.comVector)
plug(self.robot.dynamicEncoders.inertia, self.interface.inertia)
plug(self.robot.dynamicEncoders.angularmomentum,
self.interface.angMomentum)
plug(self.angMomDerivator.sout, self.interface.dangMomentum)
self.interface.dinertia.value = (0, 0, 0, 0, 0, 0)
plug(self.robot.dynamicEncoders.waist, self.interface.positionWaist)
plug(self.IMUVector.sout, self.interface.imuVector)
plug(self.interface.input, self.input)
self.robot.flextimator = self
def createDynamic(self, state, name):
# Create dynamic
self.dynamicTmp = self.robot.loadModelFromJrlDynamics(
self.robot.name + name, self.robot.modelDir, self.robot.modelName,
self.robot.specificitiesPath, self.robot.jointRankPath,
DynamicHrp2_14)
self.dynamicTmp.dimension = self.dynamicTmp.getDimension()
if self.dynamicTmp.dimension != len(self.robot.halfSitting):
raise RuntimeError(
"Dimension of half-sitting: {0} differs from dimension of robot: {1}"
.format(len(self.halfSitting), self.dynamicTmp.dimension))
# Pluging position
plug(state, self.dynamicTmp.position)
self.derivative = True
# Pluging velocity
self.robot.enableVelocityDerivator = self.derivative
if self.robot.enableVelocityDerivator:
self.dynamicTmp.velocityDerivator = Derivator_of_Vector(
'velocityDerivator')
self.dynamicTmp.velocityDerivator.dt.value = self.robot.timeStep
plug(state, self.dynamicTmp.velocityDerivator.sin)
plug(self.dynamicTmp.velocityDerivator.sout,
self.dynamicTmp.velocity)
else:
self.dynamicTmp.velocity.value = self.dynamicTmp.dimension * (0., )
# Pluging acceleration
self.robot.enableAccelerationDerivator = self.derivative
if self.robot.enableAccelerationDerivator:
self.dynamicTmp.accelerationDerivator = Derivator_of_Vector(
'accelerationDerivator')
self.dynamicTmp.accelerationDerivator.dt.value = self.robot.timeStep
plug(self.dynamicTmp.velocityDerivator.sout,
self.dynamicTmp.accelerationDerivator.sin)
plug(self.dynamicTmp.accelerationDerivator.sout,
self.dynamicTmp.acceleration)
else:
self.dynamicTmp.acceleration.value = self.dynamicTmp.dimension * (
0., )
# # --- center of mass ------------
# (self.featureCom, self.featureComDes, self.taskCom, self.gainCom) = \
# self.createCenterOfMassFeatureAndTask\
# (self.dynamicTmp, '{0}_feature_com'.format(self.robot.name),
# '{0}_feature_ref_com'.format(self.robot.name),
# '{0}_task_com'.format(self.robot.name))
# --- operational points tasks -----
self.robot.features = dict()
self.robot.tasks = dict()
self.robot.gains = dict()
for op in self.robot.OperationalPoints:
opName = op + name
self.dynamicTmp.createOpPoint(op, op)
(self.robot.features[opName], self.robot.tasks[opName], self.robot.gains[opName]) = \
self.createOperationalPointFeatureAndTask(self.dynamicTmp, op,
'{0}_feature_{1}'.format(self.robot.name, opName),
'{0}_task_{1}'.format(self.robot.name, opName))
# define a member for each operational point
w = op.split('-')
memberName = w[0]
for i in w[1:]:
memberName += i.capitalize()
setattr(self, memberName, self.robot.features[opName])
# self.robot.tasks ['com'] = self.taskCom
# self.robot.features ['com'] = self.featureCom
# self.robot.gains['com'] = self.gainCom
self.robot.features['waist' + name].selec.value = '011100'
return self.dynamicTmp
def initializeRobot(self):
"""
If the robot model is correctly loaded, this method will then
initialize the operational points, set the position to
half-sitting with null velocity/acceleration.
To finish, different tasks are initialized:
- the center of mass task used to keep the robot stability
- one task per operational point to ease robot control
"""
if not hasattr(self, 'dynamic'):
raise RuntimeError("Dynamic robot model must be initialized first")
if not hasattr(self, 'device') or self.device is None:
# raise RuntimeError("A device is already defined.")
self.device = RobotSimu(self.name + '_device')
self.device.resize(self.dynamic.getDimension())
"""
Robot timestep
"""
self.timeStep = self.device.getTimeStep()
# Compute half sitting configuration
import numpy
"""
Half sitting configuration.
"""
self.halfSitting = pinocchio.neutral(self.pinocchioModel)
self.defineHalfSitting(self.halfSitting)
self.halfSitting = numpy.array(
self.halfSitting[:3].tolist() +
[0., 0., 0.] # Replace quaternion by RPY.
+ self.halfSitting[7:].tolist())
assert self.halfSitting.shape[0] == self.dynamic.getDimension()
# Set the device limits.
def get(s):
s.recompute(0)
return s.value
def opposite(v):
return [-x for x in v]
self.dynamic.add_signals()
self.device.setPositionBounds(get(self.dynamic.lowerJl),
get(self.dynamic.upperJl))
self.device.setVelocityBounds(-get(self.dynamic.upperVl),
get(self.dynamic.upperVl))
self.device.setTorqueBounds(-get(self.dynamic.upperTl),
get(self.dynamic.upperTl))
# Freeflyer reference frame should be the same as global
# frame so that operational point positions correspond to
# position in freeflyer frame.
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 = numpy.zeros([
self.dimension,
])
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 = numpy.zeros([
self.dimension,
])
def init_online_walking(robot):
# 09.04.20 est a 100 dans dcmZmpControl_file, used to be 10
cm_conf.CTRL_MAX = 1000.0 # temporary hack
dt = robot.device.getTimeStep()
robot.timeStep = dt
# --- Pendulum parameters
robot_name = 'robot'
robot.dynamic.com.recompute(0)
robotDim = robot.dynamic.getDimension()
mass = robot.dynamic.data.mass[0]
h = robot.dynamic.com.value[2]
g = 9.81
omega = sqrt(g / h)
# --- Parameter server
robot.param_server = create_parameter_server(param_server_conf, dt)
# --- Initial feet and waist
robot.dynamic.createOpPoint('LF', robot.OperationalPointsMap['left-ankle'])
robot.dynamic.createOpPoint('RF',
robot.OperationalPointsMap['right-ankle'])
robot.dynamic.createOpPoint('WT', robot.OperationalPointsMap['waist'])
robot.dynamic.LF.recompute(0)
robot.dynamic.RF.recompute(0)
robot.dynamic.WT.recompute(0)
# -------------------------- DESIRED TRAJECTORY --------------------------
rospack = RosPack()
# -------------------------- PATTERN GENERATOR --------------------------
robot.pg = PatternGenerator('pg')
# MODIFIED WITH MY PATHS
talos_data_folder = rospack.get_path('talos_data')
robot.pg.setURDFpath(talos_data_folder + '/urdf/talos_reduced_wpg.urdf')
robot.pg.setSRDFpath(talos_data_folder + '/srdf/talos_wpg.srdf')
## END MODIFIED
robot.pg.buildModel()
robot.pg.parseCmd(":samplingperiod 0.005")
robot.pg.parseCmd(":previewcontroltime 1.6")
robot.pg.parseCmd(":omega 0.0")
robot.pg.parseCmd(':stepheight 0.05')
robot.pg.parseCmd(':doublesupporttime 0.2')
robot.pg.parseCmd(':singlesupporttime 1.0')
robot.pg.parseCmd(":armparameters 0.5")
robot.pg.parseCmd(":LimitsFeasibility 0.0")
robot.pg.parseCmd(":ZMPShiftParameters 0.015 0.015 0.015 0.015")
robot.pg.parseCmd(":TimeDistributeParameters 2.0 3.5 1.7 3.0")
robot.pg.parseCmd(":UpperBodyMotionParameters -0.1 -1.0 0.0")
robot.pg.parseCmd(":comheight 0.876681")
robot.pg.parseCmd(":setVelReference 0.1 0.0 0.0")
robot.pg.parseCmd(":SetAlgoForZmpTrajectory Naveau")
plug(robot.dynamic.position, robot.pg.position)
plug(robot.dynamic.com, robot.pg.com)
#plug(robot.dynamic.com, robot.pg.comStateSIN)
plug(robot.dynamic.LF, robot.pg.leftfootcurrentpos)
plug(robot.dynamic.RF, robot.pg.rightfootcurrentpos)
robotDim = len(robot.dynamic.velocity.value)
robot.pg.motorcontrol.value = robotDim * (0, )
robot.pg.zmppreviouscontroller.value = (0, 0, 0)
robot.pg.initState()
robot.pg.parseCmd(':setDSFeetDistance 0.162')
robot.pg.parseCmd(':NaveauOnline')
robot.pg.parseCmd(':numberstepsbeforestop 2')
robot.pg.parseCmd(':setfeetconstraint XY 0.091 0.0489')
robot.pg.parseCmd(':deleteallobstacles')
robot.pg.parseCmd(':feedBackControl false')
robot.pg.parseCmd(':useDynamicFilter true')
robot.pg.velocitydes.value = (0.1, 0.0, 0.0) # DEFAULT VALUE (0.1,0.0,0.0)
# -------------------------- TRIGGER --------------------------
robot.triggerPG = BooleanIdentity('triggerPG')
robot.triggerPG.sin.value = 0
plug(robot.triggerPG.sout, robot.pg.trigger)
# --------- Interface with controller entities -------------
wp = DummyWalkingPatternGenerator('dummy_wp')
wp.init()
# #wp.displaySignals()
wp.omega.value = omega
# 22.04 after modifying pg.cpp, new way to try and connect the waist
plug(robot.pg.waistattitudematrixabsolute, wp.waist)
plug(robot.pg.leftfootref, wp.footLeft)
plug(robot.pg.rightfootref, wp.footRight)
plug(robot.pg.comref, wp.com)
plug(robot.pg.dcomref, wp.vcom)
plug(robot.pg.ddcomref, wp.acom)
robot.wp = wp
# --- Compute the values to use them in initialization
robot.wp.comDes.recompute(0)
robot.wp.dcmDes.recompute(0)
robot.wp.zmpDes.recompute(0)
## END ADDED
# -------------------------- ESTIMATION --------------------------
# --- Base Estimation
robot.device_filters = create_device_filters(robot, dt)
robot.imu_filters = create_imu_filters(robot, dt)
robot.base_estimator = create_base_estimator(robot, dt,
base_estimator_conf)
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)
# --- 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")
estimator.omega.value = omega
estimator.mass.value = 1.0
plug(robot.cdc_estimator.c, estimator.com)
plug(robot.cdc_estimator.dc, estimator.momenta)
estimator.init()
robot.estimator = estimator
# --- Force calibration
robot.ftc = create_ft_calibrator(robot, ft_conf)
# --- ZMP estimation
zmp_estimator = SimpleZmpEstimator("zmpEst")
robot.rdynamic.createOpPoint('sole_LF', 'left_sole_link')
robot.rdynamic.createOpPoint('sole_RF', 'right_sole_link')
plug(robot.rdynamic.sole_LF, zmp_estimator.poseLeft)
plug(robot.rdynamic.sole_RF, zmp_estimator.poseRight)
plug(robot.ftc.left_foot_force_out, zmp_estimator.wrenchLeft)
plug(robot.ftc.right_foot_force_out, zmp_estimator.wrenchRight)
zmp_estimator.init()
robot.zmp_estimator = zmp_estimator
# -------------------------- ADMITTANCE CONTROL --------------------------
# --- DCM controller
Kp_dcm = [8.0] * 3
Ki_dcm = [0.0, 0.0, 0.0] # zero (to be set later)
gamma_dcm = 0.2
dcm_controller = DcmController("dcmCtrl")
dcm_controller.Kp.value = Kp_dcm
dcm_controller.Ki.value = Ki_dcm
dcm_controller.decayFactor.value = gamma_dcm
dcm_controller.mass.value = mass
dcm_controller.omega.value = omega
plug(robot.cdc_estimator.c, dcm_controller.com)
plug(robot.estimator.dcm, dcm_controller.dcm)
plug(robot.wp.zmpDes, dcm_controller.zmpDes)
plug(robot.wp.dcmDes, dcm_controller.dcmDes)
dcm_controller.init(dt)
robot.dcm_control = dcm_controller
Ki_dcm = [1.0, 1.0, 1.0] # this value is employed later
# --- CoM admittance controller
Kp_adm = [0.0, 0.0, 0.0] # zero (to be set later)
com_admittance_control = ComAdmittanceController("comAdmCtrl")
com_admittance_control.Kp.value = Kp_adm
plug(robot.zmp_estimator.zmp, com_admittance_control.zmp)
com_admittance_control.zmpDes.value = robot.wp.zmpDes.value
# should be plugged to robot.dcm_control.zmpRef
plug(robot.wp.acomDes, com_admittance_control.ddcomDes)
com_admittance_control.init(dt)
com_admittance_control.setState(robot.wp.comDes.value, [0.0, 0.0, 0.0])
robot.com_admittance_control = com_admittance_control
Kp_adm = [15.0, 15.0, 0.0] # this value is employed later
# --- Control Manager
robot.cm = create_ctrl_manager(cm_conf, dt, robot_name='robot')
robot.cm.addCtrlMode('sot_input')
robot.cm.setCtrlMode('all', 'sot_input')
robot.cm.addEmergencyStopSIN('zmp')
# -------------------------- SOT CONTROL --------------------------
# --- Upper body
robot.taskUpperBody = Task('task_upper_body')
robot.taskUpperBody.feature = FeaturePosture('feature_upper_body')
q = list(robot.dynamic.position.value)
robot.taskUpperBody.feature.state.value = q
robot.taskUpperBody.feature.posture.value = q
robot.taskUpperBody.feature.selectDof(18, True)
robot.taskUpperBody.feature.selectDof(19, True)
robot.taskUpperBody.feature.selectDof(20, True)
robot.taskUpperBody.feature.selectDof(21, True)
robot.taskUpperBody.feature.selectDof(22, True)
robot.taskUpperBody.feature.selectDof(23, True)
robot.taskUpperBody.feature.selectDof(24, True)
robot.taskUpperBody.feature.selectDof(25, True)
robot.taskUpperBody.feature.selectDof(26, True)
robot.taskUpperBody.feature.selectDof(27, True)
robot.taskUpperBody.feature.selectDof(28, True)
robot.taskUpperBody.feature.selectDof(29, True)
robot.taskUpperBody.feature.selectDof(30, True)
robot.taskUpperBody.feature.selectDof(31, True)
robot.taskUpperBody.feature.selectDof(32, True)
robot.taskUpperBody.feature.selectDof(33, True)
robot.taskUpperBody.feature.selectDof(34, True)
robot.taskUpperBody.feature.selectDof(35, True)
robot.taskUpperBody.feature.selectDof(36, True)
robot.taskUpperBody.feature.selectDof(37, True)
robot.taskUpperBody.controlGain.value = 100.0
robot.taskUpperBody.add(robot.taskUpperBody.feature.name)
plug(robot.dynamic.position, robot.taskUpperBody.feature.state)
# --- CONTACTS
robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF',
robot.OperationalPointsMap['left-ankle'])
robot.contactLF.feature.frame('desired')
robot.contactLF.gain.setConstant(300)
plug(robot.wp.footLeftDes, robot.contactLF.featureDes.position) #.errorIN?
locals()['contactLF'] = robot.contactLF
robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF',
robot.OperationalPointsMap['right-ankle'])
robot.contactRF.feature.frame('desired')
robot.contactRF.gain.setConstant(300)
plug(robot.wp.footRightDes,
robot.contactRF.featureDes.position) #.errorIN?
locals()['contactRF'] = robot.contactRF
# --- COM height
robot.taskComH = MetaTaskKineCom(robot.dynamic, name='comH')
plug(robot.wp.comDes, robot.taskComH.featureDes.errorIN)
robot.taskComH.task.controlGain.value = 100.
robot.taskComH.feature.selec.value = '100'
# --- COM
robot.taskCom = MetaTaskKineCom(robot.dynamic)
plug(robot.com_admittance_control.comRef, robot.taskCom.featureDes.errorIN)
plug(robot.com_admittance_control.dcomRef,
robot.taskCom.featureDes.errordotIN)
robot.taskCom.task.controlGain.value = 0
robot.taskCom.task.setWithDerivative(True)
robot.taskCom.feature.selec.value = '011'
# --- Waist
robot.keepWaist = MetaTaskKine6d('keepWaist', robot.dynamic, 'WT',
robot.OperationalPointsMap['waist'])
robot.keepWaist.feature.frame('desired')
robot.keepWaist.gain.setConstant(300)
plug(robot.wp.waistDes, robot.keepWaist.featureDes.position) #de base
robot.keepWaist.feature.selec.value = '111000'
locals()['keepWaist'] = robot.keepWaist
# --- SOT solver
robot.sot = SOT('sot')
robot.sot.setSize(robot.dynamic.getDimension())
# --- Plug SOT control to device through control manager
plug(robot.sot.control, robot.cm.ctrl_sot_input)
plug(robot.cm.u_safe, robot.device.control)
robot.sot.push(robot.taskUpperBody.name)
robot.sot.push(robot.contactRF.task.name)
robot.sot.push(robot.contactLF.task.name)
robot.sot.push(robot.taskComH.task.name)
robot.sot.push(robot.taskCom.task.name)
robot.sot.push(robot.keepWaist.task.name)
# --- Fix robot.dynamic inputs
plug(robot.device.velocity, robot.dynamic.velocity)
robot.dvdt = Derivator_of_Vector("dv_dt")
robot.dvdt.dt.value = dt
plug(robot.device.velocity, robot.dvdt.sin)
plug(robot.dvdt.sout, robot.dynamic.acceleration)
# -------------------------- PLOTS --------------------------
# --- ROS PUBLISHER
## THIS PARAGRAPH QUITE DIFFERENT, TO CHECK
robot.publisher = create_rospublish(robot, 'robot_publisher')
## ADDED
create_topic(robot.publisher,
robot.pg,
'comref',
robot=robot,
data_type='vector') # desired CoM
create_topic(robot.publisher,
robot.pg,
'dcomref',
robot=robot,
data_type='vector')
create_topic(robot.publisher,
robot.wp,
'waist',
robot=robot,
data_type='matrixHomo')
create_topic(robot.publisher,
robot.keepWaist.featureDes,
'position',
robot=robot,
data_type='matrixHomo')
create_topic(robot.publisher,
robot.dynamic,
'WT',
robot=robot,
data_type='matrixHomo')
create_topic(robot.publisher,
robot.pg,
'waistattitudematrixabsolute',
robot=robot,
data_type='matrixHomo') ## que font ces lignes exactement ??
create_topic(robot.publisher,
robot.pg,
'leftfootref',
robot=robot,
data_type='matrixHomo')
create_topic(robot.publisher,
robot.wp,
'footLeft',
robot=robot,
data_type='matrixHomo')
create_topic(robot.publisher,
robot.pg,
'rightfootref',
robot=robot,
data_type='matrixHomo')
create_topic(robot.publisher,
robot.wp,
'footRight',
robot=robot,
data_type='matrixHomo')
## --- TRACER
robot.tracer = TracerRealTime("com_tracer")
robot.tracer.setBufferSize(80 * (2**20))
robot.tracer.open('/tmp', 'dg_', '.dat')
robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))
addTrace(robot.tracer, robot.pg, 'waistattitudeabsolute')
# fin
addTrace(robot.tracer, robot.wp, 'comDes') # desired CoM
addTrace(robot.tracer, robot.cdc_estimator, 'c') # estimated CoM
addTrace(robot.tracer, robot.cdc_estimator, 'dc') # estimated CoM velocity
addTrace(robot.tracer, robot.pg, 'comref')
addTrace(robot.tracer, robot.pg, 'dcomref')
addTrace(robot.tracer, robot.pg, 'ddcomref')
addTrace(robot.tracer, robot.pg, 'rightfootref')
addTrace(robot.tracer, robot.pg, 'leftfootref')
addTrace(robot.tracer, robot.pg, 'rightfootcontact')
addTrace(robot.tracer, robot.pg, 'leftfootcontact')
addTrace(robot.tracer, robot.pg, 'SupportFoot')
addTrace(robot.tracer, robot.dynamic, 'com') # resulting SOT CoM
addTrace(robot.tracer, robot.dynamic, 'LF') # left foot
addTrace(robot.tracer, robot.dynamic, 'RF') # right foot
robot.tracer.start()
def init_sot_talos_balance(robot, test_folder):
cm_conf.CTRL_MAX = 1000.0 # temporary hack
dt = robot.device.getTimeStep()
robot.timeStep = dt
# --- Pendulum parameters
robot_name = 'robot'
robot.dynamic.com.recompute(0)
robotDim = robot.dynamic.getDimension()
mass = robot.dynamic.data.mass[0]
h = robot.dynamic.com.value[2]
g = 9.81
omega = sqrt(g / h)
# --- Parameter server
robot.param_server = create_parameter_server(param_server_conf, dt)
# --- Initial feet and waist
robot.dynamic.createOpPoint('LF', robot.OperationalPointsMap['left-ankle'])
robot.dynamic.createOpPoint('RF',
robot.OperationalPointsMap['right-ankle'])
robot.dynamic.createOpPoint('WT', robot.OperationalPointsMap['waist'])
robot.dynamic.LF.recompute(0)
robot.dynamic.RF.recompute(0)
robot.dynamic.WT.recompute(0)
# -------------------------- DESIRED TRAJECTORY --------------------------
rospack = RosPack()
data_folder = rospack.get_path('sot-talos-balance') + "/data/"
folder = data_folder + test_folder + '/'
# --- Trajectory generators
# --- General trigger
robot.triggerTrajGen = BooleanIdentity('triggerTrajGen')
robot.triggerTrajGen.sin.value = 0
# --- CoM
robot.comTrajGen = create_com_trajectory_generator(dt, robot)
robot.comTrajGen.x.recompute(0) # trigger computation of initial value
robot.comTrajGen.playTrajectoryFile(folder + 'CoM.dat')
plug(robot.triggerTrajGen.sout, robot.comTrajGen.trigger)
# --- Left foot
robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'LF')
robot.lfTrajGen.x.recompute(0) # trigger computation of initial value
robot.lfToMatrix = PoseRollPitchYawToMatrixHomo('lf2m')
plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)
robot.lfTrajGen.playTrajectoryFile(folder + 'LeftFoot.dat')
plug(robot.triggerTrajGen.sout, robot.lfTrajGen.trigger)
# --- Right foot
robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'RF')
robot.rfTrajGen.x.recompute(0) # trigger computation of initial value
robot.rfToMatrix = PoseRollPitchYawToMatrixHomo('rf2m')
plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)
robot.rfTrajGen.playTrajectoryFile(folder + 'RightFoot.dat')
plug(robot.triggerTrajGen.sout, robot.rfTrajGen.trigger)
# --- ZMP
robot.zmpTrajGen = create_zmp_trajectory_generator(dt, robot)
robot.zmpTrajGen.x.recompute(0) # trigger computation of initial value
# robot.zmpTrajGen.playTrajectoryFile(folder + 'ZMP.dat')
plug(robot.triggerTrajGen.sout, robot.zmpTrajGen.trigger)
# --- Waist
robot.waistTrajGen = create_orientation_rpy_trajectory_generator(
dt, robot, 'WT')
robot.waistTrajGen.x.recompute(0) # trigger computation of initial value
robot.waistMix = Mix_of_vector("waistMix")
robot.waistMix.setSignalNumber(3)
robot.waistMix.addSelec(1, 0, 3)
robot.waistMix.addSelec(2, 3, 3)
robot.waistMix.default.value = [0.0] * 6
robot.waistMix.signal("sin1").value = [0.0] * 3
plug(robot.waistTrajGen.x, robot.waistMix.signal("sin2"))
robot.waistToMatrix = PoseRollPitchYawToMatrixHomo('w2m')
plug(robot.waistMix.sout, robot.waistToMatrix.sin)
robot.waistTrajGen.playTrajectoryFile(folder + 'WaistOrientation.dat')
plug(robot.triggerTrajGen.sout, robot.waistTrajGen.trigger)
# --- Interface with controller entities
wp = DummyWalkingPatternGenerator('dummy_wp')
wp.init()
wp.omega.value = omega
#wp.waist.value = robot.dynamic.WT.value # wait receives a full homogeneous matrix, but only the rotational part is controlled
#wp.footLeft.value = robot.dynamic.LF.value
#wp.footRight.value = robot.dynamic.RF.value
#wp.com.value = robot.dynamic.com.value
#wp.vcom.value = [0.]*3
#wp.acom.value = [0.]*3
plug(robot.waistToMatrix.sout, wp.waist)
plug(robot.lfToMatrix.sout, wp.footLeft)
plug(robot.rfToMatrix.sout, wp.footRight)
plug(robot.comTrajGen.x, wp.com)
plug(robot.comTrajGen.dx, wp.vcom)
plug(robot.comTrajGen.ddx, wp.acom)
#plug(robot.zmpTrajGen.x, wp.zmp)
robot.wp = wp
# --- Compute the values to use them in initialization
robot.wp.comDes.recompute(0)
robot.wp.dcmDes.recompute(0)
robot.wp.zmpDes.recompute(0)
# -------------------------- ESTIMATION --------------------------
# --- Base Estimation
robot.device_filters = create_device_filters(robot, dt)
robot.imu_filters = create_imu_filters(robot, dt)
robot.base_estimator = create_base_estimator(robot, dt,
base_estimator_conf)
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)
# --- 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")
estimator.omega.value = omega
estimator.mass.value = 1.0
plug(robot.cdc_estimator.c, estimator.com)
plug(robot.cdc_estimator.dc, estimator.momenta)
estimator.init()
robot.estimator = estimator
# --- Force calibration
robot.ftc = create_ft_calibrator(robot, ft_conf)
# --- ZMP estimation
zmp_estimator = SimpleZmpEstimator("zmpEst")
robot.rdynamic.createOpPoint('sole_LF', 'left_sole_link')
robot.rdynamic.createOpPoint('sole_RF', 'right_sole_link')
plug(robot.rdynamic.sole_LF, zmp_estimator.poseLeft)
plug(robot.rdynamic.sole_RF, zmp_estimator.poseRight)
plug(robot.ftc.left_foot_force_out, zmp_estimator.wrenchLeft)
plug(robot.ftc.right_foot_force_out, zmp_estimator.wrenchRight)
zmp_estimator.init()
robot.zmp_estimator = zmp_estimator
# -------------------------- ADMITTANCE CONTROL --------------------------
# --- DCM controller
Kp_dcm = [8.0] * 3
Ki_dcm = [0.0, 0.0, 0.0] # zero (to be set later)
gamma_dcm = 0.2
dcm_controller = DcmController("dcmCtrl")
dcm_controller.Kp.value = Kp_dcm
dcm_controller.Ki.value = Ki_dcm
dcm_controller.decayFactor.value = gamma_dcm
dcm_controller.mass.value = mass
dcm_controller.omega.value = omega
plug(robot.cdc_estimator.c, dcm_controller.com)
plug(robot.estimator.dcm, dcm_controller.dcm)
plug(robot.wp.zmpDes, dcm_controller.zmpDes)
plug(robot.wp.dcmDes, dcm_controller.dcmDes)
dcm_controller.init(dt)
robot.dcm_control = dcm_controller
Ki_dcm = [1.0, 1.0, 1.0] # this value is employed later
# --- CoM admittance controller
Kp_adm = [0.0, 0.0, 0.0] # zero (to be set later)
com_admittance_control = ComAdmittanceController("comAdmCtrl")
com_admittance_control.Kp.value = Kp_adm
plug(robot.zmp_estimator.zmp, com_admittance_control.zmp)
com_admittance_control.zmpDes.value = robot.wp.zmpDes.value # should be plugged to robot.dcm_control.zmpRef
plug(robot.wp.acomDes, com_admittance_control.ddcomDes)
com_admittance_control.init(dt)
com_admittance_control.setState(robot.wp.comDes.value, [0.0, 0.0, 0.0])
robot.com_admittance_control = com_admittance_control
# --- Control Manager
robot.cm = create_ctrl_manager(cm_conf, dt, robot_name='robot')
robot.cm.addCtrlMode('sot_input')
robot.cm.setCtrlMode('all', 'sot_input')
robot.cm.addEmergencyStopSIN('zmp')
# -------------------------- SOT CONTROL --------------------------
# --- Upper body
robot.taskUpperBody = Task('task_upper_body')
robot.taskUpperBody.feature = FeaturePosture('feature_upper_body')
q = list(robot.dynamic.position.value)
robot.taskUpperBody.feature.state.value = q
robot.taskUpperBody.feature.posture.value = q
# robotDim = robot.dynamic.getDimension() # 38
robot.taskUpperBody.feature.selectDof(18, True)
robot.taskUpperBody.feature.selectDof(19, True)
robot.taskUpperBody.feature.selectDof(20, True)
robot.taskUpperBody.feature.selectDof(21, True)
robot.taskUpperBody.feature.selectDof(22, True)
robot.taskUpperBody.feature.selectDof(23, True)
robot.taskUpperBody.feature.selectDof(24, True)
robot.taskUpperBody.feature.selectDof(25, True)
robot.taskUpperBody.feature.selectDof(26, True)
robot.taskUpperBody.feature.selectDof(27, True)
robot.taskUpperBody.feature.selectDof(28, True)
robot.taskUpperBody.feature.selectDof(29, True)
robot.taskUpperBody.feature.selectDof(30, True)
robot.taskUpperBody.feature.selectDof(31, True)
robot.taskUpperBody.feature.selectDof(32, True)
robot.taskUpperBody.feature.selectDof(33, True)
robot.taskUpperBody.feature.selectDof(34, True)
robot.taskUpperBody.feature.selectDof(35, True)
robot.taskUpperBody.feature.selectDof(36, True)
robot.taskUpperBody.feature.selectDof(37, True)
robot.taskUpperBody.controlGain.value = 100.0
robot.taskUpperBody.add(robot.taskUpperBody.feature.name)
plug(robot.dynamic.position, robot.taskUpperBody.feature.state)
# --- CONTACTS
#define contactLF and contactRF
robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF',
robot.OperationalPointsMap['left-ankle'])
robot.contactLF.feature.frame('desired')
robot.contactLF.gain.setConstant(300)
plug(robot.wp.footLeftDes, robot.contactLF.featureDes.position) #.errorIN?
locals()['contactLF'] = robot.contactLF
robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF',
robot.OperationalPointsMap['right-ankle'])
robot.contactRF.feature.frame('desired')
robot.contactRF.gain.setConstant(300)
plug(robot.wp.footRightDes,
robot.contactRF.featureDes.position) #.errorIN?
locals()['contactRF'] = robot.contactRF
# --- COM height
robot.taskComH = MetaTaskKineCom(robot.dynamic, name='comH')
plug(robot.wp.comDes, robot.taskComH.featureDes.errorIN)
robot.taskComH.task.controlGain.value = 100.
robot.taskComH.feature.selec.value = '100'
# --- COM
robot.taskCom = MetaTaskKineCom(robot.dynamic)
plug(robot.com_admittance_control.comRef, robot.taskCom.featureDes.errorIN)
plug(robot.com_admittance_control.dcomRef,
robot.taskCom.featureDes.errordotIN)
robot.taskCom.task.controlGain.value = 0
robot.taskCom.task.setWithDerivative(True)
robot.taskCom.feature.selec.value = '011'
# --- Waist
robot.keepWaist = MetaTaskKine6d('keepWaist', robot.dynamic, 'WT',
robot.OperationalPointsMap['waist'])
robot.keepWaist.feature.frame('desired')
robot.keepWaist.gain.setConstant(300)
plug(robot.wp.waistDes, robot.keepWaist.featureDes.position)
robot.keepWaist.feature.selec.value = '111000'
locals()['keepWaist'] = robot.keepWaist
# --- SOT solver
robot.sot = SOT('sot')
robot.sot.setSize(robot.dynamic.getDimension())
# --- Plug SOT control to device through control manager
plug(robot.sot.control, robot.cm.ctrl_sot_input)
plug(robot.cm.u_safe, robot.device.control)
robot.sot.push(robot.taskUpperBody.name)
robot.sot.push(robot.contactRF.task.name)
robot.sot.push(robot.contactLF.task.name)
robot.sot.push(robot.taskComH.task.name)
robot.sot.push(robot.taskCom.task.name)
robot.sot.push(robot.keepWaist.task.name)
# --- Fix robot.dynamic inputs
plug(robot.device.velocity, robot.dynamic.velocity)
robot.dvdt = Derivator_of_Vector("dv_dt")
robot.dvdt.dt.value = dt
plug(robot.device.velocity, robot.dvdt.sin)
plug(robot.dvdt.sout, robot.dynamic.acceleration)
# -------------------------- PLOTS --------------------------
# --- ROS PUBLISHER
robot.publisher = create_rospublish(robot, 'robot_publisher')
create_topic(robot.publisher,
robot.device,
'state',
robot=robot,
data_type='vector')
create_topic(robot.publisher,
robot.base_estimator,
'q',
robot=robot,
data_type='vector')
#create_topic(robot.publisher, robot.stf, 'q', robot = robot, data_type='vector')
create_topic(robot.publisher,
robot.comTrajGen,
'x',
robot=robot,
data_type='vector') # generated CoM
create_topic(robot.publisher,
robot.comTrajGen,
'dx',
robot=robot,
data_type='vector') # generated CoM velocity
create_topic(robot.publisher,
robot.comTrajGen,
'ddx',
robot=robot,
data_type='vector') # generated CoM acceleration
create_topic(robot.publisher,
robot.wp,
'comDes',
robot=robot,
data_type='vector') # desired CoM
create_topic(robot.publisher,
robot.cdc_estimator,
'c',
robot=robot,
data_type='vector') # estimated CoM
create_topic(robot.publisher,
robot.cdc_estimator,
'dc',
robot=robot,
data_type='vector') # estimated CoM velocity
create_topic(robot.publisher,
robot.com_admittance_control,
'comRef',
robot=robot,
data_type='vector') # reference CoM
create_topic(robot.publisher,
robot.dynamic,
'com',
robot=robot,
data_type='vector') # resulting SOT CoM
create_topic(robot.publisher,
robot.dcm_control,
'dcmDes',
robot=robot,
data_type='vector') # desired DCM
create_topic(robot.publisher,
robot.estimator,
'dcm',
robot=robot,
data_type='vector') # estimated DCM
create_topic(robot.publisher,
robot.zmpTrajGen,
'x',
robot=robot,
data_type='vector') # generated ZMP
create_topic(robot.publisher,
robot.wp,
'zmpDes',
robot=robot,
data_type='vector') # desired ZMP
create_topic(robot.publisher,
robot.dynamic,
'zmp',
robot=robot,
data_type='vector') # SOT ZMP
create_topic(robot.publisher,
robot.zmp_estimator,
'zmp',
robot=robot,
data_type='vector') # estimated ZMP
create_topic(robot.publisher,
robot.dcm_control,
'zmpRef',
robot=robot,
data_type='vector') # reference ZMP
create_topic(robot.publisher,
robot.waistTrajGen,
'x',
robot=robot,
data_type='vector') # desired waist orientation
create_topic(robot.publisher,
robot.lfTrajGen,
'x',
robot=robot,
data_type='vector') # desired left foot pose
create_topic(robot.publisher,
robot.rfTrajGen,
'x',
robot=robot,
data_type='vector') # desired right foot pose
create_topic(robot.publisher,
robot.ftc,
'left_foot_force_out',
robot=robot,
data_type='vector') # calibrated left wrench
create_topic(robot.publisher,
robot.ftc,
'right_foot_force_out',
robot=robot,
data_type='vector') # calibrated right wrench
create_topic(robot.publisher,
robot.dynamic,
'LF',
robot=robot,
data_type='matrixHomo') # left foot
create_topic(robot.publisher,
robot.dynamic,
'RF',
robot=robot,
data_type='matrixHomo') # right foot
# --- TRACER
robot.tracer = TracerRealTime("com_tracer")
robot.tracer.setBufferSize(80 * (2**20))
robot.tracer.open('/tmp', 'dg_', '.dat')
robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))
addTrace(robot.tracer, robot.wp, 'comDes') # desired CoM
addTrace(robot.tracer, robot.cdc_estimator, 'c') # estimated CoM
addTrace(robot.tracer, robot.cdc_estimator, 'dc') # estimated CoM velocity
addTrace(robot.tracer, robot.com_admittance_control,
'comRef') # reference CoM
addTrace(robot.tracer, robot.dynamic, 'com') # resulting SOT CoM
addTrace(robot.tracer, robot.dcm_control, 'dcmDes') # desired DCM
addTrace(robot.tracer, robot.estimator, 'dcm') # estimated DCM
addTrace(robot.tracer, robot.dcm_control, 'zmpDes') # desired ZMP
addTrace(robot.tracer, robot.dynamic, 'zmp') # SOT ZMP
addTrace(robot.tracer, robot.zmp_estimator, 'zmp') # estimated ZMP
addTrace(robot.tracer, robot.dcm_control, 'zmpRef') # reference ZMP
addTrace(robot.tracer, robot.ftc,
'left_foot_force_out') # calibrated left wrench
addTrace(robot.tracer, robot.ftc,
'right_foot_force_out') # calibrated right wrench
addTrace(robot.tracer, robot.dynamic, 'LF') # left foot
addTrace(robot.tracer, robot.dynamic, 'RF') # right foot
robot.tracer.start()
# ----------------------------------------------------
# ------- Flex based kinematics
flex = AngleEstimator('flex')
flex.setFromSensor(False)
plug(dyn2.signal('lleg'), flex.signal('contactEmbeddedPosition'))
plug(dyn2.signal('chest'), flex.signal('sensorEmbeddedPosition'))
attitudeSensor = RPYToMatrix('attitudeSensor')
# plug attitudeSensor.out flex.sensorWorldRotation
plug(flex.signal('waistWorldPoseRPY'), dyn.signal('ffposition'))
# --- Flexibility velocity
flexV = Derivator_of_Vector('flexV')
plug(flex.signal('angles'), flexV.signal('in'))
# --- PosFF from leg contact + sensor # DEPRECIATED
posKF = WaistPoseFromSensorAndContact('posKF')
plug(dyn2.signal('lleg'), posKF.signal('contact'))
plug(dyn2.signal('chest'), posKF.signal('position'))
posKF.setFromSensor(True)
# --- DYN With true posFF
dyn.parse()
plug(zero.signal('out'), dyn.signal('velocity'))
plug(zero.signal('out'), dyn.signal('acceleration'))
plug(flex.signal('waistWorldPoseRPY'), dyn.signal('ffposition'))
# ----------------------------------------------------
# ----------------------------------------------------
# ------- Flex based kinematics
flex = AngleEstimator('flex')
flex.setFromSensor(False)
plug(dyn2.signal('lleg'), flex.signal('contactEmbeddedPosition'))
plug(dyn2.signal('chest'), flex.signal('sensorEmbeddedPosition'))
attitudeSensor = RPYToMatrix('attitudeSensor')
# plug attitudeSensor.out flex.sensorWorldRotation
plug(flex.signal('waistWorldPoseRPY'), dyn.signal('ffposition'))
# --- Flexibility velocity
flexV = Derivator_of_Vector('flexV')
plug(flex.signal('angles'), flexV.signal('in'))
# --- PosFF from leg contact + sensor # DEPRECIATED
posKF = WaistPoseFromSensorAndContact('posKF')
plug(dyn2.signal('lleg'), posKF.signal('contact'))
plug(dyn2.signal('chest'), posKF.signal('position'))
posKF.setFromSensor(True)
# --- DYN With true posFF
dyn.parse()
plug(zero.signal('out'), dyn.signal('velocity'))
plug(zero.signal('out'), dyn.signal('acceleration'))
plug(flex.signal('waistWorldPoseRPY'), dyn.signal('ffposition'))
# ----------------------------------------------------
