Skip to content
GitLab
Commit bb36c320 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

[joint-torque-ctrl] Add integral of desired joint vel in admittance control.

parent 9efab2d3
Hide whitespace changes
Inline Side-by-side
include/sot/torque_control/inverse-dynamics-balance-controller.hh
View file @ bb36c320
......@@ -262,6 +262,8 @@ namespace dynamicgraph {
Matrix6x m_J_LF;
Eigen::ColPivHouseholderQR<Matrix6x> m_J_RF_QR;
Eigen::ColPivHouseholderQR<Matrix6x> m_J_LF_QR;
tsid::math::Vector6 m_v_RF_int;
tsid::math::Vector6 m_v_LF_int;
unsigned int m_timeLast;
RobotUtil * m_robot_util;
......
include/sot/torque_control/joint-torque-controller.hh
View file @ bb36c320
......@@ -90,6 +90,7 @@ namespace dynamicgraph {
DECLARE_SIGNAL_IN(KiTorque, dynamicgraph::Vector); /// integral gain for torque feedback controller
DECLARE_SIGNAL_IN(KdTorque, dynamicgraph::Vector); /// derivative gain for torque feedback controller
DECLARE_SIGNAL_IN(KdVel, dynamicgraph::Vector); /// derivative gain for velocity feedback
DECLARE_SIGNAL_IN(KiVel, dynamicgraph::Vector); /// integral gain for velocity feedback
DECLARE_SIGNAL_IN(torque_integral_saturation, dynamicgraph::Vector); /// integral error saturation
......@@ -119,6 +120,7 @@ namespace dynamicgraph {
Eigen::VectorXd m_current_des;
Eigen::VectorXd m_tauErrIntegral; /// integral of the torque error
Eigen::VectorXd m_currentErrIntegral; /// integral of the current error
Eigen::VectorXd m_dqErrIntegral; /// integral of the velocity error
RobotUtil * m_robot_util;
......
python/cmd/cmd_hrp2_balance_ctrl.py
View file @ bb36c320
......@@ -7,8 +7,8 @@ from dynamic_graph.sot.torque_control.main import *
from dynamic_graph.sot.torque_control.utils.sot_utils import smoothly_set_signal, stop_sot
from dynamic_graph import plug
robot.timeStep=0.0015
robot = main_v3(robot, startSoT=True, go_half_sitting=True)
#go_to_position(robot.traj_gen, 30*(0.0,), 5.0)
robot = main_v3(robot, startSoT=True, go_half_sitting=0)
go_to_position(robot.traj_gen, 30*(0.0,), 5.0)
#robot.base_estimator.set_imu_weight(0.0)
plug(robot.filters.estimator_kin.dx, robot.base_estimator.joint_velocities);
......@@ -83,9 +83,9 @@ robot.ctrl_manager.setCtrlMode('all', 'pos')
create_topic(robot.ros, robot.inv_dyn.f_des_right_foot, 'f_des_R')
create_topic(robot.ros, robot.inv_dyn.f_des_left_foot, 'f_des_L')
create_topic(robot.ros, robot.inv_dyn.tau_des, 'tau_des');
create_topic(robot.ros, robot.estimator_ft.contactWrenchLeftSole, 'f_LeftSole');
create_topic(robot.ros, robot.estimator_ft.contactWrenchRightSole, 'f_RightSole');
create_topic(robot.ros, robot.ctrl_manager.pwmDes, 'i_des');
create_topic(robot.ros, robot.estimator_ft.contactWrenchLeftSole, 'f_L');
create_topic(robot.ros, robot.estimator_ft.contactWrenchRightSole, 'f_R');
create_topic(robot.ros, robot.ctrl_manager.u, 'i_des');
create_topic(robot.ros, robot.ctrl_manager.ctrl_torque, 'i_des');
create_topic(robot.ros, robot.inv_dyn.zmp_des_right_foot_local, 'cop_des_R_local')
......@@ -111,7 +111,7 @@ smoothly_set_signal(robot.torque_ctrl.KpTorque,30*(1.,))
smoothly_set_signal(robot.inv_dyn.kp_posture,30*(5.,))
robot.com_traj_gen.stop(-1)
robot.com_traj_gen.move(1, 0.05, 1.5)
robot.com_traj_gen.startSinusoid(1,-0.05,1.5)
robot.com_traj_gen.move(1, 0.03, 1.5)
robot.com_traj_gen.startSinusoid(1,-0.03,2.0)
python/dynamic_graph/sot/torque_control/create_entities_utils.py
View file @ bb36c320
......@@ -275,6 +275,7 @@ def create_torque_controller(robot, conf, motor_params, dt=0.001, robot_name="ro
torque_ctrl.KdTorque.value = tuple(conf.k_d_torque);
torque_ctrl.KiTorque.value = tuple(conf.k_i_torque);
torque_ctrl.KdVel.value = tuple(conf.k_d_vel);
torque_ctrl.KiVel.value = tuple(conf.k_i_vel);
torque_ctrl.torque_integral_saturation.value = tuple(conf.torque_integral_saturation);
torque_ctrl.coulomb_friction_compensation_percentage.value = NJ*(conf.COULOMB_FRICTION_COMPENSATION_PERCENTAGE,);
......@@ -333,8 +334,8 @@ def create_balance_controller(robot, conf, motor_params, dt, robot_name='robot')
plug(robot.com_traj_gen.dx, ctrl.com_ref_vel);
plug(robot.com_traj_gen.ddx, ctrl.com_ref_acc);
plug(robot.rf_force_traj_gen.x, ctrl.f_ref_right_foot);
plug(robot.lf_force_traj_gen.x, ctrl.f_ref_left_foot);
# plug(robot.rf_force_traj_gen.x, ctrl.f_ref_right_foot);
# plug(robot.lf_force_traj_gen.x, ctrl.f_ref_left_foot);
# rather than giving to the controller the values of gear ratios and rotor inertias
# it is better to compute directly their product in python and pass the result
......
src/inverse-dynamics-balance-controller.cpp
View file @ bb36c320
......@@ -287,6 +287,8 @@ namespace dynamicgraph
m_zmp_LF.setZero();
m_zmp.setZero();
m_com_offset.setZero();
m_v_RF_int.setZero();
m_v_LF_int.setZero();
/* Commands. */
addCommand("init",
......@@ -899,8 +901,11 @@ namespace dynamicgraph
getProfiler().start(PROFILE_DQ_ADMITTANCE);
Eigen::Vector6d v_des_RF = kp.cwiseProduct(f_des_RF - f_RF);
Eigen::Vector6d v_des_LF = kp.cwiseProduct(f_des_LF - f_LF);
Eigen::Vector6d v_des_RF = kp.cwiseProduct(f_RF - f_des_RF);
Eigen::Vector6d v_des_LF = kp.cwiseProduct(f_LF - f_des_LF);
m_v_RF_int += ki.cwiseProduct(f_RF - f_des_RF);
m_v_LF_int += ki.cwiseProduct(f_LF - f_des_LF);
m_robot->frameJacobianLocal(m_invDyn->data(), m_taskRF->frame_id(), m_J_RF);
m_robot->frameJacobianLocal(m_invDyn->data(), m_taskLF->frame_id(), m_J_LF);
......@@ -908,11 +913,11 @@ namespace dynamicgraph
m_J_RF_QR.compute(m_J_RF.rightCols(m_robot_util->m_nbJoints));
m_J_LF_QR.compute(m_J_LF.rightCols(m_robot_util->m_nbJoints));
Vector dq_adm_urdf = m_J_RF_QR.solve(v_des_RF);
Vector dq_adm_urdf = m_J_RF_QR.solve(v_des_RF+m_v_RF_int);
// SEND_MSG("J RF:\n"+toString(m_J_RF.rightCols(m_robot_util->m_nbJoints), 1), MSG_TYPE_DEBUG);
// SEND_MSG("v_des RF: "+toString(v_des_RF.transpose()), MSG_TYPE_DEBUG);
// SEND_MSG("dq_adm RF: "+toString(s.transpose()), MSG_TYPE_DEBUG);
dq_adm_urdf += m_J_LF_QR.solve(v_des_LF);
dq_adm_urdf += m_J_LF_QR.solve(v_des_LF+m_v_LF_int);
// SEND_MSG("J LF:\n"+toString(m_J_LF.rightCols(m_robot_util->m_nbJoints), 1), MSG_TYPE_DEBUG);
// SEND_MSG("v_des LF: "+toString(v_des_LF.transpose()), MSG_TYPE_DEBUG);
// SEND_MSG("dq_adm LF: "+toString(s.transpose()), MSG_TYPE_DEBUG);
......
src/joint-torque-controller.cpp
View file @ bb36c320
......@@ -39,7 +39,7 @@ namespace dynamicgraph
#define TORQUE_INTEGRAL_INPUT_SIGNALS m_KiTorqueSIN << m_torque_integral_saturationSIN
#define TORQUE_CONTROL_INPUT_SIGNALS m_jointsTorquesDesiredSIN << m_KpTorqueSIN << m_KdTorqueSIN \
<< m_coulomb_friction_compensation_percentageSIN
#define VEL_CONTROL_INPUT_SIGNALS m_dq_desSIN << m_KdVelSIN \
#define VEL_CONTROL_INPUT_SIGNALS m_dq_desSIN << m_KdVelSIN << m_KiVelSIN \
#define ALL_INPUT_SIGNALS ESTIMATOR_INPUT_SIGNALS << TORQUE_INTEGRAL_INPUT_SIGNALS << \
TORQUE_CONTROL_INPUT_SIGNALS << VEL_CONTROL_INPUT_SIGNALS << \
......@@ -75,6 +75,7 @@ namespace dynamicgraph
,CONSTRUCT_SIGNAL_IN(KiTorque, dynamicgraph::Vector) // integral gain for torque feedback controller
,CONSTRUCT_SIGNAL_IN(KdTorque, dynamicgraph::Vector) // derivative gain for torque feedback controller
,CONSTRUCT_SIGNAL_IN(KdVel, dynamicgraph::Vector) // derivative gain for velocity feedback
,CONSTRUCT_SIGNAL_IN(KiVel, dynamicgraph::Vector) // integral gain for velocity feedback
,CONSTRUCT_SIGNAL_IN(coulomb_friction_compensation_percentage, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(motorParameterKt_p, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(motorParameterKt_n, dynamicgraph::Vector)
......@@ -129,9 +130,9 @@ namespace dynamicgraph
if(!m_KiTorqueSIN.isPlugged())
return SEND_MSG("Init failed: signal m_KiTorqueSIN is not plugged", MSG_TYPE_ERROR);
/* Retrieve m_robot_util informations */
std::string localName(robot_ref);
if (isNameInRobotUtil(localName))
/* Retrieve m_robot_util informations */
std::string localName(robot_ref);
if (isNameInRobotUtil(localName))
{
m_robot_util = getRobotUtil(localName);
}
......@@ -145,11 +146,13 @@ namespace dynamicgraph
m_tau_star.setZero(m_robot_util->m_nbJoints);
m_current_des.setZero(m_robot_util->m_nbJoints);
m_tauErrIntegral.setZero(m_robot_util->m_nbJoints);
m_dqErrIntegral.setZero(m_robot_util->m_nbJoints);
}
void JointTorqueController::reset_integral()
{
m_tauErrIntegral.setZero();
m_dqErrIntegral.setZero();
}
/* --- SIGNALS ---------------------------------------------------------- */
......@@ -168,6 +171,7 @@ namespace dynamicgraph
const Eigen::VectorXd& kp = m_KpTorqueSIN(iter);
const Eigen::VectorXd& kd = m_KdTorqueSIN(iter);
const Eigen::VectorXd& kd_vel = m_KdVelSIN(iter);
const Eigen::VectorXd& ki_vel = m_KiVelSIN(iter);
const Eigen::VectorXd& tauErrInt = m_torque_error_integralSOUT(iter);
const Eigen::VectorXd& colFricCompPerc = m_coulomb_friction_compensation_percentageSIN(iter);
const Eigen::VectorXd& motorParameterKt_p = m_motorParameterKt_pSIN(iter);
......@@ -187,10 +191,12 @@ namespace dynamicgraph
if(dq.size()==(int)(m_robot_util->m_nbJoints+6))
offset = 6;
m_dqErrIntegral += m_dt * ki_vel.cwiseProduct(dq_des-dq);
for(int i=0; i<(int)m_robot_util->m_nbJoints; i++)
{
m_current_des(i) = motorModel.getCurrent(m_tau_star(i),
dq(i+offset)+kd_vel(i)*(dq_des(i)-dq(i+offset)),
dq(i+offset)+kd_vel(i)*(dq_des(i)-dq(i+offset))+m_dqErrIntegral(i),
ddq(i+offset),
motorParameterKt_p(i),
motorParameterKt_n(i),
......
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