diff --git a/plot/plot_wrenchDistrib.xml b/plot/plot_wrenchDistrib.xml new file mode 100644 index 0000000000000000000000000000000000000000..f70c034aa8afa388431d7599f5593a073801b30a --- /dev/null +++ b/plot/plot_wrenchDistrib.xml @@ -0,0 +1,107 @@ +<?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> + diff --git a/python/sot_talos_balance/test/appli_feet_admittance.py b/python/sot_talos_balance/test/appli_feet_admittance.py index bc604415f2472cd4fc1c43ddf1bbd72dce1cc627..53f19b3e63266f6e39c192e13a6fe87ef0ddb764 100644 --- a/python/sot_talos_balance/test/appli_feet_admittance.py +++ b/python/sot_talos_balance/test/appli_feet_admittance.py @@ -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 diff --git a/python/sot_talos_balance/test/test_feet_admittance.py b/python/sot_talos_balance/test/test_feet_admittance.py index 2e774304983b605dd316436bfcb6cf60a1a8edde..116cfa1b490e4851d5d7524632a31aff85ccfb0e 100644 --- a/python/sot_talos_balance/test/test_feet_admittance.py +++ b/python/sot_talos_balance/test/test_feet_admittance.py @@ -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)')