[frames] placement, velocity and acceleration

src/algorithm/frames.hpp

@@ -25,7 +25,7 @@ namespace se3
 {
 
   /**
-   * @brief      Updates the position of each frame contained in the model
+   * @brief      Updates the placement of each frame contained in the model
    *
    * @param[in]  model  The kinematic model.
    * @param      data   Data associated to model.
@@ -48,14 +48,62 @@ namespace se3
                                       const Eigen::VectorXd & q);
 
   /**
-   * @brief      Returns the jacobian of the frame expresssed either expressed in the LOCAL frame coordinate system or in the WORLD coordinate system,
+   * @brief      Updates the placement of the given frame.
+   *
+   * @param[in]  model        The kinematic model.
+   * @param      data         Data associated to model.
+   * @param[in]  frame_id     Id of the operational Frame.
+   *
+   * @return     A reference to the frame placement stored in data.oMf[frame_id]
+   *
+   * @warning    One of the algorithms forwardKinematics should have been called first
+   */
+  inline const SE3 & frameForwardKinematics(const Model & model,
+                                            Data & data,
+                                            const Model::FrameIndex frame_id);
+
+  /**
+   * @brief      Returns the spatial velocity of the frame expressed in the LOCAL frame coordinate system.
+   *             You must first call se3::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
+   * @param[out] frame_v     The spatial velocity of the Frame expressed in the coordinates Frame.
+   *
+   * @warning    Fist or second order forwardKinematics should have been called first
+   */
+  void getFrameVelocity(const Model & model,
+                        const Data & data,
+                        const Model::FrameIndex frame_id,
+                        Motion & frame_v);
+
+  /**
+   * @brief      Returns the spatial acceleration of the frame expressed in the LOCAL frame coordinate system.
+   *             You must first call se3::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
+   */
+  void getFrameAcceleration(const Model & model,
+                            const Data & data,
+                            const Model::FrameIndex frame_id,
+                            Motion & frame_a);
+
+
+  /**
+   * @brief      Returns the jacobian of the frame expressed either in the LOCAL frame coordinate system or in the WORLD coordinate system,
    *             depending on the value of the template parameter rf.
    *             You must first call se3::computeJointJacobians followed by se3::framesForwardKinematics to update placement values in data structure.
    *
    * @tparam     rf Reference frame in which the columns of the Jacobian are expressed.
-   
+   *
    * @remark     Similarly to se3::getJointJacobian with LOCAL or WORLD templated 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
+   *             in the local coordinates of the frame, or if rl == WORLD, it returns the Jacobian expressed of the point coincident with the origin
    *             and expressed in a coordinate system aligned with the WORLD.
    *
    * @param[in]  model       The kinematic model
@@ -63,7 +111,7 @@ namespace se3
    * @param[in]  frame_id    Id of the operational Frame
    * @param[out] J           The Jacobian of the Frame expressed in the coordinates Frame.
    *
-   * @warning    The function se3::computeJointJacobians and se3::framesForwardKinematics should have been called first.
+   * @warning    The functions se3::computeJointJacobians and se3::framesForwardKinematics should have been called first.
    */
   template<ReferenceFrame rf>
   void getFrameJacobian(const Model & model,

src/algorithm/frames.hxx

@@ -53,6 +53,43 @@ namespace se3
     forwardKinematics(model, data, q);
     framesForwardKinematics(model, data);
   }
+
+  inline const SE3 & frameForwardKinematics(const Model & model,
+                                            Data & data,
+                                            const Model::FrameIndex frame_id)
+  {
+    const Frame & frame = model.frames[frame_id];
+    const Model::JointIndex & parent = frame.parent;
+    if (frame.placement.isIdentity())
+      data.oMf[frame_id] = data.oMi[parent];
+    else
+      data.oMf[frame_id] = data.oMi[parent]*frame.placement;
+    return data.oMf[frame_id];
+  }
+
+  void getFrameVelocity(const Model & model,
+                        const Data & data,
+                        const Model::FrameIndex frame_id,
+                        Motion & frame_v)
+  {
+    assert(model.check(data) && "data is not consistent with model.");
+
+    const Frame & frame = model.frames[frame_id];
+    const Model::JointIndex & parent = frame.parent;
+    frame_v = frame.placement.actInv(data.v[parent]);
+  }
+
+  void getFrameAcceleration(const Model & model,
+                            const Data & data,
+                            const Model::FrameIndex frame_id,
+                            Motion & frame_a)
+  {
+    assert(model.check(data) && "data is not consistent with model.");
+
+    const Frame & frame = model.frames[frame_id];
+    const Model::JointIndex & parent = frame.parent;
+    frame_a = frame.placement.actInv(data.a[parent]);
+  }
   
   template<ReferenceFrame rf>
   inline void getFrameJacobian(const Model & model,

unittest/frames.cpp

@@ -60,6 +60,82 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
 
 }
 
+BOOST_AUTO_TEST_CASE ( test_single_kinematics )
+{
+  using namespace Eigen;
+  using namespace se3;
+
+  se3::Model model;
+  se3::buildModels::humanoidSimple(model);
+  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
+  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
+  const SE3 & framePlacement = SE3::Random();
+  model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
+  Model::FrameIndex frame_idx = model.getFrameId(frame_name);
+  se3::Data data(model);
+  se3::Data data_ref(model);
+
+  VectorXd q = VectorXd::Ones(model.nq);
+  q.middleRows<4> (3).normalize();
+
+  forwardKinematics(model, data, q);
+  frameForwardKinematics(model, data, frame_idx);
+
+  framesForwardKinematics(model, data_ref, q);
+
+  BOOST_CHECK(data.oMf[frame_idx].isApprox(data_ref.oMf[frame_idx]));
+}
+
+BOOST_AUTO_TEST_CASE ( test_velocity )
+{
+  using namespace Eigen;
+  using namespace se3;
+
+  se3::Model model;
+  se3::buildModels::humanoidSimple(model);
+  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
+  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
+  const SE3 & framePlacement = SE3::Random();
+  model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
+  Model::FrameIndex frame_idx = model.getFrameId(frame_name);
+  se3::Data data(model);
+
+  VectorXd q = VectorXd::Ones(model.nq);
+  q.middleRows<4> (3).normalize();
+  VectorXd v = VectorXd::Ones(model.nv);
+  forwardKinematics(model, data, q, v);
+
+  Motion vf;
+  getFrameVelocity(model, data, frame_idx, vf);
+
+  BOOST_CHECK(vf.isApprox(framePlacement.actInv(data.v[parent_idx])));
+}
+
+BOOST_AUTO_TEST_CASE ( test_acceleration )
+{
+  using namespace Eigen;
+  using namespace se3;
+
+  se3::Model model;
+  se3::buildModels::humanoidSimple(model);
+  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
+  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
+  const SE3 & framePlacement = SE3::Random();
+  model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
+  Model::FrameIndex frame_idx = model.getFrameId(frame_name);
+  se3::Data data(model);
+
+  VectorXd q = VectorXd::Ones(model.nq);
+  q.middleRows<4> (3).normalize();
+  VectorXd v = VectorXd::Ones(model.nv);
+  VectorXd a = VectorXd::Ones(model.nv);
+  forwardKinematics(model, data, q, v, a);
+
+  Motion af;
+  getFrameAcceleration(model, data, frame_idx, af);
+
+  BOOST_CHECK(af.isApprox(framePlacement.actInv(data.a[parent_idx])));
+}
 
 BOOST_AUTO_TEST_CASE ( test_jacobian )
 {
@@ -86,7 +162,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
   BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));
   Data::Matrix6x Jjj(6,model.nv); Jjj.fill(0);
   Data::Matrix6x Jff(6,model.nv); Jff.fill(0);
-  getFrameJacobian(model,data,idx,Jff);
+  getFrameJacobian<LOCAL>(model,data,idx,Jff);
   getJointJacobian<LOCAL>(model, data_ref, parent_idx, Jjj);
 
   Motion nu_frame = Motion(Jff*q_dot);