Commit 1c005f45 authored by Gabriele Buondonno's avatar Gabriele Buondonno

[test_feet_admittance] Add CoM control

parent 365cf900
<?xml version='1.0' encoding='UTF-8'?>
<root>
<tabbed_widget name="Main Window" parent="main_window">
<plotmatrix tab_name="plot" columns="3" rows="2">
<plot col="0" row="0">
<range bottom="-0.066212" right="51.116000" top="2.714707" left="41.117000"/>
<limitY/>
<curve G="0" name="/sot/distribute/wrenchLeft/data.0" custom_transform="noTransform" B="255" R="0"/>
<curve G="160" name="/sot/ftc/left_foot_force_out/data.0" custom_transform="noTransform" B="164" R="160"/>
<transform value="noTransform"/>
</plot>
<plot col="0" row="1">
<range bottom="-2.745565" right="51.116000" top="0.066965" left="41.117000"/>
<limitY/>
<curve G="0" name="/sot/distribute/wrenchRight/data.0" custom_transform="noTransform" B="255" R="255"/>
<curve G="0" name="/sot/ftc/right_foot_force_out/data.0" custom_transform="noTransform" B="255" R="0"/>
<transform value="noTransform"/>
</plot>
<plot col="1" row="0">
<range bottom="-39.112365" right="51.116000" top="0.953960" left="41.117000"/>
<limitY/>
<curve G="128" name="/sot/distribute/wrenchLeft/data.1" custom_transform="noTransform" B="0" R="0"/>
<curve G="0" name="/sot/ftc/left_foot_force_out/data.1" custom_transform="noTransform" B="0" R="128"/>
<transform value="noTransform"/>
</plot>
<plot col="1" row="1">
<range bottom="-0.952687" right="51.116000" top="39.060148" left="41.117000"/>
<limitY/>
<curve G="0" name="/sot/distribute/wrenchRight/data.1" custom_transform="noTransform" B="128" R="0"/>
<curve G="128" name="/sot/ftc/right_foot_force_out/data.1" custom_transform="noTransform" B="0" R="0"/>
<transform value="noTransform"/>
</plot>
<plot col="2" row="0">
<range bottom="400.779123" right="51.116000" top="443.812853" left="41.117000"/>
<limitY/>
<curve G="0" name="/sot/distribute/wrenchLeft/data.2" custom_transform="noTransform" B="0" R="255"/>
<curve G="128" name="/sot/ftc/left_foot_force_out/data.2" custom_transform="noTransform" B="0" R="128"/>
<transform value="noTransform"/>
</plot>
<plot col="2" row="1">
<range bottom="442.325429" right="51.116000" top="461.762601" left="41.117000"/>
<limitY/>
<curve G="128" name="/sot/distribute/wrenchRight/data.2" custom_transform="noTransform" B="128" R="0"/>
<curve G="0" name="/sot/ftc/right_foot_force_out/data.2" custom_transform="noTransform" B="0" R="255"/>
<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/distribute/wrenchLeft;/sot/distribute/wrenchRight;/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>
......@@ -8,6 +8,7 @@ from dynamic_graph.sot.core import Task, FeaturePosture
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT
from dynamic_graph.sot.core import operator
from math import sqrt
import numpy as np
......@@ -42,19 +43,23 @@ robot.dynamic.WT.recompute(0)
# --- 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')
robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'LF')
robot.lfToMatrix = PoseRollPitchYawToMatrixHomo('lf2m')
plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)
robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'RF')
robot.rfToMatrix = PoseRollPitchYawToMatrixHomo('rf2m')
plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)
# --- Interface with controller entities
wp = DummyWalkingPatternGenerator('dummy_wp')
wp.init()
wp.omega.value = omega
wp.waist.value = robot.dynamic.WT.value # wait receives a full homogeneous matrix, but only the rotational part is controlled
wp.footLeft.value = robot.dynamic.LF.value
wp.footRight.value = robot.dynamic.RF.value
plug(robot.lfPosTrajGen.x, wp.footPositionLeft) # if given, footPositionLeft overrides the translation in footLeft
plug(robot.rfPosTrajGen.x, wp.footPositionRight) # if given, footPositionRight overrides the translation in footRight
wp.waist.value = robot.dynamic.WT.value # waist receives a full homogeneous matrix, but only the rotational part is controlled
plug(robot.lfToMatrix.sout, wp.footLeft)
plug(robot.rfToMatrix.sout, wp.footRight)
plug(robot.comTrajGen.x, wp.com)
plug(robot.comTrajGen.dx, wp.vcom)
plug(robot.comTrajGen.ddx, wp.acom)
......@@ -134,15 +139,59 @@ robot.zmp_estimator = zmp_estimator
# -------------------------- ADMITTANCE CONTROL --------------------------
# --- CoM control
Kp_com = [0.]*2 + [4.]
comErr = operator.Substract_of_vector('comErr')
plug(robot.wp.comDes, comErr.sin1)
plug(robot.cdc_estimator.c, comErr.sin2)
robot.comErr = comErr
comControl = operator.Multiply_of_vector('comControl')
comControl.sin0.value = Kp_com
plug(robot.comErr.sout, comControl.sin1)
robot.comControl = comControl
forceControl = operator.Add_of_vector('forceControl')
plug(robot.comControl.sout, forceControl.sin1)
forceControl.sin2.value = [0.0, 0.0, mass*g]
robot.forceControl = forceControl
wrenchControl = operator.Mix_of_vector('wrenchControl')
wrenchControl.setSignalNumber(3)
wrenchControl.addSelec(1, 0, 3)
wrenchControl.addSelec(2, 3, 3)
wrenchControl.default.value = [0.0]*6
plug(robot.forceControl.sout, wrenchControl.signal("sin1"))
wrenchControl.signal("sin2").value = [0.0]*3
robot.wrenchControl = wrenchControl
# --- Distribute wrench
distribute = SimpleDistributeWrench('distribute')
plug(robot.e2q.quaternion, distribute.q)
distribute.rho.value = 0.5
plug(robot.wrenchControl.sout, distribute.wrenchDes)
distribute.init(robot_name)
robot.distribute = distribute
# --- Trick to command both feet controllers at the same time
robot.admBF_Kp = Selec_of_vector('admBF_Kp')
robot.admBF_Kp.selec(0,6)
robot.admBF_Kp.sin.value = [0.0]*2 + [3.0] + [0.0]*3
robot.admBF_dqSaturation = Selec_of_vector('admBF_dqSaturation')
robot.admBF_dqSaturation.selec(0,6)
robot.admBF_dqSaturation.sin.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# --- Left foot
controller = AdmittanceControllerEndEffector("admLF")
plug(robot.ftc.left_foot_force_out, controller.force)
plug(robot.e2q.quaternion, controller.q)
controller.Kp.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
controller.Kd.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
controller.w_forceDes.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
controller.dqSaturation.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
plug(robot.admBF_Kp.sout, controller.Kp)
controller.Kd.value = [1.0] * 6
plug(robot.distribute.wrenchLeft, controller.w_forceDes)
plug(robot.admBF_dqSaturation.sout, controller.dqSaturation)
controller.init(dt, "right_sole_link", 'leg_right_6_joint')
......@@ -153,10 +202,10 @@ controller = AdmittanceControllerEndEffector("admRF")
plug(robot.ftc.right_foot_force_out, controller.force)
plug(robot.e2q.quaternion, controller.q)
controller.Kp.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
controller.Kd.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
controller.w_forceDes.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
controller.dqSaturation.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
plug(robot.admBF_Kp.sout, controller.Kp)
controller.Kd.value = [1.0] * 6
plug(robot.distribute.wrenchRight, controller.w_forceDes)
plug(robot.admBF_dqSaturation.sout, controller.dqSaturation)
controller.init(dt, "left_sole_link", 'leg_right_6_joint')
......@@ -245,7 +294,7 @@ locals()['contactRF'] = robot.contactRF
robot.taskCom = MetaTaskKineCom(robot.dynamic)
plug(robot.wp.comDes,robot.taskCom.featureDes.errorIN)
robot.taskCom.task.controlGain.value = 10
robot.taskCom.task.setWithDerivative(True)
robot.taskCom.feature.selec.value = '011' # needed ?
# --- Waist
robot.keepWaist = MetaTaskKine6d('keepWaist',robot.dynamic,'WT',robot.OperationalPointsMap['waist'])
......@@ -348,6 +397,10 @@ create_topic(robot.publisher, robot.zmp_estimator, 'zmp', robot = robot, data_ty
#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.distribute, 'wrenchLeft', robot = robot, data_type='vector')
create_topic(robot.publisher, robot.distribute, 'wrenchRight', robot = robot, data_type='vector')
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
......
......@@ -7,6 +7,9 @@ run_test('appli_feet_admittance.py')
run_ft_calibration('robot.ftc')
raw_input("Wait before running the test")
print('Set saturation value')
runCommandClient('robot.admBF_dqSaturation.sin.value = [0.0, 0.0, 0.01, 0.0, 0.0, 0.0]')
raw_input("Wait before dumping the data")
runCommandClient('dump_tracer(robot.tracer)')
......
Markdown is supported
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