From 51bc53decc0201d7020e3bdc0fdb9e475180c424 Mon Sep 17 00:00:00 2001
From: Valenza Florian <fvalenza@laas.fr>
Date: Mon, 1 Feb 2016 17:21:52 +0100
Subject: [PATCH] [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 | 1 +
src/algorithm/extra-frames.hpp | 130 +++++++++++++++++++++++++++++++++
src/python/algorithms.hpp | 49 +++++++++++++
src/python/data.hpp | 2 +
4 files changed, 182 insertions(+)
create mode 100644 src/algorithm/extra-frames.hpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 00917e200..4d1337eb5 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/extra-frames.hpp
)
SET(${PROJECT_NAME}_SIMULATION_HEADERS
diff --git a/src/algorithm/extra-frames.hpp b/src/algorithm/extra-frames.hpp
new file mode 100644
index 000000000..3b810f83b
--- /dev/null
+++ b/src/algorithm/extra-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_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__
+
diff --git a/src/python/algorithms.hpp b/src/python/algorithms.hpp
index fdfb0fd9f..82bfd8566 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/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)"),
diff --git a/src/python/data.hpp b/src/python/data.hpp
index 189579df4..239125520 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>,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")
--
GitLab