[frames] frameVelocity and frameAcceleration return value in C++

bindings/python/algorithm/expose-frames.cpp

@@ -58,26 +58,6 @@ namespace pinocchio
       updateFramePlacements(model,data);
   
       return get_frame_jacobian_time_variation_proxy(model, data, frame_id, rf);
-    }        
-
-    static Motion get_frame_velocity_proxy(const Model & model,
-                                           Data & data,
-                                           const Model::FrameIndex frame_id
-                                           )
-    {
-      Motion v;
-      getFrameVelocity(model,data,frame_id,v);
-      return v;
-    }
-
-    static Motion get_frame_acceleration_proxy(const Model & model,
-                                               Data & data,
-                                               const Model::FrameIndex frame_id
-                                               )
-    {
-      Motion a;
-      getFrameAcceleration(model,data,frame_id,a);
-      return a;
     }
     
     void exposeFramesAlgo()
@@ -97,13 +77,13 @@ namespace pinocchio
               bp::return_value_policy<bp::return_by_value>());
 
       bp::def("getFrameVelocity",
-              (Motion (*)(const Model &, Data &, const Model::FrameIndex))&get_frame_velocity_proxy,
+              &getFrameVelocity<double,0,JointCollectionDefaultTpl>,
               bp::args("Model","Data","Operational frame ID (int)"),
               "Returns the spatial velocity of the frame expressed in the LOCAL frame coordinate system."
               "Fist or second order forwardKinematics should be called first.");
 
       bp::def("getFrameAcceleration",
-              (Motion (*)(const Model &, Data &, const Model::FrameIndex))&get_frame_acceleration_proxy,
+              &getFrameAcceleration<double,0,JointCollectionDefaultTpl>,
               bp::args("Model","Data","Operational frame ID (int)"),
               "Returns the spatial velocity of the frame expressed in the LOCAL frame coordinate system."
               "Second order forwardKinematics should be called first.");

src/algorithm/frames.hpp

@@ -60,6 +60,24 @@ namespace pinocchio
                                       const Eigen::MatrixBase<ConfigVectorType> & q);
 
 
+  /**
+   * @brief      Returns the spatial velocity of the frame expressed in the LOCAL frame coordinate system.
+   *             You must first call pinocchio::forwardKinematics to update placement and velocity values in data structure.
+   *
+   * @param[in]  model       The kinematic model
+   * @param[in]  data        Data associated to model
+   * @param[in]  frame_id    Id of the operational Frame
+   *
+   * @return     The spatial velocity of the Frame expressed in the coordinates Frame.
+   *
+   * @warning    Fist or second order forwardKinematics should have been called first
+   */
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+  inline MotionTpl<Scalar, Options>
+  getFrameVelocity(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                   const DataTpl<Scalar,Options,JointCollectionTpl> & data,
+                   const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id);
+
   /**
    * @brief      Returns the spatial velocity of the frame expressed in the LOCAL frame coordinate system.
    *             You must first call pinocchio::forwardKinematics to update placement and velocity values in data structure.
@@ -69,13 +87,58 @@ namespace pinocchio
    * @param[in]  frame_id    Id of the operational Frame
    * @param[out] frame_v     The spatial velocity of the Frame expressed in the coordinates Frame.
    *
+   * @deprecated This function is now deprecated. Use the return-value version instead (since: 19 feb 2019)
+   *
    * @warning    Fist or second order forwardKinematics should have been called first
    */
   template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename MotionLike>
