[WIP] test_dcm_zmp_control_flex_openloop

python/sot_talos_balance/create_entities_utils.py

@@ -113,6 +113,13 @@ def create_config_trajectory_generator(dt, robot):
     jtg.init(dt, N_CONFIG)
     return jtg
 
+def create_torque_trajectory_generator(dt, robot):
+    N_CONFIG = N_JOINTS + 6
+    jtg = NdTrajectoryGenerator("torqueTrajGen")
+    jtg.initial_value.value = [0.] * N_CONFIG
+    jtg.trigger.value = 1.0
+    jtg.init(dt, N_CONFIG)
+    return jtg
 
 def create_com_trajectory_generator(dt, robot):
     comTrajGen = NdTrajectoryGenerator("comTrajGen")
@@ -680,6 +687,10 @@ def load_folder(robot, folder, zmp=False):
             robot.phaseTrajGen.playTrajectoryFile(folder + 'Phase.dat')
         except AttributeError:
             pass
+        try:
+            robot.torqueTrajGen.playTrajectoryFile(folder + 'Torques.dat')
+        except AttributeError:
+            pass
 
 def reload_folder(robot, folder, zmp=False):
     set_trigger(robot, False)

python/sot_talos_balance/test/appli_dcm_zmp_control_flex_openloop.py

