[task-se3-equality] Add method to get frame-id

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

@@ -74,6 +74,8 @@ namespace tsid
       void Kp(ConstRefVector Kp);
       void Kd(ConstRefVector Kp);
 
+      Index frame_id() const;
+
     protected:
       
       std::string m_frame_name;

src/tasks/task-se3-equality.cpp

@@ -129,6 +129,11 @@ namespace tsid
       return m_constraint.matrix()*dv + m_drift.toVector();
     }
 
+    Index TaskSE3Equality::frame_id() const
+    {
+      return m_frame_id;
+    }
+
     const ConstraintBase & TaskSE3Equality::getConstraint() const
     {
       return m_constraint;