Commit 012d2802 authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[test_dcmZmpFootControl] Raise foot works

parent 66b55bb0
<?xml version='1.0' encoding='UTF-8'?>
<root>
<root version="2.3.0">
<tabbed_widget name="Main Window" parent="main_window">
<plotmatrix columns="3" tab_name="plot" rows="2">
<plotmatrix rows="2" columns="3" tab_name="plot">
<plot col="0" row="0">
<range bottom="-0.003287" top="0.001892" right="11.600000" left="6.605000"/>
<range bottom="-0.003286" left="2.648000" right="7.651000" top="0.001841"/>
<limitY/>
<curve G="0" name="/sot/cdc_estimator/c/data.0" B="0" R="255" custom_transform="noTransform"/>
<curve G="255" name="/sot/dummy_wp/comDes/data.0" B="0" R="0" custom_transform="noTransform"/>
<curve G="0" name="/sot/robot_dynamic/com/data.0" B="255" R="0" custom_transform="noTransform"/>
<curve name="/sot/cdc_estimator/c/data.0" G="100" B="160" custom_transform="noTransform" R="20"/>
<curve name="/sot/dummy_wp/comDes/data.0" G="0" B="247" custom_transform="noTransform" R="247"/>
<curve name="/sot/robot_dynamic/com/data.0" G="180" B="127" custom_transform="noTransform" R="80"/>
<transform value="noTransform"/>
</plot>
<plot col="0" row="1">
<range bottom="-0.000122" top="0.000075" right="11.600000" left="6.605000"/>
<range bottom="-0.000039" left="2.648000" right="7.640000" top="0.000026"/>
<limitY/>
<curve G="160" name="/sot/cdc_estimator/dc/data.0" B="164" R="160" custom_transform="noTransform"/>
<curve name="/sot/cdc_estimator/dc/data.0" G="100" B="160" custom_transform="noTransform" R="20"/>
<transform value="noTransform"/>
</plot>
<plot col="1" row="0">
<range bottom="-0.027679" top="0.051894" right="11.600000" left="6.605000"/>
<range bottom="-0.027569" left="2.648000" right="7.651000" top="0.026282"/>
<limitY/>
<curve G="0" name="/sot/cdc_estimator/c/data.1" B="0" R="255" custom_transform="noTransform"/>
<curve G="255" name="/sot/dummy_wp/comDes/data.1" B="0" R="0" custom_transform="noTransform"/>
<curve G="0" name="/sot/robot_dynamic/com/data.1" B="255" R="0" custom_transform="noTransform"/>
<curve name="/sot/cdc_estimator/c/data.1" G="116" B="13" custom_transform="noTransform" R="5"/>
<curve name="/sot/dummy_wp/comDes/data.1" G="51" B="238" custom_transform="noTransform" R="0"/>
<curve name="/sot/robot_dynamic/com/data.1" G="83" B="29" custom_transform="noTransform" R="244"/>
<transform value="noTransform"/>
</plot>
<plot col="1" row="1">
<range bottom="-0.058624" top="0.058105" right="11.600000" left="6.605000"/>
<range bottom="-0.039159" left="2.648000" right="7.640000" top="0.039146"/>
<limitY/>
<curve G="128" name="/sot/cdc_estimator/dc/data.1" B="0" R="128" custom_transform="noTransform"/>
<curve name="/sot/cdc_estimator/dc/data.1" G="116" B="13" custom_transform="noTransform" R="5"/>
<transform value="noTransform"/>
</plot>
<plot col="2" row="0">
<range bottom="0.874104" top="0.875963" right="11.600000" left="6.605000"/>
<range bottom="0.874383" left="2.648000" right="7.651000" top="0.875654"/>
<limitY/>
<curve G="0" name="/sot/cdc_estimator/c/data.2" B="0" R="255" custom_transform="noTransform"/>
<curve G="255" name="/sot/dummy_wp/comDes/data.2" B="0" R="0" custom_transform="noTransform"/>
<curve G="0" name="/sot/robot_dynamic/com/data.2" B="255" R="0" custom_transform="noTransform"/>
<curve name="/sot/cdc_estimator/c/data.2" G="19" B="24" custom_transform="noTransform" R="255"/>
<curve name="/sot/dummy_wp/comDes/data.2" G="119" B="119" custom_transform="noTransform" R="0"/>
<curve name="/sot/robot_dynamic/com/data.2" G="52" B="136" custom_transform="noTransform" R="142"/>
<transform value="noTransform"/>
</plot>
<plot col="2" row="1">
<range bottom="-0.000285" top="0.000288" right="11.600000" left="6.605000"/>
<range bottom="-0.000078" left="2.648000" right="7.640000" top="0.000123"/>
<limitY/>
<curve G="0" name="/sot/cdc_estimator/dc/data.2" B="0" R="128" custom_transform="noTransform"/>
<curve name="/sot/cdc_estimator/dc/data.2" G="19" B="24" custom_transform="noTransform" R="255"/>
<transform value="noTransform"/>
</plot>
</plotmatrix>
<currentPlotMatrix index="0"/>
</tabbed_widget>
<use_relative_time_offset enabled="1"/>
<!-- - - - - - - - - - - - - - - -->
<!-- - - - - - - - - - - - - - - -->
<Plugins>
<DataLoad_CSV>
<plugin ID="DataLoad_CSV">
<default time_axis=""/>
</DataLoad_CSV>
<DataLoad_ROS_bags>
<selected_topics list=""/>
</DataLoad_ROS_bags>
<DataLoad_ULog>
<no_params/>
</DataLoad_ULog>
<ROS_Topic_Streamer>
<selected_topics list="/gazebo/link_states;/gazebo/model_states;/sot/cdc_estimator/c;/sot/cdc_estimator/dc;/sot/comAdmCtrl/comRef;/sot/dcmCtrl/dcmDes;/sot/dcmCtrl/zmpDes;/sot/dcmCtrl/zmpRef;/sot/dummy/dcm;/sot/robot_dynamic/com;/sot/robot_dynamic/zmp;/sot/zmpEst/zmp;/sot/dummy_wp/comDes"/>
</ROS_Topic_Streamer>
<RosoutPublisherROS/>
<TopicPublisherROS/>
</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>
<previouslyLoadedStreamer name="ROS_Topic_Streamer"/>
<!-- - - - - - - - - - - - - - - -->
<previouslyLoaded_Datafiles/>
<previouslyLoaded_Streamer name="ROS_Topic_Streamer"/>
<!-- - - - - - - - - - - - - - - -->
<customMathEquations/>
<snippets>
<snippet name="1st_derivative">
......@@ -103,5 +112,6 @@ function quaternionToYaw(x, y, z, w)
<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.simple_controller_6d import SimpleController6d
from sot_talos_balance.foot_force_difference_controller import FootForceDifferenceController
import sot_talos_balance.talos.parameter_server_conf as param_server_conf
......@@ -83,11 +84,21 @@ robot.rhoScalar = Component_of_vector("rho_scalar")
robot.rhoScalar.setIndex(0)
plug(robot.rhoTrajGen.x, robot.rhoScalar.sin)
# --- Phase
robot.phaseTrajGen = create_scalar_trajectory_generator(dt, 0., 'phaseTrajGen')
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)
# --- 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)
......@@ -165,7 +176,7 @@ robot.cdc_estimator = cdc_estimator
# --- DCM Estimation
estimator = DummyDcmEstimator("dummy")
estimator.omega.value = omega
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)
......@@ -199,7 +210,7 @@ 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.wp.omegaDes, dcm_controller.omega)
plug(robot.cdc_estimator.c,dcm_controller.com)
plug(robot.estimator.dcm,dcm_controller.dcm)
......@@ -217,7 +228,8 @@ Ki_dcm = [1.0,1.0,1.0] # this value is employed later
distribute = create_distribute_wrench(distribute_conf)
plug(robot.e2q.quaternion, distribute.q)
plug(robot.dcm_control.wrenchRef, distribute.wrenchDes)
plug(robot.rhoScalar.sout, distribute.rho)
plug(robot.wp.rhoDes, distribute.rho)
plug(robot.wp.phaseDes, distribute.phase)
distribute.init(robot_name)
robot.distribute = distribute
......@@ -245,7 +257,7 @@ vdcDamping = 0.
controller = FootForceDifferenceController("footController")
controller.init()
controller.phase.value = 0
plug(robot.wp.phaseDes, controller.phase)
controller.dfzAdmittance.value = 0.
......@@ -311,7 +323,7 @@ plug(robot.dynamic.position, robot.taskUpperBody.feature.state)
#define contactLF and contactRF
robot.contactLF = MetaTaskKine6d('contactLF',robot.dynamic,'LF',robot.OperationalPointsMap['left-ankle'])
robot.contactLF.feature.frame('desired')
robot.contactLF.gain.setConstant(0)
robot.contactLF.gain.setConstant(1)
plug(robot.wp.footLeftDes, robot.contactLF.featureDes.position) #.errorIN?
plug(robot.ffdc.vLeft, robot.contactLF.featureDes.velocity)
robot.contactLF.task.setWithDerivative(True)
......@@ -319,7 +331,7 @@ locals()['contactLF'] = robot.contactLF
robot.contactRF = MetaTaskKine6d('contactRF',robot.dynamic,'RF',robot.OperationalPointsMap['right-ankle'])
robot.contactRF.feature.frame('desired')
robot.contactRF.gain.setConstant(0)
robot.contactRF.gain.setConstant(1)
plug(robot.wp.footRightDes, robot.contactRF.featureDes.position) #.errorIN?
plug(robot.ffdc.vRight, robot.contactRF.featureDes.velocity)
robot.contactRF.task.setWithDerivative(True)
......
......@@ -65,9 +65,10 @@ if c:
c2 = ask_for_confirmation("Confirm raising the foot?")
if c2:
print("Raising the foot...")
runCommandClient('robot.distribute.phase.value = -1')
runCommandClient('robot.phaseTrajGen.set(0,-1)')
runCommandClient('h = robot.dynamic.LF.value[2][3]')
runCommandClient('robot.lfTrajGen.move(2,h+0.05,10.0)')
runCommandClient('robot.contactLF.gain.setConstant(300)')
sleep(10.0)
print("Foot has been raised!")
c3 = ask_for_confirmation("Put the foot back?")
......@@ -79,7 +80,8 @@ if c:
print("Putting the foot back...")
runCommandClient('robot.lfTrajGen.move(2,h,10.0)')
sleep(10.0)
runCommandClient('robot.distribute.phase.value = 0')
runCommandClient('robot.contactLF.gain.setConstant(1)')
runCommandClient('robot.phaseTrajGen.set(0,0)')
print("The foot is back in position!")
else:
print("Not putting the foot back")
......
Supports Markdown
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