From 66d86c4c1cf81c29f27b30642bdc014eb9f0a451 Mon Sep 17 00:00:00 2001
From: Gabriele Buondonno <gbuondon@laas.fr>
Date: Mon, 8 Apr 2019 17:06:08 +0200
Subject: [PATCH] [test_dcmZmpControl_distribute] Added F/T calibration and
control manager
---
.../test/appli_dcmZmpControl_distribute.py | 34 ++++++++++++++++---
.../test/test_dcmZmpControl_distribute.py | 17 +++-------
2 files changed, 34 insertions(+), 17 deletions(-)
diff --git a/python/sot_talos_balance/test/appli_dcmZmpControl_distribute.py b/python/sot_talos_balance/test/appli_dcmZmpControl_distribute.py
index 3259b17..31ba4e2 100644
--- a/python/sot_talos_balance/test/appli_dcmZmpControl_distribute.py
+++ b/python/sot_talos_balance/test/appli_dcmZmpControl_distribute.py
@@ -1,6 +1,8 @@
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
@@ -99,14 +101,17 @@ 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.device_filters.ft_LF_filter.x_filtered,zmp_estimator.wrenchLeft)
-plug(robot.device_filters.ft_RF_filter.x_filtered,zmp_estimator.wrenchRight)
+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
@@ -135,7 +140,7 @@ dcm_controller.init(dt)
robot.dcm_control = dcm_controller
-Ki_dcm = [0.1,0.1,0.1] # this value is employed later
+Ki_dcm = [1.0,1.0,1.0] # this value is employed later
# --- Distribute wrench
distribute = DistributeWrench('distribute')
@@ -158,7 +163,14 @@ com_admittance_control.setState(comDes,[0.0,0.0,0.0])
robot.com_admittance_control = com_admittance_control
-Kp_adm = [0.1,0.1,0.0] # this value is employed later
+Kp_adm = [20.0,10.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')
+robot.cm.addEmergencyStopSIN('distribute')
# -------------------------- SOT CONTROL --------------------------
@@ -274,7 +286,10 @@ locals()['keepWaist'] = robot.keepWaist
# --- SOT solver
robot.sot = SOT('sot')
robot.sot.setSize(robot.dynamic.getDimension())
-plug(robot.sot.control,robot.device.control)
+
+# --- 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)
@@ -321,6 +336,15 @@ create_topic(robot.publisher, robot.distribute, 'wrenchLeft', robot = robot, dat
create_topic(robot.publisher, robot.distribute, 'wrenchRight', robot = robot, data_type='vector') # reference 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.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("zmp_tracer")
robot.tracer.setBufferSize(80*(2**20))
diff --git a/python/sot_talos_balance/test/test_dcmZmpControl_distribute.py b/python/sot_talos_balance/test/test_dcmZmpControl_distribute.py
index c02179d..ee23ed7 100644
--- a/python/sot_talos_balance/test/test_dcmZmpControl_distribute.py
+++ b/python/sot_talos_balance/test/test_dcmZmpControl_distribute.py
@@ -1,32 +1,25 @@
'''Test CoM admittance control as described in paper.'''
-from sot_talos_balance.utils.run_test_utils import run_test, runCommandClient, evalCommandClient
+from sot_talos_balance.utils.run_test_utils import *
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_distribute.py')
-sleep(5.0)
+run_ft_calibration('robot.ftc')
+raw_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.distribute.emergencyStop,robot.cm.emergencyStop_distribute)')
runCommandClient('plug(robot.distribute.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)')
--
GitLab