[C++] Added algorithms to compute position and jacobian of extra_frames, and binded it in python

Conflicts:

	CMakeLists.txt
	src/python/algorithms.hpp

CMakeLists.txt

@@ -140,6 +140,7 @@ SET(${PROJECT_NAME}_ALGORITHM_HEADERS
   algorithm/non-linear-effects.hpp
   algorithm/joint-limits.hpp
   algorithm/energy.hpp
+  algorithm/extra-frames.hpp
   )
 
 SET(${PROJECT_NAME}_SIMULATION_HEADERS

src/algorithm/extra-frames.hpp

+//
+// Copyright (c) 2015 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_extra_frames_hpp__
+#define __se3_extra_frames_hpp__
+
+#include "pinocchio/multibody/visitor.hpp"
+#include "pinocchio/multibody/model.hpp"
+
+#include "pinocchio/algorithm/kinematics.hpp"
+
+namespace se3
+{
+
+
+
+  /**
+   * @brief      Update the position of each extra frame
+   *
+   * @param[in]  model  The kinematic model
+   * @param      data   Data associated to model
+   * @warning    One of the algorithms forwardKinematics should have been called first
+   */
+  inline void extraFramesForwardKinematic(const Model & model,
+                                          Data & data
+                                          );
+
+  /**
+   * @brief      Compute Kinematics of full model, then the position of each extra frame
+   *
+   * @param[in]  model                    The kinematic model
+   * @param      data                     Data associated to model
+   * @param[in]  q                        Configuration vector
+   */
+  inline void extraFramesForwardKinematic(const Model & model,
+                                          Data & data,
+                                          const Eigen::VectorXd & q
+                                          );
+
+  /**
+   * @brief      Return the jacobian of the extra frame in the world frame or
+     in the local frame depending on the template argument.
+   *
+   * @param[in]  model       The kinematic model
+   * @param[in]  data        Data associated to model
+   * @param[in]  frame_id    Id of the extra frame we want to compute the jacobian
+   * @param      J           The filled Jacobian Matrix
+   *
+   * @tparam     localFrame  Express the jacobian in the local or global frame
+   * 
+   * @warning    The function computeJacobians should have been called first
+   */
+  template<bool localFrame>
+  inline void getExtraFrameJacobian(const Model & model,
+                                    const Data& data,
+                                    Model::Index frame_id,
+                                    Eigen::MatrixXd & J
+                                    );
+ 
+} // namespace se3 
+
+/* --- Details -------------------------------------------------------------------- */
+namespace se3 
+{
+  
+  
+inline void  extraFramesForwardKinematic(const Model & model,
+                                         Data & data
+                                         )
+{
+  using namespace se3;
+
+  for (Model::Index i=0; i < (Model::Index) model.nExtraFrames; ++i)
+  {
+    const Model::Index & parent = model.extra_frames[i].parent_id;
+    data.oMef[i] = (data.oMi[parent] * model.extra_frames[i].frame_placement);
+  }
+}
+
+inline void  extraFramesForwardKinematic(const Model & model,
+                                         Data & data,
+                                         const Eigen::VectorXd & q
+                                         )
+{
+  using namespace se3;
+
+  forwardKinematics(model, data, q);
+
+  extraFramesForwardKinematic(model, data);
+}
+
+
+
+template<bool localFrame>
+inline void getExtraFrameJacobian(const Model & model, const Data& data,
+     Model::Index frame_id, Eigen::MatrixXd & J)
+{
+  assert( J.rows() == data.J.rows() );
+  assert( J.cols() == data.J.cols() );
+
+  const Model::Index & parent = model.extra_frames[frame_id].parent_id;
+  const SE3 & oMframe = data.oMef[frame_id];
+
+  int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1;
+
+  for(int j=colRef;j>=0;j=data.parents_fromRow[(Model::Index)j])
+    {
+      if(! localFrame )   J.col(j) = data.J.col(j);
+      else                J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
+    }
+}
+
+} // namespace se3
+
+#endif // ifndef __se3_extra_frames_hpp__
+

src/python/algorithms.hpp

@@ -29,6 +29,7 @@
 #include "pinocchio/algorithm/crba.hpp"
 #include "pinocchio/algorithm/kinematics.hpp"
 #include "pinocchio/algorithm/jacobian.hpp"
+#include "pinocchio/algorithm/extra-frames.hpp"
 #include "pinocchio/algorithm/center-of-mass.hpp"
 #include "pinocchio/algorithm/joint-limits.hpp"
 #include "pinocchio/algorithm/energy.hpp"
@@ -106,6 +107,25 @@ namespace se3
   return J;
       }
       
