Commit 9a544fc7 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

[torque-ctrl] Add saturation for joint velocity integral

parent dbebb310
......@@ -79,6 +79,7 @@ namespace dynamicgraph {
DYNAMIC_GRAPH_ENTITY_DECL();
public: /* --- SIGNALS --- */
DECLARE_SIGNAL_IN(jointsPositions, dynamicgraph::Vector); /// q
DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector); /// dq
DECLARE_SIGNAL_IN(jointsAccelerations, dynamicgraph::Vector); /// ddq
DECLARE_SIGNAL_IN(jointsTorques, dynamicgraph::Vector); /// estimated joints torques tau
......@@ -120,6 +121,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_dqDesIntegral; /// integral of the desired velocity
Eigen::VectorXd m_dqErrIntegral; /// integral of the velocity error
RobotUtil * m_robot_util;
......
......@@ -33,7 +33,7 @@ namespace dynamicgraph
<< m_motorParameterKv_pSIN << m_motorParameterKv_nSIN \
<< m_motorParameterKa_pSIN << m_motorParameterKa_nSIN << m_polySignDqSIN
#define ESTIMATOR_INPUT_SIGNALS m_jointsVelocitiesSIN << m_jointsAccelerationsSIN << \
#define ESTIMATOR_INPUT_SIGNALS m_jointsPositionsSIN << m_jointsVelocitiesSIN << m_jointsAccelerationsSIN << \
m_jointsTorquesSIN << m_jointsTorquesDerivativeSIN
#define TORQUE_INTEGRAL_INPUT_SIGNALS m_KiTorqueSIN << m_torque_integral_saturationSIN
......@@ -65,6 +65,7 @@ namespace dynamicgraph
JointTorqueController::
JointTorqueController( const std::string & name )
: Entity(name)
,CONSTRUCT_SIGNAL_IN(jointsPositions, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(jointsAccelerations, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(jointsTorques, dynamicgraph::Vector)
......@@ -146,6 +147,7 @@ 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_dqDesIntegral.setZero(m_robot_util->m_nbJoints);
m_dqErrIntegral.setZero(m_robot_util->m_nbJoints);
}
......@@ -161,6 +163,7 @@ namespace dynamicgraph
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
{
const Eigen::VectorXd& q = m_jointsPositionsSIN(iter);
const Eigen::VectorXd& dq = m_jointsVelocitiesSIN(iter);
const Eigen::VectorXd& ddq = m_jointsAccelerationsSIN(iter);
const Eigen::VectorXd& tau = m_jointsTorquesSIN(iter);
......@@ -192,11 +195,29 @@ namespace dynamicgraph
offset = 6;
m_dqErrIntegral += m_dt * ki_vel.cwiseProduct(dq_des-dq);
const Eigen::VectorXd& err_int_sat = m_torque_integral_saturationSIN(iter);
// saturate
bool saturating = false;
for(int i=0; i<(int)m_robot_util->m_nbJoints; i++)
{
if(m_dqErrIntegral(i) > err_int_sat(i))
{
saturating = true;
m_dqErrIntegral(i) = err_int_sat(i);
}
else if(m_dqErrIntegral(i) < -err_int_sat(i))
{
saturating = true;
m_dqErrIntegral(i) = -err_int_sat(i);
}
}
if(saturating)
SEND_INFO_STREAM_MSG("Saturate dqErr integral: "+toString(m_dqErrIntegral.head<12>()));
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))+m_dqErrIntegral(i),
dq(i+offset)+kd_vel(i)*(dq_des(i)-dq(i+offset)) + m_dqErrIntegral(i), //ki_vel(i)*(m_dqDesIntegral(i)-q(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