Avoid allocation for rotating the jacobian into world orientation

include/tsid/tasks/task-se3-equality.hpp

@@ -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;
     };
-    
+
   }
 }
 

src/tasks/task-se3-equality.cpp

@@ -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();