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

[test_dcmZmpControl_estimator] Raising foot

parent 34df6623
No related branches found
Tags v1.2.0
No related merge requests found
Hide whitespace changes
Inline Side-by-side
plot/plot_sot_zmp.xml 0 → 100644
+ 103
− 0
View file @ ea904413
<?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>
plot/plot_zmp.xml
+ 15
− 17
View file @ ea904413
<?xml version='1.0' encoding='UTF-8'?>
<root>
<tabbed_widget parent="main_window" name="Main Window">
<plotmatrix rows="2" columns="3" tab_name="plot">
<tabbed_widget name="Main Window" parent="main_window">
<plotmatrix tab_name="plot" rows="2" columns="3">
<plot col="0" row="0">
<range top="0.000748" bottom="-0.010062" left="11.716000" right="16.709000"/>
<range left="14.262000" right="19.255000" bottom="0.000035" top="0.000037"/>
<limitY/>
<curve B="255" G="0" R="0" name="/sot/zmpEst/zmp/data.0" custom_transform="noTransform"/>
<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 top="0.002393" bottom="-0.013736" left="11.716000" right="16.709000"/>
<range left="14.262000" right="19.255000" bottom="-0.002360" top="-0.002360"/>
<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"/>
<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 top="0.070737" bottom="-0.046684" left="11.716000" right="16.709000"/>
<range left="14.262000" right="19.255000" bottom="-0.092751" top="-0.092750"/>
<limitY/>
<curve B="0" G="128" R="0" name="/sot/zmpEst/zmp/data.1" custom_transform="noTransform"/>
<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 top="0.057134" bottom="-0.032148" left="11.716000" right="16.709000"/>
<range left="14.262000" right="19.255000" bottom="-0.083197" top="-0.083196"/>
<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"/>
<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 top="0.000047" bottom="-0.000097" left="11.716000" right="16.709000"/>
<range left="14.262000" right="19.255000" bottom="0.000100" top="0.000100"/>
<limitY/>
<curve B="0" G="0" R="255" name="/sot/zmpEst/zmp/data.2" custom_transform="noTransform"/>
<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 top="0.100000" bottom="-0.100000" left="11.716000" right="16.709000"/>
<range left="14.262000" right="19.255000" bottom="0.874043" top="0.874049"/>
<limitY/>
<curve B="128" G="128" R="0" name="/sot/robot_dynamic/zmp/data.2" custom_transform="noTransform"/>
<curve G="128" custom_transform="noTransform" R="0" B="0" name="/sot/cdc_estimator/c/data.2"/>
<transform value="noTransform"/>
</plot>
</plotmatrix>
......@@ -55,7 +53,7 @@
<no_params/>
</DataLoad_ULog>
<ROS_Topic_Streamer>
<selected_topics list="/sot/robot_dynamic/zmp;/sot/zmpEst/zmp;/sot/robot_dynamic/com"/>
<selected_topics list="/sot/cdc_estimator/c;/sot/zmpEst/zmp"/>
</ROS_Topic_Streamer>
<RosoutPublisherROS/>
<TopicPublisherROS/>
......
python/sot_talos_balance/create_entities_utils.py
+ 10
− 0
View file @ ea904413
......@@ -80,6 +80,16 @@ def create_com_trajectory_generator(dt, robot):
comTrajGen.init(dt, 3)
return comTrajGen
def create_position_trajectory_generator(dt, robot, signal_name):
trajGen = NdTrajectoryGenerator(signal_name+"TrajGen")
M = robot.dynamic.signal(signal_name).value
v = [ M[i][3] for i in range(3) ]
trajGen.initial_value.value = v
trajGen.trigger.value = 1.0
trajGen.init(dt, 3)
return trajGen
def create_joint_controller(Kp):
controller = JointPositionController("posctrl")
......
python/sot_talos_balance/test/appli_dcmZmpControl_estimator.py
+ 11
− 7
View file @ ea904413
......@@ -30,12 +30,7 @@ omega = sqrt(g/h)
# --- Parameter server
robot.param_server = create_parameter_server(param_server_conf,dt)
# -------------------------- DESIRED TRAJECTORY --------------------------
# --- Desired CoM
robot.comTrajGen = create_com_trajectory_generator(dt,robot)
# --- Desired feet and waist
# --- Initial feet and waist
robot.dynamic.createOpPoint('LF',robot.OperationalPointsMap['left-ankle'])
robot.dynamic.createOpPoint('RF',robot.OperationalPointsMap['right-ankle'])
robot.dynamic.createOpPoint('WT',robot.OperationalPointsMap['waist'])
......@@ -43,14 +38,23 @@ robot.dynamic.LF.recompute(0)
robot.dynamic.RF.recompute(0)
robot.dynamic.WT.recompute(0)
# -------------------------- DESIRED TRAJECTORY --------------------------
# --- Trajectory generators
robot.comTrajGen = create_com_trajectory_generator(dt,robot)
robot.lfPosTrajGen = create_position_trajectory_generator(dt, robot, 'LF')
robot.rfPosTrajGen = create_position_trajectory_generator(dt, robot, 'RF')
# --- Walking pattern generator
wp = DummyWalkingPatternGenerator('dummy_wp')
wp.init()
wp.omega.value = omega
wp.waist.value = robot.dynamic.WT.value
wp.footLeft.value = robot.dynamic.LF.value
wp.footRight.value = robot.dynamic.RF.value
wp.waist.value = robot.dynamic.WT.value
plug(robot.lfPosTrajGen.x, wp.footPositionLeft)
plug(robot.rfPosTrajGen.x, wp.footPositionRight)
plug(robot.comTrajGen.x, wp.com)
plug(robot.comTrajGen.dx, wp.vcom)
plug(robot.comTrajGen.ddx, wp.acom)
......
python/sot_talos_balance/test/test_dcmZmpControl_estimator.py
+ 19
− 65
View file @ ea904413
......@@ -19,71 +19,25 @@ runCommandClient('robot.com_admittance_control.Kp.value = Kp_adm')
runCommandClient('robot.dcm_control.resetDcmIntegralError()')
runCommandClient('robot.dcm_control.Ki.value = Ki_dcm')
sleep(30.0)
runCommandClient('dump_tracer(robot.tracer)')
# --- DISPLAY
comDes_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dcm_control.name') + '-dcmDes.dat') # desired CoM (workaround)
comEst_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.cdc_estimator.name') + '-c.dat') # estimated CoM
comRef_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.com_admittance_control.name') + '-comRef.dat') # reference CoM
comSOT_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dynamic.name') + '-com.dat') # resulting SOT CoM
dcmDes_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dcm_control.name') + '-dcmDes.dat') # desired DCM
dcmEst_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.estimator.name') + '-dcm.dat') # estimated DCM
zmpDes_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dcm_control.name') + '-zmpDes.dat') # desired ZMP
zmpSOT_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dynamic.name') + '-zmp.dat') # SOT ZMP
zmpEst_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.zmp_estimator.name') + '-zmp.dat') # estimated ZMP
zmpRef_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dcm_control.name') + '-zmpRef.dat') # reference ZMP
plt.ion()
plt.figure()
plt.plot(comDes_data[:,1],'b--')
plt.plot(comEst_data[:,1],'b-')
plt.plot(comRef_data[:,1],'b:')
plt.plot(comSOT_data[:,1],'b-.')
plt.plot(comDes_data[:,2],'r--')
plt.plot(comEst_data[:,2],'r-')
plt.plot(comRef_data[:,2],'r:')
plt.plot(comSOT_data[:,2],'r-.')
plt.plot(comDes_data[:,3],'g--')
plt.plot(comEst_data[:,3],'g-')
plt.plot(comRef_data[:,3],'g:')
plt.plot(comSOT_data[:,3],'g-.')
plt.title('COM')
plt.legend(['Desired x', 'Estimated x', 'Reference x', 'SOT x',
'Desired y', 'Estimated y', 'Reference y', 'SOT y',
'Desired z', 'Estimated z', 'Reference z', 'SOT z'])
plt.figure()
plt.plot(dcmDes_data[:,1],'b--')
plt.plot(dcmEst_data[:,1],'b-')
plt.plot(dcmDes_data[:,2],'r--')
plt.plot(dcmEst_data[:,2],'r-')
plt.title('DCM')
plt.legend(['Desired x', 'Estimated x', 'Desired y', 'Estimated y'])
plt.figure()
plt.plot(zmpDes_data[:,1],'b--')
plt.plot(zmpSOT_data[:,1],'b-')
plt.plot(zmpRef_data[:,1],'b:')
plt.plot(zmpDes_data[:,2],'r--')
plt.plot(zmpSOT_data[:,2],'r-')
plt.plot(zmpRef_data[:,2],'r:')
plt.title('ZMP')
plt.legend(['Desired x', 'SOT x', 'Reference x',
'Desired y', 'SOT y', 'Reference y'])
zmpErrSOT = zmpSOT_data - zmpDes_data
plt.figure()
plt.plot(zmpErrSOT[:,1],'b-')
plt.plot(zmpErrSOT[:,2],'r-')
plt.title('ZMP SOT error')
plt.legend(['Error on x','Error on y'])
c = ask_for_confirmation("Do you want to raise the foot?")
if c:
runCommandClient('robot.comTrajGen.move(1,-0.08,10.0)')
print("Putting the robot in position...")
sleep(10.0)
print("Robot is in position!")
c2 = ask_for_confirmation("Confirm raising the foot?")
if c2:
print("Raising the foot...")
runCommandClient('h = robot.dynamic.LF.value[2][3]')
runCommandClient('robot.lfPosTrajGen.move(2,h+0.05,10.0)')
sleep(10.0)
print("Foot has been raised!")
else:
print("Not raising the foot")
else:
print("Not raising the foot")
raw_input("Wait before leaving the simulation")
runCommandClient('dump_tracer(robot.tracer)')
python/sot_talos_balance/utils/run_test_utils.py
+ 10
− 9
View file @ ea904413
......@@ -92,16 +92,17 @@ def run_test(appli,verbosity=1):
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
def ask_for_confirmation(text):
c = raw_input(text+" [y/N] ")
try:
return strtobool(c)
except:
return False
def run_ft_calibration(sensor_name,force=False):
cb = False
if force:
cb = True
else:
c = raw_input("Calibrate force sensors? [y/N] ")
try:
cb = strtobool(c)
except:
cb = False
cb = force
if not cb:
cb = ask_for_confirmation("Calibrate force sensors?")
if cb:
raw_input("Wait before running the calibration")
print("Calibrating sensors...")
......
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