[estimator] Test with fake estimator

python/sot_talos_balance/test/appli_dcmZmpControl_estimator.py

+from sot_talos_balance.create_entities_utils import *
+import sot_talos_balance.control_manager_conf          as param_server_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
+
+from dynamic_graph.tracer_real_time import TracerRealTime
+
+from sot_talos_balance.euler_to_quat import EulerToQuat
+
+robot.timeStep = robot.device.getTimeStep()
+dt = robot.timeStep;
+
+# -------------------------- DESIRED TRAJECTORY --------------------------
+
+# --- Desired values
+robot.dynamic.com.recompute(0)
+comDes = robot.dynamic.com.value
+#comDes = list(robot.dynamic.com.value)
+#comDes[0] += 0.001
+#comDes[1] += 0.001
+#comDes = tuple(comDes)
+dcmDes = comDes
+zmpDes = comDes[:2] + (0.0,)
+ddcomDes = (0.0,0.0,0.0)
+
+# --- Pendulum parameters
+robot_name='robot'
+robotDim = robot.dynamic.getDimension()
+mass = robot.dynamic.data.mass[0]
+h = robot.dynamic.com.value[2]
+g = 9.81
+omega = sqrt(g/h)
+
+# -------------------------- ESTIMATION --------------------------
+
+# --- Conversion
+e2q = EulerToQuat('e2q')
+plug(robot.device.state,e2q.euler)
+robot.e2q = e2q
+
+# --- General Estimation
+robot.param_server = create_parameter_server(param_server_conf,dt)
+cdc_estimator = DcmEstimator('cdc_estimator')
+cdc_estimator.init(dt, robot_name)
+plug(robot.e2q.quaternion, cdc_estimator.q)
+plug(robot.device.velocity, 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
+
+# --- filters
+filters = Bunch()
+filters.ft_RF_filter  = create_butter_lp_filter_Wn_04_N_2("ft_RF_filter", dt, 6)
+filters.ft_LF_filter  = create_butter_lp_filter_Wn_04_N_2("ft_LF_filter", dt, 6)
+plug(robot.device.forceRLEG, filters.ft_RF_filter.x)
+plug(robot.device.forceLLEG, filters.ft_LF_filter.x)
+robot.device_filters = filters
+
+# --- ZMP estimation
+zmp_estimator = SimpleZmpEstimator("zmpEst")
+robot.dynamic.createOpPoint('sole_LF','left_sole_link')
+robot.dynamic.createOpPoint('sole_RF','right_sole_link')
+plug(robot.dynamic.sole_LF,zmp_estimator.poseLeft)
+plug(robot.dynamic.sole_RF,zmp_estimator.poseRight)
+plug(robot.device_filters.ft_LF_filter.x_filtered,zmp_estimator.wrenchLeft)
+plug(robot.device_filters.ft_RF_filter.x_filtered,zmp_estimator.wrenchRight)
+#plug(robot.device.forceLLEG,zmp_estimator.wrenchLeft)
+#plug(robot.device.forceRLEG,zmp_estimator.wrenchRight)
+zmp_estimator.init()
+robot.zmp_estimator = zmp_estimator
+
+# -------------------------- ADMITTANCE CONTROL --------------------------
+
+# --- DCM controller
+Kp_dcm = [1.0,1.0,1.0]
+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)
+
+dcm_controller.zmpDes.value = zmpDes # plug a signal here
+dcm_controller.dcmDes.value = dcmDes # plug a signal here
+
+dcm_controller.init(dt)
+
+robot.dcm_control = dcm_controller
+
+Ki_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 = zmpDes     # should be plugged to robot.dcm_control.zmpRef
+com_admittance_control.ddcomDes.value = ddcomDes # plug a signal here
+
+com_admittance_control.init(dt)
+com_admittance_control.setState(comDes,[0.0,0.0,0.0])
+
+robot.com_admittance_control = com_admittance_control
+
+Kp_adm = [1.0,1.0,0.0] # this value is employed later
+
+# -------------------------- 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)
+robot.contactLF.keep()
+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)
+robot.contactRF.keep()
+locals()['contactRF'] = robot.contactRF
+
+# --- 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)
+
+# --- Waist
+robot.keepWaist = MetaTaskKine6d('keepWaist',robot.dynamic,'WT',robot.OperationalPointsMap['waist'])
+robot.keepWaist.feature.frame('desired')
+robot.keepWaist.gain.setConstant(300)
+robot.keepWaist.keep()
+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(robot.sot.control,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)
+
+# --- 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')        
+
+rospub_signalName = '{0}_{1}'.format('fake', 'comDes')
+topicname = '/sot/{0}/{1}'.format('fake', 'comDes')
+robot.publisher.add('vector',rospub_signalName,topicname)
+plug(robot.dcm_control.dcmDes, robot.publisher.signal(rospub_signalName))                                 # desired CoM (workaround)
+
+create_topic(robot.publisher, robot.cdc_estimator, 'c', robot = robot, data_type='vector')                # estimated CoM
+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.dcm_control, '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
+
+# --- TRACER
+robot.tracer = TracerRealTime("zmp_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.dcm_control, 'dcmDes')             # desired CoM (workaround)
+addTrace(robot.tracer, robot.cdc_estimator, 'c')                # estimated CoM (to be modified)
+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 (already added)
+addTrace(robot.tracer, robot.estimator, 'dcm')                  # estimated DCM
+
+addTrace(robot.tracer, robot.dcm_control, 'zmpDes')             # desired ZMP
+addTrace(robot.tracer, robot.zmp_estimator, 'zmp')              # estimated ZMP
+addTrace(robot.tracer, robot.dcm_control, 'zmpRef')             # reference ZMP
+addTrace(robot.tracer, robot.dynamic, 'zmp')                    # SOT ZMP
+
+# -------------------------- SIMULATION --------------------------
+
+robot.tracer.start()
+

python/sot_talos_balance/test/test_dcmZmpControl_estimator.py

+'''Test CoM admittance control as described in paper.'''
+from sot_talos_balance.utils.run_test_utils import run_test, runCommandClient, evalCommandClient
+from time import sleep
+
+from sot_talos_balance.utils.gazebo_utils import apply_force
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+run_test('appli_dcmZmpControl_estimator.py')
+
+sleep(5.0)
+
+# Connect ZMP reference and reset controllers
+print('Connect ZMP reference')
+runCommandClient('plug(robot.dcm_control.zmpRef,robot.com_admittance_control.zmpDes)')
+runCommandClient('robot.com_admittance_control.setState(comDes,[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')
+
+#sleep(5.0)
+
+#print('Kick the robot...')
+#apply_force([-1000.0,0,0],0.01)
+#print('... kick!')
+
+#sleep(5.0)
+
+sleep(30.0)
+
+runCommandClient('dump_tracer(robot.tracer)')
+
+# --- DISPLAY
+comDes_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dcm_control.name') + '-dcmDes.dat')             # desired CoM (workaround)
+comEst_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.cdc_estimator.name') + '-c.dat')                # estimated CoM
+comRef_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.com_admittance_control.name') + '-comRef.dat')  # reference CoM
+comSOT_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dynamic.name') + '-com.dat')                    # resulting SOT CoM
+
+dcmDes_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dcm_control.name') + '-dcmDes.dat')             # desired DCM
+dcmEst_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.estimator.name') + '-dcm.dat')                  # estimated DCM
+
+zmpDes_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dcm_control.name') + '-zmpDes.dat')             # desired ZMP
+zmpSOT_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dynamic.name') + '-zmp.dat')                    # SOT ZMP
+zmpEst_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.zmp_estimator.name') + '-zmp.dat')              # estimated ZMP
+zmpRef_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dcm_control.name') + '-zmpRef.dat')             # reference ZMP
+
+
+plt.ion()
+
+plt.figure()
+plt.plot(comDes_data[:,1],'b--')
+plt.plot(comEst_data[:,1],'b-')
+plt.plot(comRef_data[:,1],'b:')
+plt.plot(comSOT_data[:,1],'b-.')
+plt.plot(comDes_data[:,2],'r--')
+plt.plot(comEst_data[:,2],'r-')
+plt.plot(comRef_data[:,2],'r:')
+plt.plot(comSOT_data[:,2],'r-.')
+plt.plot(comDes_data[:,3],'g--')
+plt.plot(comEst_data[:,3],'g-')
+plt.plot(comRef_data[:,3],'g:')
+plt.plot(comSOT_data[:,3],'g-.')
+plt.title('COM')
+plt.legend(['Desired x', 'Estimated x', 'Reference x', 'SOT x',
+            'Desired y', 'Estimated y', 'Reference y', 'SOT y',
+            'Desired z', 'Estimated z', 'Reference z', 'SOT z'])
+
+plt.figure()
+plt.plot(dcmDes_data[:,1],'b--')
+plt.plot(dcmEst_data[:,1],'b-')
+plt.plot(dcmDes_data[:,2],'r--')
+plt.plot(dcmEst_data[:,2],'r-')
+plt.title('DCM')
+plt.legend(['Desired x', 'Estimated x', 'Desired y', 'Estimated y'])
+
+plt.figure()
+plt.plot(zmpDes_data[:,1],'b--')
+plt.plot(zmpSOT_data[:,1],'b-')
+plt.plot(zmpRef_data[:,1],'b:')
+plt.plot(zmpDes_data[:,2],'r--')
+plt.plot(zmpSOT_data[:,2],'r-')
+plt.plot(zmpRef_data[:,2],'r:')
+plt.title('ZMP')
+plt.legend(['Desired x', 'SOT x', 'Reference x',
+            'Desired y', 'SOT y', 'Reference y'])
+
+zmpErrSOT = zmpSOT_data - zmpDes_data
+
+plt.figure()
+plt.plot(zmpErrSOT[:,1],'b-')
+plt.plot(zmpErrSOT[:,2],'r-')
+plt.title('ZMP SOT error')
+plt.legend(['Error on x','Error on y'])
+
+raw_input("Wait before leaving the simulation")
+