appli_dcm_estimator.py

Go to the documentation of this file.

# flake8: noqa
from math import sqrt
 
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT, Derivator_of_Vector
from dynamic_graph.sot.core.meta_tasks_kine import (
    MetaTaskKine6d,
    MetaTaskKineCom,
    gotoNd,
)
from dynamic_graph.sot.dynamic_pinocchio import DynamicPinocchio
from dynamic_graph.tracer_real_time import TracerRealTime
 
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 *
 
robot.timeStep = robot.device.getTimeStep()
dt = robot.timeStep
 
# -------------------------- DESIRED TRAJECTORY --------------------------
 
robot.comTrajGen = create_com_trajectory_generator(dt, robot)
 
# --- Pendulum parameters
robot_name = "robot"
robotDim = robot.dynamic.getDimension()
mass = robot.dynamic.data.mass[0]
h = robot.dynamic.com.value[2]
g = 9.81
omega = sqrt(g / h)
 
# -------------------------- ESTIMATION --------------------------
 
# --- Base Estimation
robot.param_server = create_parameter_server(param_server_conf, dt)
robot.device_filters = create_device_filters(robot, dt)
robot.imu_filters = create_imu_filters(robot, dt)
robot.base_estimator = create_base_estimator(robot, dt, base_estimator_conf)
# robot.be_filters              = create_be_filters(robot, dt)
 
# --- Conversion
e2q = EulerToQuat("e2q")
plug(robot.base_estimator.q, e2q.euler)
robot.e2q = e2q
 
# --- Kinematic computations
robot.rdynamic = DynamicPinocchio("real_dynamics")
robot.rdynamic.setModel(robot.dynamic.model)
robot.rdynamic.setData(robot.rdynamic.model.createData())
plug(robot.base_estimator.q, robot.rdynamic.position)
robot.rdynamic.velocity.value = [0.0] * robotDim
robot.rdynamic.acceleration.value = [0.0] * robotDim
 
# --- CoM Estimation
cdc_estimator = DcmEstimator("cdc_estimator")
cdc_estimator.init(dt, robot_name)
plug(robot.e2q.quaternion, cdc_estimator.q)
plug(robot.base_estimator.v, cdc_estimator.v)
robot.cdc_estimator = cdc_estimator
 
# --- DCM Estimation
estimator = DummyDcmEstimator("dummy")
estimator.omega.value = omega
estimator.mass.value = 1.0
plug(robot.cdc_estimator.c, estimator.com)
plug(robot.cdc_estimator.dc, estimator.momenta)
estimator.init()
robot.estimator = estimator
 
# --- Force calibration
robot.ftc = create_ft_calibrator(robot, ft_conf)
 
# --- ZMP estimation (disconnected)
zmp_estimator = SimpleZmpEstimator("zmpEst")
robot.rdynamic.createOpPoint("sole_LF", "left_sole_link")
robot.rdynamic.createOpPoint("sole_RF", "right_sole_link")
plug(robot.rdynamic.sole_LF, zmp_estimator.poseLeft)
plug(robot.rdynamic.sole_RF, zmp_estimator.poseRight)
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
 
# --- Control Manager
robot.cm = create_ctrl_manager(cm_conf, dt, robot_name="robot")
robot.cm.addCtrlMode("sot_input")
robot.cm.setCtrlMode("all", "sot_input")
robot.cm.addEmergencyStopSIN("zmp")
 
# -------------------------- SOT CONTROL --------------------------
 
# --- CONTACTS
# define contactLF and contactRF
robot.contactLF = MetaTaskKine6d(
    "contactLF", robot.dynamic, "LF", robot.OperationalPointsMap["left-ankle"]
)
robot.contactLF.feature.frame("desired")
robot.contactLF.gain.setConstant(100)
robot.contactLF.keep()
locals()["contactLF"] = robot.contactLF
 
robot.contactRF = MetaTaskKine6d(
    "contactRF", robot.dynamic, "RF", robot.OperationalPointsMap["right-ankle"]
)
robot.contactRF.feature.frame("desired")
robot.contactRF.gain.setConstant(100)
robot.contactRF.keep()
locals()["contactRF"] = robot.contactRF
 
# --- COM
robot.taskCom = MetaTaskKineCom(robot.dynamic)
robot.dynamic.com.recompute(0)
robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value
robot.taskCom.task.controlGain.value = 10
 
# --- SOT solver
robot.sot = SOT("sot")
robot.sot.setSize(robot.dynamic.getDimension())
 
# --- Plug SOT control to device through control manager
plug(robot.sot.control, robot.cm.ctrl_sot_input)
plug(robot.cm.u_safe, robot.device.control)
 
plug(robot.comTrajGen.x, robot.taskCom.featureDes.errorIN)
 
robot.sot.push(robot.contactRF.task.name)
robot.sot.push(robot.contactLF.task.name)
robot.sot.push(robot.taskCom.task.name)
robot.device.control.recompute(0)
 
# --- Fix robot.dynamic inputs
plug(robot.device.velocity, robot.dynamic.velocity)
robot.dvdt = Derivator_of_Vector("dv_dt")
robot.dvdt.dt.value = dt
plug(robot.device.velocity, robot.dvdt.sin)
plug(robot.dvdt.sout, robot.dynamic.acceleration)
 
# -------------------------- PLOTS --------------------------
 
# --- ROS PUBLISHER
robot.publisher = create_rospublish(robot, "robot_publisher")
 
create_topic(
    robot.publisher, robot.cdc_estimator, "c", robot=robot, data_type="vector"
)  # estimated CoM
create_topic(
    robot.publisher, robot.cdc_estimator, "dc", robot=robot, data_type="vector"
)  # estimated CoM velocity
create_topic(
    robot.publisher, robot.estimator, "dcm", robot=robot, data_type="vector"
)  # estimated DCM
 
create_topic(
    robot.publisher, robot.zmp_estimator, "zmp", robot=robot, data_type="vector"
)  # estimated ZMP
create_topic(
    robot.publisher,
    robot.zmp_estimator,
    "emergencyStop",
    robot=robot,
    data_type="boolean",
)  # ZMP emergency stop
create_topic(
    robot.publisher, robot.dynamic, "com", robot=robot, data_type="vector"
)  # SOT CoM
create_topic(
    robot.publisher, robot.dynamic, "zmp", robot=robot, data_type="vector"
)  # SOT ZMP
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