Skip to content
Snippets Groups Projects
Commit 7cafefed authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[test_dcmZmpControl_file] Add trigger

parent ca0675f2
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
plot/plot_com_gen.xml 0 → 100644
+ 119
− 0
View file @ 7cafefed
<?xml version='1.0' encoding='UTF-8'?>
<root>
<tabbed_widget parent="main_window" name="Main Window">
<plotmatrix columns="3" tab_name="plot" rows="3">
<plot row="0" col="0">
<range left="41.381000" right="51.374000" bottom="-0.100635" top="0.099365"/>
<limitY/>
<curve name="/sot/comTrajGen/x/data.0" custom_transform="noTransform" B="255" G="0" R="0"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="0">
<range left="41.381000" right="51.374000" bottom="-0.100000" top="0.100000"/>
<limitY/>
<curve name="/sot/comTrajGen/dx/data.0" custom_transform="noTransform" B="255" G="0" R="255"/>
<transform value="noTransform"/>
</plot>
<plot row="2" col="0">
<range left="41.381000" right="51.374000" bottom="-0.100000" top="0.100000"/>
<limitY/>
<curve name="/sot/comTrajGen/ddx/data.0" custom_transform="noTransform" B="164" G="160" R="160"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="1">
<range left="41.381000" right="51.374000" bottom="-0.100663" top="0.099337"/>
<limitY/>
<curve name="/sot/comTrajGen/x/data.1" custom_transform="noTransform" B="0" G="128" R="0"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="1">
<range left="41.381000" right="51.374000" bottom="-0.100000" top="0.100000"/>
<limitY/>
<curve name="/sot/comTrajGen/dx/data.1" custom_transform="noTransform" B="128" G="0" R="0"/>
<transform value="noTransform"/>
</plot>
<plot row="2" col="1">
<range left="41.381000" right="51.374000" bottom="-0.100000" top="0.100000"/>
<limitY/>
<curve name="/sot/comTrajGen/ddx/data.1" custom_transform="noTransform" B="0" G="128" R="128"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="2">
<range left="41.381000" right="51.374000" bottom="0.776681" top="0.976681"/>
<limitY/>
<curve name="/sot/comTrajGen/x/data.2" custom_transform="noTransform" B="0" G="0" R="255"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="2">
<range left="41.381000" right="51.374000" bottom="-0.100000" top="0.100000"/>
<limitY/>
<curve name="/sot/comTrajGen/dx/data.2" custom_transform="noTransform" B="128" G="128" R="0"/>
<transform value="noTransform"/>
</plot>
<plot row="2" col="2">
<range left="41.381000" right="51.374000" bottom="-0.100000" top="0.100000"/>
<limitY/>
<curve name="/sot/comTrajGen/ddx/data.2" custom_transform="noTransform" B="0" G="0" R="128"/>
<transform value="noTransform"/>
</plot>
</plotmatrix>
<currentPlotMatrix index="0"/>
</tabbed_widget>
<use_relative_time_offset enabled="1"/>
<Plugins>
<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="/sot/comTrajGen/dx;/sot/comTrajGen/x;/sot/comTrajGen/ddx"/>
</ROS_Topic_Streamer>
<RosoutPublisherROS/>
<TopicPublisherROS/>
</Plugins>
<previouslyLoadedStreamer 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>
python/sot_talos_balance/test/appli_dcmZmpControl_file.py
+ 12
− 0
View file @ 7cafefed
......@@ -50,10 +50,15 @@ folder = data_folder + test_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
robot.comTrajGen.playTrajectoryFile(folder+'CoM.dat')
plug(robot.triggerTrajGen.sout, robot.comTrajGen.trigger)
# --- Left foot
robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'LF')
......@@ -62,6 +67,7 @@ robot.lfTrajGen.x.recompute(0) # trigger computation of initial value
robot.lfToMatrix = PoseRollPitchYawToMatrixHomo('lf2m')
plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)
robot.lfTrajGen.playTrajectoryFile(folder+'LeftFoot.dat')
plug(robot.triggerTrajGen.sout, robot.lfTrajGen.trigger)
# --- Right foot
robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'RF')
......@@ -70,11 +76,13 @@ robot.rfTrajGen.x.recompute(0) # trigger computation of initial value
robot.rfToMatrix = PoseRollPitchYawToMatrixHomo('rf2m')
plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)
robot.rfTrajGen.playTrajectoryFile(folder+'RightFoot.dat')
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
robot.zmpTrajGen.playTrajectoryFile(folder+'ZMP.dat')
plug(robot.triggerTrajGen.sout, robot.zmpTrajGen.trigger)
# --- Waist
robot.waistTrajGen = create_orientation_rpy_trajectory_generator(dt, robot, 'WT')
......@@ -91,6 +99,7 @@ plug(robot.waistTrajGen.x, robot.waistMix.signal("sin2"))
robot.waistToMatrix = PoseRollPitchYawToMatrixHomo('w2m')
plug(robot.waistMix.sout, robot.waistToMatrix.sin)
robot.waistTrajGen.playTrajectoryFile(folder+'WaistOrientation.dat')
plug(robot.triggerTrajGen.sout, robot.waistTrajGen.trigger)
# --- Interface with controller entities
......@@ -375,6 +384,9 @@ plug(robot.dvdt.sout,robot.dynamic.acceleration)
robot.publisher = create_rospublish(robot, 'robot_publisher')
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
......
python/sot_talos_balance/test/test_dcmZmpControl_file.py
+ 4
− 3
View file @ 7cafefed
......@@ -10,11 +10,12 @@ runCommandClient('test_folder = "' + test_folder + '"')
run_test('appli_dcmZmpControl_file.py')
#run_ft_calibration('robot.ftc')
#raw_input("Wait before running the test")
run_ft_calibration('robot.ftc')
raw_input("Wait before running the test")
# Connect ZMP reference and reset controllers
print('Connect ZMP reference')
print('Trigger trajectory')
runCommandClient('robot.triggerTrajGen.sin.value = 1')
# runCommandClient('plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)')
runCommandClient('plug(robot.dcm_control.zmpRef,robot.com_admittance_control.zmpDes)')
runCommandClient('robot.com_admittance_control.setState(robot.wp.comDes.value,[0.0,0.0,0.0])')
......
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