Commit 9efab2d3 authored by andreadelprete's avatar andreadelprete
Browse files

Add admittance control in balance controller and torque controller

parent 82c81563
......@@ -115,6 +115,8 @@ namespace dynamicgraph {
DECLARE_SIGNAL_IN(kd_posture, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_admittance, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ki_admittance, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(w_com, double);
DECLARE_SIGNAL_IN(w_feet, double);
......@@ -152,6 +154,7 @@ namespace dynamicgraph {
DECLARE_SIGNAL_OUT(tau_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(M, dynamicgraph::Matrix);
DECLARE_SIGNAL_OUT(dv_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dq_admittance, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(f_des_right_foot, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(f_des_left_foot, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(zmp_des_right_foot, dynamicgraph::Vector);
......@@ -254,6 +257,12 @@ namespace dynamicgraph {
tsid::math::Vector m_q_urdf;
tsid::math::Vector m_v_urdf;
typedef se3::Data::Matrix6x Matrix6x;
Matrix6x m_J_RF;
Matrix6x m_J_LF;
Eigen::ColPivHouseholderQR<Matrix6x> m_J_RF_QR;
Eigen::ColPivHouseholderQR<Matrix6x> m_J_LF_QR;
unsigned int m_timeLast;
RobotUtil * m_robot_util;
......
......@@ -68,8 +68,6 @@ namespace dynamicgraph {
* plug(estimator.jointsTorques, jtc.jointsTorques);
* jtc.KpTorque.value = N_DOF*(10.0,);
* jtc.KiTorque.value = N_DOF*(0.01,);
* jtc.k_tau.value = ...
* jtc.k_v.value = ...
* jtc.init(dt);
*
* DETAILS
......@@ -87,10 +85,13 @@ namespace dynamicgraph {
DECLARE_SIGNAL_IN(jointsTorquesDerivative,dynamicgraph::Vector); /// estimated joints torques derivative dtau
DECLARE_SIGNAL_IN(jointsTorquesDesired, dynamicgraph::Vector); /// desired joints torques tauDes
// DECLARE_SIGNAL_IN(jointsTorquesDesiredDerivative, dynamicgraph::Vector);/// desired joints torques derivative dtauDes
DECLARE_SIGNAL_IN(dq_des, dynamicgraph::Vector); /// desired joint velocities
DECLARE_SIGNAL_IN(KpTorque, dynamicgraph::Vector); /// proportional gain for torque feedback controller
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(torque_integral_saturation, dynamicgraph::Vector); /// integral error saturation
// DECLARE_SIGNAL_IN(dq_threshold, dynamicgraph::Vector); /// velocity sign threshold
// DECLARE_SIGNAL_IN(ddq_threshold, dynamicgraph::Vector); /// acceleration sign threshold
......
......@@ -96,26 +96,43 @@ namespace dynamicgraph {
};
template<typename T>
std::string toString(const T& v)
std::string toString(const T& v, const int precision=3, const int width=-1)
{
std::stringstream ss;
ss<<v;
if(width>precision)
ss<<std::fixed<<std::setw(width)<<std::setprecision(precision)<<v;
else
ss<<std::fixed<<std::setprecision(precision)<<v;
return ss.str();
}
template<typename T>
std::string toString(const std::vector<T>& v, const std::string separator=", ")
std::string toString(const std::vector<T>& v, const int precision=3, const int width=-1,
const std::string separator=", ")
{
std::stringstream ss;
for(int i=0; i<v.size()-1; i++)
ss<<v[i]<<separator;
ss<<v[v.size()-1];
if(width>precision)
{
for(int i=0; i<v.size()-1; i++)
ss<<std::fixed<<std::setw(width)<<std::setprecision(precision)<<v[i]<<separator;
ss<<std::fixed<<std::setw(width)<<std::setprecision(precision)<<v[v.size()-1];
}
else
{
for(int i=0; i<v.size()-1; i++)
ss<<std::fixed<<std::setprecision(precision)<<v[i]<<separator;
ss<<std::fixed<<std::setprecision(precision)<<v[v.size()-1];
}
return ss.str();
}
template<typename T, int N>
std::string toString(const Eigen::Matrix<T, N, 1, 0, N, 1>& v, const std::string separator=", ",
const int precision=3, const int width=-1)
// template<typename T, int N>
// std::string toString(const Eigen::Matrix<T, N, 1, 0, N, 1>& v, const std::string separator=", ",
// const int precision=3, const int width=-1)
template<typename T>
std::string toString(const Eigen::MatrixBase<T>& v,
const int precision=3, const int width=-1, const std::string separator=", ")
{
std::stringstream ss;
if(width>precision)
......
......@@ -270,9 +270,11 @@ def create_torque_controller(robot, conf, motor_params, dt=0.001, robot_name="ro
plug(robot.estimator_ft.jointsTorques, torque_ctrl.jointsTorques);
torque_ctrl.jointsTorquesDesired.value = NJ*(0.0,);
torque_ctrl.jointsTorquesDerivative.value = NJ*(0.0,);
torque_ctrl.dq_des.value = NJ*(0.0,);
torque_ctrl.KpTorque.value = tuple(conf.k_p_torque);
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.torque_integral_saturation.value = tuple(conf.torque_integral_saturation);
torque_ctrl.coulomb_friction_compensation_percentage.value = NJ*(conf.COULOMB_FRICTION_COMPENSATION_PERCENTAGE,);
......@@ -311,6 +313,7 @@ def create_balance_controller(robot, conf, motor_params, dt, robot_name='robot')
plug(robot.estimator_ft.contactWrenchRightSole, ctrl.wrench_right_foot);
plug(robot.estimator_ft.contactWrenchLeftSole, ctrl.wrench_left_foot);
plug(ctrl.tau_des, robot.torque_ctrl.jointsTorquesDesired);
plug(ctrl.dq_admittance, robot.torque_ctrl.dq_des);
plug(ctrl.tau_des, robot.estimator_ft.tauDes);
plug(ctrl.right_foot_pos, robot.rf_traj_gen.initial_value);
......@@ -358,6 +361,8 @@ def create_balance_controller(robot, conf, motor_params, dt, robot_name='robot')
ctrl.kd_posture.value = conf.kd_posture;
ctrl.kp_pos.value = conf.kp_pos;
ctrl.kd_pos.value = conf.kd_pos;
ctrl.kp_admittance.value = conf.kp_admittance;
ctrl.ki_admittance.value = conf.ki_admittance;
ctrl.w_com.value = conf.w_com;
ctrl.w_feet.value = conf.w_feet;
......
......@@ -45,11 +45,13 @@ create_topic(robot.ros, robot.inv_dyn.left_foot_pos, 'lf_pose');
create_topic(robot.ros, robot.inv_dyn.left_foot_vel, 'lf_vel');
create_topic(robot.ros, robot.inv_dyn.left_foot_acc, 'lf_acc');
create_topic(robot.ros, robot.inv_dyn.left_foot_acc_des, 'lf_acc_des');
create_topic(robot.ros, robot.inv_dyn.dq_admittance, 'dq_adm');
#robot.torque_ctrl.velLeakingRate.value = 30*(0.,)
#robot.torque_ctrl.reset_ddq_integral();
ent.ctrl_manager.setCtrlMode('all','torque')
robot.base_estimator.set_imu_weight(1.0);
robot.inv_dyn.dq_admittance.recompute(0)
robot.base_estimator.set_imu_weight(0.0);
plug(robot.encoders.sout, robot.base_estimator.joint_positions)
#plug(robot.device.gyrometer, robot.base_estimator.gyroscope);
plug(robot.filters.estimator_kin.dx, robot.base_estimator.joint_velocities);
......
......@@ -632,6 +632,11 @@ namespace dynamicgraph
// if both weights are zero set them to a small positive value to avoid division by zero
if(wR==0.0 && wL==0.0)
{
SEND_WARNING_STREAM_MSG("The robot is flying!"+
("- forceRLEG: "+toString(ftrf.transpose()))+
"- forceLLEG: "+toString(ftlf.transpose())+
"- m_right_foot_is_stable: "+toString(m_right_foot_is_stable)+
"- m_left_foot_is_stable: "+toString(m_left_foot_is_stable));
wR = 1e-3;
wL = 1e-3;
}
......@@ -1000,7 +1005,6 @@ namespace dynamicgraph
// if both weights are zero set them to a small positive value to avoid division by zero
if(wR==0.0 && wL==0.0)
{
SEND_WARNING_STREAM_MSG("The robot is flying!!!");
wR = 1e-3;
wL = 1e-3;
}
......
......@@ -287,7 +287,7 @@ namespace dynamicgraph
for(unsigned int i=0; i<m_ctrlInputsSIN.size(); i++)
{
const dynamicgraph::Vector& ctrl = (*m_ctrlInputsSIN[i])(iter);
SEND_MSG(toString(iter)+") tau =" +toString(ctrl," ",1,4)+m_ctrlModes[i], MSG_TYPE_ERROR);
SEND_MSG(toString(iter)+") tau =" +toString(ctrl,1,4," ")+m_ctrlModes[i], MSG_TYPE_ERROR);
}
}
......
......@@ -56,14 +56,20 @@ namespace dynamicgraph
#define REQUIRE_FINITE(A) assert(is_finite(A))
//Size to be aligned "-------------------------------------------------------"
#define PROFILE_TAU_DES_COMPUTATION "InverseDynamicsBalanceController: desired tau"
#define PROFILE_HQP_SOLUTION "InverseDynamicsBalanceController: HQP"
#define PROFILE_PREPARE_INV_DYN "InverseDynamicsBalanceController: prepare inv-dyn"
#define PROFILE_READ_INPUT_SIGNALS "InverseDynamicsBalanceController: read input signals"
#define PROFILE_TAU_DES_COMPUTATION "InvDynBalCtrl: desired tau"
#define PROFILE_HQP_SOLUTION "InvDynBalCtrl: HQP"
#define PROFILE_PREPARE_INV_DYN "InvDynBalCtrl: prepare inv-dyn"
#define PROFILE_READ_INPUT_SIGNALS "InvDynBalCtrl: read input signals"
#define PROFILE_DQ_ADMITTANCE "InvDynBalCtrl: dq admittance"
#define ZERO_FORCE_THRESHOLD 1e-3
#define INPUT_SIGNALS m_com_ref_posSIN \
#define ADMITTANCE_SIGNALS m_kp_admittanceSIN \
<< m_ki_admittanceSIN \
<< m_wrench_left_footSIN \
<< m_wrench_right_footSIN
#define INV_DYN_SIGNALS m_com_ref_posSIN \
<< m_com_ref_velSIN \
<< m_com_ref_accSIN \
<< m_rf_ref_posSIN \
......@@ -117,10 +123,10 @@ namespace dynamicgraph
<< m_qSIN \
<< m_vSIN \
<< m_wrench_baseSIN \
<< m_wrench_left_footSIN \
<< m_wrench_right_footSIN \
<< m_active_jointsSIN
#define INPUT_SIGNALS ADMITTANCE_SIGNALS << INV_DYN_SIGNALS
#define OUTPUT_SIGNALS m_tau_desSOUT \
<< m_f_des_right_footSOUT \
<< m_f_des_left_footSOUT \
......@@ -147,6 +153,7 @@ namespace dynamicgraph
<< m_right_foot_acc_desSOUT \
<< m_left_foot_acc_desSOUT \
<< m_dv_desSOUT \
<< m_dq_admittanceSOUT \
<< m_MSOUT
/// Define EntityClassName here rather than in the header file
......@@ -195,6 +202,8 @@ namespace dynamicgraph
,CONSTRUCT_SIGNAL_IN(kd_posture, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(kp_pos, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(kd_pos, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(kp_admittance, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(ki_admittance, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(w_com, double)
,CONSTRUCT_SIGNAL_IN(w_feet, double)
,CONSTRUCT_SIGNAL_IN(w_posture, double)
......@@ -223,7 +232,7 @@ namespace dynamicgraph
,CONSTRUCT_SIGNAL_IN(wrench_left_foot, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(wrench_right_foot, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(active_joints, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_OUT(tau_des, dynamicgraph::Vector, INPUT_SIGNALS)
,CONSTRUCT_SIGNAL_OUT(tau_des, dynamicgraph::Vector, INV_DYN_SIGNALS)
,CONSTRUCT_SIGNAL_OUT(f_des_right_foot, dynamicgraph::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(f_des_left_foot, dynamicgraph::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(zmp_des_right_foot, dynamicgraph::Vector, m_f_des_right_footSOUT)
......@@ -241,6 +250,9 @@ namespace dynamicgraph
m_zmp_left_footSOUT<<
m_zmp_right_footSOUT)
,CONSTRUCT_SIGNAL_OUT(dv_des, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(dq_admittance, dg::Vector, ADMITTANCE_SIGNALS <<
m_f_des_right_footSOUT <<
m_f_des_left_footSOUT)
,CONSTRUCT_SIGNAL_OUT(M, dg::Matrix, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(com, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(com_vel, dg::Vector, m_tau_desSOUT)
......@@ -443,6 +455,8 @@ namespace dynamicgraph
m_f.setZero(24);
m_q_urdf.setZero(m_robot->nq());
m_v_urdf.setZero(m_robot->nv());
m_J_RF.setZero(6, m_robot->nv());
m_J_LF.setZero(6, m_robot->nv());
m_invDyn = new InverseDynamicsFormulationAccForce("invdyn", *m_robot);
......@@ -768,8 +782,8 @@ namespace dynamicgraph
{
SEND_ERROR_STREAM_MSG("HQP solver failed to find a solution: "+toString(sol.status));
SEND_DEBUG_STREAM_MSG(tsid::solvers::HQPDataToString(hqpData, false));
SEND_DEBUG_STREAM_MSG("q="+toString(q_sot, ", ", 1,5));
SEND_DEBUG_STREAM_MSG("v="+toString(v_sot, ", ", 1,5));
SEND_DEBUG_STREAM_MSG("q="+toString(q_sot.transpose(),1,5));
SEND_DEBUG_STREAM_MSG("v="+toString(v_sot.transpose(),1,5));
s.setZero();
return s;
}
......@@ -865,6 +879,51 @@ namespace dynamicgraph
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(dq_admittance, dynamicgraph::Vector)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal dq_admittance before initialization!");
return s;
}
if(s.size()!=m_robot_util->m_nbJoints)
s.resize(m_robot_util->m_nbJoints);
const Eigen::Vector6d& f_des_RF = m_f_des_right_footSOUT(iter);
const Eigen::Vector6d& f_des_LF = m_f_des_left_footSOUT(iter);
const Eigen::Vector6d& f_RF = m_wrench_right_footSIN(iter);
const Eigen::Vector6d& f_LF = m_wrench_left_footSIN(iter);
const Eigen::Vector6d& kp = m_kp_admittanceSIN(iter);
const Eigen::Vector6d& ki = m_ki_admittanceSIN(iter);
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);
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);
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);
// 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);
// 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);
m_robot_util->joints_urdf_to_sot(dq_adm_urdf, s);
getProfiler().stop(PROFILE_DQ_ADMITTANCE);
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(com_acc_des, dynamicgraph::Vector)
{
if(!m_initSucceeded)
......
......@@ -39,9 +39,11 @@ 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 ALL_INPUT_SIGNALS ESTIMATOR_INPUT_SIGNALS << TORQUE_INTEGRAL_INPUT_SIGNALS << \
TORQUE_CONTROL_INPUT_SIGNALS << MODEL_INPUT_SIGNALS
TORQUE_CONTROL_INPUT_SIGNALS << VEL_CONTROL_INPUT_SIGNALS << \
MODEL_INPUT_SIGNALS
#define ALL_OUTPUT_SIGNALS m_uSOUT << m_torque_error_integralSOUT << m_smoothSignDqSOUT
namespace dynamicgraph = ::dynamicgraph;
......@@ -63,14 +65,16 @@ namespace dynamicgraph
JointTorqueController::
JointTorqueController( const std::string & name )
: Entity(name)
,CONSTRUCT_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(jointsAccelerations, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(jointsTorques, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(jointsTorquesDesired, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(jointsTorquesDerivative,dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(jointsAccelerations, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(jointsTorques, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(jointsTorquesDesired, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(jointsTorquesDerivative, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(dq_des, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(KpTorque, dynamicgraph::Vector) // proportional gain for torque feedback controller
,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(coulomb_friction_compensation_percentage, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(motorParameterKt_p, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(motorParameterKt_n, dynamicgraph::Vector)
......@@ -84,6 +88,7 @@ namespace dynamicgraph
,CONSTRUCT_SIGNAL_IN(torque_integral_saturation, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_OUT(u, dynamicgraph::Vector, ESTIMATOR_INPUT_SIGNALS <<
TORQUE_CONTROL_INPUT_SIGNALS <<
VEL_CONTROL_INPUT_SIGNALS <<
MODEL_INPUT_SIGNALS <<
m_torque_error_integralSOUT)
,CONSTRUCT_SIGNAL_OUT(torque_error_integral, dynamicgraph::Vector, m_jointsTorquesSIN <<
......@@ -127,15 +132,15 @@ namespace dynamicgraph
/* Retrieve m_robot_util informations */
std::string localName(robot_ref);
if (isNameInRobotUtil(localName))
{
m_robot_util = getRobotUtil(localName);
}
else
{
SEND_MSG("You should have an entity controller manager initialized before",MSG_TYPE_ERROR);
return;
}
{
m_robot_util = getRobotUtil(localName);
}
else
{
SEND_MSG("You should have an entity controller manager initialized before",MSG_TYPE_ERROR);
return;
}
m_dt = timestep;
m_tau_star.setZero(m_robot_util->m_nbJoints);
m_current_des.setZero(m_robot_util->m_nbJoints);
......@@ -159,8 +164,10 @@ namespace dynamicgraph
const Eigen::VectorXd& dtau = m_jointsTorquesDerivativeSIN(iter);
const Eigen::VectorXd& tau_d = m_jointsTorquesDesiredSIN(iter);
// const Eigen::VectorXd& dtau_d = m_jointsTorquesDesiredDerivativeSIN(iter);
const Eigen::VectorXd& dq_des = m_dq_desSIN(iter);
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& 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);
......@@ -182,7 +189,9 @@ namespace dynamicgraph
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), ddq(i+offset),
m_current_des(i) = motorModel.getCurrent(m_tau_star(i),
dq(i+offset)+kd_vel(i)*(dq_des(i)-dq(i+offset)),
ddq(i+offset),
motorParameterKt_p(i),
motorParameterKt_n(i),
motorParameterKf_p(i)*colFricCompPerc(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