-  void getFrameVelocity(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
-                        const DataTpl<Scalar,Options,JointCollectionTpl> & data,
-                        const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
-                        const MotionDense<MotionLike> & frame_v);
+  PINOCCHIO_DEPRECATED
+  inline void getFrameVelocity(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                               const DataTpl<Scalar,Options,JointCollectionTpl> & data,
+                               const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
+                               const MotionDense<MotionLike> & frame_v)
+  {
+    frame_v.derived() = getFrameVelocity(model, data, frame_id);
+  }
+
+  /**
+   * @brief      Returns the spatial acceleration of the frame expressed in the LOCAL frame coordinate system.
+   *             You must first call pinocchio::forwardKinematics to update placement values in data structure.
+   *
+   * @param[in]  model       The kinematic model
+   * @param[in]  data        Data associated to model
+   * @param[in]  frame_id    Id of the operational Frame
+   *
+   * @return The spatial acceleration of the Frame expressed in the coordinates Frame.
+   *
+   * @warning    Second order forwardKinematics should have been called first
+   */
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+  inline MotionTpl<Scalar, Options>
+  getFrameAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                       const DataTpl<Scalar,Options,JointCollectionTpl> & data,
+                       const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id);
+
+  /**
+   * @brief      Returns the spatial acceleration of the frame expressed in the LOCAL frame coordinate system.
+   *             You must first call pinocchio::forwardKinematics to update placement values in data structure.
+   *
+   * @param[in]  model       The kinematic model
+   * @param[in]  data        Data associated to model
+   * @param[in]  frame_id    Id of the operational Frame
+   * @param[out] frame_a     The spatial acceleration of the Frame expressed in the coordinates Frame.
+   *
+   * @deprecated This function is now deprecated. Use the return-value version instead (since: 19 feb 2019)
+   *
+   * @warning    Second order forwardKinematics should have been called first
+   */
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename MotionLike>
+  PINOCCHIO_DEPRECATED
+  inline void getFrameAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                                   const DataTpl<Scalar,Options,JointCollectionTpl> & data,
+                                   const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
+                                   const MotionDense<MotionLike> & frame_a)
+  { frame_a.derived() = getFrameAcceleration(model, data, frame_id); }
 
   /**
    * @brief      Returns the jacobian of the frame expressed either expressed in the LOCAL frame coordinate system or in the WORLD coordinate system,
@@ -131,23 +194,6 @@ namespace pinocchio
                             const FrameIndex frameId,
                             const Eigen::MatrixBase<Matrix6Like> & J);
 
-   /**
-   * @brief      Returns the spatial acceleration of the frame expressed in the LOCAL frame coordinate system.
-   *             You must first call pinocchio::forwardKinematics to update placement values in data structure.
-   *
-   * @param[in]  model       The kinematic model
-   * @param[in]  data        Data associated to model
-   * @param[in]  frame_id    Id of the operational Frame
-   * @param[out] frame_a     The spatial acceleration of the Frame expressed in the coordinates Frame.
-   *
-   * @warning    Second order forwardKinematics should have been called first
-   */
-  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename MotionLike>
-  void getFrameAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
-                            const DataTpl<Scalar,Options,JointCollectionTpl> & data,
-                            const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
-                            const MotionDense<MotionLike> & frame_a);
-
   /**
    * @brief      Returns the jacobian of the frame expresssed either expressed in the LOCAL frame coordinate system or in the WORLD coordinate system,
    *             depending on the value of rf.
@@ -155,7 +201,7 @@ namespace pinocchio
    *
    * @tparam     rf Reference frame in which the columns of the Jacobian are expressed.
    * @deprecated This function is now deprecated. Please call pinocchio::getFrameJacobian for same functionality.
-   
+   *
    * @remark     Similarly to pinocchio::getJointJacobian with LOCAL or WORLD parameters, if rf == LOCAL, this function returns the Jacobian of the frame expressed
    *             in the local coordinates of the frame, or if rl == WORDL, it returns the Jacobian expressed of the point coincident with the origin
    *             and expressed in a coordinate system aligned with the WORLD.

src/algorithm/frames.hxx

@@ -74,11 +74,11 @@ namespace pinocchio
     updateFramePlacements(model, data);
   }
 
-  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename MotionLike>
-  void getFrameVelocity(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
-                        const DataTpl<Scalar,Options,JointCollectionTpl> & data,
-                        const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
-                        const MotionDense<MotionLike> & frame_v)
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+  inline MotionTpl<Scalar, Options>
+  getFrameVelocity(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                   const DataTpl<Scalar,Options,JointCollectionTpl> & data,
+                   const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id)
   {
     assert(model.check(data) && "data is not consistent with model.");
     
@@ -86,14 +86,14 @@ namespace pinocchio
 
     const typename Model::Frame & frame = model.frames[frame_id];
     const typename Model::JointIndex & parent = frame.parent;
-    const_cast<MotionLike &>(frame_v.derived()) = frame.placement.actInv(data.v[parent]);
-  }
+    return frame.placement.actInv(data.v[parent]);
+  }  
 
-  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename MotionLike>
-  void getFrameAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
-                            const DataTpl<Scalar,Options,JointCollectionTpl> & data,
-                            const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
-                            const MotionDense<MotionLike> & frame_a)
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+  inline MotionTpl<Scalar, Options>
+  getFrameAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                       const DataTpl<Scalar,Options,JointCollectionTpl> & data,
+                       const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id)
   {
     assert(model.check(data) && "data is not consistent with model.");
 
@@ -101,7 +101,7 @@ namespace pinocchio
     
     const typename Model::Frame & frame = model.frames[frame_id];
     const typename Model::JointIndex & parent = frame.parent;
-    const_cast<MotionLike &>(frame_a.derived()) = frame.placement.actInv(data.a[parent]);
+    return frame.placement.actInv(data.a[parent]);
   }
   
   template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>

unittest/frames.cpp

@@ -148,8 +148,7 @@ BOOST_AUTO_TEST_CASE ( test_velocity )
   VectorXd v = VectorXd::Ones(model.nv);
   forwardKinematics(model, data, q, v);
 
-  Motion vf;
-  getFrameVelocity(model, data, frame_idx, vf);
+  Motion vf = getFrameVelocity(model, data, frame_idx);
 
   BOOST_CHECK(vf.isApprox(framePlacement.actInv(data.v[parent_idx])));
 }
@@ -174,8 +173,7 @@ BOOST_AUTO_TEST_CASE ( test_acceleration )
   VectorXd a = VectorXd::Ones(model.nv);
   forwardKinematics(model, data, q, v, a);
 
-  Motion af;
-  getFrameAcceleration(model, data, frame_idx, af);
+  Motion af = getFrameAcceleration(model, data, frame_idx);
 
   BOOST_CHECK(af.isApprox(framePlacement.actInv(data.a[parent_idx])));
 }