Commit b7ece956 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

Improve TaskSE3Equality: apply mask before returning task acceleration,...

Improve TaskSE3Equality: apply mask before returning task acceleration, desired task acceleration, position error and velocity error.
parent a0ccdf7b
......@@ -60,13 +60,32 @@ namespace tsid
void setReference(TrajectorySample & ref);
const TrajectorySample & getReference() const;
/** Return the desired task acceleration (after applying the specified mask).
* The value is expressed in local frame is the local_frame flag is true,
* otherwise it is expressed in a local world-oriented frame.
*/
const Vector & getDesiredAcceleration() const;
/** Return the task acceleration (after applying the specified mask).
* The value is expressed in local frame is the local_frame flag is true,
* otherwise it is expressed in a local world-oriented frame.
*/
Vector getAcceleration(ConstRefVector dv) const;
virtual void setMask(math::ConstRefVector mask);
/** Return the position tracking error (after applying the specified mask).
* The error is expressed in local frame is the local_frame flag is true,
* otherwise it is expressed in a local world-oriented frame.
*/
const Vector & position_error() const;
/** Return the velocity tracking error (after applying the specified mask).
* The error is expressed in local frame is the local_frame flag is true,
* otherwise it is expressed in a local world-oriented frame.
*/
const Vector & velocity_error() const;
const Vector & position() const;
const Vector & velocity() const;
const Vector & position_ref() const;
......@@ -81,10 +100,10 @@ namespace tsid
/**
* @brief Specifies if the jacobian and desired acceloration should be
* expressed in the local frame or the local world oriented frame.
* expressed in the local frame or the local world-oriented frame.
*
* @param local_frame If true, represent jacobian and acceloration in the
* local frame. If false, represent them in the local world oriented frame.
* local frame. If false, represent them in the local world-oriented frame.
*/
void useLocalFrame(bool local_frame);
......@@ -94,14 +113,16 @@ namespace tsid
Index m_frame_id;
Motion m_p_error, m_v_error;
Vector m_p_error_vec, m_v_error_vec;
Vector m_p_error_masked_vec, m_v_error_masked_vec;
Vector m_p, m_v;
Vector m_p_ref, m_v_ref_vec;
Motion m_v_ref, m_a_ref;
SE3 m_M_ref, m_wMl;
Vector m_Kp;
Vector m_Kd;
Vector m_a_des;
Vector m_a_des, m_a_des_masked;
Motion m_drift;
Vector m_drift_masked;
Matrix6x m_J;
Matrix6x m_J_rotated;
ConstraintEquality m_constraint;
......
......@@ -56,6 +56,7 @@ namespace tsid
m_mask.resize(6);
m_mask.fill(1.);
setMask(m_mask);
m_local_frame = true;
}
......@@ -63,7 +64,12 @@ namespace tsid
void TaskSE3Equality::setMask(math::ConstRefVector mask)
{
TaskMotion::setMask(mask);
m_constraint.resize(dim(), (unsigned int)m_J.cols());
int n = dim();
m_constraint.resize(n, (unsigned int)m_J.cols());
m_p_error_masked_vec.resize(n);
m_v_error_masked_vec.resize(n);
m_drift_masked.resize(n);
m_a_des_masked.resize(n);
}
int TaskSE3Equality::dim() const
......@@ -102,12 +108,12 @@ namespace tsid
const Vector & TaskSE3Equality::position_error() const
{
return m_p_error_vec;
return m_p_error_masked_vec;
}
const Vector & TaskSE3Equality::velocity_error() const
{
return m_v_error_vec;
return m_v_error_masked_vec;
}
const Vector & TaskSE3Equality::position() const
......@@ -132,12 +138,12 @@ namespace tsid
const Vector & TaskSE3Equality::getDesiredAcceleration() const
{
return m_a_des;
return m_a_des_masked;
}
Vector TaskSE3Equality::getAcceleration(ConstRefVector dv) const
{
return m_constraint.matrix()*dv + m_drift.toVector();
return m_constraint.matrix()*dv + m_drift_masked;
}
Index TaskSE3Equality::frame_id() const
......@@ -192,7 +198,7 @@ namespace tsid
m_drift = m_wMl.act(m_drift);
// desired acc in local oriented frame
// desired acc in local world-oriented frame
m_a_des = - m_Kp.cwiseProduct(m_p_error_vec)
- m_Kd.cwiseProduct(m_v_error.toVector())
+ m_a_ref.toVector();
......@@ -212,6 +218,10 @@ namespace tsid
m_constraint.matrix().row(idx) = m_J.row(i);
m_constraint.vector().row(idx) = (m_a_des - m_drift.toVector()).row(i);
m_a_des_masked(idx) = m_a_des(i);
m_drift_masked(idx) = m_drift.toVector()(i);
m_p_error_masked_vec(idx) = m_p_error_vec(i);
m_v_error_masked_vec(idx) = m_v_error_vec(i);
idx += 1;
}
......
Markdown is supported
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