+      static Eigen::MatrixXd extra_frame_jacobian_proxy(const ModelHandler& model, 
+                                                        DataHandler & data,
+                                                        const VectorXd_fx & q,
+                                                        Model::Index frame_id,
+                                                        bool local,
+                                                        bool update_geometry
+                                                        )
+      {
+        Eigen::MatrixXd J( 6,model->nv ); J.setZero();
+
+        if (update_geometry)
+          computeJacobians( *model,*data,q );
+
+        if(local) getExtraFrameJacobian<true> (*model, *data, frame_id, J);
+        else getExtraFrameJacobian<false> (*model, *data, frame_id, J);
+        
+        return J;
+      }
+
       static void compute_jacobians_proxy(const ModelHandler& model, 
                DataHandler & data,
                const VectorXd_fx & q)
@@ -128,6 +148,15 @@ namespace se3
         forwardKinematics(*model,*data,q,qdot);
       }
 
+
+      static void kinematics_proxy(const ModelHandler& model, 
+                                   DataHandler & data,
+                                   const VectorXd_fx & q
+                                   )
+      {
+        extraFramesForwardKinematic( *model,*data,q );
+      }
+
       static void fk_2_proxy(const ModelHandler& model,
                            DataHandler & data,
                            const VectorXd_fx & q,
@@ -252,6 +281,14 @@ namespace se3
     "Compute the placements and spatial velocities of all the frames of the kinematic "
     "tree and put the results in data.");
 
+
+  bp::def("extraFramesKinematics",extra_frames_kinematics_proxy,
+    bp::args("Model","Data",
+       "Configuration q (size Model::nq)",
+       "Velocity v (size Model::nv)"),
+    "Compute the placements and spatial velocities of all the extra frames "
+    "and put the results in data.");
+
   bp::def("geometry",fk_0_proxy,
     bp::args("Model","Data",
         "Configuration q (size Model::nq)"),
@@ -284,6 +321,18 @@ namespace se3
     "the demanded one if update_geometry is set to false. It is therefore outrageously costly wrt a dedicated "
     "call. Function to be used only for prototyping.");
 
+  bp::def("extraFramejacobian",extra_frame_jacobian_proxy,
+    bp::args("Model","Data",
+       "Configuration q (size Model::nq)",
+       "Extra frame ID (int)",
+       "frame (true = local, false = world)",
+       "update_geometry (true = recompute the kinematics)"),
+    "Call computeJacobians if update_geometry is true. If not, user should call computeJacobians first."
+    "Then call getJacobian and return the resulted jacobian matrix. Attention: if update_geometry is true, the "
+    "function computes all the jacobians of the model, even if just outputing "
+    "the demanded one. It is therefore outrageously costly wrt a dedicated "
+    "call. Use only with update_geometry for prototyping.");
+
   bp::def("computeJacobians",compute_jacobians_proxy,
     bp::args("Model","Data",
        "Configuration q (size Model::nq)"),

src/python/data.hpp

@@ -74,6 +74,7 @@ namespace se3
 	  .ADD_DATA_PROPERTY(std::vector<Motion>,v,"Body velocity")
 	  .ADD_DATA_PROPERTY(std::vector<Force>,f,"Body force")
 	  .ADD_DATA_PROPERTY(std::vector<SE3>,oMi,"Body absolute placement (wrt world)")
+    .ADD_DATA_PROPERTY(std::vector<SE3>,oMef,"extra_frames absolute placement (wrt world)")
 	  .ADD_DATA_PROPERTY(std::vector<SE3>,liMi,"Body relative placement (wrt parent)")
 	  .ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,tau,"Joint forces")
     .ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,nle,"Non Linear Effects")
@@ -111,6 +112,7 @@ namespace se3
       IMPL_DATA_PROPERTY(std::vector<Motion>,v,"Body velocity")
       IMPL_DATA_PROPERTY(std::vector<Force>,f,"Body force")
       IMPL_DATA_PROPERTY(std::vector<SE3>,oMi,"Body absolute placement (wrt world)")
+      IMPL_DATA_PROPERTY(std::vector<SE3>,oMef,"extra_frames absolute placement (wrt world)")
       IMPL_DATA_PROPERTY(std::vector<SE3>,liMi,"Body relative placement (wrt parent)")
       IMPL_DATA_PROPERTY_CONST(Eigen::VectorXd,tau,"Joint forces")
       IMPL_DATA_PROPERTY_CONST(Eigen::VectorXd,nle,"Non Linear Effects")