Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • lscherrer/sot-talos-balance
  • imaroger/sot-talos-balance
  • pfernbac/sot-talos-balance
  • ostasse/sot-talos-balance
  • gsaurel/sot-talos-balance
  • loco-3d/sot-talos-balance
6 results
Show changes
Hide whitespace changes
Inline Side-by-side
Showing
with 1185 additions and 894 deletions
plot/plot_wrench_distrib.xml 0 → 100644
View file @ e217f893
<?xml version='1.0' encoding='UTF-8'?>
<root version="2.3.0">
<tabbed_widget parent="main_window" name="Main Window">
<plotmatrix rows="2" columns="3" tab_name="plot">
<plot row="0" col="0">
<range bottom="-64.965137" top="117.614916" left="-21.733000" right="-11.738000"/>
<limitY/>
<curve R="20" B="160" G="100" name="/sot/dcmCtrl/wrenchRef/data.0" custom_transform="noTransform"/>
<curve R="247" B="247" G="0" name="/sot/distribute/wrenchRef/data.0" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="0">
<range bottom="-64.965137" top="117.614916" left="-21.733000" right="-11.738000"/>
<limitY/>
<curve R="80" B="127" G="180" name="/sot/distribute/wrenchLeft/data.0" custom_transform="noTransform"/>
<curve R="20" B="160" G="100" name="/sot/distribute/wrenchRight/data.0" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="1">
<range bottom="-70.960893" top="83.557378" left="-21.733000" right="-11.738000"/>
<limitY/>
<curve R="5" B="13" G="116" name="/sot/dcmCtrl/wrenchRef/data.1" custom_transform="noTransform"/>
<curve R="0" B="238" G="51" name="/sot/distribute/wrenchRef/data.1" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="1">
<range bottom="-70.960893" top="83.557378" left="-21.733000" right="-11.738000"/>
<limitY/>
<curve R="244" B="29" G="83" name="/sot/distribute/wrenchLeft/data.1" custom_transform="noTransform"/>
<curve R="5" B="13" G="116" name="/sot/distribute/wrenchRight/data.1" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="2">
<range bottom="885.570204" top="885.570204" left="-21.733000" right="-11.738000"/>
<limitY/>
<curve R="255" B="24" G="19" name="/sot/dcmCtrl/wrenchRef/data.2" custom_transform="noTransform"/>
<curve R="0" B="119" G="119" name="/sot/distribute/wrenchRef/data.2" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="2">
<range bottom="-22.139255" top="907.709459" left="-21.733000" right="-11.738000"/>
<limitY/>
<curve R="142" B="136" G="52" name="/sot/distribute/wrenchLeft/data.2" custom_transform="noTransform"/>
<curve R="255" B="24" G="19" name="/sot/distribute/wrenchRight/data.2" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
</plotmatrix>
<plotmatrix rows="2" columns="3" tab_name="plot">
<plot row="0" col="0">
<range bottom="58.812999" top="59.369892" left="24.225000" right="34.222000"/>
<limitY/>
<curve R="241" B="193" G="76" name="/sot/ftc/left_foot_force_out/data.0" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="0">
<range bottom="-59.654389" top="-59.188734" left="24.225000" right="34.222000"/>
<limitY/>
<curve R="188" B="34" G="189" name="/sot/ftc/right_foot_force_out/data.0" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="1">
<range bottom="-46.926824" top="-45.127294" left="24.225000" right="34.222000"/>
<limitY/>
<curve R="148" B="189" G="103" name="/sot/ftc/left_foot_force_out/data.1" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="1">
<range bottom="45.588683" top="47.350587" left="24.225000" right="34.222000"/>
<limitY/>
<curve R="31" B="180" G="119" name="/sot/ftc/right_foot_force_out/data.1" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="2">
<range bottom="416.204087" top="417.069832" left="24.225000" right="34.222000"/>
<limitY/>
<curve R="23" B="207" G="190" name="/sot/ftc/left_foot_force_out/data.2" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="2">
<range bottom="445.899472" top="446.756994" left="24.225000" right="34.222000"/>
<limitY/>
<curve R="214" B="40" G="39" name="/sot/ftc/right_foot_force_out/data.2" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
</plotmatrix>
<currentPlotMatrix index="1"/>
</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 status="idle" ID="RosoutPublisherROS"/>
<plugin status="idle" ID="TopicPublisherROS"/>
</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>
plot/plot_xy.xml 0 → 100644
View file @ e217f893
<?xml version='1.0' encoding='UTF-8'?>
<root>
<tabbed_widget name="Main Window" parent="main_window">
<plotmatrix tab_name="plot" columns="4" rows="2">
<plot row="0" col="0">
<range left="247.596000" top="1.410900" right="297.594000" bottom="1.410636"/>
<limitY/>
<curve R="0" B="255" custom_transform="noTransform" G="0" name="/sot/PYRENE/state/data.0"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="0">
<range left="247.596000" top="-0.000291" right="297.594000" bottom="-0.000803"/>
<limitY/>
<curve R="0" B="0" custom_transform="noTransform" G="128" name="/sot/PYRENE/state/data.1"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="1">
<range left="247.585000" top="0.016967" right="297.583000" bottom="0.016326"/>
<limitY/>
<curve R="255" B="0" custom_transform="noTransform" G="0" name="/sot/base_estimator/q/data.0"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="1">
<range left="247.585000" top="-0.003585" right="297.583000" bottom="-0.011928"/>
<limitY/>
<curve R="255" B="255" custom_transform="noTransform" G="0" name="/sot/base_estimator/q/data.1"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="2">
<range left="247.596000" top="1.408120" right="297.594000" bottom="1.407480"/>
<limitY/>
<curve R="0" B="128" custom_transform="noTransform" G="0" name="/sot/stf/q/data.0"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="2">
<range left="247.596000" top="0.001227" right="297.594000" bottom="-0.007114"/>
<limitY/>
<curve R="0" B="128" custom_transform="noTransform" G="128" name="/sot/stf/q/data.1"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="3">
<range left="247.585000" top="1.445758" right="297.594000" bottom="-0.018522"/>
<limitY/>
<curve R="0" B="255" custom_transform="noTransform" G="0" name="/sot/PYRENE/state/data.0"/>
<curve R="255" B="0" custom_transform="noTransform" G="0" name="/sot/base_estimator/q/data.0"/>
<curve R="0" B="128" custom_transform="noTransform" G="0" name="/sot/stf/q/data.0"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="3">
<range left="247.585000" top="0.001348" right="297.594000" bottom="-0.012049"/>
<limitY/>
<curve R="0" B="0" custom_transform="noTransform" G="128" name="/sot/PYRENE/state/data.1"/>
<curve R="255" B="255" custom_transform="noTransform" G="0" name="/sot/base_estimator/q/data.1"/>
<curve R="0" B="128" custom_transform="noTransform" G="128" name="/sot/stf/q/data.1"/>
<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/base_estimator/q;/sot/PYRENE/state;/sot/stf/q"/>
</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>
plot/plot_xy_gz.xml 0 → 100644
View file @ e217f893
<?xml version='1.0' encoding='UTF-8'?>
<root>
<tabbed_widget name="Main Window" parent="main_window">
<plotmatrix tab_name="plot" columns="5" rows="2">
<plot col="0" row="0">
<range top="0.011684" right="129.915000" bottom="0.011660" left="124.919000"/>
<limitY/>
<curve R="0" B="255" custom_transform="noTransform" name="/sot/PYRENE/state/data.0" G="0"/>
<transform value="noTransform"/>
</plot>
<plot col="0" row="1">
<range top="-0.003682" right="129.915000" bottom="-0.003690" left="124.919000"/>
<limitY/>
<curve R="0" B="0" custom_transform="noTransform" name="/sot/PYRENE/state/data.1" G="128"/>
<transform value="noTransform"/>
</plot>
<plot col="1" row="0">
<range top="0.016814" right="129.915000" bottom="0.016808" left="124.919000"/>
<limitY/>
<curve R="255" B="0" custom_transform="noTransform" name="/sot/base_estimator/q/data.0" G="0"/>
<transform value="noTransform"/>
</plot>
<plot col="1" row="1">
<range top="-0.008695" right="129.915000" bottom="-0.008702" left="124.919000"/>
<limitY/>
<curve R="255" B="255" custom_transform="noTransform" name="/sot/base_estimator/q/data.1" G="0"/>
<transform value="noTransform"/>
</plot>
<plot col="2" row="0">
<range top="0.007967" right="129.915000" bottom="0.007961" left="124.919000"/>
<limitY/>
<curve R="0" B="128" custom_transform="noTransform" name="/sot/stf/q/data.0" G="0"/>
<transform value="noTransform"/>
</plot>
<plot col="2" row="1">
<range top="-0.003877" right="129.915000" bottom="-0.003885" left="124.919000"/>
<limitY/>
<curve R="0" B="128" custom_transform="noTransform" name="/sot/stf/q/data.1" G="128"/>
<transform value="noTransform"/>
</plot>
<plot col="3" row="0">
<range top="0.110470" right="129.925000" bottom="0.110463" left="124.925000"/>
<limitY/>
<curve R="160" B="164" custom_transform="noTransform" name="/gazebo/model_states/pose.1/position/x" G="160"/>
<transform value="noTransform"/>
</plot>
<plot col="3" row="1">
<range top="-0.001122" right="129.925000" bottom="-0.001152" left="124.925000"/>
<limitY/>
<curve R="128" B="0" custom_transform="noTransform" name="/gazebo/model_states/pose.1/position/y" G="128"/>
<transform value="noTransform"/>
</plot>
<plot col="4" row="0">
<range top="0.113033" right="129.925000" bottom="0.005399" left="124.919000"/>
<limitY/>
<curve R="160" B="164" custom_transform="noTransform" name="/gazebo/model_states/pose.1/position/x" G="160"/>
<curve R="0" B="255" custom_transform="noTransform" name="/sot/PYRENE/state/data.0" G="0"/>
<curve R="255" B="0" custom_transform="noTransform" name="/sot/base_estimator/q/data.0" G="0"/>
<curve R="0" B="128" custom_transform="noTransform" name="/sot/stf/q/data.0" G="0"/>
<transform value="noTransform"/>
</plot>
<plot col="4" row="1">
<range top="-0.000934" right="129.925000" bottom="-0.008892" left="124.919000"/>
<limitY/>
<curve R="128" B="0" custom_transform="noTransform" name="/gazebo/model_states/pose.1/position/y" G="128"/>
<curve R="0" B="0" custom_transform="noTransform" name="/sot/PYRENE/state/data.1" G="128"/>
<curve R="255" B="255" custom_transform="noTransform" name="/sot/base_estimator/q/data.1" G="0"/>
<curve R="0" B="128" custom_transform="noTransform" name="/sot/stf/q/data.1" G="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/base_estimator/q;/sot/PYRENE/state;/sot/stf/q;/gazebo/model_states"/>
</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>
plot/plot_xy_gznostf.xml 0 → 100644
View file @ e217f893
<?xml version='1.0' encoding='UTF-8'?>
<root>
<tabbed_widget parent="main_window" name="Main Window">
<plotmatrix tab_name="plot" rows="2" columns="4">
<plot row="0" col="0">
<range bottom="-0.000001" top="-0.000001" right="26.164000" left="0.000000"/>
<limitY/>
<curve B="255" name="/sot/PYRENE/state/data.0" R="0" G="0" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="0">
<range bottom="0.000072" top="0.000072" right="26.164000" left="0.000000"/>
<limitY/>
<curve B="0" name="/sot/PYRENE/state/data.1" R="0" G="128" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="1">
<range bottom="-0.100000" top="0.100000" right="0.000000" left="0.000000"/>
<limitY/>
<curve B="0" name="/sot/base_estimator/q/data.0" R="255" G="0" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="1">
<range bottom="-0.100000" top="0.100000" right="0.000000" left="0.000000"/>
<limitY/>
<curve B="255" name="/sot/base_estimator/q/data.1" R="255" G="0" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="2">
<range bottom="-0.041579" top="-0.041371" right="26.170000" left="0.000000"/>
<limitY/>
<curve B="164" name="/gazebo/model_states/pose.1/position/x" R="160" G="160" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="2">
<range bottom="-0.002136" top="-0.002134" right="26.170000" left="0.000000"/>
<limitY/>
<curve B="0" name="/gazebo/model_states/pose.1/position/y" R="128" G="128" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="3">
<range bottom="-0.042613" top="0.001038" right="26.170000" left="0.000000"/>
<limitY/>
<curve B="164" name="/gazebo/model_states/pose.1/position/x" R="160" G="160" custom_transform="noTransform"/>
<curve B="255" name="/sot/PYRENE/state/data.0" R="0" G="0" custom_transform="noTransform"/>
<curve B="0" name="/sot/base_estimator/q/data.0" R="255" G="0" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="3">
<range bottom="-0.002191" top="0.000127" right="26.170000" left="0.000000"/>
<limitY/>
<curve B="0" name="/gazebo/model_states/pose.1/position/y" R="128" G="128" custom_transform="noTransform"/>
<curve B="0" name="/sot/PYRENE/state/data.1" R="0" G="128" custom_transform="noTransform"/>
<curve B="255" name="/sot/base_estimator/q/data.1" R="255" G="0" custom_transform="noTransform"/>
<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="/gazebo/model_states;/sot/base_estimator/q;/sot/PYRENE/state"/>
</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>
plot/plot_xy_madgwick.xml 0 → 100644
View file @ e217f893
<?xml version='1.0' encoding='UTF-8'?>
<root>
<tabbed_widget name="Main Window" parent="main_window">
<plotmatrix columns="4" tab_name="plot" rows="2">
<plot row="0" col="0">
<range bottom="-0.099558" left="442.267000" right="447.265000" top="0.100442"/>
<limitY/>
<curve custom_transform="noTransform" R="0" B="255" G="0" name="/sot/PYRENE/state/data.0"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="0">
<range bottom="-0.099979" left="442.267000" right="447.265000" top="0.100021"/>
<limitY/>
<curve custom_transform="noTransform" R="0" B="0" G="128" name="/sot/PYRENE/state/data.1"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="1">
<range bottom="-0.003379" left="442.267000" right="447.265000" top="-0.003307"/>
<limitY/>
<curve custom_transform="noTransform" R="255" B="0" G="0" name="/sot/base_estimator/q/data.0"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="1">
<range bottom="-0.000029" left="442.267000" right="447.265000" top="0.000134"/>
<limitY/>
<curve custom_transform="noTransform" R="255" B="255" G="0" name="/sot/base_estimator/q/data.1"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="2">
<range bottom="-0.038807" left="442.271000" right="447.271000" top="-0.038783"/>
<limitY/>
<curve custom_transform="noTransform" R="160" B="164" G="160" name="/gazebo/model_states/pose.1/position/x"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="2">
<range bottom="-0.001875" left="442.271000" right="447.271000" top="-0.001875"/>
<limitY/>
<curve custom_transform="noTransform" R="128" B="0" G="128" name="/gazebo/model_states/pose.1/position/y"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="3">
<range bottom="-0.039787" left="442.267000" right="447.271000" top="0.001423"/>
<limitY/>
<curve custom_transform="noTransform" R="160" B="164" G="160" name="/gazebo/model_states/pose.1/position/x"/>
<curve custom_transform="noTransform" R="0" B="255" G="0" name="/sot/PYRENE/state/data.0"/>
<curve custom_transform="noTransform" R="255" B="0" G="0" name="/sot/base_estimator/q/data.0"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="3">
<range bottom="-0.001926" left="442.267000" right="447.271000" top="0.000181"/>
<limitY/>
<curve custom_transform="noTransform" R="128" B="0" G="128" name="/gazebo/model_states/pose.1/position/y"/>
<curve custom_transform="noTransform" R="0" B="0" G="128" name="/sot/PYRENE/state/data.1"/>
<curve custom_transform="noTransform" R="255" B="255" G="0" name="/sot/base_estimator/q/data.1"/>
<transform value="noTransform"/>
</plot>
</plotmatrix>
<plotmatrix columns="2" tab_name="plot" rows="2">
<plot row="0" col="0">
<range bottom="0.000010" left="436.472000" right="441.463000" top="0.000163"/>
<limitY/>
<curve custom_transform="noTransform" R="160" B="164" G="160" name="/sot/imu_filter/imu_quat/data.0"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="0">
<range bottom="-0.143066" left="436.472000" right="441.463000" top="-0.143065"/>
<limitY/>
<curve custom_transform="noTransform" R="128" B="0" G="0" name="/sot/imu_filter/imu_quat/data.2"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="1">
<range bottom="0.989713" left="436.472000" right="441.463000" top="0.989713"/>
<limitY/>
<curve custom_transform="noTransform" R="128" B="0" G="128" name="/sot/imu_filter/imu_quat/data.1"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="1">
<range bottom="-0.000153" left="436.472000" right="441.463000" top="0.000099"/>
<limitY/>
<curve custom_transform="noTransform" R="0" B="255" G="0" name="/sot/imu_filter/imu_quat/data.3"/>
<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="/gazebo/model_states;/sot/base_estimator/q;/sot/PYRENE/state;/sot/imu_filter/imu_quat"/>
</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>
plot/plot_xy_madgwick_w.xml 0 → 100644
View file @ e217f893
<?xml version='1.0' encoding='UTF-8'?>
<root>
<tabbed_widget name="Main Window" parent="main_window">
<plotmatrix columns="4" rows="2" tab_name="plot">
<plot col="0" row="0">
<range left="39.953000" right="47.367000" bottom="-0.100000" top="0.100000"/>
<limitY/>
<curve name="/sot/PYRENE/state/data.0" R="0" G="0" custom_transform="noTransform" B="255"/>
<transform value="noTransform"/>
</plot>
<plot col="0" row="1">
<range left="39.953000" right="47.367000" bottom="-0.100000" top="0.100000"/>
<limitY/>
<curve name="/sot/PYRENE/state/data.1" R="0" G="128" custom_transform="noTransform" B="0"/>
<transform value="noTransform"/>
</plot>
<plot col="1" row="0">
<range left="39.953000" right="47.367000" bottom="0.002214" top="0.002901"/>
<limitY/>
<curve name="/sot/base_estimator/q/data.0" R="255" G="0" custom_transform="noTransform" B="0"/>
<transform value="noTransform"/>
</plot>
<plot col="1" row="1">
<range left="39.953000" right="47.367000" bottom="-0.001142" top="-0.000983"/>
<limitY/>
<curve name="/sot/base_estimator/q/data.1" R="255" G="0" custom_transform="noTransform" B="255"/>
<transform value="noTransform"/>
</plot>
<plot col="2" row="0">
<range left="39.950000" right="47.377000" bottom="-0.041712" top="-0.041588"/>
<limitY/>
<curve name="/gazebo/model_states/pose.1/position/x" R="160" G="160" custom_transform="noTransform" B="164"/>
<transform value="noTransform"/>
</plot>
<plot col="2" row="1">
<range left="39.950000" right="47.377000" bottom="-0.003048" top="-0.003048"/>
<limitY/>
<curve name="/gazebo/model_states/pose.1/position/y" R="128" G="128" custom_transform="noTransform" B="0"/>
<transform value="noTransform"/>
</plot>
<plot col="3" row="0">
<range left="39.950000" right="47.377000" bottom="-0.042824" top="0.004000"/>
<limitY/>
<curve name="/gazebo/model_states/pose.1/position/x" R="160" G="160" custom_transform="noTransform" B="164"/>
<curve name="/sot/PYRENE/state/data.0" R="0" G="0" custom_transform="noTransform" B="255"/>
<curve name="/sot/base_estimator/q/data.0" R="255" G="0" custom_transform="noTransform" B="0"/>
<transform value="noTransform"/>
</plot>
<plot col="3" row="1">
<range left="39.950000" right="47.377000" bottom="-0.003124" top="0.000076"/>
<limitY/>
<curve name="/gazebo/model_states/pose.1/position/y" R="128" G="128" custom_transform="noTransform" B="0"/>
<curve name="/sot/PYRENE/state/data.1" R="0" G="128" custom_transform="noTransform" B="0"/>
<curve name="/sot/base_estimator/q/data.1" R="255" G="0" custom_transform="noTransform" B="255"/>
<transform value="noTransform"/>
</plot>
</plotmatrix>
<plotmatrix columns="3" rows="2" tab_name="plot">
<plot col="0" row="0">
<range left="7.497000" right="20.147000" bottom="-0.044145" top="1.025466"/>
<limitY/>
<curve name="/sot/imu_filter/imu_quat/data.0" R="160" G="160" custom_transform="noTransform" B="164"/>
<transform value="noTransform"/>
</plot>
<plot col="0" row="1">
<range left="7.497000" right="20.147000" bottom="-0.086185" top="0.002038"/>
<limitY/>
<curve name="/sot/imu_filter/imu_quat/data.2" R="128" G="0" custom_transform="noTransform" B="0"/>
<transform value="noTransform"/>
</plot>
<plot col="1" row="0">
<range left="7.497000" right="20.147000" bottom="-0.023977" top="1.024974"/>
<limitY/>
<curve name="/sot/base_estimator/w_lf_filtered/data" R="0" G="0" custom_transform="noTransform" B="128"/>
<curve name="/sot/imu_filter/imu_quat/data.1" R="128" G="128" custom_transform="noTransform" B="0"/>
<transform value="noTransform"/>
</plot>
<plot col="1" row="1">
<range left="7.497000" right="20.147000" bottom="-0.002490" top="0.002785"/>
<limitY/>
<curve name="/sot/imu_filter/imu_quat/data.3" R="0" G="0" custom_transform="noTransform" B="255"/>
<transform value="noTransform"/>
</plot>
<plot col="2" row="0">
<range left="7.497000" right="20.147000" bottom="0.931296" top="1.001674"/>
<limitY/>
<curve name="/sot/base_estimator/w_lf/data" R="255" G="0" custom_transform="noTransform" B="255"/>
<curve name="/sot/base_estimator/w_lf_filtered/data" R="0" G="0" custom_transform="noTransform" B="128"/>
<curve name="/sot/base_estimator/w_rf/data" R="0" G="128" custom_transform="noTransform" B="128"/>
<curve name="/sot/base_estimator/w_rf_filtered/data" R="160" G="160" custom_transform="noTransform" B="164"/>
<transform value="noTransform"/>
</plot>
<plot col="2" row="1">
<range left="7.497000" right="20.147000" bottom="259.765124" top="644.775929"/>
<limitY/>
<curve name="/sot/ft_LF_filter/x_filtered/data.2" R="0" G="128" custom_transform="noTransform" B="0"/>
<curve name="/sot/ft_RF_filter/x_filtered/data.2" R="255" G="0" custom_transform="noTransform" B="0"/>
<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="/gazebo/model_states;/sot/base_estimator/q;/sot/base_estimator/w_lf;/sot/base_estimator/w_lf_filtered;/sot/base_estimator/w_rf;/sot/base_estimator/w_rf_filtered;/sot/ft_LF_filter/x_filtered;/sot/ft_RF_filter/x_filtered;/sot/imu_filter/imu_quat;/sot/PYRENE/forceLLEG;/sot/PYRENE/state;/sot/PYRENE/forceRLEG"/>
</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>
plot/plot_xyz_gz.xml 0 → 100644
View file @ e217f893
<?xml version='1.0' encoding='UTF-8'?>
<root>
<tabbed_widget name="Main Window" parent="main_window">
<plotmatrix columns="4" tab_name="plot" rows="3">
<plot col="0" row="0">
<range left="0.000000" top="1.448911" right="50.000000" bottom="-0.035340"/>
<limitY/>
<curve B="255" custom_transform="noTransform" R="0" name="/sot/PYRENE/state/data.0" G="0"/>
<transform value="noTransform"/>
</plot>
<plot col="0" row="1">
<range left="0.000000" top="0.056388" right="50.000000" bottom="-0.078851"/>
<limitY/>
<curve B="0" custom_transform="noTransform" R="0" name="/sot/PYRENE/state/data.1" G="128"/>
<transform value="noTransform"/>
</plot>
<plot col="0" row="2">
<range left="0.000000" top="1.020286" right="50.000000" bottom="1.012320"/>
<limitY/>
<curve B="0" custom_transform="noTransform" R="128" name="/sot/PYRENE/state/data.2" G="0"/>
<transform value="noTransform"/>
</plot>
<plot col="1" row="0">
<range left="0.000000" top="1.445344" right="50.000000" bottom="-0.041356"/>
<limitY/>
<curve B="0" custom_transform="noTransform" R="255" name="/sot/base_estimator/q/data.0" G="0"/>
<transform value="noTransform"/>
</plot>
<plot col="1" row="1">
<range left="0.000000" top="0.058376" right="50.000000" bottom="-0.086501"/>
<limitY/>
<curve B="255" custom_transform="noTransform" R="255" name="/sot/base_estimator/q/data.1" G="0"/>
<transform value="noTransform"/>
</plot>
<plot col="1" row="2">
<range left="0.000000" top="1.019692" right="50.000000" bottom="1.008006"/>
<limitY/>
<curve B="255" custom_transform="noTransform" R="0" name="/sot/base_estimator/q/data.2" G="0"/>
<transform value="noTransform"/>
</plot>
<plot col="2" row="0">
<range left="0.001000" top="1.494741" right="50.001000" bottom="-0.078522"/>
<limitY/>
<curve B="164" custom_transform="noTransform" R="160" name="/gazebo/model_states/pose.1/position/x" G="160"/>
<transform value="noTransform"/>
</plot>
<plot col="2" row="1">
<range left="0.001000" top="0.040600" right="50.001000" bottom="-0.090886"/>
<limitY/>
<curve B="0" custom_transform="noTransform" R="128" name="/gazebo/model_states/pose.1/position/y" G="128"/>
<transform value="noTransform"/>
</plot>
<plot col="2" row="2">
<range left="0.001000" top="1.024913" right="50.001000" bottom="1.010724"/>
<limitY/>
<curve B="0" custom_transform="noTransform" R="0" name="/gazebo/model_states/pose.1/position/z" G="128"/>
<transform value="noTransform"/>
</plot>
<plot col="3" row="0">
<range left="0.000000" top="1.494741" right="50.001000" bottom="-0.078522"/>
<limitY/>
<curve B="164" custom_transform="noTransform" R="160" name="/gazebo/model_states/pose.1/position/x" G="160"/>
<curve B="255" custom_transform="noTransform" R="0" name="/sot/PYRENE/state/data.0" G="0"/>
<curve B="0" custom_transform="noTransform" R="255" name="/sot/base_estimator/q/data.0" G="0"/>
<transform value="noTransform"/>
</plot>
<plot col="3" row="1">
<range left="0.000000" top="0.058493" right="50.001000" bottom="-0.091322"/>
<limitY/>
<curve B="0" custom_transform="noTransform" R="128" name="/gazebo/model_states/pose.1/position/y" G="128"/>
<curve B="0" custom_transform="noTransform" R="0" name="/sot/PYRENE/state/data.1" G="128"/>
<curve B="255" custom_transform="noTransform" R="255" name="/sot/base_estimator/q/data.1" G="0"/>
<transform value="noTransform"/>
</plot>
<plot col="3" row="2">
<range left="0.000000" top="1.024983" right="50.001000" bottom="1.007877"/>
<limitY/>
<curve B="0" custom_transform="noTransform" R="0" name="/gazebo/model_states/pose.1/position/z" G="128"/>
<curve B="0" custom_transform="noTransform" R="128" name="/sot/PYRENE/state/data.2" G="0"/>
<curve B="255" custom_transform="noTransform" R="0" name="/sot/base_estimator/q/data.2" G="0"/>
<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="/gazebo/model_states;/sot/base_estimator/q;/sot/PYRENE/state"/>
</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>
plot/plot_zmp.xml 0 → 100644
View file @ e217f893
<?xml version='1.0' encoding='UTF-8'?>
<root>
<tabbed_widget name="Main Window" parent="main_window">
<plotmatrix tab_name="plot" rows="2" columns="3">
<plot col="0" row="0">
<range left="14.262000" right="19.255000" bottom="0.000035" top="0.000037"/>
<limitY/>
<curve G="0" custom_transform="noTransform" R="0" B="255" name="/sot/zmpEst/zmp/data.0"/>
<transform value="noTransform"/>
</plot>
<plot col="0" row="1">
<range left="14.262000" right="19.255000" bottom="-0.002360" top="-0.002360"/>
<limitY/>
<curve G="0" custom_transform="noTransform" R="128" B="0" name="/sot/cdc_estimator/c/data.0"/>
<transform value="noTransform"/>
</plot>
<plot col="1" row="0">
<range left="14.262000" right="19.255000" bottom="-0.092751" top="-0.092750"/>
<limitY/>
<curve G="128" custom_transform="noTransform" R="0" B="0" name="/sot/zmpEst/zmp/data.1"/>
<transform value="noTransform"/>
</plot>
<plot col="1" row="1">
<range left="14.262000" right="19.255000" bottom="-0.083197" top="-0.083196"/>
<limitY/>
<curve G="0" custom_transform="noTransform" R="0" B="255" name="/sot/cdc_estimator/c/data.1"/>
<transform value="noTransform"/>
</plot>
<plot col="2" row="0">
<range left="14.262000" right="19.255000" bottom="0.000100" top="0.000100"/>
<limitY/>
<curve G="0" custom_transform="noTransform" R="255" B="0" name="/sot/zmpEst/zmp/data.2"/>
<transform value="noTransform"/>
</plot>
<plot col="2" row="1">
<range left="14.262000" right="19.255000" bottom="0.874043" top="0.874049"/>
<limitY/>
<curve G="128" custom_transform="noTransform" R="0" B="0" name="/sot/cdc_estimator/c/data.2"/>
<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/cdc_estimator/c;/sot/zmpEst/zmp"/>
</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>
plot/plot_zmp_com.xml 0 → 100644
View file @ e217f893
<?xml version='1.0' encoding='UTF-8'?>
<root>
<tabbed_widget parent="main_window" name="Main Window">
<plotmatrix tab_name="plot" columns="4" rows="2">
<plot row="0" col="0">
<range top="0.001234" bottom="-0.003271" left="21.177000" right="26.172000"/>
<limitY/>
<curve B="255" G="0" R="0" custom_transform="noTransform" name="/sot/dummy_wp/comDes/data.0"/>
<curve B="0" G="255" R="0" custom_transform="noTransform" name="/sot/comAdmCtrl/comRef/data.0"/>
<curve B="0" G="0" R="255" custom_transform="noTransform" name="/sot/cdc_estimator/c/data.0"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="0">
<range top="0.000071" bottom="-0.000148" left="21.177000" right="26.172000"/>
<limitY/>
<curve B="255" G="0" R="0" custom_transform="noTransform" name="/sot/dcmCtrl/zmpDes/data.0"/>
<curve B="0" G="255" R="0" custom_transform="noTransform" name="/sot/dcmCtrl/zmpRef/data.0"/>
<curve B="0" G="0" R="255" custom_transform="noTransform" name="/sot/zmpEst/zmp/data.0"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="1">
<range top="0.026275" bottom="-0.027328" left="21.177000" right="26.172000"/>
<limitY/>
<curve B="255" G="0" R="0" custom_transform="noTransform" name="/sot/dummy_wp/comDes/data.1"/>
<curve B="0" G="255" R="0" custom_transform="noTransform" name="/sot/comAdmCtrl/comRef/data.1"/>
<curve B="0" G="0" R="255" custom_transform="noTransform" name="/sot/cdc_estimator/c/data.1"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="1">
<range top="0.031396" bottom="-0.039032" left="21.177000" right="26.172000"/>
<limitY/>
<curve B="255" G="0" R="0" custom_transform="noTransform" name="/sot/dcmCtrl/zmpDes/data.1"/>
<curve B="0" G="255" R="0" custom_transform="noTransform" name="/sot/dcmCtrl/zmpRef/data.1"/>
<curve B="0" G="0" R="255" custom_transform="noTransform" name="/sot/zmpEst/zmp/data.1"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="2">
<range top="0.875949" bottom="0.874358" left="21.177000" right="26.172000"/>
<limitY/>
<curve B="255" G="0" R="0" custom_transform="noTransform" name="/sot/dummy_wp/comDes/data.2"/>
<curve B="0" G="255" R="0" custom_transform="noTransform" name="/sot/comAdmCtrl/comRef/data.2"/>
<curve B="0" G="0" R="255" custom_transform="noTransform" name="/sot/cdc_estimator/c/data.2"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="2">
<range top="0.000012" bottom="-0.000079" left="21.177000" right="26.172000"/>
<limitY/>
<curve B="255" G="0" R="0" custom_transform="noTransform" name="/sot/dcmCtrl/zmpDes/data.2"/>
<curve B="0" G="255" R="0" custom_transform="noTransform" name="/sot/dcmCtrl/zmpRef/data.2"/>
<curve B="0" G="0" R="255" custom_transform="noTransform" name="/sot/zmpEst/zmp/data.2"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="3">
<range top="-23.556048" bottom="-23.558718" left="21.177000" right="26.172000"/>
<limitY/>
<curve name="/sot/ftc/left_foot_force_out/data.2" B="0" custom_transform="noTransform" R="255" G="0"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="3">
<range top="888.944024" bottom="888.921802" left="21.177000" right="26.172000"/>
<limitY/>
<curve name="/sot/ftc/right_foot_force_out/data.2" B="255" custom_transform="noTransform" R="0" G="0"/>
<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="/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_wp/comDes;/sot/dummy/dcm;/sot/robot_dynamic/com;/sot/robot_dynamic/zmp;/sot/zmpEst/zmp;/sot/ftc/left_foot_force_out;/sot/ftc/right_foot_force_out"/>
</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>
pyproject.toml 0 → 100644
View file @ e217f893
[tool.black]
exclude = "cmake"
python/sot_talos_balance/create_entities_utils.py deleted 100644 → 0
View file @ ca7bad3e
from sot_talos_balance.control_manager import ControlManager
from sot_talos_balance.example import Example
from sot_talos_balance.parameter_server import ParameterServer
from dynamic_graph.tracer_real_time import TracerRealTime
from time import sleep
from sot_talos_balance.base_estimator import BaseEstimator
from sot_talos_balance.madgwickahrs import MadgwickAHRS
from sot_talos_balance.imu_offset_compensation import ImuOffsetCompensation
from sot_talos_balance.dcm_estimator import DcmEstimator
from sot_talos_balance.ft_calibration import FtCalibration
from sot_talos_balance.euler_to_quat import EulerToQuat
from sot_talos_balance.quat_to_euler import QuatToEuler
from dynamic_graph.sot.core.operator import Mix_of_vector
from dynamic_graph.sot.core.operator import Selec_of_vector
from dynamic_graph.sot.core.operator import MatrixHomoToPoseQuaternion
from sot_talos_balance.nd_trajectory_generator import NdTrajectoryGenerator
from sot_talos_balance.joint_position_controller import JointPositionController
from sot_talos_balance.admittance_controller import AdmittanceController
from sot_talos_balance.admittance_controller_end_effector import AdmittanceControllerEndEffector
from sot_talos_balance.dummy_dcm_estimator import DummyDcmEstimator
from sot_talos_balance.com_admittance_controller import ComAdmittanceController
from sot_talos_balance.dcm_controller import DcmController
from sot_talos_balance.dcm_com_controller import DcmComController
from sot_talos_balance.simple_zmp_estimator import SimpleZmpEstimator
from sot_talos_balance.distribute_wrench import DistributeWrench
# python
from sot_talos_balance.utils.filter_utils import *
from sot_talos_balance.utils.sot_utils import Bunch
from dynamic_graph import plug
from dynamic_graph.ros import RosPublish
N_JOINTS = 32;
def create_extend_mix(n_in,n_out):
assert n_out>n_in
mix_of_vector = Mix_of_vector( "mix " + str(n_in) + "-" + str(n_out) )
mix_of_vector.setSignalNumber(3)
n_diff = n_out-n_in
mix_of_vector.addSelec(1,0,n_diff)
mix_of_vector.addSelec(2,n_diff,n_in)
mix_of_vector.default.value=[0.0]*n_out
mix_of_vector.signal("sin1").value = [0.0]*n_diff
mix_of_vector.signal("sin2").value = [2.0]*n_in
return mix_of_vector
def create_joint_trajectory_generator(dt):
jtg = NdTrajectoryGenerator("jtg")
jtg.initial_value.value = tuple(N_JOINTS*[0.0])
jtg.trigger.value = 1.0
jtg.init(dt, N_JOINTS)
return jtg
def create_config_trajectory_generator(dt):
N_CONFIG = N_JOINTS + 6
jtg = NdTrajectoryGenerator("jtg")
jtg.initial_value.value = tuple(N_CONFIG*[0.0])
jtg.trigger.value = 1.0
jtg.init(dt, N_CONFIG)
return jtg
def create_com_trajectory_generator(dt,robot):
comTrajGen = NdTrajectoryGenerator("comTrajGen")
comTrajGen.initial_value.value = robot.dynamic.com.value
comTrajGen.trigger.value = 1.0
comTrajGen.init(dt, 3)
return comTrajGen
def create_joint_controller(Kp):
controller = JointPositionController("posctrl")
controller.Kp.value = Kp
return controller
def create_end_effector_admittance_controller(Kp, timeStep, robot):
controller = AdmittanceControllerEndEffector("admittanceController")
controller.Kp.value = Kp
# Plug the force and joint configuration
plug(robot.device.joint_angles, controller.jointPosition)
plug(robot.device.forceRARM, controller.force)
controller.forceDes.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
controller.init(timeStep, "wrist_left_ft_link")
return controller
def create_joint_admittance_controller(joint,Kp,dt,robot,filter=False):
controller = AdmittanceController("jadmctrl")
controller.Kp.value = Kp
robot.stateselec = Selec_of_vector("state_selec")
robot.stateselec.selec(joint+6,joint+7)
plug(robot.device.state,robot.stateselec.sin)
plug(robot.stateselec.sout,controller.state)
robot.tauselec = Selec_of_vector("tau_selec")
robot.tauselec.selec(joint,joint+1)
if filter and hasattr(robot,'device_filters'):
plug(robot.device_filters.torque_filter.x_filtered,robot.tauselec.sin)
else:
plug(robot.device.ptorque,robot.tauselec.sin)
plug(robot.tauselec.sout,controller.tau)
controller.tauDes.value = [0.0]
controller.init(dt, 1)
controller.setPosition([ robot.device.state.value[joint+6] ])
return controller
def create_imu_offset_compensation(robot, dt):
imu_offset_compensation = ImuOffsetCompensation('imu_offset_comp');
imu_offset_compensation.init(dt);
plug(robot.device.accelerometer, imu_offset_compensation.accelerometer_in);
plug(robot.device.gyrometer, imu_offset_compensation.gyrometer_in);
return imu_offset_compensation;
def create_device_filters(robot, dt):
robot.vselec = Selec_of_vector("vselec")
robot.vselec.selec(6,6+N_JOINTS)
plug(robot.device.velocity,robot.vselec.sin)
filters = Bunch();
filters.joints_kin = create_chebi1_checby2_series_filter("joints_kin", dt, N_JOINTS);
filters.ft_RF_filter = create_butter_lp_filter_Wn_04_N_2("ft_RF_filter", dt, 6);
filters.ft_LF_filter = create_butter_lp_filter_Wn_04_N_2("ft_LF_filter", dt, 6);
filters.ft_RH_filter = create_chebi1_checby2_series_filter("ft_RH_filter", dt, 6);
filters.ft_LH_filter = create_chebi1_checby2_series_filter("ft_LH_filter", dt, 6);
filters.torque_filter = create_chebi1_checby2_series_filter("ptorque_filter", dt, N_JOINTS);
filters.acc_filter = create_chebi1_checby2_series_filter("acc_filter", dt, 3);
filters.gyro_filter = create_chebi1_checby2_series_filter("gyro_filter", dt, 3);
filters.vel_filter = create_butter_lp_filter_Wn_04_N_2("vel_filter", dt, N_JOINTS);
plug(robot.device.joint_angles, filters.joints_kin.x);
plug(robot.device.forceRLEG, filters.ft_RF_filter.x);
plug(robot.device.forceLLEG, filters.ft_LF_filter.x);
plug(robot.device.forceRARM, filters.ft_RH_filter.x);
plug(robot.device.forceLARM, filters.ft_LH_filter.x);
plug(robot.device.ptorque, filters.torque_filter.x);
plug(robot.vselec.sout, filters.vel_filter.x);
# switch following lines if willing to use imu offset compensation
#~ plug(robot.imu_offset_compensation.accelerometer_out, filters.acc_filter.x);
plug(robot.device.accelerometer, filters.acc_filter.x);
#~ plug(robot.imu_offset_compensation.gyrometer_out, filters.gyro_filter.x);
plug(robot.device.gyrometer, filters.gyro_filter.x);
return filters
def create_be_filters(robot, dt):
be_filters = Bunch();
be_filters.test = create_chebi1_checby2_series_filter("test_filter", dt, N_JOINTS);
plug(robot.base_estimator.q, be_filters.test.x);
return be_filters
def create_ctrl_manager(conf, dt, robot_name='robot'):
ctrl_manager = ControlManager("ctrl_man");
ctrl_manager.init(dt, robot_name)
ctrl_manager.u_max.value = conf.NJ*(conf.CTRL_MAX,);
# Init should be called before addCtrlMode
# because the size of state vector must be known.
return ctrl_manager;
def create_base_estimator(robot, dt, conf, robot_name="robot"):
base_estimator = BaseEstimator('base_estimator')
base_estimator.init(dt, robot_name)
plug(robot.device.joint_angles, base_estimator.joint_positions) # device.state, device.joint_angles or device.motor_angles ?
plug(robot.device_filters.ft_LF_filter.x_filtered, base_estimator.forceLLEG)
plug(robot.device_filters.ft_RF_filter.x_filtered, base_estimator.forceRLEG)
plug(robot.device_filters.ft_LF_filter.dx, base_estimator.dforceLLEG)
plug(robot.device_filters.ft_RF_filter.dx, base_estimator.dforceRLEG)
plug(robot.vselec.sout, base_estimator.joint_velocities)
# plug(robot.device_filters.vel_filter.x_filtered, base_estimator.joint_velocities)
plug(robot.imu_filters.imu_quat, base_estimator.imu_quaternion)
plug(robot.device_filters.gyro_filter.x_filtered, base_estimator.gyroscope)
plug(robot.device_filters.acc_filter.x_filtered, base_estimator.accelerometer)
base_estimator.K_fb_feet_poses.value = conf.K_fb_feet_poses
base_estimator.w_lf_in.value = conf.w_lf_in
base_estimator.w_rf_in.value = conf.w_rf_in
base_estimator.set_imu_weight(conf.w_imu)
base_estimator.set_stiffness_right_foot(conf.K)
base_estimator.set_stiffness_left_foot(conf.K)
base_estimator.set_zmp_std_dev_right_foot(conf.std_dev_zmp)
base_estimator.set_zmp_std_dev_left_foot(conf.std_dev_zmp)
base_estimator.set_normal_force_std_dev_right_foot(conf.std_dev_fz)
base_estimator.set_normal_force_std_dev_left_foot(conf.std_dev_fz)
base_estimator.set_zmp_margin_right_foot(conf.zmp_margin)
base_estimator.set_zmp_margin_left_foot(conf.zmp_margin)
base_estimator.set_normal_force_margin_right_foot(conf.normal_force_margin)
base_estimator.set_normal_force_margin_left_foot(conf.normal_force_margin)
base_estimator.set_right_foot_sizes(conf.RIGHT_FOOT_SIZES)
base_estimator.set_left_foot_sizes(conf.LEFT_FOOT_SIZES)
return base_estimator
def create_imu_filters(robot, dt):
imu_filter = MadgwickAHRS('imu_filter');
imu_filter.init(dt);
plug(robot.device_filters.acc_filter.x_filtered, imu_filter.accelerometer); # no IMU compensation
plug(robot.device_filters.gyro_filter.x_filtered, imu_filter.gyroscope); # no IMU compensation
return imu_filter;
def addTrace(tracer, entity, signalName):
"""
Add a signal to a tracer
"""
signal = '{0}.{1}'.format(entity.name, signalName);
filename = '{0}-{1}'.format(entity.name, signalName);
tracer.add(signal, filename);
def addSignalsToTracer(tracer, device, outputs):
for sign in outputs :
addTrace(tracer,device,sign);
return
def create_tracer(robot,entity,tracer_name, outputs=None):
tracer = TracerRealTime(tracer_name)
tracer.setBufferSize(80*(2**20))
tracer.open('/tmp','dg_','.dat')
robot.device.after.addSignal('{0}.triger'.format(tracer.name))
if outputs is not None:
addSignalsToTracer(tracer, entity, outputs)
return tracer
def reset_tracer(device,tracer):
tracer.stop();
sleep(0.2);
tracer.close();
sleep(0.2);
tracer.clear();
sleep(0.2);
tracer = create_tracer(device, tracer.name);
return tracer;
def dump_tracer(tracer):
tracer.stop();
sleep(0.2);
tracer.dump()
sleep(0.2);
tracer.close();
def create_rospublish(robot, name):
rospub = RosPublish(name)
robot.device.after.addSignal(rospub.name+'.trigger')
return rospub
def create_topic(rospub, entity, signalName, robot=None, data_type='vector'):
if not entity.hasSignal(signalName): # check needed to prevent creation of broken topic
raise AttributeError( 'Entity %s does not have signal %s' % (entity.name, signalName) )
rospub_signalName = '{0}_{1}'.format(entity.name, signalName)
topicname = '/sot/{0}/{1}'.format(entity.name, signalName)
rospub.add(data_type,rospub_signalName,topicname)
plug(entity.signal(signalName), rospub.signal(rospub_signalName))
if robot is not None:
robot.device.after.addSignal( '{0}.{1}'.format(entity.name, signalName) )
def create_dummy_dcm_estimator(robot):
from math import sqrt
estimator = DummyDcmEstimator("dummy")
robot.dynamic.com.recompute(0)
mass = robot.dynamic.data.mass[0]
h = robot.dynamic.com.value[2]
g = 9.81
omega = sqrt(g/h)
estimator.mass.value = mass
estimator.omega.value = omega
plug(robot.dynamic.com,estimator.com)
plug(robot.dynamic.momenta,estimator.momenta)
estimator.init()
return estimator
def create_cdc_dcm_estimator(robot):
from math import sqrt
estimator = DummyDcmEstimator("dummy")
robot.dynamic.com.recompute(0)
h = robot.dynamic.com.value[2]
g = 9.81
omega = sqrt(g/h)
estimator.mass.value = 1.0
estimator.omega.value = omega
plug(robot.cdc_estimator.c, estimator.com)
plug(robot.cdc_estimator.dc,estimator.momenta)
estimator.init()
return estimator
def create_com_admittance_controller(Kp,dt,robot):
controller = ComAdmittanceController("comAdmCtrl")
controller.Kp.value = Kp
plug(robot.dynamic.zmp,controller.zmp)
robot.dynamic.zmp.recompute(0)
controller.zmpDes.value = robot.dynamic.zmp.value
controller.ddcomDes.value = [0.0,0.0,0.0]
controller.init(dt)
robot.dynamic.com.recompute(0)
controller.setState(robot.dynamic.com.value,[0.0,0.0,0.0])
return controller
def create_dcm_controller(Kp,Ki,dt,robot,dcmSignal):
from math import sqrt
controller = DcmController("dcmCtrl")
robot.dynamic.com.recompute(0)
mass = robot.dynamic.data.mass[0]
h = robot.dynamic.com.value[2]
g = 9.81
omega = sqrt(g/h)
controller.Kp.value = Kp
controller.Ki.value = Ki
controller.decayFactor.value = 0.2
controller.mass.value = mass
controller.omega.value = omega
plug(robot.dynamic.com,controller.com)
plug(dcmSignal,controller.dcm)
robot.dynamic.zmp.recompute(0)
controller.zmpDes.value = robot.dynamic.zmp.value
controller.dcmDes.value = robot.dynamic.zmp.value
controller.init(dt)
return controller
def create_dcm_com_controller(Kp,Ki,dt,robot,dcmSignal):
from math import sqrt
controller = DcmComController("dcmComCtrl")
robot.dynamic.com.recompute(0)
mass = robot.dynamic.data.mass[0]
h = robot.dynamic.com.value[2]
g = 9.81
omega = sqrt(g/h)
controller.Kp.value = Kp
controller.Ki.value = Ki
controller.decayFactor.value = 0.2
controller.mass.value = mass
controller.omega.value = omega
controller.ddcomDes.value = [0.0,0.0,0.0]
plug(dcmSignal,controller.dcm)
robot.dynamic.com.recompute(0)
controller.comDes.value = robot.dynamic.com.value
controller.dcmDes.value = (robot.dynamic.com.value[0], robot.dynamic.com.value[1], 0.0)
controller.init(dt)
return controller
def create_parameter_server(conf, dt, robot_name='robot'):
param_server = ParameterServer("param_server");
# Init should be called before addCtrlMode
# because the size of state vector must be known.
param_server.init(dt, conf.urdfFileName, robot_name)
# Set the map from joint name to joint ID
for key in conf.mapJointNameToID:
param_server.setNameToId(key,conf.mapJointNameToID[key])
# Set the map joint limits for each id
for key in conf.mapJointLimits:
param_server.setJointLimitsFromId(key,conf.mapJointLimits[key][0], \
conf.mapJointLimits[key][1])
# Set the force limits for each id
for key in conf.mapForceIdToForceLimits:
param_server.setForceLimitsFromId(key,tuple(conf.mapForceIdToForceLimits[key][0]), \
tuple(conf.mapForceIdToForceLimits[key][1]))
# Set the force sensor id for each sensor name
for key in conf.mapNameToForceId:
param_server.setForceNameToForceId(key,conf.mapNameToForceId[key])
# Set the map from the urdf joint list to the sot joint list
param_server.setJointsUrdfToSot(conf.urdftosot)
# Set the foot frame name
for key in conf.footFrameNames:
param_server.setFootFrameName(key,conf.footFrameNames[key])
# Set IMU hosting joint name
param_server.setImuJointName(conf.ImuJointName)
param_server.setRightFootForceSensorXYZ(conf.rightFootSensorXYZ);
param_server.setRightFootSoleXYZ(conf.rightFootSoleXYZ);
return param_server;
def create_example(robot_name='robot', firstAdd = 0., secondAdd = 0.):
example = Example('example')
example.firstAddend.value = firstAdd
example.secondAddend.value = secondAdd
example.init(robot_name)
return example
def create_dcm_estimator(robot, dt, robot_name='robot'):
dcm_estimator = DcmEstimator('dcm_estimator')
dcm_estimator.init(dt, robot_name)
plug(robot.base_estimator.q, dcm_estimator.q)
plug(robot.base_estimator.v, dcm_estimator.v)
return dcm_estimator
def create_zmp_estimator(robot,filter=False):
estimator = SimpleZmpEstimator("zmpEst")
plug(robot.dynamic.LF,estimator.poseLeft)
plug(robot.dynamic.RF,estimator.poseRight)
if filter and hasattr(robot,'device_filters'):
plug(robot.device_filters.ft_LF_filter.x_filtered,estimator.wrenchLeft)
plug(robot.device_filters.ft_RF_filter.x_filtered,estimator.wrenchRight)
else:
plug(robot.device.forceLLEG,estimator.wrenchLeft)
plug(robot.device.forceRLEG,estimator.wrenchRight)
estimator.init()
return estimator
def create_ft_calibrator(robot,conf):
ftc = FtCalibration('ftc')
ftc.init(robot_name)
ftc.setRightFootWeight(conf.rfw)
ftc.setLeftFootWeight(conf.lfw)
plug(robot.device_filters.ft_RF_filter.x_filtered, ftc.right_foot_force_in)
plug(robot.device_filters.ft_LF_filter.x_filtered, ftc.left_foot_force_in)
return ftc
\ No newline at end of file
python/sot_talos_balance/extract_torque_motorenc_joint_enc_batteries.py deleted 100644 → 0
View file @ ca7bad3e
import rosbag
import rospy
import subprocess, yaml
bagname= '2017-07-24-08-04-30.bag'
info_dict = yaml.load(subprocess.Popen(['rosbag', 'info', '--yaml', bagname], stdout=subprocess.PIPE).communicate()[0])
bag = rosbag.Bag(bagname)
ps=bag.read_messages(topics=['/power_status'])
introdata=bag.read_messages(topics=['/data'])
start_time=rospy.Time()
class message:
"""Store torque, motor encoder, joint encoder"""
motorenc = 0
jointenc = 0
torquesensor = 0
current = 0
def extract_data():
jointnames = ('torso_1','torso_2', 'head_1','head_2', \
'arm_left_1','arm_left_2','arm_left_3','arm_left_4','arm_left_5','arm_left_6','arm_left_7', \
'arm_right_1','arm_right_2','arm_right_3','arm_right_4','arm_right_5','arm_right_6','arm_right_7', \
'leg_left_1','leg_left_2','leg_left_3','leg_left_4','leg_left_5','leg_left_6','leg_left_7', \
'leg_right_1','leg_right_2','leg_right_3','leg_right_4','leg_right_5','leg_right_6','leg_right_7')
filesnames={}
files={}
for jn in jointnames:
filesnames[jn] = '/BackupFiles/devel-src/data/'+ jn + '_me_je.dat'
files[jn] = open(filesnames[jn],'w')
start=1
for apw in introdata:
messages={}
if start==1:
start_time=apw.timestamp
start=0
for jn in jointnames:
messages[jn] = message()
for adbdata in apw.message.doubles:
for jn in jointnames:
if jn + '_motor_position'==adbdata.name:
messages[jn].motorenc=adbdata.value
if jn + '_motor_absolute_encoder_position'==adbdata.name:
messages[jn].jointenc=adbdata.value
if jn + '_motor_torque_sensor'==adbdata.name:
messages[jn].torquesensor=adbdata.value
if jn + '_motor_effort'==adbdata.name:
messages[jn].current=adbdata.value
for jn in jointnames:
afile = files[jn]
diff_secs = apw.timestamp.secs-start_time.secs
diff_nanosecs=apw.timestamp.nsecs-start_time.nsecs
if (diff_nanosecs<0):
diff_nanosecs=1000000000+diff_nanosecs
diff_secs= diff_secs-1
afile.write(str(messages[jn].motorenc) + ' ' + \
str(messages[jn].jointenc) + ' ' + \
str(messages[jn].torquesensor) + ' ' + \
str(messages[jn].current) + ' ' + \
str(diff_secs)+'.'+str(diff_nanosecs).zfill(9)+'\n');
for jn in jointnames:
files[jn].close()
def extract_batteries_data():
f=open('/BackupFiles/devel-src/data/batteries.dat','w')
introdata=bag.read_messages(topics=['/data'])
apw=introdata.next()
start_time=apw.timestamp
for aps in ps:
diff_secs = aps.timestamp.secs-start_time.secs
diff_nanosecs=aps.timestamp.nsecs-start_time.nsecs
if (diff_nanosecs<0):
diff_nanosecs=1000000000+diff_nanosecs
diff_secs= diff_secs-1
f.write(str(aps.message.charge) + ' ' +\
str(aps.message.current) + ' ' +\
str(aps.message.input_voltage) + ' ' +\
str(aps.message.battery_voltage) + ' ' +\
str(aps.message.regulator_voltage) + ' '+ \
str(aps.message.lower_body_voltage) + ' '+ \
str(aps.message.charger_voltage) + ' ' +\
str(aps.message.upper_body_voltage) + ' ' +\
str(diff_secs)+'.'+str(diff_nanosecs).zfill(9)+'\n')
f.close()
# main
extract_data()
extract_batteries_data()
bag.close()
python/sot_talos_balance/motor_parameters.py deleted 100644 → 0
View file @ ca7bad3e
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 9 13:55:16 2015
@author: adelpret
"""
from numpy import zeros as zeros
NJ = 32;
GEAR_RATIOS = (150.0, 101.0, 100.0, 144.0, 100.0, 101.0,
150.0, 101.0, 100.0, 144.0, 100.0, 101.0,
100.0, 100.0,
100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0,
100.0,
100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0,
100.0,
100.0,200.0);
ROTOR_INERTIAS = (8.6e-6, 34e-6, 34.0e-5, 61.0e-5, 34.0e-6, 21.0e-6,
8.6e-6, 34e-6, 34.0e-5, 61.0e-5, 34.0e-6, 21.0e-6,
34.0e-6, 34.0e-6,
34.0e-6, 34.0e-6, 21.0e-6, 21.0e-6, 5.8e-6, 1.3e-6, 1.3e-07,
1.3,
34.0e-6, 34.0e-6, 21.0e-6, 21.0e-6, 5.8e-6, 1.3e-6, 1.3e-07,
1.3,
1.3,1.3);
### New motor parameters with current measurment (used by ForceTorqueEstimator and JointTorqueController) ###
Kt_p=zeros(NJ)+1.0;
Kt_n=zeros(NJ)+1.0;
Kf_p=zeros(NJ);
Kf_n=zeros(NJ);
Kv_p=zeros(NJ);
Kv_n=zeros(NJ);
Ka_p=zeros(NJ);
Ka_n=zeros(NJ);
deadzone=zeros(NJ);
K_bemf=zeros(NJ); # used by ControlManager to compensate back-EMF
cur_sens_gains = zeros(NJ)+1.0;
cur_sens_gains[0] = 1.256534 #Using 20161114_135332_rhy_static
deadzone[0] = 0.588671 #Using 20161114_135332_rhy_static
deadzone[0] = 0.575506 #Using 20161114_143152_rhy_vel
K_bemf[0] = 1.169309 # [Amp/Rad.s-1] Using 20161114_143152_rhy_vel
Kt_p[0] = 0.055302 #Using 20161114_135332_rhy_static
Kt_n[0] = 0.053055 #Using 20161114_135332_rhy_static
Kv_p[0] = 0.777466 #Using 20161114_143152_rhy_vel
Kv_n[0] = 0.779525 #Using 20161114_143152_rhy_vel
Kf_p[0] = 0.463513 #Using 20161114_143152_rhy_vel
Kf_n[0] = 0.231212 #Using 20161114_143152_rhy_vel
cur_sens_gains[1] = 0.997354 #Using 20161114_144232_rhr_static
deadzone[1] = 0.571099 #Using 20161114_144232_rhr_static
deadzone[1] = 0.564055 #Using 20161114_150356_rhr_vel
K_bemf[1] = 1.120005 # [Amp/Rad.s-1] Using 20161114_150356_rhr_vel
Kt_p[1] = 0.061092 #Using 20161114_144232_rhr_static
Kt_n[1] = 0.057921 #Using 20161114_144232_rhr_static
Kv_p[1] = 0.583937 #Using 20161114_150356_rhr_vel
Kv_n[1] = 0.471164 #Using 20161114_150356_rhr_vel
Kf_p[1] = 0.139620 #Using 20161114_150356_rhr_vel
Kf_n[1] = 0.622044 #Using 20161114_150356_rhr_vel
cur_sens_gains[2] = 0.884396 #Using 20161114_150722_rhp_static
deadzone[2] = 0.671750 #Using 20161114_150722_rhp_static
deadzone[2] = 0.461780 #Using 20161114_151812_rhp_vel
K_bemf[2] = 1.171485 # [Amp/Rad.s-1] Using 20161114_151812_rhp_vel
Kt_p[2] = 0.093924 #Using 20161114_150722_rhp_static
Kt_n[2] = 0.074335 #Using 20161114_150722_rhp_static
Kv_p[2] = 0.236554 #Using 20161114_151812_rhp_vel
Kv_n[2] = 0.200306 #Using 20161114_151812_rhp_vel
Kf_p[2] = 0.322370 #Using 20161114_151812_rhp_vel
Kf_n[2] = 0.955010 #Using 20161114_151812_rhp_vel
cur_sens_gains[3] = 0.898618 #Using 20161114_152140_rk_static
deadzone[3] = 0.611894 #Using 20161114_152140_rk_static
deadzone[3] = 0.594111 #Using 20161114_153220_rk_vel
K_bemf[3] = 1.062028 # [Amp/Rad.s-1] Using 20161114_153220_rk_vel
Kt_p[3] = 0.074025 #Using 20161114_152140_rk_static
Kt_n[3] = 0.070763 #Using 20161114_152140_rk_static
Kv_p[3] = 0.310712 #Using 20161114_153220_rk_vel
Kv_n[3] = 0.302653 #Using 20161114_153220_rk_vel
Kf_p[3] = 0.562304 #Using 20161114_153220_rk_vel
Kf_n[3] = 0.590003 #Using 20161114_153220_rk_vel
cur_sens_gains[4] = 0.989997 #Using 20161114_153739_rap_static
deadzone[4] = 0.485198 #Using 20161114_153739_rap_static
deadzone[4] = 0.615647 #Using 20161114_154559_rap_vel
K_bemf[4] = 0.787735 # [Amp/Rad.s-1] Using 20161114_154559_rap_vel
Kt_p[4] = 0.082806 #Using 20161114_153739_rap_static
Kt_n[4] = 0.088764 #Using 20161114_153739_rap_static
Kv_p[4] = 0.222277 #Using 20161114_154559_rap_vel
Kv_n[4] = 0.225662 #Using 20161114_154559_rap_vel
Kf_p[4] = 0.559252 #Using 20161114_154559_rap_vel
Kf_n[4] = 0.259040 #Using 20161114_154559_rap_vel
cur_sens_gains[5] = 1.007775 #Using 20161114_154945_rar_static
deadzone[5] = 0.595250 #Using 20161114_154945_rar_static
deadzone[5] = 0.617705 #Using 20161114_160038_rar_vel
K_bemf[5] = 0.456973 # [Amp/Rad.s-1] Using 20161114_160038_rar_vel
Kt_p[5] = 0.155565 #Using 20161114_154945_rar_static
Kt_n[5] = 0.156830 #Using 20161114_154945_rar_static
Kv_p[5] = 0.511848 #Using 20161114_160038_rar_vel
Kv_n[5] = 0.517686 #Using 20161114_160038_rar_vel
Kf_p[5] = 0.564020 #Using 20161114_160038_rar_vel
Kf_n[5] = 0.321447 #Using 20161114_160038_rar_vel
cur_sens_gains[6]= 1.250000 #Using 20171002_163413_lhy_static (forced by hand)
deadzone[6] = 0.295741 #Using 20171002_163413_lhy_static
deadzone[6] = 0.627764 #Using 20171002_151718_lhy_vel
K_bemf[6] = 1.168112 # [Amp/Rad.s-1] Using 20171002_151718_lhy_vel
Kt_p[6] = 0.057042 #Using 20171002_163413_lhy_static
Kt_n[6] = 0.054307 #Using 20171002_163413_lhy_static
Kv_p[6] = 0.833485 #Using 20171002_151718_lhy_vel
Kv_n[6] = 0.835893 #Using 20171002_151718_lhy_vel
Kf_p[6] = 0.444239 #Using 20171002_151718_lhy_vel
Kf_n[6] = 0.251280 #Using 20171002_151718_lhy_vel
cur_sens_gains[7]= 1.004305 #Using 20171002_164436_lhr_static
deadzone[7] = 0.640682 #Using 20171002_164436_lhr_static
deadzone[7] = 0.601328 #Using 20171002_153334_lhr_vel
K_bemf[7] = 1.161687 # [Amp/Rad.s-1] Using 20171002_153334_lhr_vel
Kt_p[7] = 0.069472 #Using 20171002_164436_lhr_static
Kt_n[7] = 0.058696 #Using 20171002_164436_lhr_static
Kv_p[7] = 0.363950 #Using 20171002_153334_lhr_vel
Kv_n[7] = 0.421599 #Using 20171002_153334_lhr_vel
Kf_p[7] = 0.606632 #Using 20171002_153334_lhr_vel
Kf_n[7] = 0.095194 #Using 20171002_153334_lhr_vel
cur_sens_gains[8]= 0.901796 #Using 20171002_165335_lhp_static
deadzone[8] = 0.456045 #Using 20171002_165335_lhp_static
deadzone[8] = 0.607772 #Using 20171002_154449_lhp_vel
K_bemf[8] = 0.975598 # [Amp/Rad.s-1] Using 20171002_154449_lhp_vel
Kt_p[8] = 0.076617 #Using 20171002_165335_lhp_static
Kt_n[8] = 0.087909 #Using 20171002_165335_lhp_static
Kv_p[8] = 0.122464 #Using 20171002_154449_lhp_vel
Kv_n[8] = 0.181315 #Using 20171002_154449_lhp_vel
Kf_p[8] = 0.597530 #Using 20171002_154449_lhp_vel
Kf_n[8] = 0.375285 #Using 20171002_154449_lhp_vel
cur_sens_gains[9]= 0.897802 #Using 20170113_151748_lk_static
deadzone[9] = 0.628737 #Using 20170113_151748_lk_static
deadzone[9] = 0.621934 #Using 20170113_152924_lk_const_vel
K_bemf[9] = 1.057187 # [Amp/Rad.s-1] Using 20170113_152924_lk_const_vel
Kt_p[9] = 0.071932 #Using 20170113_151748_lk_static
Kt_n[9] = 0.065418 #Using 20170113_151748_lk_static
Kv_p[9] = 0.331607 #Using 20170113_152924_lk_const_vel
Kv_n[9] = 0.351136 #Using 20170113_152924_lk_const_vel
Kf_p[9] = 0.363123 #Using 20170113_152924_lk_const_vel
Kf_n[9] = 0.699079 #Using 20170113_152924_lk_const_vel
cur_sens_gains[10]= 0.978598 #Using 20170113_154007_lap_static
deadzone[10] = 0.577901 #Using 20170113_154007_lap_static
deadzone[10] = 0.582928 #Using 20170113_154834_lap_const_vel
K_bemf[10] = 0.806259 # [Amp/Rad.s-1] Using 20170113_154834_lap_const_vel
Kt_p[10] = 0.092789 #Using 20170113_154007_lap_static
Kt_n[10] = 0.093060 #Using 20170113_154007_lap_static
Kv_p[10] = 0.542156 #Using 20170113_154834_lap_const_vel
Kv_n[10] = 0.469827 #Using 20170113_154834_lap_const_vel
Kf_p[10] = 0.063421 #Using 20170113_154834_lap_const_vel
Kf_n[10] = 0.806020 #Using 20170113_154834_lap_const_vel
cur_sens_gains[11]= 0.995794 #Using 20170113_155150_lar_static
deadzone[11] = 0.643120 #Using 20170113_155150_lar_static
deadzone[11] = 0.645305 #Using 20170113_160057_lar_const_vel
K_bemf[11] = 0.474213 # [Amp/Rad.s-1] Using 20170113_160057_lar_const_vel
Kt_p[11] = 0.147175 #Using 20170113_155150_lar_static
Kt_n[11] = 0.150098 #Using 20170113_155150_lar_static
Kv_p[11] = 0.342627 #Using 20170113_160057_lar_const_vel
Kv_n[11] = 0.351680 #Using 20170113_160057_lar_const_vel
Kf_p[11] = 0.263935 #Using 20170113_160057_lar_const_vel
Kf_n[11] = 0.430722 #Using 20170113_160057_lar_const_vel
# take averages for p and n
for i in range(NJ):
Kt_av = (Kt_n[i] + Kt_p[i])/2
Kt_n[i]=Kt_av
Kt_p[i]=Kt_av
Kv_av = (Kv_n[i] + Kv_p[i])/2
Kv_n[i]=Kv_av
Kv_p[i]=Kv_av
Kf_av = (Kf_n[i] + Kf_p[i])/2
Kf_n[i]=Kf_av
Kf_p[i]=Kf_av
python/sot_talos_balance/talos/admittance_ctrl_conf.py deleted 100644 → 0
View file @ ca7bad3e
from math import sqrt
import numpy as np
kp_force = 6*(0.0,)
ki_force = 6*(0.0,)
kp_vel = 32*(0.0,)
ki_vel = 32*(0.0,)
force_integral_saturation = (0.0, 0.0, 160.0, 20.0, 20.0, 0.0)
force_integral_deadzone = (5.0, 5.0, 5.0, 0.5, 0.5, 0.5)
python/sot_talos_balance/talos/balance_ctrl_conf.py deleted 100644 → 0
View file @ ca7bad3e
from math import sqrt
import numpy as np
''' *********************** USER-PARAMETERS *********************** '''
COM_DES = (0.00, 0.0, 0.81);
# CONTROLLER GAINS
NJ = 32;
kp_posture = NJ*(100.0,); # proportional gain of postural task
kd_posture = NJ*(0*sqrt(kp_posture[0]),);
kp_pos = NJ*(0.0,); # proportional gain of position controller
kd_pos = NJ*(0*sqrt(kp_pos[0]),);
kp_constr = .0*1.0; # constraint proportional feedback gain
kd_constr = 0*sqrt(kp_constr); # constraint derivative feedback gain
kp_com = .0*30.0;
kd_com = 0.0;
kp_feet = .0*30.0;
kd_feet = 0.0;
kp_admittance = (0,0,0,.0*1,.0*1,0.);
ki_admittance = 6*(0.0,);
constraint_mask = np.array([True, True, True, True, True, True]).T;
ee_mask = np.array([True, True, True, True, True, True]).T;
# CONTROLLER WEIGTHS
w_com = .0*1.0;
w_feet = .0*1.0;
w_posture = 1e-3; # weight of postural task
w_forces = .0*1e-6;
w_base_orientation = 0.0;
w_torques = 0.0;
# CONTACT PARAMETERS
RIGHT_FOOT_SIZES = (0.130, -0.100, 0.056, -0.075); # pos x, neg x, pos y, neg y size
LEFT_FOOT_SIZES = (0.130, -0.100, 0.075, -0.056); # pos x, neg x, pos y, neg y size
RIGHT_FOOT_SIZES = (0.130, -0.100, 0.056, -0.056); # pos x, neg x, pos y, neg y size
RIGHT_FOOT_CONTACT_POINTS = ((RIGHT_FOOT_SIZES[0], RIGHT_FOOT_SIZES[0], RIGHT_FOOT_SIZES[1], RIGHT_FOOT_SIZES[1]),
(RIGHT_FOOT_SIZES[3], RIGHT_FOOT_SIZES[2], RIGHT_FOOT_SIZES[3], RIGHT_FOOT_SIZES[2]),
(-0.105, -0.105, -0.105, -0.105)); # contact points in local reference frame
LEFT_FOOT_CONTACT_POINTS = np.matrix([[LEFT_FOOT_SIZES[0], LEFT_FOOT_SIZES[3], -0.105],
[LEFT_FOOT_SIZES[0], LEFT_FOOT_SIZES[2], -0.105],
[LEFT_FOOT_SIZES[1], LEFT_FOOT_SIZES[3], -0.105],
[LEFT_FOOT_SIZES[1], LEFT_FOOT_SIZES[2], -0.105]]).T # contact points in local reference frame
FOOT_CONTACT_NORMAL = (0.0, 0.0, 1.0);
mu = np.array([0.3, 0.1]); # force and moment friction coefficient
fMin = 1.0; # minimum normal force
fMax = 1e3; # maximum normal force
RF_FORCE_DES = (0, 0, 300, 0, 0, 0.);
LF_FORCE_DES = (0, 0, 300, 0, 0, 0.);
python/sot_talos_balance/talos/balance_ctrl_sim_conf.py deleted 100644 → 0
View file @ ca7bad3e
from balance_ctrl_conf import *
# CONTROLLER GAINS
kp_posture = NJ*(400.0,);
kd_posture = NJ*(sqrt(kp_posture[0]),);
kd_constr = 0.0*2*sqrt(kp_constr); # constraint derivative feedback gain
kd_com = 0.0*2*sqrt(kp_com);
kd_feet = 0.0*2*sqrt(kp_feet);
python/sot_talos_balance/talos/base_estimator_conf.py deleted 100644 → 0
View file @ ca7bad3e
''' *********************** USER-PARAMETERS OF BASE ESTIMATOR *********************** '''
# K = (4034, 23770, 239018, 707, 502, 936); #HRP2
# K = (1., 1., 1., 1., 1., 1.);
K = (1e6, 1e6, 1e6, 1e6, 1e6, 1e6);
std_dev_zmp = 0.02
std_dev_fz = 50.
normal_force_margin = 30.
zmp_margin = 0.002
w_imu = 0.0;
beta = 0.00329
K_fb_feet_poses = 0.0; # gain used for updating foot positions
RIGHT_FOOT_SIZES = (0.130, -0.100, 0.056, -0.075); # pos x, neg x, pos y, neg y size
LEFT_FOOT_SIZES = (0.130, -0.100, 0.075, -0.056); # pos x, neg x, pos y, neg y size
w_lf_in = 1.
w_rf_in = 1.
#mu = 0.3; # force friction coefficient
python/sot_talos_balance/talos/base_estimator_sim_conf.py deleted 100644 → 0
View file @ ca7bad3e
''' *********************** USER-PARAMETERS OF BASE ESTIMATOR *********************** '''
from base_estimator_conf import *
K = 6*(1e7,);
python/sot_talos_balance/talos/control_manager_sim_conf.py deleted 100644 → 0
View file @ ca7bad3e
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 9 13:55:16 2015
@author: adelpret
"""
from control_manager_conf import *
TAU_MAX = 1e6; # security check of ControlManager
CURRENT_MAX = 1e6; # security check of ControlManager
CTRL_MAX = 1e6; # max desired current (security check of ControlManager)
python/sot_talos_balance/talos/current_controller_conf.py deleted 100644 → 0
View file @ ca7bad3e
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 9 13:55:16 2015
@author: adelpret
"""
import numpy as np
NJ = 32;
CURRENT_OFFSET_ITERS = 200; # Number of itertion while control is disabled to calibrate current sensors
CURRENT_MAX = 8.0; # max motor current (security check of ControlManager)
CTRL_MAX = 10.0; # max desired current (security check of ControlManager)
CTRL_SATURATION = 2047; # saturation of the control signal
IN_OUT_GAIN = 102.4; # factor to convert from a [-20.0 ; 20.0] Ampers value to the [-2048 ; 2048] 12bit DAC register
percentage_dead_zone_compensation = NJ*[0.,]; # percentage of dead zone to compensate (used by ControlManager)
i_max_dz_comp = NJ*[0.05,]; # value of current tracking error at which deadzone is completely compensated
percentage_bemf_compensation = NJ*[0.,]
current_sensor_offsets_low_level = NJ*[0.,]
kp_current = NJ*[0.,];
ki_current = np.array(NJ*[0.,]);
percentage_bemf_compensation[0] = 0.9
percentage_bemf_compensation[1] = 0.9 # a bit unstable
percentage_bemf_compensation[2] = 0.9
percentage_bemf_compensation[3] = 0.9
percentage_bemf_compensation[4] = 1.0
percentage_bemf_compensation[5] = 0.9
percentage_bemf_compensation[6] = 0.9
percentage_bemf_compensation[7] = 0.9
percentage_bemf_compensation[8] = 0.9
percentage_bemf_compensation[9] = 0.9
percentage_bemf_compensation[10] = 0.9
percentage_bemf_compensation[11] = 0.9
percentage_dead_zone_compensation[0] = 0.8
percentage_dead_zone_compensation[1] = 0.8
percentage_dead_zone_compensation[2] = 0.8 # could go up to 1.2
percentage_dead_zone_compensation[3] = 0.8
percentage_dead_zone_compensation[4] = 0.8 # a bit unstable
percentage_dead_zone_compensation[5] = 0.8
percentage_dead_zone_compensation[6] = 0.8
percentage_dead_zone_compensation[7] = 0.8
percentage_dead_zone_compensation[8] = 0.8
percentage_dead_zone_compensation[9] = 0.8
percentage_dead_zone_compensation[10] = 0.8 # very asymettric error, a bit unstable on negative currents, may need investigation
percentage_dead_zone_compensation[11] = 0.8 # getting some vibrations for negative errors, may need investigation
i_max_dz_comp[0] = 0.03
current_sensor_offsets_low_level[0] = 0.04
current_sensor_offsets_low_level[2] = -0.05
current_sensor_offsets_low_level[5] = 0.02
current_sensor_offsets_low_level[6] = 0.01
current_sensor_offsets_low_level[7] = 0.01
current_sensor_offsets_low_level[8] = -0.01
current_sensor_offsets_low_level[10] = -0.11
current_sensor_offsets_low_level[11] = -0.04
ki_current[0] = 3e-3 # could go higher
ki_current[1] = 3e-3 # could go higher
ki_current[2] = 1e-3 # could probably go higher
ki_current[3] = 3e-3
ki_current[4] = 1e-3 # could probably go higher
ki_current[5] = 3e-3 # could probably go higher
ki_current[6] = 1e-3
ki_current[7] = 1e-3
ki_current[8] = 1e-3
ki_current[9] = 1e-3
ki_current[10] = 1e-3
ki_current[11] = 1e-3
#ki_current[:] = 0.0