Commit 1eb2b2eb authored by Julian Viereck's avatar Julian Viereck
Browse files

Avoid allocation for rotating the jacobian into world orientation

parent a89f9735
......@@ -34,7 +34,7 @@ namespace tsid
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef math::Index Index;
typedef trajectories::TrajectorySample TrajectorySample;
typedef math::Vector Vector;
......@@ -80,7 +80,7 @@ namespace tsid
void useLocalFrame(bool local_frame);
protected:
std::string m_frame_name;
Index m_frame_id;
Motion m_p_error, m_v_error;
......@@ -94,11 +94,12 @@ namespace tsid
Vector m_a_des;
Motion m_drift;
Matrix6x m_J;
Matrix6x m_J_rotated;
ConstraintEquality m_constraint;
TrajectorySample m_ref;
bool m_local_frame;
};
}
}
......
......@@ -52,6 +52,7 @@ namespace tsid
m_Kd.setZero(6);
m_a_des.setZero(6);
m_J.setZero(6, robot.nv());
m_J_rotated.setZero(6, robot.nv());
m_mask.resize(6);
m_mask.fill(1.);
......@@ -189,7 +190,9 @@ namespace tsid
- m_Kd.cwiseProduct(m_v_error.toVector())
+ m_a_ref.toVector();
m_J = m_wMl.toActionMatrix() * m_J;
// Use an explicit temporary `m_J_rotated` here to avoid allocations.
m_J_rotated.noalias() = m_wMl.toActionMatrix() * m_J;
m_J = m_J_rotated;
}
m_v_error_vec = m_v_error.toVector();
......
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