+# flake8: noqa
+from math import sqrt
+
+import numpy as np
+
+import sot_talos_balance.talos.base_estimator_conf as base_estimator_conf
+import sot_talos_balance.talos.control_manager_conf as cm_conf
+import sot_talos_balance.talos.ft_calibration_conf as ft_conf
+import sot_talos_balance.talos.hip_flexibility_compensation_conf as hipFlexCompConfig
+import sot_talos_balance.talos.parameter_server_conf as param_server_conf
+from dynamic_graph import plug
+from dynamic_graph.sot.core import SOT, Derivator_of_Vector, FeaturePosture, MatrixHomoToPoseQuaternion, Task
+from dynamic_graph.sot.core.matrix_util import matrixToTuple
+from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
+from dynamic_graph.sot.dynamics_pinocchio import DynamicPinocchio
+from dynamic_graph.tracer_real_time import TracerRealTime
+from sot_talos_balance.create_entities_utils import *
+
+cm_conf.CTRL_MAX = 10.0  # temporary hack
+
+robot.timeStep = robot.device.getTimeStep()
+dt = robot.timeStep
+robot.device.setControlInputType("noInteg")
+
+# --- 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 --------------------------
+
+folder = None
+if test_folder is not None:
+    if sot_talos_balance_folder:
+        from rospkg import RosPack
+        rospack = RosPack()
+
+        folder = rospack.get_path('sot_talos_balance') + "/data/" + test_folder
+    else:
+        folder = test_folder
+    if folder[-1] != '/':
+        folder += '/'
+
+# --- Trajectory generators
+
+# --- General trigger
+robot.triggerTrajGen = BooleanIdentity('triggerTrajGen')
+set_trigger(robot, False)
+
+# --- CoM
+robot.comTrajGen = create_com_trajectory_generator(dt, robot)
+robot.comTrajGen.x.recompute(0)  # trigger computation of initial value
+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)
+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)
+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
+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)
+plug(robot.triggerTrajGen.sout, robot.waistTrajGen.trigger)
+
+# --- Phase
+robot.phaseTrajGen = create_scalar_trajectory_generator(dt, 0., 'phaseTrajGen')
+robot.phaseTrajGen.x.recompute(0)  # trigger computation of initial value
+robot.phaseScalar = Component_of_vector("phase_scalar")
+robot.phaseScalar.setIndex(0)
+plug(robot.phaseTrajGen.x, robot.phaseScalar.sin)
+robot.phaseInt = RoundDoubleToInt("phase_int")
+plug(robot.phaseScalar.sout, robot.phaseInt.sin)
+plug(robot.triggerTrajGen.sout, robot.phaseTrajGen.trigger)
+
+# --- Torque
+robot.torqueTrajGen = create_torque_trajectory_generator(dt, robot, 'WT')
+robot.torqueTrajGen.x.recompute(0)  # trigger computation of initial value
+
+robot.torqueSelec = Selec_of_vector("torque_selec")
+robot.torqueSelec.selec(6, 38)
+plug(robot.torqueTrajGen.x, robot.torqueSelec.sin)
+
+# --- Load files
+load_folder(robot, folder)
+
+# --- Interface with controller entities
+
+wp = DummyWalkingPatternGenerator('dummy_wp')
+wp.init()
+wp.omega.value = omega
+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)
+#if folder is not None:
+#    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)
+
+# robot.be_filters              = create_be_filters(robot, dt)
+
+# --- Conversion
+e2q = EulerToQuat('e2q')
+plug(robot.base_estimator.q, e2q.euler)
+robot.e2q = e2q
+
+# --- Kinematic computations
+robot.rdynamic = DynamicPinocchio("real_dynamics")
+robot.rdynamic.setModel(robot.dynamic.model)
+robot.rdynamic.setData(robot.rdynamic.model.createData())
+plug(robot.base_estimator.q, robot.rdynamic.position)
+robot.rdynamic.velocity.value = [0.0] * robotDim
+robot.rdynamic.acceleration.value = [0.0] * robotDim
+
+# --- CoM Estimation
+cdc_estimator = DcmEstimator('cdc_estimator')
+cdc_estimator.init(dt, robot_name)
+plug(robot.e2q.quaternion, cdc_estimator.q)
+plug(robot.base_estimator.v, cdc_estimator.v)
+robot.cdc_estimator = cdc_estimator
+
+# --- DCM Estimation
+estimator = DummyDcmEstimator("dummy")
+plug(robot.wp.omegaDes, estimator.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)
+Kz_dcm = [0.] * 3
+gamma_dcm = 0.2
+
+dcm_controller = DcmController("dcmCtrl")
+
+dcm_controller.Kp.value = Kp_dcm
+dcm_controller.Ki.value = Ki_dcm
+dcm_controller.Kz.value = Kz_dcm
+dcm_controller.decayFactor.value = gamma_dcm
+dcm_controller.mass.value = mass
+plug(robot.wp.omegaDes, dcm_controller.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)
+
+plug(robot.zmp_estimator.zmp, dcm_controller.zmp)
+
+dcm_controller.init(dt)
+
+robot.dcm_control = dcm_controller
+
+Ki_dcm = [1.0, 1.0, 1.0]  # this value is employed later
+
+Kz_dcm = [0.0, 0.0, 0.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
+
+robotDim = robot.dynamic.getDimension()  # 38
+for i in range(18, robotDim):
+    robot.taskUpperBody.feature.selectDof(i, 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 = 100.
+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())
+
+# --- State integrator
+robot.integrate = SimpleStateIntegrator("integrate")
+robot.integrate.init(dt)
+robot.integrate.setState(robot.device.state.value)
+robot.integrate.setVelocity(robot.dynamic.getDimension() * [0.])
+
+# --- Hip flexibility compensation --------------------------------
+
+robot.hipComp = create_hip_flexibility_compensation(robot, hipFlexCompConfig, robot_name)
+plug(robot.torqueSelec.sout, robot.hipComp.tau)
+plug(robot.phaseInt.sout, robot.hipComp.phase)
+if not flexi:
+    robot.hipComp.K_l.value = float('inf')  #disable
+    robot.hipComp.K_r.value = float('inf')  #disable
+
+# --- Plug SOT control to device through control manager, state integrator and hip flexibility compensation
+plug(robot.sot.control, robot.cm.ctrl_sot_input)
+plug(robot.cm.u_safe, robot.integrate.control)
+plug(robot.integrate.state, robot.hipComp.q_des)
+plug(robot.hipComp.q_cmd, 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)
+# robot.sot.push(robot.taskPos.name)
+# robot.device.control.recompute(0) # this crashes as it employs joint sensors which are not ready yet
+
+# --- Delay
+robot.delay_pos = DelayVector("delay_pos")
+robot.delay_pos.setMemory(robot.device.state.value)
+robot.device.before.addSignal(robot.delay_pos.name + '.current')
+plug(robot.integrate.state, robot.delay_pos.sin)
+
+robot.delay_vel = DelayVector("delay_vel")
+robot.delay_vel.setMemory(robot.dynamic.getDimension() * [0.])
+robot.device.before.addSignal(robot.delay_vel.name + '.current')
+plug(robot.cm.u_safe, robot.delay_vel.sin)
+
+# --- Plug integrator instead of device
+plug(robot.delay_pos.previous, robot.pselec.sin)
+plug(robot.pselec.sout, robot.base_estimator.joint_positions)
+plug(robot.delay_vel.previous, robot.vselec.sin)
+
+# --- Fix robot.dynamic inputs
+plug(robot.delay_pos.previous, robot.dynamic.position)
+plug(robot.delay_vel.previous, robot.dynamic.velocity)
+robot.dvdt = Derivator_of_Vector("dv_dt")
+robot.dvdt.dt.value = dt
+plug(robot.delay_vel.previous, 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.device, 'forceLLEG', robot = robot, data_type='vector')               # measured left wrench
+#create_topic(robot.publisher, robot.device, 'forceRLEG', robot = robot, data_type='vector')               # measured right wrench
+
+#create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
+#create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
+
+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
+
+create_topic(robot.publisher, robot.hipComp, 'delta_q', robot=robot, data_type='vector')
+create_topic(robot.publisher, robot.hipComp, 'q_cmd', robot=robot, data_type='vector')
+create_topic(robot.publisher, robot.hipComp, 'q_des', robot=robot, data_type='vector')
+create_topic(robot.publisher, robot.hipComp, 'tau', robot=robot, data_type='vector')
+create_topic(robot.publisher, robot.hipComp, 'tau_filt', robot=robot, data_type='vector')
+
+# --- 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()
+

python/sot_talos_balance/test/test_dcm_zmp_control_flex_openloop.py

+'''Test CoM admittance control as described in paper'''
+from sys import argv
+from time import sleep
+
+from sot_talos_balance.utils.run_test_utils import (ask_for_confirmation, get_file_folder, run_ft_calibration,
+                                                    run_test, runCommandClient)
+
+try:
+    # Python 2
+    input = raw_input  # noqa
+except NameError:
+    pass
+
+test_folder, sot_talos_balance_folder = get_file_folder(argv)
+
+flexi = ask_for_confirmation('Compensate flexibility?')
+if flexi:
+    print('Compensating flexibility')
+    runCommandClient('flexi = False')
+else:
+    print('Not compensating flexibility')
+    runCommandClient('flexi = False')
+
+run_test('appli_dcm_zmp_control_flex.py')
+
+run_ft_calibration('robot.ftc')
+
+if flexi:
+    input("Wait before activating flexibility")
+    cmd_l = 'robot.hipComp.K_l.value = hipFlexCompConfig.flexibility_left'
+    cmd_r = 'robot.hipComp.K_r.value = hipFlexCompConfig.flexibility_right'
+    runCommandClient(cmd_l + '; ' + cmd_r)
+
+input("Wait before running the test")
+
+# Connect ZMP reference and reset controllers
+print('Connect ZMP reference')
+runCommandClient('plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)')
+runCommandClient('plug(robot.dcm_control.zmpRef,robot.com_admittance_control.zmpDes)')
+runCommandClient('robot.com_admittance_control.setState(robot.wp.comDes.value,[0.0,0.0,0.0])')
+runCommandClient('robot.com_admittance_control.Kp.value = Kp_adm')
+runCommandClient('robot.dcm_control.resetDcmIntegralError()')
+runCommandClient('robot.dcm_control.Ki.value = Ki_dcm')
+runCommandClient('robot.dcm_control.Kz.value = Kz_dcm')
+
+if test_folder is not None:
+    c = ask_for_confirmation('Execute trajectory?')
+    if c:
+        print('Executing the trajectory')
+        runCommandClient('set_trigger(robot, True)')
+    else:
+        print('Not executing the trajectory')
+else:
+    c = ask_for_confirmation("Execute a sinusoid?")
+    if c:
+        print("Putting the robot in position...")
+        runCommandClient('robot.comTrajGen.move(1,-0.025,1.0)')
+        sleep(1.0)
+        print("Robot is in position!")
+
+        c2 = ask_for_confirmation("Confirm executing the sinusoid?")
+        if c2:
+            print("Executing the sinusoid...")
+            runCommandClient('robot.comTrajGen.startSinusoid(1,0.025,2.0)')
+            print("Sinusoid started!")
+        else:
+            print("Not executing the sinusoid")
+
+        c3 = ask_for_confirmation("Put the robot back?")
+        if c3:
+            print("Stopping the robot...")
+            runCommandClient('robot.comTrajGen.stop(1)')
+            sleep(5.0)
+            print("Putting the robot back...")
+            runCommandClient('robot.comTrajGen.move(1,0.0,1.0)')
+            sleep(1.0)