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>
This diff is collapsed.
'''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!')
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment