Commit 597577dd authored by Valenza Florian's avatar Valenza Florian
Browse files

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

Conflicts:

	CMakeLists.txt
	src/python/algorithms.hpp
parent 64dcf1ff
......@@ -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
......
//
// 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__
......@@ -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
......
......@@ -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)
......
......@@ -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)"),
......
......@@ -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")
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment