[frames] Add optional reference frame to getFrameAcceleration

src/algorithm/frames.hpp

@@ -101,14 +101,15 @@ namespace pinocchio
                    const ReferenceFrame rf = ReferenceFrame::LOCAL);
 
   /**
-   * @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.
+   * @brief      Returns the spatial acceleration of the Frame expressed in the desired reference frame.
+   *             You must first call pinocchio::forwardKinematics to update placement, velocity and acceleration 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[in]  rf          Reference frame in which the acceleration is expressed.
    *
-   * @return The spatial acceleration of the Frame expressed in the coordinates Frame.
+   * @return The spatial acceleration of the Frame expressed in the desired reference frame.
    *
    * @warning    Second order forwardKinematics should have been called first
    */
@@ -116,7 +117,8 @@ namespace pinocchio
   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);
+                       const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
+                       const ReferenceFrame rf = ReferenceFrame::LOCAL);
 
   /**
    * @brief      Returns the jacobian of the frame expressed either expressed in the LOCAL frame coordinate system or in the WORLD coordinate system,

src/algorithm/frames.hxx

@@ -94,15 +94,29 @@ namespace pinocchio
   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)
+                       const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
+                       const ReferenceFrame rf)
   {
     assert(model.check(data) && "data is not consistent with model.");
 
     typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
-    
+    typedef typename Model::Motion Motion;
+
     const typename Model::Frame & frame = model.frames[frame_id];
-    const typename Model::JointIndex & parent = frame.parent;
-    return frame.placement.actInv(data.a[parent]);
+    const typename Model::SE3 & oMi = data.oMi[frame.parent];
+    const typename Model::Motion & a = data.a[frame.parent];
+    switch(rf)
+    {
+      case ReferenceFrame::LOCAL:
+        return frame.placement.actInv(a);
+      case ReferenceFrame::WORLD:
+        return oMi.act(a);
+      case ReferenceFrame::LOCAL_WORLD_ALIGNED:
+        return Motion(oMi.rotation() * (a.linear() + a.angular().cross(frame.placement.translation())),
+                      oMi.rotation() * a.angular());
+      default:
+        throw std::invalid_argument("Bad reference frame.");
+    }
   }
   
   template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>

unittest/frames.cpp

@@ -186,6 +186,16 @@ BOOST_AUTO_TEST_CASE ( test_acceleration )
   Motion af = getFrameAcceleration(model, data, frame_idx);
 
   BOOST_CHECK(af.isApprox(framePlacement.actInv(data.a[parent_idx])));
+
+  pinocchio::Data data_ref(model);
+  forwardKinematics(model, data_ref, q, v, a);
+  updateFramePlacements(model, data_ref);
+  Motion a_ref = getFrameAcceleration(model, data_ref, frame_idx);
+
+  BOOST_CHECK(a_ref.isApprox(getFrameAcceleration(model,data,frame_idx)));
+  BOOST_CHECK(a_ref.isApprox(getFrameAcceleration(model,data,frame_idx,ReferenceFrame::LOCAL)));
+  BOOST_CHECK(data_ref.oMf[frame_idx].act(a_ref).isApprox(getFrameAcceleration(model,data,frame_idx,ReferenceFrame::WORLD)));
+  BOOST_CHECK(SE3(data_ref.oMf[frame_idx].rotation(), Eigen::Vector3d::Zero()).act(a_ref).isApprox(getFrameAcceleration(model,data,frame_idx,ReferenceFrame::LOCAL_WORLD_ALIGNED)));
 }
 
 BOOST_AUTO_TEST_CASE ( test_get_frame_jacobian )