diff --git a/CMakeLists.txt b/CMakeLists.txt
index 17f60833959d5ba96a07f8f7c478c08284b6a5d8..b8e97d8244f92002fce302a744dbaefb993e74ee 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -140,6 +140,7 @@ SET(${PROJECT_NAME}_ALGORITHM_HEADERS
algorithm/non-linear-effects.hpp
algorithm/joint-limits.hpp
algorithm/energy.hpp
+ algorithm/operational-frames.hpp
)
SET(${PROJECT_NAME}_SIMULATION_HEADERS
diff --git a/src/algorithm/operational-frames.hpp b/src/algorithm/operational-frames.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..419bf9e9d4a483e36a14cfb91c7baa1e3a9a0693
--- /dev/null
+++ b/src/algorithm/operational-frames.hpp
@@ -0,0 +1,130 @@
+//
+// 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_operational_frames_hpp__
+#define __se3_operational_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 framesForwardKinematic(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 framesForwardKinematic(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 getFrameJacobian(const Model & model,
+ const Data& data,
+ Model::Index frame_id,
+ Data::Matrix6x & J
+ );
+
+} // namespace se3
+
+/* --- Details -------------------------------------------------------------------- */
+namespace se3
+{
+
+
+inline void framesForwardKinematic(const Model & model,
+ Data & data
+ )
+{
+ using namespace se3;
+
+ for (Model::Index i=0; i < (Model::Index) model.nOperationalFrames; ++i)
+ {
+ const Model::Index & parent = model.operational_frames[i].parent_id;
+ data.oMof[i] = (data.oMi[parent] * model.operational_frames[i].frame_placement);
+ }
+}
+
+inline void framesForwardKinematic(const Model & model,
+ Data & data,
+ const Eigen::VectorXd & q
+ )
+{
+ using namespace se3;
+
+ forwardKinematics(model, data, q);
+
+ framesForwardKinematic(model, data);
+}
+
+
+
+template<bool localFrame>
+inline void getFrameJacobian(const Model & model, const Data& data,
+ Model::Index frame_id, Data::Matrix6x & J)
+{
+ assert( J.rows() == data.J.rows() );
+ assert( J.cols() == data.J.cols() );
+
+ const Model::Index & parent = model.operational_frames[frame_id].parent_id;
+ const SE3 & oMframe = data.oMof[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_operational_frames_hpp__
+
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index c42f1b3450128251550439103c69bcf99fef29be..5bd77d650832b6fc647339b7108aabd2298fc40d 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -147,7 +147,7 @@ namespace se3
Eigen::VectorXd tau; // Joint forces
Eigen::VectorXd nle; // Non linear effects
- std::vector<SE3> oMef; // Absolute position of extra frames
+ std::vector<SE3> oMof; // Absolute position of extra frames
std::vector<Inertia> Ycrb; // Inertia of the sub-tree composit rigid body
Eigen::MatrixXd M; // Joint Inertia
diff --git a/src/multibody/model.hxx b/src/multibody/model.hxx
index 5092f0e8069022ade5a949248d71854a386e8540..e58a089105f2050ff42b5d43cdfc25a644a1d40e 100644
--- a/src/multibody/model.hxx
+++ b/src/multibody/model.hxx
@@ -247,6 +247,7 @@ namespace se3
,liMi((std::size_t)ref.nbody)
,tau(ref.nv)
,nle(ref.nv)
+ ,oMof((std::size_t)ref.nOperationalFrames)
,Ycrb((std::size_t)ref.nbody)
,M(ref.nv,ref.nv)
,Fcrb((std::size_t)ref.nbody)
diff --git a/src/python/algorithms.hpp b/src/python/algorithms.hpp
index 4237ff476a92d02db37b8429e5e70c0ea90571cc..ae65919db88b5675e71205cb11c3783b4b60fa2b 100644
--- a/src/python/algorithms.hpp
+++ b/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/operational-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 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) getFrameJacobian<true> (*model, *data, frame_id, J);
+ else getFrameJacobian<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
+ )
+ {
+ FramesForwardKinematic( *model,*data,q );
+ }
+
static void fk_2_proxy(const ModelHandler& model,
DataHandler & data,
const VectorXd_fx & q,
@@ -263,6 +292,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("FramesKinematics",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 operational frames "
+ "and put the results in data.");
+
bp::def("geometry",fk_0_proxy,
bp::args("Model","Data",
"Configuration q (size Model::nq)"),
@@ -295,6 +332,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("Framejacobian",frame_jacobian_proxy,
+ bp::args("Model","Data",
+ "Configuration q (size Model::nq)",
+ "Operational 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)"),
diff --git a/src/python/data.hpp b/src/python/data.hpp
index 189579df4a6ec7762d124ff42fbf069e13b6f413..8c4a7f1b104351e40c23baca92ee81235eb0759c 100644
--- a/src/python/data.hpp
+++ b/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>,oMof,"operational_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>,oMof,"operational_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")