joints: add accessor to JointData fields

src/multibody/joint/joint-data-base.hpp

@@ -22,7 +22,9 @@
   typedef TYPENAME traits<Joint>::UTypeConstRef UTypeConstRef;      \
   typedef TYPENAME traits<Joint>::UTypeRef UTypeRef;      \
   typedef TYPENAME traits<Joint>::DTypeConstRef DTypeConstRef;      \
-  typedef TYPENAME traits<Joint>::UDTypeConstRef UDTypeConstRef
+  typedef TYPENAME traits<Joint>::DTypeRef DTypeRef;      \
+  typedef TYPENAME traits<Joint>::UDTypeConstRef UDTypeConstRef; \
+  typedef TYPENAME traits<Joint>::UDTypeRef UDTypeRef
   
 #ifdef __clang__
 
@@ -53,7 +55,9 @@
   UTypeConstRef U_accessor() const { return U; } \
   UTypeRef U_accessor() { return U; } \
   DTypeConstRef Dinv_accessor() const { return Dinv; } \
-  UDTypeConstRef UDinv_accessor() const { return UDinv; }
+  DTypeRef Dinv_accessor() { return Dinv; } \
+  UDTypeConstRef UDinv_accessor() const { return UDinv; } \
+  UDTypeRef UDinv_accessor() { return UDinv; }
   
 #define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE \
   typedef const Constraint_t & ConstraintTypeConstRef; \
@@ -67,7 +71,9 @@
   typedef const U_t & UTypeConstRef; \
   typedef U_t & UTypeRef; \
   typedef const D_t & DTypeConstRef; \
-  typedef const UD_t & UDTypeConstRef;
+  typedef D_t & DTypeRef; \
+  typedef const UD_t & UDTypeConstRef; \
+  typedef UD_t & UDTypeRef;
 
 namespace pinocchio
 {
@@ -95,7 +101,9 @@ namespace pinocchio
     UTypeConstRef U() const     { return derived().U_accessor(); }
     UTypeRef U()           { return derived().U_accessor(); }
     DTypeConstRef Dinv() const  { return derived().Dinv_accessor(); }
+    DTypeRef Dinv()  { return derived().Dinv_accessor(); }
     UDTypeConstRef UDinv() const { return derived().UDinv_accessor(); }
+    UDTypeRef UDinv() { return derived().UDinv_accessor(); }
 
     std::string shortname() const { return derived().shortname(); }
     static std::string classname() { return Derived::classname(); }

src/multibody/joint/joint-generic.hpp

@@ -54,7 +54,9 @@ namespace pinocchio
     typedef U_t UTypeConstRef;
     typedef U_t UTypeRef;
     typedef D_t DTypeConstRef;
+    typedef D_t DTypeRef;
     typedef UD_t UDTypeConstRef;
+    typedef UD_t UDTypeRef;
 
     typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> ConfigVector_t;
     typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> TangentVector_t;
@@ -109,12 +111,11 @@ namespace pinocchio
       BOOST_MPL_ASSERT((boost::mpl::contains<typename JointDataVariant::types,JointDataDerived>));
     }
     
-    /// Define all the standard accessors
+    // Define all the standard accessors
     Constraint_t S_accessor() const { return S(); }
     Transformation_t M_accessor() const { return M(); }
     Motion_t v_accessor() const { return v(); }
     Bias_t c_accessor() const { return c(); }
-    Bias_t c_accessor() { return c(); }
     U_t U_accessor() const { return U(); }
     D_t Dinv_accessor() const { return Dinv(); }
     UD_t UDinv_accessor() const { return UDinv(); }

src/multibody/joint/joint-mimic.hpp

@@ -308,18 +308,26 @@ namespace pinocchio
     }
     
     // Accessors
-    ConstraintTypeConstRef S_accessor() const { return S; }
-    ConstraintTypeRef S_accessor() { return S; }
-    TansformTypeConstRef M_accessor() const { return jdata_ref.M; }
-    TansformTypeRef M_accessor() { return jdata_ref.M; }
-    MotionTypeConstRef v_accessor() const { return jdata_ref.v; }
-    MotionTypeRef v_accessor() { return jdata_ref.v; }
-    BiasTypeConstRef c_accessor() const { return jdata_ref.c; }
-    BiasTypeRef c_accessor() { return jdata_ref.c; }
-    UTypeConstRef U_accessor() const { return jdata_ref.U; }
-    UTypeRef U_accessor() { return jdata_ref.U; }
-    DTypeConstRef Dinv_accessor() const { return jdata_ref.Dinv; }
-    UDTypeConstRef UDinv_accessor() const { return jdata_ref.UDinv; }
+    ConstraintTypeConstRef S_accessor() const { return m_S; }
+    ConstraintTypeRef S_accessor() { return m_S; }
+    
+    TansformTypeConstRef M_accessor() const { return m_jdata_ref.M; }
+    TansformTypeRef M_accessor() { return m_jdata_ref.M; }
+    
+    MotionTypeConstRef v_accessor() const { return m_jdata_ref.v; }
+    MotionTypeRef v_accessor() { return m_jdata_ref.v; }
+    
+    BiasTypeConstRef c_accessor() const { return m_jdata_ref.c; }
+    BiasTypeRef c_accessor() { return m_jdata_ref.c; }
+    
+    UTypeConstRef U_accessor() const { return m_jdata_ref.U; }
+    UTypeRef U_accessor() { return m_jdata_ref.U; }
+    
+    DTypeConstRef Dinv_accessor() const { return m_jdata_ref.Dinv; }
+    DTypeRef Dinv_accessor() { return m_jdata_ref.Dinv; }
+    
+    UDTypeConstRef UDinv_accessor() const { return m_jdata_ref.UDinv; }
+    UDTypeRef UDinv_accessor() { return m_jdata_ref.UDinv; }
     
     template<class JointModel>
     friend struct JointModelMimic;