test_dcm_zmp_control_ffdc

8a0abc45 · Gabriele Buondonno · 7f3188ae · 8a0abc45 · 8a0abc45 · 8a0abc45
Commit 8a0abc45 authored Sep 03, 2019 by Gabriele Buondonno
--- a/plot/plot_wrench_distrib.xml
+++ b/plot/plot_wrench_distrib.xml
+<?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>
+
--- a/python/sot_talos_balance/test/appli_dcm_zmp_control_ffdc.py
+++ b/python/sot_talos_balance/test/appli_dcm_zmp_control_ffdc.py
--- a/python/sot_talos_balance/test/test_dcm_zmp_control_ffdc.py
+++ b/python/sot_talos_balance/test/test_dcm_zmp_control_ffdc.py
+'''Test CoM admittance control as described in paper'''
+from sot_talos_balance.utils.run_test_utils import *
+from time import sleep
+
+from sys import argv
+test_folder, sot_talos_balance_folder = get_file_folder(argv)
+
+run_test('appli_dcm_zmp_control_ffdc.py')
+
+run_ft_calibration('robot.ftc')
+
+raw_input("Wait before running the test")
+
+# Connect ZMP reference and reset controllers
+print('Set controller')
+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(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')
+
+raw_input("Wait before activating foot force difference control")
+runCommandClient('robot.ffdc.dfzAdmittance.value = dfzAdmittance')
+runCommandClient('robot.ffdc.vdcFrequency.value = vdcFrequency')
+runCommandClient('robot.ffdc.vdcDamping.value = vdcDamping')
+
+if test_folder is not None:
+    c = ask_for_confirmation('Execute trajectory?')
+    if c:
+        print('Executing the trajectory')
+        runCommandClient('robot.triggerTrajGen.sin.value = 1')
+    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)
+            print("The robot is back in position!")
+        else:
+            print("Not putting the robot back")
+    else:
+        print("Not executing the sinusoid")
+
+    c = ask_for_confirmation("Raise the foot?")
+    if c:
+        print("Putting the robot in position...")
+        runCommandClient('robot.comTrajGen.move(1,-0.08,10.0)')
+        runCommandClient('robot.rhoTrajGen.move(0,0.4,10.0)')
+        sleep(10.0)
+        print("Robot is in position!")
+
+        foot_on_ground = True
+
+        c2 = ask_for_confirmation("Confirm raising the foot?")
+        if c2:
+            print("Raising the foot...")
+            runCommandClient('robot.phaseTrajGen.set(0,-1)')
+            runCommandClient('h = robot.dynamic.LF.value[2][3]')
+            runCommandClient('robot.lfTrajGen.move(2,h+0.05,10.0)')
+            sleep(10.0)
+            print("Foot has been raised!")
+            foot_on_ground = False
+            c3 = ask_for_confirmation("Put the foot back?")
+            if c3:
+                print("Putting the foot back...")
+                runCommandClient('robot.lfTrajGen.move(2,h,10.0)')
+                sleep(10.0)
+                runCommandClient('robot.phaseTrajGen.set(0,0)')
+                print("The foot is back in position!")
+                foot_on_ground = True
+            else:
+                print("Not putting the foot back")
+        else:
+            print("Not raising the foot")
+
+        if foot_on_ground:
+            c4 = ask_for_confirmation("Put the robot back?")
+            if c4:
+                print("Putting the robot back...")
+                runCommandClient('robot.comTrajGen.move(1,0.0,10.0)')
+                runCommandClient('robot.rhoTrajGen.move(0,0.5,10.0)')
+                sleep(10.0)
+                print("The robot is back in position!")
+            else:
+                print("Not putting the robot back")
+    else:
+        print("Not raising the foot")
+
+#raw_input("Wait before dumping the data")
+
+#runCommandClient('dump_tracer(robot.tracer)')
+
+print('Bye!')
+