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 ...@@ -34,7 +34,7 @@ namespace tsid
{ {
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef math::Index Index; typedef math::Index Index;
typedef trajectories::TrajectorySample TrajectorySample; typedef trajectories::TrajectorySample TrajectorySample;
typedef math::Vector Vector; typedef math::Vector Vector;
...@@ -80,7 +80,7 @@ namespace tsid ...@@ -80,7 +80,7 @@ namespace tsid
void useLocalFrame(bool local_frame); void useLocalFrame(bool local_frame);
protected: protected:
std::string m_frame_name; std::string m_frame_name;
Index m_frame_id; Index m_frame_id;
Motion m_p_error, m_v_error; Motion m_p_error, m_v_error;
...@@ -94,11 +94,12 @@ namespace tsid ...@@ -94,11 +94,12 @@ namespace tsid
Vector m_a_des; Vector m_a_des;
Motion m_drift; Motion m_drift;
Matrix6x m_J; Matrix6x m_J;
Matrix6x m_J_rotated;
ConstraintEquality m_constraint; ConstraintEquality m_constraint;
TrajectorySample m_ref; TrajectorySample m_ref;
bool m_local_frame; bool m_local_frame;
}; };
} }
} }
......
...@@ -52,6 +52,7 @@ namespace tsid ...@@ -52,6 +52,7 @@ namespace tsid
m_Kd.setZero(6); m_Kd.setZero(6);
m_a_des.setZero(6); m_a_des.setZero(6);
m_J.setZero(6, robot.nv()); m_J.setZero(6, robot.nv());
m_J_rotated.setZero(6, robot.nv());
m_mask.resize(6); m_mask.resize(6);
m_mask.fill(1.); m_mask.fill(1.);
...@@ -189,7 +190,9 @@ namespace tsid ...@@ -189,7 +190,9 @@ namespace tsid
- m_Kd.cwiseProduct(m_v_error.toVector()) - m_Kd.cwiseProduct(m_v_error.toVector())
+ m_a_ref.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(); 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