Commit 5820f126 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Update for dynamic-graph-python v4

parent 34160caf
# flake8: noqa # flake8: noqa
from math import sqrt from math import sqrt
from dynamic_graph.sot.core.task import Task import dynamic_graph.sot_talos_balance.talos.base_estimator_conf as base_estimator_conf
from dynamic_graph.sot.core.feature_posture import FeaturePosture import dynamic_graph.sot_talos_balance.talos.control_manager_conf as cm_conf
from dynamic_graph.sot.core.sot import SOT import dynamic_graph.sot_talos_balance.talos.ft_calibration_conf as ft_conf
import dynamic_graph.sot_talos_balance.talos.parameter_server_conf as param_server_conf
from dynamic_graph import plug
from dynamic_graph.sot.core.derivator import Derivator_of_Vector from dynamic_graph.sot.core.derivator import Derivator_of_Vector
from dynamic_graph.sot.core.feature_posture import FeaturePosture
from dynamic_graph.sot.core.matrix_util import matrixToTuple from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
from dynamic_graph.sot.core.sot import SOT
from dynamic_graph.sot.core.task import Task
from dynamic_graph.sot.dynamic_pinocchio import DynamicPinocchio from dynamic_graph.sot.dynamic_pinocchio import DynamicPinocchio
from dynamic_graph.sot_talos_balance.create_entities_utils import *
from dynamic_graph.tracer_real_time import TracerRealTime from dynamic_graph.tracer_real_time import TracerRealTime
from rospkg import RosPack from rospkg import RosPack
import dynamic_graph.sot_talos_balance.talos.base_estimator_conf as base_estimator_conf
import dynamic_graph.sot_talos_balance.talos.control_manager_conf as cm_conf
import dynamic_graph.sot_talos_balance.talos.ft_calibration_conf as ft_conf
import dynamic_graph.sot_talos_balance.talos.parameter_server_conf as param_server_conf
from dynamic_graph.sot_talos_balance.create_entities_utils import *
from dynamic_graph import plug
def init_sot_talos_balance(robot, test_folder): def init_sot_talos_balance(robot, test_folder):
cm_conf.CTRL_MAX = 1000.0 # temporary hack cm_conf.CTRL_MAX = 1000.0 # temporary hack
...@@ -42,6 +39,7 @@ def init_sot_talos_balance(robot, test_folder): ...@@ -42,6 +39,7 @@ def init_sot_talos_balance(robot, test_folder):
robot.dynamic.createOpPoint('LF', robot.OperationalPointsMap['left-ankle']) robot.dynamic.createOpPoint('LF', robot.OperationalPointsMap['left-ankle'])
robot.dynamic.createOpPoint('RF', robot.OperationalPointsMap['right-ankle']) robot.dynamic.createOpPoint('RF', robot.OperationalPointsMap['right-ankle'])
robot.dynamic.createOpPoint('WT', robot.OperationalPointsMap['waist']) robot.dynamic.createOpPoint('WT', robot.OperationalPointsMap['waist'])
robot.dynamic.add_signals()
robot.dynamic.LF.recompute(0) robot.dynamic.LF.recompute(0)
robot.dynamic.RF.recompute(0) robot.dynamic.RF.recompute(0)
robot.dynamic.WT.recompute(0) robot.dynamic.WT.recompute(0)
...@@ -97,8 +95,8 @@ def init_sot_talos_balance(robot, test_folder): ...@@ -97,8 +95,8 @@ def init_sot_talos_balance(robot, test_folder):
robot.waistMix.setSignalNumber(3) robot.waistMix.setSignalNumber(3)
robot.waistMix.addSelec(1, 0, 3) robot.waistMix.addSelec(1, 0, 3)
robot.waistMix.addSelec(2, 3, 3) robot.waistMix.addSelec(2, 3, 3)
robot.waistMix.default.value = [0.0] * 6 # robot.waistMix.default.value = np.array([0.0] * 6)
robot.waistMix.signal("sin1").value = [0.0] * 3 robot.waistMix.signal("sin1").value = np.array([0.0] * 3)
plug(robot.waistTrajGen.x, robot.waistMix.signal("sin2")) plug(robot.waistTrajGen.x, robot.waistMix.signal("sin2"))
robot.waistToMatrix = PoseRollPitchYawToMatrixHomo('w2m') robot.waistToMatrix = PoseRollPitchYawToMatrixHomo('w2m')
...@@ -155,9 +153,10 @@ def init_sot_talos_balance(robot, test_folder): ...@@ -155,9 +153,10 @@ def init_sot_talos_balance(robot, test_folder):
robot.rdynamic = DynamicPinocchio("real_dynamics") robot.rdynamic = DynamicPinocchio("real_dynamics")
robot.rdynamic.setModel(robot.dynamic.model) robot.rdynamic.setModel(robot.dynamic.model)
robot.rdynamic.setData(robot.rdynamic.model.createData()) robot.rdynamic.setData(robot.rdynamic.model.createData())
robot.rdynamic.add_signals()
plug(robot.base_estimator.q, robot.rdynamic.position) plug(robot.base_estimator.q, robot.rdynamic.position)
robot.rdynamic.velocity.value = [0.0] * robotDim robot.rdynamic.velocity.value = np.array([0.0] * robotDim)
robot.rdynamic.acceleration.value = [0.0] * robotDim robot.rdynamic.acceleration.value = np.array([0.0] * robotDim)
# --- CoM Estimation # --- CoM Estimation
cdc_estimator = DcmEstimator('cdc_estimator') cdc_estimator = DcmEstimator('cdc_estimator')
...@@ -182,6 +181,7 @@ def init_sot_talos_balance(robot, test_folder): ...@@ -182,6 +181,7 @@ def init_sot_talos_balance(robot, test_folder):
zmp_estimator = SimpleZmpEstimator("zmpEst") zmp_estimator = SimpleZmpEstimator("zmpEst")
robot.rdynamic.createOpPoint('sole_LF', 'left_sole_link') robot.rdynamic.createOpPoint('sole_LF', 'left_sole_link')
robot.rdynamic.createOpPoint('sole_RF', 'right_sole_link') robot.rdynamic.createOpPoint('sole_RF', 'right_sole_link')
robot.rdynamic.add_signals()
plug(robot.rdynamic.sole_LF, zmp_estimator.poseLeft) plug(robot.rdynamic.sole_LF, zmp_estimator.poseLeft)
plug(robot.rdynamic.sole_RF, zmp_estimator.poseRight) plug(robot.rdynamic.sole_RF, zmp_estimator.poseRight)
plug(robot.ftc.left_foot_force_out, zmp_estimator.wrenchLeft) plug(robot.ftc.left_foot_force_out, zmp_estimator.wrenchLeft)
...@@ -192,8 +192,8 @@ def init_sot_talos_balance(robot, test_folder): ...@@ -192,8 +192,8 @@ def init_sot_talos_balance(robot, test_folder):
# -------------------------- ADMITTANCE CONTROL -------------------------- # -------------------------- ADMITTANCE CONTROL --------------------------
# --- DCM controller # --- DCM controller
Kp_dcm = [8.0] * 3 Kp_dcm = np.array([8.0] * 3)
Ki_dcm = [0.0, 0.0, 0.0] # zero (to be set later) Ki_dcm = np.array([0.0, 0.0, 0.0]) # zero (to be set later)
gamma_dcm = 0.2 gamma_dcm = 0.2
dcm_controller = DcmController("dcmCtrl") dcm_controller = DcmController("dcmCtrl")
...@@ -214,10 +214,10 @@ def init_sot_talos_balance(robot, test_folder): ...@@ -214,10 +214,10 @@ def init_sot_talos_balance(robot, test_folder):
robot.dcm_control = dcm_controller robot.dcm_control = dcm_controller
Ki_dcm = [1.0, 1.0, 1.0] # this value is employed later Ki_dcm = np.array([1.0, 1.0, 1.0]) # this value is employed later
# --- CoM admittance controller # --- CoM admittance controller
Kp_adm = [0.0, 0.0, 0.0] # zero (to be set later) Kp_adm = np.array([0.0, 0.0, 0.0]) # zero (to be set later)
com_admittance_control = ComAdmittanceController("comAdmCtrl") com_admittance_control = ComAdmittanceController("comAdmCtrl")
com_admittance_control.Kp.value = Kp_adm com_admittance_control.Kp.value = Kp_adm
...@@ -226,7 +226,7 @@ def init_sot_talos_balance(robot, test_folder): ...@@ -226,7 +226,7 @@ def init_sot_talos_balance(robot, test_folder):
plug(robot.wp.acomDes, com_admittance_control.ddcomDes) plug(robot.wp.acomDes, com_admittance_control.ddcomDes)
com_admittance_control.init(dt) com_admittance_control.init(dt)
com_admittance_control.setState(robot.wp.comDes.value, [0.0, 0.0, 0.0]) com_admittance_control.setState(robot.wp.comDes.value, np.array([0.0, 0.0, 0.0]))
robot.com_admittance_control = com_admittance_control robot.com_admittance_control = com_admittance_control
...@@ -242,7 +242,7 @@ def init_sot_talos_balance(robot, test_folder): ...@@ -242,7 +242,7 @@ def init_sot_talos_balance(robot, test_folder):
robot.taskUpperBody = Task('task_upper_body') robot.taskUpperBody = Task('task_upper_body')
robot.taskUpperBody.feature = FeaturePosture('feature_upper_body') robot.taskUpperBody.feature = FeaturePosture('feature_upper_body')
q = list(robot.dynamic.position.value) q = robot.dynamic.position.value
robot.taskUpperBody.feature.state.value = q robot.taskUpperBody.feature.state.value = q
robot.taskUpperBody.feature.posture.value = q robot.taskUpperBody.feature.posture.value = q
...@@ -313,6 +313,8 @@ def init_sot_talos_balance(robot, test_folder): ...@@ -313,6 +313,8 @@ def init_sot_talos_balance(robot, test_folder):
robot.sot.setSize(robot.dynamic.getDimension()) robot.sot.setSize(robot.dynamic.getDimension())
# --- Plug SOT control to device through control manager # --- Plug SOT control to device through control manager
robot.cm.add_commands()
robot.cm.add_signals()
plug(robot.sot.control, robot.cm.ctrl_sot_input) plug(robot.sot.control, robot.cm.ctrl_sot_input)
plug(robot.cm.u_safe, robot.device.control) plug(robot.cm.u_safe, robot.device.control)
......
# flake8: noqa # flake8: noqa
from math import sqrt from math import sqrt
from dynamic_graph.sot.core.task import Task import dynamic_graph.sot_talos_balance.talos.base_estimator_conf as base_estimator_conf
from dynamic_graph.sot.core.feature_posture import FeaturePosture import dynamic_graph.sot_talos_balance.talos.control_manager_conf as cm_conf
import dynamic_graph.sot_talos_balance.talos.ft_calibration_conf as ft_conf
import dynamic_graph.sot_talos_balance.talos.parameter_server_conf as param_server_conf
from dynamic_graph import plug from dynamic_graph import plug
from dynamic_graph.sot.core.sot import SOT
from dynamic_graph.sot.core.derivator import Derivator_of_Vector from dynamic_graph.sot.core.derivator import Derivator_of_Vector
from dynamic_graph.sot.core.feature_posture import FeaturePosture
from dynamic_graph.sot.core.matrix_util import matrixToTuple from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKineCom, gotoNd from dynamic_graph.sot.core.sot import SOT
from dynamic_graph.sot.core.task import Task
from dynamic_graph.sot.dynamic_pinocchio import DynamicPinocchio from dynamic_graph.sot.dynamic_pinocchio import DynamicPinocchio
from dynamic_graph.sot.pattern_generator import PatternGenerator
from dynamic_graph.sot_talos_balance.create_entities_utils import *
from dynamic_graph.tracer_real_time import TracerRealTime from dynamic_graph.tracer_real_time import TracerRealTime
from rospkg import RosPack from rospkg import RosPack
import dynamic_graph.sot_talos_balance.talos.base_estimator_conf as base_estimator_conf
import dynamic_graph.sot_talos_balance.talos.control_manager_conf as cm_conf
import dynamic_graph.sot_talos_balance.talos.ft_calibration_conf as ft_conf
import dynamic_graph.sot_talos_balance.talos.parameter_server_conf as param_server_conf
from dynamic_graph.sot_talos_balance.create_entities_utils import *
from dynamic_graph.sot.pattern_generator import PatternGenerator
from dynamic_graph import plug
def init_online_walking(robot): def init_online_walking(robot):
# 09.04.20 est a 100 dans dcmZmpControl_file, used to be 10 # 09.04.20 est a 100 dans dcmZmpControl_file, used to be 10
...@@ -46,6 +41,7 @@ def init_online_walking(robot): ...@@ -46,6 +41,7 @@ def init_online_walking(robot):
robot.dynamic.createOpPoint('LF', robot.OperationalPointsMap['left-ankle']) robot.dynamic.createOpPoint('LF', robot.OperationalPointsMap['left-ankle'])
robot.dynamic.createOpPoint('RF', robot.OperationalPointsMap['right-ankle']) robot.dynamic.createOpPoint('RF', robot.OperationalPointsMap['right-ankle'])
robot.dynamic.createOpPoint('WT', robot.OperationalPointsMap['waist']) robot.dynamic.createOpPoint('WT', robot.OperationalPointsMap['waist'])
robot.dynamic.add_signals()
robot.dynamic.LF.recompute(0) robot.dynamic.LF.recompute(0)
robot.dynamic.RF.recompute(0) robot.dynamic.RF.recompute(0)
robot.dynamic.WT.recompute(0) robot.dynamic.WT.recompute(0)
...@@ -88,8 +84,8 @@ def init_online_walking(robot): ...@@ -88,8 +84,8 @@ def init_online_walking(robot):
plug(robot.dynamic.LF, robot.pg.leftfootcurrentpos) plug(robot.dynamic.LF, robot.pg.leftfootcurrentpos)
plug(robot.dynamic.RF, robot.pg.rightfootcurrentpos) plug(robot.dynamic.RF, robot.pg.rightfootcurrentpos)
robotDim = len(robot.dynamic.velocity.value) robotDim = len(robot.dynamic.velocity.value)
robot.pg.motorcontrol.value = robotDim * (0, ) robot.pg.motorcontrol.value = np.array(robotDim * (0, ))
robot.pg.zmppreviouscontroller.value = (0, 0, 0) robot.pg.zmppreviouscontroller.value = np.array((0, 0, 0))
robot.pg.initState() robot.pg.initState()
...@@ -103,7 +99,7 @@ def init_online_walking(robot): ...@@ -103,7 +99,7 @@ def init_online_walking(robot):
robot.pg.parseCmd(':feedBackControl false') robot.pg.parseCmd(':feedBackControl false')
robot.pg.parseCmd(':useDynamicFilter true') robot.pg.parseCmd(':useDynamicFilter true')
robot.pg.velocitydes.value = (0.1, 0.0, 0.0) # DEFAULT VALUE (0.1,0.0,0.0) robot.pg.velocitydes.value = np.array((0.1, 0.0, 0.0)) # DEFAULT VALUE (0.1,0.0,0.0)
# -------------------------- TRIGGER -------------------------- # -------------------------- TRIGGER --------------------------
...@@ -158,9 +154,10 @@ def init_online_walking(robot): ...@@ -158,9 +154,10 @@ def init_online_walking(robot):
robot.rdynamic = DynamicPinocchio("real_dynamics") robot.rdynamic = DynamicPinocchio("real_dynamics")
robot.rdynamic.setModel(robot.dynamic.model) robot.rdynamic.setModel(robot.dynamic.model)
robot.rdynamic.setData(robot.rdynamic.model.createData()) robot.rdynamic.setData(robot.rdynamic.model.createData())
robot.rdynamic.add_signals()
plug(robot.base_estimator.q, robot.rdynamic.position) plug(robot.base_estimator.q, robot.rdynamic.position)
robot.rdynamic.velocity.value = [0.0] * robotDim robot.rdynamic.velocity.value = np.array([0.0] * robotDim)
robot.rdynamic.acceleration.value = [0.0] * robotDim robot.rdynamic.acceleration.value = np.array([0.0] * robotDim)
# --- CoM Estimation # --- CoM Estimation
cdc_estimator = DcmEstimator('cdc_estimator') cdc_estimator = DcmEstimator('cdc_estimator')
...@@ -185,6 +182,7 @@ def init_online_walking(robot): ...@@ -185,6 +182,7 @@ def init_online_walking(robot):
zmp_estimator = SimpleZmpEstimator("zmpEst") zmp_estimator = SimpleZmpEstimator("zmpEst")
robot.rdynamic.createOpPoint('sole_LF', 'left_sole_link') robot.rdynamic.createOpPoint('sole_LF', 'left_sole_link')
robot.rdynamic.createOpPoint('sole_RF', 'right_sole_link') robot.rdynamic.createOpPoint('sole_RF', 'right_sole_link')
robot.rdynamic.add_signals()
plug(robot.rdynamic.sole_LF, zmp_estimator.poseLeft) plug(robot.rdynamic.sole_LF, zmp_estimator.poseLeft)
plug(robot.rdynamic.sole_RF, zmp_estimator.poseRight) plug(robot.rdynamic.sole_RF, zmp_estimator.poseRight)
plug(robot.ftc.left_foot_force_out, zmp_estimator.wrenchLeft) plug(robot.ftc.left_foot_force_out, zmp_estimator.wrenchLeft)
...@@ -195,8 +193,8 @@ def init_online_walking(robot): ...@@ -195,8 +193,8 @@ def init_online_walking(robot):
# -------------------------- ADMITTANCE CONTROL -------------------------- # -------------------------- ADMITTANCE CONTROL --------------------------
# --- DCM controller # --- DCM controller
Kp_dcm = [8.0] * 3 Kp_dcm = np.array([8.0] * 3)
Ki_dcm = [0.0, 0.0, 0.0] # zero (to be set later) Ki_dcm = np.array([0.0, 0.0, 0.0]) # zero (to be set later)
gamma_dcm = 0.2 gamma_dcm = 0.2
dcm_controller = DcmController("dcmCtrl") dcm_controller = DcmController("dcmCtrl")
...@@ -217,10 +215,10 @@ def init_online_walking(robot): ...@@ -217,10 +215,10 @@ def init_online_walking(robot):
robot.dcm_control = dcm_controller robot.dcm_control = dcm_controller
Ki_dcm = [1.0, 1.0, 1.0] # this value is employed later Ki_dcm = np.array([1.0, 1.0, 1.0]) # this value is employed later
# --- CoM admittance controller # --- CoM admittance controller
Kp_adm = [0.0, 0.0, 0.0] # zero (to be set later) Kp_adm = np.array([0.0, 0.0, 0.0]) # zero (to be set later)
com_admittance_control = ComAdmittanceController("comAdmCtrl") com_admittance_control = ComAdmittanceController("comAdmCtrl")
com_admittance_control.Kp.value = Kp_adm com_admittance_control.Kp.value = Kp_adm
...@@ -230,11 +228,11 @@ def init_online_walking(robot): ...@@ -230,11 +228,11 @@ def init_online_walking(robot):
plug(robot.wp.acomDes, com_admittance_control.ddcomDes) plug(robot.wp.acomDes, com_admittance_control.ddcomDes)
com_admittance_control.init(dt) com_admittance_control.init(dt)
com_admittance_control.setState(robot.wp.comDes.value, [0.0, 0.0, 0.0]) com_admittance_control.setState(robot.wp.comDes.value, np.array([0.0, 0.0, 0.0]))
robot.com_admittance_control = com_admittance_control robot.com_admittance_control = com_admittance_control
Kp_adm = [15.0, 15.0, 0.0] # this value is employed later Kp_adm = np.array([15.0, 15.0, 0.0]) # this value is employed later
# --- Control Manager # --- Control Manager
robot.cm = create_ctrl_manager(cm_conf, dt, robot_name='robot') robot.cm = create_ctrl_manager(cm_conf, dt, robot_name='robot')
...@@ -248,7 +246,7 @@ def init_online_walking(robot): ...@@ -248,7 +246,7 @@ def init_online_walking(robot):
robot.taskUpperBody = Task('task_upper_body') robot.taskUpperBody = Task('task_upper_body')
robot.taskUpperBody.feature = FeaturePosture('feature_upper_body') robot.taskUpperBody.feature = FeaturePosture('feature_upper_body')
q = list(robot.dynamic.position.value) q = robot.dynamic.position.value
robot.taskUpperBody.feature.state.value = q robot.taskUpperBody.feature.state.value = q
robot.taskUpperBody.feature.posture.value = q robot.taskUpperBody.feature.posture.value = q
...@@ -318,6 +316,7 @@ def init_online_walking(robot): ...@@ -318,6 +316,7 @@ def init_online_walking(robot):
robot.sot.setSize(robot.dynamic.getDimension()) robot.sot.setSize(robot.dynamic.getDimension())
# --- Plug SOT control to device through control manager # --- Plug SOT control to device through control manager
robot.cm.add_signals()
plug(robot.sot.control, robot.cm.ctrl_sot_input) plug(robot.sot.control, robot.cm.ctrl_sot_input)
plug(robot.cm.u_safe, robot.device.control) plug(robot.cm.u_safe, robot.device.control)
......
Supports Markdown
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