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

[test_zmpEstimator] Added F/T calibration

parent 66d86c4c
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
plot/plot_zmp.xml 0 → 100644
+ 103
− 0
View file @ d631df20
<?xml version='1.0' encoding='UTF-8'?>
<root>
<tabbed_widget parent="main_window" name="Main Window">
<plotmatrix rows="2" columns="3" tab_name="plot">
<plot col="0" row="0">
<range top="0.000748" bottom="-0.010062" left="11.716000" right="16.709000"/>
<limitY/>
<curve B="255" G="0" R="0" name="/sot/zmpEst/zmp/data.0" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot col="0" row="1">
<range top="0.002393" bottom="-0.013736" left="11.716000" right="16.709000"/>
<limitY/>
<curve B="164" G="160" R="160" name="/sot/robot_dynamic/com/data.0" custom_transform="noTransform"/>
<curve B="255" G="0" R="255" name="/sot/robot_dynamic/zmp/data.0" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot col="1" row="0">
<range top="0.070737" bottom="-0.046684" left="11.716000" right="16.709000"/>
<limitY/>
<curve B="0" G="128" R="0" name="/sot/zmpEst/zmp/data.1" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot col="1" row="1">
<range top="0.057134" bottom="-0.032148" left="11.716000" right="16.709000"/>
<limitY/>
<curve B="0" G="128" R="128" name="/sot/robot_dynamic/com/data.1" custom_transform="noTransform"/>
<curve B="128" G="0" R="0" name="/sot/robot_dynamic/zmp/data.1" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot col="2" row="0">
<range top="0.000047" bottom="-0.000097" left="11.716000" right="16.709000"/>
<limitY/>
<curve B="0" G="0" R="255" name="/sot/zmpEst/zmp/data.2" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot col="2" row="1">
<range top="0.100000" bottom="-0.100000" left="11.716000" right="16.709000"/>
<limitY/>
<curve B="128" G="128" R="0" name="/sot/robot_dynamic/zmp/data.2" 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="/sot/robot_dynamic/zmp;/sot/zmpEst/zmp;/sot/robot_dynamic/com"/>
</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_zmpEstimator.py
+ 19
− 4
View file @ d631df20
from sot_talos_balance.create_entities_utils import *
import sot_talos_balance.talos.parameter_server_conf as param_server_conf
import sot_talos_balance.talos.ft_calibration_conf as ft_conf
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT
......@@ -14,6 +16,9 @@ robot.comTrajGen = create_com_trajectory_generator(dt,robot);
# -------------------------- ESTIMATION --------------------------
# --- Parameter server
robot.param_server = create_parameter_server(param_server_conf,dt)
# --- filters
filters = Bunch();
filters.ft_RF_filter = create_butter_lp_filter_Wn_04_N_2("ft_RF_filter", dt, 6)
......@@ -22,16 +27,17 @@ plug(robot.device.forceRLEG, filters.ft_RF_filter.x)
plug(robot.device.forceLLEG, filters.ft_LF_filter.x)
robot.device_filters = filters
# --- Force calibration
robot.ftc = create_ft_calibrator(robot,ft_conf)
# --- ZMP estimation (disconnected)
zmp_estimator = SimpleZmpEstimator("zmpEst")
robot.dynamic.createOpPoint('sole_LF','left_sole_link')
robot.dynamic.createOpPoint('sole_RF','right_sole_link')
plug(robot.dynamic.sole_LF,zmp_estimator.poseLeft)
plug(robot.dynamic.sole_RF,zmp_estimator.poseRight)
plug(robot.device_filters.ft_LF_filter.x_filtered,zmp_estimator.wrenchLeft)
plug(robot.device_filters.ft_RF_filter.x_filtered,zmp_estimator.wrenchRight)
#plug(robot.device.forceLLEG,zmp_estimator.wrenchLeft)
#plug(robot.device.forceRLEG,zmp_estimator.wrenchRight)
plug(robot.ftc.left_foot_force_out,zmp_estimator.wrenchLeft)
plug(robot.ftc.right_foot_force_out,zmp_estimator.wrenchRight)
zmp_estimator.init()
robot.zmp_estimator = zmp_estimator
......@@ -88,6 +94,15 @@ create_topic(robot.publisher, robot.dynamic, 'zmp', robot = robot, data_type='ve
create_topic(robot.publisher, robot.device, 'forceLLEG', robot = robot, data_type='vector') # force on left foot
create_topic(robot.publisher, robot.device, 'forceRLEG', robot = robot, data_type='vector') # force on right foot
#create_topic(robot.publisher, robot.device, 'forceLLEG', robot = robot, data_type='vector') # measured left wrench
#create_topic(robot.publisher, robot.device, 'forceRLEG', robot = robot, data_type='vector') # measured right wrench
#create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
#create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot = robot, data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot = robot, data_type='vector') # calibrated right wrench
# --- TRACER
robot.tracer = TracerRealTime("zmp_tracer")
robot.tracer.setBufferSize(80*(2**20))
......
python/sot_talos_balance/test/test_zmpEstimator.py
+ 4
− 1
View file @ d631df20
from sot_talos_balance.utils.run_test_utils import run_test, runCommandClient, evalCommandClient
from sot_talos_balance.utils.run_test_utils import *
from time import sleep
import matplotlib.pyplot as plt
......@@ -6,6 +6,9 @@ import numpy as np
run_test('appli_zmpEstimator.py')
run_ft_calibration('robot.ftc')
raw_input("Wait before running the test")
sleep(2.0)
runCommandClient('robot.comTrajGen.move(1,-0.025,1.0)')
sleep(20.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