Commit 8a0abc45 authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

test_dcm_zmp_control_ffdc

parent 7f3188ae
<?xml version='1.0' encoding='UTF-8'?>
<root version="2.3.0">
<tabbed_widget name="Main Window" parent="main_window">
<plotmatrix rows="2" columns="3" tab_name="plot">
<plot row="0" col="0">
<range top="-1.926198" bottom="-2.704815" left="0.000000" right="11.113000"/>
<limitY/>
<curve name="/sot/dcmCtrl/wrenchRef/data.0" R="20" G="100" custom_transform="noTransform" B="160"/>
<curve name="/sot/distribute/wrenchRef/data.0" R="247" G="0" custom_transform="noTransform" B="247"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="0">
<range top="-0.947004" bottom="-1.368506" left="0.000000" right="11.113000"/>
<limitY/>
<curve name="/sot/distribute/wrenchLeft/data.0" R="80" G="180" custom_transform="noTransform" B="127"/>
<curve name="/sot/distribute/wrenchRight/data.0" R="20" G="100" custom_transform="noTransform" B="160"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="1">
<range top="1.292420" bottom="1.087894" left="0.000000" right="11.113000"/>
<limitY/>
<curve name="/sot/dcmCtrl/wrenchRef/data.1" R="5" G="116" custom_transform="noTransform" B="13"/>
<curve name="/sot/distribute/wrenchRef/data.1" R="0" G="51" custom_transform="noTransform" B="238"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="1">
<range top="0.994414" bottom="0.195436" left="0.000000" right="11.113000"/>
<limitY/>
<curve name="/sot/distribute/wrenchLeft/data.1" R="244" G="83" custom_transform="noTransform" B="29"/>
<curve name="/sot/distribute/wrenchRight/data.1" R="5" G="116" custom_transform="noTransform" B="13"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="2">
<range top="885.570204" bottom="885.570203" left="0.000000" right="11.113000"/>
<limitY/>
<curve name="/sot/dcmCtrl/wrenchRef/data.2" R="255" G="19" custom_transform="noTransform" B="24"/>
<curve name="/sot/distribute/wrenchRef/data.2" R="0" G="119" custom_transform="noTransform" B="119"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="2">
<range top="442.786545" bottom="442.783658" left="0.000000" right="11.113000"/>
<limitY/>
<curve name="/sot/distribute/wrenchLeft/data.2" R="142" G="52" custom_transform="noTransform" B="136"/>
<curve name="/sot/distribute/wrenchRight/data.2" R="255" G="19" custom_transform="noTransform" B="24"/>
<transform value="noTransform"/>
</plot>
</plotmatrix>
<currentPlotMatrix index="0"/>
</tabbed_widget>
<use_relative_time_offset enabled="1"/>
<!-- - - - - - - - - - - - - - - -->
<!-- - - - - - - - - - - - - - - -->
<Plugins>
<plugin ID="DataLoad_CSV">
<default time_axis=""/>
</plugin>
<plugin ID="DataLoad_ROS_bags">
<use_header_stamp value="false"/>
<use_renaming_rules value="true"/>
<discard_large_arrays value="true"/>
<max_array_size value="100"/>
</plugin>
<plugin ID="DataLoad_ULog"/>
<plugin ID="ROS_Topic_Streamer">
<use_header_stamp value="false"/>
<use_renaming_rules value="true"/>
<discard_large_arrays value="true"/>
<max_array_size value="100"/>
</plugin>
<plugin ID="RosoutPublisherROS" status="idle"/>
<plugin ID="TopicPublisherROS" status="idle"/>
</Plugins>
<!-- - - - - - - - - - - - - - - -->
<previouslyLoaded_Datafiles/>
<previouslyLoaded_Streamer name="ROS_Topic_Streamer"/>
<!-- - - - - - - - - - - - - - - -->
<customMathEquations/>
<snippets>
<snippet name="1st_derivative">
<global>var prevX = 0
var prevY = 0</global>
<equation>dx = time - prevX
dy = value - prevY
prevX = time
prevY = value
return dy/dx</equation>
</snippet>
<snippet name="1st_order_lowpass">
<global>var prevY = 0
var alpha = 0.1</global>
<equation>prevY = alpha * value + (1.-alpha) * prevY
return prevY</equation>
</snippet>
<snippet name="sum_A_B">
<global></global>
<equation>return $$PLOT_A$$ + $$PLOT_B$$</equation>
</snippet>
<snippet name="yaw_from_quaternion">
<global>// source: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
function quaternionToYaw(x, y, z, w)
{
// yaw (z-axis rotation)
t1 = 2.0 * (w * z + x * y);
t2 = 1.0 - 2.0 * (y * y + z * z);
yaw = Math.atan2(t1, t2);
return yaw
}</global>
<equation>return quaternionToYaw(x, y, z, w);</equation>
</snippet>
</snippets>
<!-- - - - - - - - - - - - - - - -->
</root>
from sot_talos_balance.create_entities_utils import *
from sot_talos_balance.round_double_to_int import RoundDoubleToInt
from sot_talos_balance.foot_force_difference_controller import FootForceDifferenceController
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.distribute_conf as distribute_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
cm_conf.CTRL_MAX = 1000.0 # temporary hack
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 --------------------------
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')
robot.triggerTrajGen.sin.value = 0
# --- 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)
# --- Rho
robot.rhoTrajGen = create_scalar_trajectory_generator(dt, 0.5, 'rhoTrajGen')
robot.rhoTrajGen.x.recompute(0) # trigger computation of initial value
robot.rhoScalar = Component_of_vector("rho_scalar")
robot.rhoScalar.setIndex(0)
plug(robot.rhoTrajGen.x, robot.rhoScalar.sin)
plug(robot.triggerTrajGen.sout, robot.rhoTrajGen.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)
# --- Load files
if folder is not None:
robot.comTrajGen.playTrajectoryFile(folder+'CoM.dat')
robot.lfTrajGen.playTrajectoryFile(folder+'LeftFoot.dat')
robot.rfTrajGen.playTrajectoryFile(folder+'RightFoot.dat')
robot.zmpTrajGen.playTrajectoryFile(folder+'ZMP.dat')
robot.waistTrajGen.playTrajectoryFile(folder+'WaistOrientation.dat')
robot.rhoTrajGen.playTrajectoryFile(folder+'Rho.dat')
robot.phaseTrajGen.playTrajectoryFile(folder+'Phase.dat')
# --- Interface with controller entities
wp = DummyWalkingPatternGenerator('dummy_wp')
wp.init()
wp.omega.value = omega
plug(robot.rhoScalar.sout, wp.rho)
plug(robot.phaseInt.sout, wp.phase)
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)
from dynamic_graph.sot.core import MatrixHomoToPoseQuaternion
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)
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
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)
dcm_controller.init(dt)
robot.dcm_control = dcm_controller
Ki_dcm = [1.0,1.0,1.0] # this value is employed later
# --- Distribute wrench
distribute = create_distribute_wrench(distribute_conf)
plug(robot.e2q.quaternion, distribute.q)
plug(robot.dcm_control.wrenchRef, distribute.wrenchDes)
plug(robot.wp.rhoDes, distribute.rho)
plug(robot.wp.phaseDes, distribute.phase)
distribute.init(robot_name)
robot.distribute = distribute
# --- 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.distribute.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
# --- Foot force difference control
dfzAdmittance = -1e-4
vdcFrequency = -1.
vdcDamping = 0.
gainSwing = 300.
gainStance = 1.
gainDouble = 1.
controller = FootForceDifferenceController("footController")
controller.init()
plug(robot.wp.phaseDes, controller.phase)
controller.dfzAdmittance.value = 0.
plug(robot.ftc.right_foot_force_out, controller.wrenchRight)
plug(robot.ftc.left_foot_force_out, controller.wrenchLeft)
plug(robot.distribute.surfaceWrenchRight, controller.wrenchRightDes)
plug(robot.distribute.surfaceWrenchLeft, controller.wrenchLeftDes)
controller.vdcFrequency.value = 0.
controller.vdcDamping.value = 0.
plug(robot.wp.footRightDes, controller.posRightDes)
plug(robot.wp.footLeftDes, controller.posLeftDes)
plug(robot.dynamic.RF, controller.posRight)
plug(robot.dynamic.LF, controller.posLeft)
controller.gainSwing.value = gainSwing
controller.gainStance.value = gainStance
controller.gainDouble.value = gainDouble
robot.ffdc = 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')
robot.cm.addEmergencyStopSIN('distribute')
# -------------------------- 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')
plug(robot.ffdc.gainLeft,robot.contactLF.task.controlGain)
plug(robot.wp.footLeftDes, robot.contactLF.featureDes.position) #.errorIN?
plug(robot.ffdc.vLeft, robot.contactLF.featureDes.velocity)
robot.contactLF.task.setWithDerivative(True)
locals()['contactLF'] = robot.contactLF
robot.contactRF = MetaTaskKine6d('contactRF',robot.dynamic,'RF',robot.OperationalPointsMap['right-ankle'])
robot.contactRF.feature.frame('desired')
plug(robot.ffdc.gainRight,robot.contactRF.task.controlGain)
plug(robot.wp.footRightDes, robot.contactRF.featureDes.position) #.errorIN?
plug(robot.ffdc.vRight, robot.contactRF.featureDes.velocity)
robot.contactRF.task.setWithDerivative(True)
locals()['contactRF'] = robot.contactRF
# --- COM height
robot.taskComH = MetaTaskKineCom(robot.dynamic, name='comH')
plug(robot.wp.comDes,robot.taskComH.featureDes.errorIN)
robot.taskComH.task.controlGain.value = 100.
robot.taskComH.feature.selec.value = '100'
# --- COM
robot.taskCom = MetaTaskKineCom(robot.dynamic)
plug(robot.com_admittance_control.comRef,robot.taskCom.featureDes.errorIN)
plug(robot.com_admittance_control.dcomRef,robot.taskCom.featureDes.errordotIN)
robot.taskCom.task.controlGain.value = 0
robot.taskCom.task.setWithDerivative(True)
robot.taskCom.feature.selec.value = '011'
# --- Waist
robot.keepWaist = MetaTaskKine6d('keepWaist',robot.dynamic,'WT',robot.OperationalPointsMap['waist'])
robot.keepWaist.feature.frame('desired')
robot.keepWaist.gain.setConstant(300)
plug(robot.wp.waistDes, robot.keepWaist.featureDes.position)
robot.keepWaist.feature.selec.value = '111000'
locals()['keepWaist'] = robot.keepWaist
# --- SOT solver
robot.sot = SOT('sot')
robot.sot.setSize(robot.dynamic.getDimension())
# --- Plug SOT control to device through control manager
plug(robot.sot.control,robot.cm.ctrl_sot_input)
plug(robot.cm.u_safe,robot.device.control)
robot.sot.push(robot.taskUpperBody.name)
robot.sot.push(robot.contactRF.task.name)
robot.sot.push(robot.contactLF.task.name)
robot.sot.push(robot.taskComH.task.name)
robot.sot.push(robot.taskCom.task.name)
robot.sot.push(robot.keepWaist.task.name)
# 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.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.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
create_topic(robot.publisher, robot.dcm_control, 'wrenchRef', robot = robot, data_type='vector') # unoptimized reference wrench
create_topic(robot.publisher, robot.distribute, 'wrenchLeft', robot = robot, data_type='vector') # reference left wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRight', robot = robot, data_type='vector') # reference right wrench
create_topic(robot.publisher, robot.distribute, 'surfaceWrenchLeft', robot = robot, data_type='vector') # reference surface left wrench
create_topic(robot.publisher, robot.distribute, 'surfaceWrenchRight', robot = robot, data_type='vector') # reference surface right wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot = robot, data_type='vector') # optimized reference wrench
#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.zmp_estimator, 'copRight', robot = robot, data_type='vector')
create_topic(robot.publisher, robot.zmp_estimator, 'copLeft', robot = robot, data_type='vector')
## --- TRACER
#robot.tracer = TracerRealTime("com_tracer")