def test_ros(robot):
# SCRIPT PARAMETERS
j = 0; # index of joint under analysis
N = 300; # test duration (in number of timesteps)
dt = 0.001; # time step
estimationDelay = 10; # delay introduced by the estimation [number of time steps]
createRosTopics = 1; # 1=true, 0=false
# CONSTANTS
nj = 30; # number of joints
rad2deg = 180/3.14;
app = Application(robot);
dq_des = nj*(0.0,);
app.robot.device.control.value = dq_des;
if(createRosTopics==1):
ros = RosExport('rosExport');
ros.add('vector', 'robotStateRos', 'state');
plug(robot.device.state, ros.robotStateRos);
# robot.device.after.addSignal('rosExport.robotStateRos')
return ros
def test_ros(robot):
# SCRIPT PARAMETERS
j = 0
# index of joint under analysis
N = 300
# test duration (in number of timesteps)
dt = 0.001
# time step
estimationDelay = 10
# delay introduced by the estimation [number of time steps]
createRosTopics = 1
# 1=true, 0=false
# CONSTANTS
nj = 30
# number of joints
rad2deg = 180 / 3.14
app = Application(robot)
dq_des = nj * (0.0, )
app.robot.device.control.value = dq_des
if (createRosTopics == 1):
ros = RosExport('rosExport')
ros.add('vector', 'robotStateRos', 'state')
plug(robot.device.state, ros.robotStateRos)
# robot.device.after.addSignal('rosExport.robotStateRos')
return ros
def __init__(self, robot):
self.robot = robot
self.ros = RosExport('rosExportMocap')
self.ros.add('matrixHomoStamped', "chest",
"/evart/hrp2_head_sf/hrp2_head_sf")
self.mocapFilter = MocapDataFilter('MocapDataFilter')
plug(self.ros.signal('chest'), self.mocapFilter.sin)
self.mocapSignal = self.mocapFilter.sout
class HRP2MocapToSOT:
def __init__(self, robot):
self.robot = robot
self.ros = RosExport('rosExportMocap')
self.ros.add('matrixHomoStamped', "chest",
"/evart/hrp2_head_sf/hrp2_head_sf")
self.mocapFilter = MocapDataFilter('MocapDataFilter')
plug(self.ros.signal('chest'), self.mocapFilter.sin)
self.mocapSignal = self.mocapFilter.sout
def initialize(self):
m = np.matrix([
[-0.8530, 0.5208, -0.0360, 0.0049],
[-0.5206, -0.8537, -0.0146, -0.0078],
[-0.0384, 0.0063, 0.9993, 0.2158], [0, 0, 0, 1.0000]
]) #transformation from the chest-markers frame to the chest frame
self.chestMarkersSot = OpPointModifier(
'chestMarkersSot'
) # gets the position of the chest-markers frame in sot frame
self.chestMarkersSot.setEndEffector(False)
self.chestMarkersSot.setTransformation(matrixToTuple(m))
plug(self.robot.dynamic.chest, self.chestMarkersSot.positionIN)
self.chestMarkersSot.position.recompute(1)
self.mchestsot = np.matrix(self.chestMarkersSot.position.value)
self.mchestmocap = np.matrix(self.ros.signal('chest').value)
self.mtransformMocap2ISot = np.linalg.inv(
self.mchestsot
) * self.mchestmocap #transforms the mocap frame to the initial-sot frame
self.chestMarkerMocapInISot = Multiply_of_matrixHomo(
"chestMarkerMocap"
) #gets position of chest-markers frame in sot-initial frame
self.chestMarkerMocapInISot.sin1.value = matrixToTuple(
self.mtransformMocap2ISot)
plug(self.mocapSignal, self.chestMarkerMocapInISot.sin2)
self.chestMarkersSotInverse = Inverse_of_matrixHomo(
"chestMarkersSotInverse"
) #inverse of the homomatrix for position of markers in local sot
plug(self.chestMarkersSot.position, self.chestMarkersSotInverse.sin)
self.robotPositionISot = Multiply_of_matrixHomo("robotPositionInSot")
plug(self.chestMarkerMocapInISot.sout, self.robotPositionISot.sin1)
plug(self.chestMarkersSotInverse.sout, self.robotPositionISot.sin2)
self.robotPositionInMocap = Multiply_of_matrixHomo(
"robotPositionInMocap")
plug(self.mocapSignal, self.robotPositionInMocap.sin1)
plug(self.chestMarkersSotInverse.sout, self.robotPositionInMocap.sin2)
class HRP2MocapToSOT:
def __init__(self, robot):
self.robot = robot
self.ros = RosExport("rosExportMocap")
self.ros.add("matrixHomoStamped", "chest", "/evart/hrp2_head_sf/hrp2_head_sf")
self.mocapFilter = MocapDataFilter("MocapDataFilter")
plug(self.ros.signal("chest"), self.mocapFilter.sin)
self.mocapSignal = self.mocapFilter.sout
def initialize(self):
m = np.matrix(
[
[-0.8530, 0.5208, -0.0360, 0.0049],
[-0.5206, -0.8537, -0.0146, -0.0078],
[-0.0384, 0.0063, 0.9993, 0.2158],
[0, 0, 0, 1.0000],
]
) # transformation from the chest-markers frame to the chest frame
self.chestMarkersSot = OpPointModifier(
"chestMarkersSot"
) # gets the position of the chest-markers frame in sot frame
self.chestMarkersSot.setEndEffector(False)
self.chestMarkersSot.setTransformation(matrixToTuple(m))
plug(self.robot.dynamic.chest, self.chestMarkersSot.positionIN)
self.chestMarkersSot.position.recompute(1)
self.mchestsot = np.matrix(self.chestMarkersSot.position.value)
self.mchestmocap = np.matrix(self.ros.signal("chest").value)
self.mtransformMocap2ISot = (
np.linalg.inv(self.mchestsot) * self.mchestmocap
) # transforms the mocap frame to the initial-sot frame
self.chestMarkerMocapInISot = Multiply_of_matrixHomo(
"chestMarkerMocap"
) # gets position of chest-markers frame in sot-initial frame
self.chestMarkerMocapInISot.sin1.value = matrixToTuple(self.mtransformMocap2ISot)
plug(self.mocapSignal, self.chestMarkerMocapInISot.sin2)
self.chestMarkersSotInverse = Inverse_of_matrixHomo(
"chestMarkersSotInverse"
) # inverse of the homomatrix for position of markers in local sot
plug(self.chestMarkersSot.position, self.chestMarkersSotInverse.sin)
self.robotPositionISot = Multiply_of_matrixHomo("robotPositionInSot")
plug(self.chestMarkerMocapInISot.sout, self.robotPositionISot.sin1)
plug(self.chestMarkersSotInverse.sout, self.robotPositionISot.sin2)
self.robotPositionInMocap = Multiply_of_matrixHomo("robotPositionInMocap")
plug(self.mocapSignal, self.robotPositionInMocap.sin1)
plug(self.chestMarkersSotInverse.sout, self.robotPositionInMocap.sin2)
def __init__(self, robot):
self.robot = robot
self.ros = RosExport("rosExportMocap")
self.ros.add("matrixHomoStamped", "chest", "/evart/hrp2_head_sf/hrp2_head_sf")
self.mocapFilter = MocapDataFilter("MocapDataFilter")
plug(self.ros.signal("chest"), self.mocapFilter.sin)
self.mocapSignal = self.mocapFilter.sout
def __init__(self, motion, yamlData):
checkDict('object-name', yamlData)
checkDict('position', yamlData)
Control.__init__(self, motion, yamlData)
self.robot = motion.robot
self.objectName = yamlData['object-name']
self.frameName = yamlData['frame-name']
self.position = yamlData['position']
obj = motion.environment.get(self.objectName)
if not obj:
raise RuntimeError('object does not exist')
self.robotPositionFromVisp = RobotPositionFromVisp(
'robotPositionFromViSP' + str(id(yamlData)))
# Convert ViSP frame into usual dynamic-graph frame.
self.robotPositionFromVisp.setSensorTransformation(
(( 0., 0., 1., 0.),
( 0., -1., 0., 0.),
(-1., 0., 0., 0.),
( 0., 0., 0., 1.))
)
if motion.ros:
self.ros = motion.ros
else:
self.ros = RosExport('rosExport')
self.ros.add('matrixHomoStamped', self.objectName, self.position)
self.robotPositionFromVisp.plannedObjectPosition.value = \
obj.plannedPosition.dgRotationMatrix()
plug(self.ros.signal(self.objectName),
self.robotPositionFromVisp.cMo)
plug(self.ros.signal(self.objectName + 'Timestamp'),
self.robotPositionFromVisp.cMoTimestamp)
# Plug wMc/wMr to robotPositionFromVisp
plug(motion.robot.frames[self.frameName].position,
self.robotPositionFromVisp.wMc)
plug(motion.robot.dynamic.waist, self.robotPositionFromVisp.wMr)
#connect to mocap
from dynamic_graph.ros import RosExport
rosIn = RosExport('rosExportMocap')
rosIn.add('matrixHomoStamped', "chest", "/evart/chest9M/chest9M")
robot.device.after.addDownsampledSignal('rosExportMocap.chest',1)
rosIn.add('matrixHomoStamped', "left_foot", "/evart/hrp2_14_LeftFoot/hrp2_14_LeftFoot")
robot.device.after.addDownsampledSignal('rosExportMocap.left_foot',1)
rosIn.add('matrixHomoStamped', "left_hand", "/evart/hand_flag4m/hand_flag4m")
robot.device.after.addDownsampledSignal('rosExportMocap.left_hand',1)
#republish mocap
ros.add('matrix', 'chest_ros', 'chest');
plug(rosIn.chest, ros.chest_ros);
tracer=create_tracer(robot.device)
addTrace(tracer,rosIn,'chest')
addTrace(tracer,rosIn,'left_foot')
addTrace(tracer,rosIn,'left_hand')
#MOCAP:**************************************
ssh tflayols@hakusan-vm
export ROS_MASTER_URI=http://hrp2014c:11311
roslaunch evart_bridge start.launch&
rosservice call /evart/list_segments
rosservice call /evart/track_segments chest9M chest9M
rosservice call /evart/track_segments hand_flag4m hand_flag4m
rosservice call /evart/track_segments hrp2_14_LeftFoot hrp2_14_LeftFoot
class ControlViSP(Control):
yaml_tag = u'visp'
def __init__(self, motion, yamlData):
checkDict('object-name', yamlData)
checkDict('position', yamlData)
Control.__init__(self, motion, yamlData)
self.robot = motion.robot
self.objectName = yamlData['object-name']
self.frameName = yamlData['frame-name']
self.position = yamlData['position']
obj = motion.environment.get(self.objectName)
if not obj:
raise RuntimeError('object does not exist')
self.robotPositionFromVisp = RobotPositionFromVisp(
'robotPositionFromViSP' + str(id(yamlData)))
# Convert ViSP frame into usual dynamic-graph frame.
self.robotPositionFromVisp.setSensorTransformation(
(( 0., 0., 1., 0.),
( 0., -1., 0., 0.),
(-1., 0., 0., 0.),
( 0., 0., 0., 1.))
)
if motion.ros:
self.ros = motion.ros
else:
self.ros = RosExport('rosExport')
self.ros.add('matrixHomoStamped', self.objectName, self.position)
self.robotPositionFromVisp.plannedObjectPosition.value = \
obj.plannedPosition.dgRotationMatrix()
plug(self.ros.signal(self.objectName),
self.robotPositionFromVisp.cMo)
plug(self.ros.signal(self.objectName + 'Timestamp'),
self.robotPositionFromVisp.cMoTimestamp)
# Plug wMc/wMr to robotPositionFromVisp
plug(motion.robot.frames[self.frameName].position,
self.robotPositionFromVisp.wMc)
plug(motion.robot.dynamic.waist, self.robotPositionFromVisp.wMr)
def start(self, name, feetFollowerWithCorrection):
I = ((1.,0.,0.,0.), (0.,1.,0.,0.), (0.,0.,1.,0.), (0.,0.,0.,1.))
self.estimator = ErrorEstimator(name)
self.estimator.setReferenceTrajectory(
feetFollowerWithCorrection.referenceTrajectory.name)
plug(feetFollowerWithCorrection.referenceTrajectory.waist,
self.estimator.planned)
self.estimator.setSensorToWorldTransformation(I)
#FIXME: we should change the reference point accordingly
# with the current contact point.
plug(self.robot.dynamic.signal('Jleft-ankle'),
self.estimator.referencePointJacobian)
self.estimator.plannedCommand.value = self.robot.device.state.value
if type(self.robot.device) == RobotSimu:
self.estimator.realCommand.value = self.robot.device.state.value
else:
self.estimator.realCommand.value = self.robot.device.robotState.value
plug(self.robotPositionFromVisp.position, self.estimator.position)
plug(self.robotPositionFromVisp.positionTimestamp,
self.estimator.positionTimestamp)
self.setupTrace(self.estimator)
return self.estimator
def interactiveStart(self, name, feetFollowerWithCorrection):
while len(self.ros.signals()) == 0:
raw_input("Press enter after starting ROS visp_tracker node.")
while len(self.ros.signal(self.objectName).value) < 1:
raw_input("Tracking not started...")
return self.start(name, feetFollowerWithCorrection)
def canStart(self):
if not self.ros:
return False
if len(self.ros.signals()) == 0:
return False
if len(self.ros.signal(self.objectName).value) < 1:
return False
if len(self.ros.signal(self.objectName + 'Timestamp').value) < 1:
return False
return True
def setupTrace(self, errorEstimator):
self.setupTraceErrorEstimator(self.estimator)
for s in [self.objectName, self.objectName + 'Timestamp']:
addTrace(self.robot, self.trace, self.ros.name, s)
for s in ['cMo', 'cMoTimestamp',
'plannedObjectPosition',
'position',
'positionTimestamp',
'dbgcMo',
'dbgPosition',
'dbgrMc']:
addTrace(self.robot, self.trace, self.robotPositionFromVisp.name, s)
def __str__(self):
msg = "ViSP moving edge tracker control element (frame: {0}, object: {1})"
return msg.format(self.frameName, self.objectName)
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 __init__(self, robot):
self.robot = robot
self.ros = RosExport('rosExportMocap')
self.ros.add('matrixHomoStamped', "chest", "/evart/hrp2_head_sf/PO")
self.mocapSignal = self.ros.signal('chest')
def __init__(self, robot):
self.robot = robot
self.ros = RosExport("rosExportMocap")
self.ros.add("matrixHomoStamped", "chest", "/evart/hrp2_head_sf/PO")
self.mocapSignal = self.ros.signal("chest")
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.setKfeRopes(matrixToTuple(np.diag((10000,10000,10000))))
self.setKfvRopes(matrixToTuple(np.diag((300,300,800))))
self.setKteRopes(matrixToTuple(np.diag((600,600,600))))
self.setKtvRopes(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 __init__(self, filename, robot, solver, defaultDirectories,
logger = None):
if not logger:
logger = initializeLogging()
self.defaultDirectories = defaultDirectories
self.robot = robot
self.solver = solver
self.logger = logger
self.logger.info('loading motion plan file \'{0}\''.format(filename))
self.plan = yaml.load(
open(searchFile(filename, defaultDirectories), "r"))
self.duration = float(self.plan['duration'])
# Middleware proxies.
self.corba = CorbaServer('corba_server')
self.ros = RosExport('rosExport')
# Supervisor.
self.supervisor = Supervisor('supervisor')
self.robot.device.after.addSignal(self.supervisor.name + '.trigger')
self.supervisor.setSolver(self.solver.sot.name)
# Load plan.
self.logger.debug('loading environment')
self.loadEnvironment()
self.logger.debug('loading motion elements')
self.loadMotion()
self.logger.debug('loading control elements')
self.loadControl()
# For now, only 1 feet follower is allowed (must start at t=0).
feetFollowerElement = find(lambda e: type(e) == MotionWalk, self.motion)
hasControl = len(self.control) > 0
self.supervisor.setPostureFeature(
feetFollowerElement.feetFollower.postureFeature.name)
# Plug motion signals which depend on control.
for m in self.motion:
if type(m) == MotionVisualPoint:
# FIXME: this is so wrong.
plug(self.ros.signal(m.objectName),
m.vispPointProjection.cMo)
plug(self.ros.signal(m.objectName + 'Timestamp'),
m.vispPointProjection.cMoTimestamp)
if hasControl and feetFollowerElement:
self.feetFollower = FeetFollowerGraphWithCorrection(
robot, solver, feetFollowerElement.feetFollower,
MotionPlanErrorEstimationStrategy,
maxX = self.maxX, maxY = self.maxY,
maxTheta = self.maxTheta)
self.feetFollower.errorEstimationStrategy.motionPlan = self
#FIXME: not enough generic
self.feetFollower.feetFollower.setFootsteps(
2., makeFootsteps(self.footsteps))
elif feetFollowerElement:
self.feetFollower = feetFollowerElement.feetFollower
else:
self.feetFollower = None
self.logger.debug('motion plan created with success')
class MotionPlan(object):
robot = None
solver = None
feetFollower = None
corba = None
ros = None
plan = None
duration = 0
motion = []
control = []
footsteps = []
environment = {}
trace = None
started = False
maxX = FeetFollowerGraphWithCorrection.maxX
maxY = FeetFollowerGraphWithCorrection.maxY
maxTheta = FeetFollowerGraphWithCorrection.maxTheta
def __init__(self, filename, robot, solver, defaultDirectories,
logger = None):
if not logger:
logger = initializeLogging()
self.defaultDirectories = defaultDirectories
self.robot = robot
self.solver = solver
self.logger = logger
self.logger.info('loading motion plan file \'{0}\''.format(filename))
self.plan = yaml.load(
open(searchFile(filename, defaultDirectories), "r"))
self.duration = float(self.plan['duration'])
# Middleware proxies.
self.corba = CorbaServer('corba_server')
self.ros = RosExport('rosExport')
# Supervisor.
self.supervisor = Supervisor('supervisor')
self.robot.device.after.addSignal(self.supervisor.name + '.trigger')
self.supervisor.setSolver(self.solver.sot.name)
# Load plan.
self.logger.debug('loading environment')
self.loadEnvironment()
self.logger.debug('loading motion elements')
self.loadMotion()
self.logger.debug('loading control elements')
self.loadControl()
# For now, only 1 feet follower is allowed (must start at t=0).
feetFollowerElement = find(lambda e: type(e) == MotionWalk, self.motion)
hasControl = len(self.control) > 0
self.supervisor.setPostureFeature(
feetFollowerElement.feetFollower.postureFeature.name)
# Plug motion signals which depend on control.
for m in self.motion:
if type(m) == MotionVisualPoint:
# FIXME: this is so wrong.
plug(self.ros.signal(m.objectName),
m.vispPointProjection.cMo)
plug(self.ros.signal(m.objectName + 'Timestamp'),
m.vispPointProjection.cMoTimestamp)
if hasControl and feetFollowerElement:
self.feetFollower = FeetFollowerGraphWithCorrection(
robot, solver, feetFollowerElement.feetFollower,
MotionPlanErrorEstimationStrategy,
maxX = self.maxX, maxY = self.maxY,
maxTheta = self.maxTheta)
self.feetFollower.errorEstimationStrategy.motionPlan = self
#FIXME: not enough generic
self.feetFollower.feetFollower.setFootsteps(
2., makeFootsteps(self.footsteps))
elif feetFollowerElement:
self.feetFollower = feetFollowerElement.feetFollower
else:
self.feetFollower = None
self.logger.debug('motion plan created with success')
def loadEnvironment(self):
if not 'environment' in self.plan:
return
for obj in self.plan['environment']:
checkDict('object', obj)
checkDict('name', obj['object'])
self.environment[obj['object']['name']] = \
EnvironmentObject(self, obj['object'])
self.logger.debug('adding object \'{0}\''.format(obj['object']['name']))
def loadMotion(self):
if not 'motion' in self.plan or not self.plan['motion']:
return
if 'maximum-correction-per-step' in self.plan:
self.maxX = self.plan['maximum-correction-per-step']['x']
self.maxY = self.plan['maximum-correction-per-step']['y']
self.maxTheta = self.plan['maximum-correction-per-step']['theta']
motionClasses = [MotionWalk, MotionJoint, MotionTask, MotionVisualPoint]
for motion in self.plan['motion']:
if len(motion.items()) != 1:
raise RuntimeError('each motion should have only one type')
(tag, data) = motion.items()[0]
cls = find(lambda c: c.yaml_tag == tag, motionClasses)
if not cls:
raise RuntimeError('invalid motion element')
self.motion.append(cls(self, data, self.defaultDirectories))
self.logger.debug('adding motion element \'{0}\''.format(tag))
if self.trace:
for motion in self.motion:
motion.setupTrace(self.trace)
def loadControl(self):
if not 'control' in self.plan or not self.plan['control']:
return
controlClasses = [ControlConstant, ControlMocap, ControlViSP,
ControlHueblob, ControlVirtualSensor]
for control in self.plan['control']:
if len(control.items()) != 1:
raise RuntimeError('each control should have only one type')
(tag, data) = control.items()[0]
cls = find(lambda c: c.yaml_tag == tag, controlClasses)
if not cls:
raise RuntimeError('invalid control element')
self.control.append(cls(self, data))
self.logger.debug('adding control element \'{0}\''.format(tag))
def __str__(self):
res = 'Motion:\n'
res += '-------\n'
for motion in self.motion:
res += '\t* {0}\n'.format(str(motion))
res += '\n'
res += 'Control:\n'
res += '--------\n'
for control in self.control:
res += '\t* {0}\n'.format(str(control))
res += '\n'
res += str(self.feetFollower)
res += '\n\n'
res += self.supervisor.display()
return res
def start(self):
if self.started:
self.logger.info('already started')
return
if not self.canStart():
self.logger.info('failed to start')
return
self.started = True
self.logger.info('execution starts')
if self.feetFollower:
self.feetFollower.start()
# Remove default tasks and let the supervisor take over the
# tasks management.
self.solver.sot.clear()
tOrigin = self.feetFollower.feetFollower.getStartTime()
self.supervisor.setOrigin(max(0., tOrigin))
def canStart(self):
canStart = reduce(lambda acc, c: c.canStart() and acc,
self.control, True)
if not canStart:
return False
if self.feetFollower:
return self.feetFollower.canStart()
else:
return True
class HRP2ModelBaseFlexEstimatorIMUForce(DGIMUModelBaseFlexEstimation):
def __init__(self, robot, name="flextimator"):
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.0e-2,) * 6
+ (1e-15,) * 2
+ (1.0e-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.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, 0.0, 1.57))
self.interface.setRightHandSensorTransformation((0.0, 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.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.Peg)
(self.contact2OpPoint, self.contact2Pos, self.contact2) = self.createContact("contact2", self.Prl2, self.Peg)
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)
# Mocap signal
self.ros = RosExport("rosExportMocap")
self.ros.add("matrixHomoStamped", "chest", "/evart/hrp2_head_sf/hrp2_head_sf")
# Filtering
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()
# Measurement reconstruction
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
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")
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.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")
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.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 = self.robot.timeStep
# self.angMomDerivator = PositionStateReconstructor (name+'angMomDerivator')
# 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 initAbsolutePoses(self):
self.drift.init()
def createContact(self, name, prl, peg):
self.contactOpPoint = OpPointModifier(name + "_opPoint")
self.contactOpPoint.setEndEffector(False)
self.contactOpPoint.setTransformation(matrixToTuple(prl))
plug(self.robot.dynamic.chest, self.contactOpPoint.positionIN)
self.contactPos = MatrixHomoToPose(name + "_pos")
plug(self.contactOpPoint.position, self.contactPos.sin)
self.contact = Stack_of_vector(name)
self.contact.sin1.value = peg
plug(self.contactPos.sout, self.contact.sin2)
self.contact.selec1(0, 3)
self.contact.selec2(0, 3)
return (self.contactOpPoint, self.contactPos, self.contact)
