[test_feet_admittance] First draft

python/sot_talos_balance/test/appli_feet_admittance.py

+from sot_talos_balance.create_entities_utils import *
+import sot_talos_balance.talos.parameter_server_conf   as param_server_conf
+import sot_talos_balance.talos.control_manager_conf    as cm_conf
+import sot_talos_balance.talos.base_estimator_conf     as base_estimator_conf
+import sot_talos_balance.talos.ft_calibration_conf     as ft_conf
+from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
+from dynamic_graph.sot.core import Task, FeaturePosture
+from dynamic_graph.sot.core.matrix_util import matrixToTuple
+from dynamic_graph import plug
+from dynamic_graph.sot.core import SOT
+from math import sqrt
+import numpy as np
+
+from dynamic_graph.tracer_real_time import TracerRealTime
+
+from dynamic_graph.sot.dynamics_pinocchio import DynamicPinocchio
+
+robot.timeStep = robot.device.getTimeStep()
+dt = robot.timeStep
+
+# --- 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 --------------------------
+
+# --- Trajectory generators
+robot.comTrajGen = create_com_trajectory_generator(dt,robot)
+robot.lfPosTrajGen  = create_position_trajectory_generator(dt, robot, 'LF')
+robot.rfPosTrajGen  = create_position_trajectory_generator(dt, robot, 'RF')
+
+# --- 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
+plug(robot.lfPosTrajGen.x, wp.footPositionLeft)  # if given, footPositionLeft overrides the translation in footLeft
+plug(robot.rfPosTrajGen.x, wp.footPositionRight) # if given, footPositionRight overrides the translation in footRight
+plug(robot.comTrajGen.x, wp.com)
+plug(robot.comTrajGen.dx, wp.vcom)
+plug(robot.comTrajGen.ddx, 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)
+
+# -------------------------- 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.be_filters              = create_be_filters(robot, dt)
+
+# --- Reference frame
+
+rf = SimpleReferenceFrame('rf')
+rf.init(robot_name)
+plug(robot.dynamic.LF, rf.footLeft)
+plug(robot.dynamic.RF, rf.footRight)
+robot.rf = rf
+
+# --- State transformation
+stf = StateTransformation("stf")
+stf.init()
+plug(robot.rf.referenceFrame,stf.referenceFrame)
+plug(robot.base_estimator.q,stf.q_in)
+plug(robot.base_estimator.v,stf.v_in)
+robot.stf = stf
+
+# --- Conversion
+e2q = EulerToQuat('e2q')
+plug(robot.stf.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.stf.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.stf.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 --------------------------
+
+# --- Left foot
+controller = AdmittanceControllerEndEffector("admLF")
+
+plug(robot.ftc.left_foot_force_out, controller.force)
+plug(robot.e2q.quaternion, controller.q)
+controller.Kp.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+controller.Kd.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+controller.w_forceDes.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+controller.dqSaturation.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+
+controller.init(dt, "right_sole_link", 'leg_right_6_joint')
+
+robot.admLF = controller
+
+# --- Right foot
+controller = AdmittanceControllerEndEffector("admRF")
+
+plug(robot.ftc.right_foot_force_out, controller.force)
+plug(robot.e2q.quaternion, controller.q)
+controller.Kp.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+controller.Kd.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+controller.w_forceDes.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+controller.dqSaturation.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+
+controller.init(dt, "left_sole_link", 'leg_right_6_joint')
+
+robot.admRF = controller
+
+# --- 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
+
+# --- Left foot
+robot.contactLF = MetaTaskKine6d('contactLF',robot.dynamic,'LF',robot.OperationalPointsMap['left-ankle'])
+# handMgrip = np.eye(4)
+# handMgrip[0:3, 3] = (0.1, 0, 0)
+# robot.contactLF.opmodif = matrixToTuple(handMgrip)
+robot.contactLF.feature.frame('desired')
+# robot.contactLF.feature.selec.value = '111111'
+robot.contactLF.task.setWithDerivative(True)
+robot.contactLF.task.controlGain.value = 0
+robot.contactLF.feature.position.value = np.eye(4)
+robot.contactLF.feature.velocity.value = [0., 0., 0., 0., 0., 0.]
+robot.contactLF.featureDes.position.value = np.eye(4)
+# robot.contactLF.gain.setConstant(300)
+# plug(robot.wp.footLeftDes, robot.contactLF.featureDes.position) #.errorIN?
+plug(robot.admLF.dq, robot.contactLF.featureDes.velocity)
+locals()['contactLF'] = robot.contactLF
+
+# --- Right foot
+robot.contactRF = MetaTaskKine6d('contactRF',robot.dynamic,'RF',robot.OperationalPointsMap['right-ankle'])
+# handMgrip = np.eye(4)
+# handMgrip[0:3, 3] = (0.1, 0, 0)
+# robot.contactRF.opmodif = matrixToTuple(handMgrip)
+robot.contactRF.feature.frame('desired')
+# robot.contactRF.feature.selec.value = '111111'
+robot.contactRF.task.setWithDerivative(True)
+robot.contactRF.task.controlGain.value = 0
+robot.contactRF.feature.position.value = np.eye(4)
+robot.contactRF.feature.velocity.value = [0., 0., 0., 0., 0., 0.]
+robot.contactRF.featureDes.position.value = np.eye(4)
+# robot.contactRF.gain.setConstant(300)
+# plug(robot.wp.footLeftDes, robot.contactRF.featureDes.position) #.errorIN?
+plug(robot.admRF.dq, robot.contactRF.featureDes.velocity)
+locals()['contactRF'] = robot.contactRF
+
+# --- COM
+robot.taskCom = MetaTaskKineCom(robot.dynamic)
+plug(robot.wp.comDes,robot.taskCom.featureDes.errorIN)
+robot.taskCom.task.controlGain.value = 10
+robot.taskCom.task.setWithDerivative(True)
+
+# --- 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
+
+# # --- Posture
+# robot.taskPos = Task ('task_pos')
+# robot.taskPos.feature = FeaturePosture('feature_pos')
+#
+# q = list(robot.dynamic.position.value)
+# robot.taskPos.feature.state.value = q
+# robot.taskPos.feature.posture.value = q
+
+# robotDim = robot.dynamic.getDimension() # 38
+#robot.taskPos.feature.selectDof(6,True)
+#robot.taskPos.feature.selectDof(7,True)
+#robot.taskPos.feature.selectDof(8,True)
+#robot.taskPos.feature.selectDof(9,True)
+#robot.taskPos.feature.selectDof(10,True)
+#robot.taskPos.feature.selectDof(11,True)
+#robot.taskPos.feature.selectDof(12,True)
+#robot.taskPos.feature.selectDof(13,True)
+#robot.taskPos.feature.selectDof(14,True)
+#robot.taskPos.feature.selectDof(15,True)
+#robot.taskPos.feature.selectDof(16,True)
+#robot.taskPos.feature.selectDof(17,True)
+#robot.taskPos.feature.selectDof(18,True)
+#robot.taskPos.feature.selectDof(19,True)
+#robot.taskPos.feature.selectDof(20,True)
+#robot.taskPos.feature.selectDof(21,True)
+#robot.taskPos.feature.selectDof(22,True)
+#robot.taskPos.feature.selectDof(23,True)
+#robot.taskPos.feature.selectDof(24,True)
+#robot.taskPos.feature.selectDof(25,True)
+#robot.taskPos.feature.selectDof(26,True)
+#robot.taskPos.feature.selectDof(27,True)
+#robot.taskPos.feature.selectDof(28,True)
+#robot.taskPos.feature.selectDof(29,True)
+#robot.taskPos.feature.selectDof(30,True)
+#robot.taskPos.feature.selectDof(31,True)
+#robot.taskPos.feature.selectDof(32,True)
+#robot.taskPos.feature.selectDof(33,True)
+#robot.taskPos.feature.selectDof(34,True)
+#robot.taskPos.feature.selectDof(35,True)
+#robot.taskPos.feature.selectDof(36,True)
+#robot.taskPos.feature.selectDof(37,True)
+
+#robot.taskPos.controlGain.value = 100.0
+#robot.taskPos.add(robot.taskPos.feature.name)
+#plug(robot.dynamic.position, robot.taskPos.feature.state)
+
+# --- 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.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
+
+# --- Fix robot.dynamic inputs
+plug(robot.device.velocity,robot.dynamic.velocity)
+from dynamic_graph.sot.core import Derivator_of_Vector
+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.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.dynamic, 'com', robot = robot, data_type='vector')                    # resulting SOT CoM
+
+create_topic(robot.publisher, robot.estimator, 'dcm', robot = robot, data_type='vector')                  # estimated DCM
+
+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.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.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
+
+# --- 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.dynamic, 'com')                    # resulting SOT CoM
+
+addTrace(robot.tracer, robot.estimator, 'dcm')                  # estimated DCM
+
+addTrace(robot.tracer, robot.dynamic, 'zmp')                    # SOT ZMP
+addTrace(robot.tracer, robot.zmp_estimator, 'zmp')              # estimated 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
+
+robot.tracer.start()
+

python/sot_talos_balance/test/test_feet_admittance.py

+'''Test feet admittance control'''
+from sot_talos_balance.utils.run_test_utils import *
+from time import sleep
+
+run_test('appli_feet_admittance.py')
+
+run_ft_calibration('robot.ftc')
+raw_input("Wait before running the test")
+
+raw_input("Wait before dumping the data")
+
+runCommandClient('dump_tracer(robot.tracer)')
+