diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4d1337eb53b3bac75b300664efc07d172add47a1..93988c81df2ea454b1fd753585881cd5e1de95f4 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -140,7 +140,6 @@ 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
@@ -158,7 +157,6 @@ SET(${PROJECT_NAME}_PYTHON_HEADERS
python/joint-dense.hpp
python/joints-models.hpp
python/joints-variant.hpp
- python/frame.hpp
python/model.hpp
python/data.hpp
python/algorithms.hpp
diff --git a/src/algorithm/extra-frames.hpp b/src/algorithm/extra-frames.hpp
deleted file mode 100644
index 3b810f83bedfbec4ec2a4599d21a60e4002f6aa1..0000000000000000000000000000000000000000
--- a/src/algorithm/extra-frames.hpp
+++ /dev/null
@@ -1,130 +0,0 @@
-//
-// 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/multibody/model.hpp b/src/multibody/model.hpp
index 4ffd5c8bd1e65e3ed07485343201548afbab2796..e5ad22eb491692f0fc6a02d88620be6419580d2d 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -39,26 +39,6 @@ namespace se3
{
class Model;
class Data;
- struct Frame
- {
- typedef std::size_t Index;
-
- Frame(const std::string & name, Index parent, const SE3 & placement): name(name)
- , parent_id(parent)
- , frame_placement(placement)
- {
- }
-
- bool operator == (const Frame & other) const
- {
- return name == other.name && parent_id == other.parent_id
- && frame_placement == other.frame_placement ;
- }
-
- std::string name;
- Index parent_id;
- SE3 frame_placement;
- };
class Model
{
@@ -69,7 +49,6 @@ namespace se3
int nv; // Dimension of the velocity vector space
int nbody; // Number of bodies (= number of joints + 1)
int nFixBody; // Number of fixed-bodies (= number of fixed-joints)
- int nExtraFrames; // Number of extra frames
std::vector<Inertia> inertias; // Spatial inertias of the body <i> in the supporting joint frame <i>
std::vector<SE3> jointPlacements; // Placement (SE3) of the input of joint <i> in parent joint output <li>
@@ -84,8 +63,6 @@ namespace se3
std::vector<bool> fix_hasVisual; // True iff fixed-body <i> has a visual mesh.
std::vector<std::string> fix_bodyNames;// Name of fixed-joint <i>
- std::vector<Frame> extra_frames;
-
Motion gravity; // Spatial gravity
static const Eigen::Vector3d gravity981; // Default 3D gravity (=(0,0,-9.81))
@@ -94,7 +71,6 @@ namespace se3
, nv(0)
, nbody(1)
, nFixBody(0)
- , nExtraFrames(0)
, inertias(1)
, jointPlacements(1)
, joints(1)
@@ -131,18 +107,6 @@ namespace se3
Index getJointId( const std::string & name ) const;
bool existJointName( const std::string & name ) const;
const std::string& getJointName( Index index ) const;
-
- Index getFrameId ( const std::string & name ) const;
- bool existFrame ( const std::string & name ) const;
- const std::string & getFrameName ( Index index ) const;
- const Index& getFrameParent( const std::string & name ) const;
- const Index& getFrameParent( Index index ) const;
- const SE3 & getJointToFrameTransform( const std::string & name ) const;
- const SE3 & getJointToFrameTransform( Index index ) const;
-
- void addFrame ( const Frame & frame );
- void addFrame ( const std::string & name, Index index, const SE3 & placement );
-
};
class Data
@@ -164,8 +128,6 @@ namespace se3
Eigen::VectorXd tau; // Joint forces
Eigen::VectorXd nle; // Non linear effects
- std::vector<SE3> oMef; // 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 9499154f89d8f8203b57ddf89b15db1dec359b49..be4aa1980dfea392d4cef02f9e789289de0a9544 100644
--- a/src/multibody/model.hxx
+++ b/src/multibody/model.hxx
@@ -27,8 +27,6 @@
#include "pinocchio/multibody/joint/joint-variant.hpp"
#include <iostream>
-#include <boost/bind.hpp>
-
namespace se3
{
inline std::ostream& operator<< (std::ostream & os, const Model & model)
@@ -175,67 +173,6 @@ namespace se3
return names[index];
}
- inline Model::Index Model::getFrameId ( const std::string & name ) const
- {
- std::vector<Frame>::const_iterator it = std::find_if( extra_frames.begin()
- , extra_frames.end()
- , boost::bind(&Frame::name, _1) == name
- );
- return Model::Index(it - extra_frames.begin());
- }
-
- inline bool Model::existFrame ( const std::string & name ) const
- {
- return std::find_if( extra_frames.begin(), extra_frames.end(), boost::bind(&Frame::name, _1) == name) != extra_frames.end();
- }
-
- inline const std::string & Model::getFrameName ( Index index ) const
- {
- return extra_frames[index].name;
- }
-
- inline const Model::Index& Model::getFrameParent( const std::string & name ) const
- {
- std::vector<Frame>::const_iterator it = std::find_if( extra_frames.begin()
- , extra_frames.end()
- , boost::bind(&Frame::name, _1) == name
- );
- return it->parent_id;
- }
-
- inline const Model::Index& Model::getFrameParent( Model::Index index ) const
- {
- return extra_frames[index].parent_id;
- }
-
- inline const SE3 & Model::getJointToFrameTransform( const std::string & name) const
- {
- assert(existFrame(name) && "The Frame you requested does not exist");
- std::vector<Frame>::const_iterator it = std::find_if( extra_frames.begin()
- , extra_frames.end()
- , boost::bind(&Frame::name, _1) == name
- );
- return it->frame_placement;
- }
-
- inline const SE3 & Model::getJointToFrameTransform( Model::Index index ) const
- {
- return extra_frames[index].frame_placement;
- }
-
- inline void Model::addFrame ( const Frame & frame )
- {
- if( !existFrame(frame.name) )
- extra_frames.push_back(frame);nExtraFrames++;
- }
-
- inline void Model::addFrame ( const std::string & name, Index index, const SE3 & placement)
- {
- if( !existFrame(name) )
- addFrame(Frame(name, index, placement));
- }
-
-
inline Data::Data (const Model & ref)
:model(ref)
,joints(0)
@@ -247,7 +184,6 @@ namespace se3
,liMi((std::size_t)ref.nbody)
,tau(ref.nv)
,nle(ref.nv)
- ,oMef((std::size_t)ref.nExtraFrames)
,Ycrb((std::size_t)ref.nbody)
,M(ref.nv,ref.nv)
,Fcrb((std::size_t)ref.nbody)
diff --git a/src/multibody/parser/urdf.hxx b/src/multibody/parser/urdf.hxx
index 2cbdd1bd1c5e6595717d5a00baa63ab9ead9004a..ebf258f2c8bb05057b6d439cae06f2b9383b90e1 100644
--- a/src/multibody/parser/urdf.hxx
+++ b/src/multibody/parser/urdf.hxx
@@ -345,9 +345,6 @@ namespace se3
//add the fixed Body in the model for the viewer
model.addFixedBody(parent_joint_id, nextPlacementOffset, link->name, has_visual);
-
- // Add a frame in the model to keep trace of this fixed joint
- model.addFrame(joint->name, parent_joint_id, nextPlacementOffset);
//for the children of the current link, set their parent to be
//the the parent of the current link.
diff --git a/src/python/algorithms.hpp b/src/python/algorithms.hpp
index 82bfd856691374eb918622ade66ebc181d2ad7fb..fdfb0fd9f0e8e7910ffd0bf41cb34dd86c798f66 100644
--- a/src/python/algorithms.hpp
+++ b/src/python/algorithms.hpp
@@ -29,7 +29,6 @@
#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"
@@ -107,25 +106,6 @@ 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)
@@ -148,15 +128,6 @@ 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,
@@ -281,14 +252,6 @@ 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)"),
@@ -321,18 +284,6 @@ 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 23912552061c501d9b55d377d3dfe7c126287ee0..189579df4a6ec7762d124ff42fbf069e13b6f413 100644
--- a/src/python/data.hpp
+++ b/src/python/data.hpp
@@ -74,7 +74,6 @@ 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")
@@ -112,7 +111,6 @@ 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")
diff --git a/src/python/frame.hpp b/src/python/frame.hpp
deleted file mode 100644
index a76df1697a845902b3fbe20196961cb5e877634c..0000000000000000000000000000000000000000
--- a/src/python/frame.hpp
+++ /dev/null
@@ -1,83 +0,0 @@
-//
-// Copyright (c) 2016 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_python_frame_hpp__
-#define __se3_python_frame_hpp__
-
-#include <eigenpy/exception.hpp>
-#include <eigenpy/eigenpy.hpp>
-#include "pinocchio/multibody/model.hpp"
-
-
-namespace se3
-{
- namespace python
- {
- namespace bp = boost::python;
-
- struct FramePythonVisitor
- : public boost::python::def_visitor< FramePythonVisitor >
- {
- typedef eigenpy::UnalignedEquivalent<SE3>::type SE3_fx;
- typedef Model::Index Index;
-
- public:
-
- static PyObject* convert(Frame const& f)
- {
- return boost::python::incref(boost::python::object(f).ptr());
- }
-
- template<class PyClass>
- void visit(PyClass& cl) const
- {
- cl
- .def(bp::init<const std::string&,Index, const SE3_fx&> ((bp::arg("name"),bp::arg("parent id"), bp::arg("placement")),
- "Initialize from name, parent id and placement wrt parent joint."))
-
- .def("name", &FramePythonVisitor::getName)
- .def("parent_id", &FramePythonVisitor::getParentId)
- .def("placement", &FramePythonVisitor::getPlcaementWrtParentJoint)
- ;
- }
-
-
- static std::string getName( const Frame & self) { return self.name; }
- static Index getParentId( const Frame & self) { return self.parent_id; }
- static SE3_fx getPlcaementWrtParentJoint( const Frame & self) { return self.frame_placement; }
-
- static void expose()
- {
- bp::class_<Frame>("Frame",
- "ExtraFrames.\n\n",
- bp::no_init
- )
- .def(FramePythonVisitor())
- ;
-
- bp::to_python_converter< Frame,FramePythonVisitor >();
- }
-
-
- };
-
-
- } // namespace python
-} // namespace se3
-
-#endif // ifndef __se3_python_frame_hpp__
-
diff --git a/src/python/model.hpp b/src/python/model.hpp
index 6caba7c4bc47c067f273361234fe8621363ca5e3..96162c6badede77df3e79878727da537d8775d20 100644
--- a/src/python/model.hpp
+++ b/src/python/model.hpp
@@ -132,10 +132,6 @@ namespace se3
.add_property("fix_hasVisual", bp::make_function(&ModelPythonVisitor::fix_hasVisual, bp::return_internal_reference<>()) )
.add_property("fix_bodyNames", bp::make_function(&ModelPythonVisitor::fix_bodyNames, bp::return_internal_reference<>()) )
- .def("getFrameParentJoint", &ModelPythonVisitor::getFrameParent)
- .def("getFramePositionInParent", &ModelPythonVisitor::getJointToFrameTransform)
- .def("addExtraFrame", &ModelPythonVisitor::addExtraFrame)
- .add_property("extra_frames", bp::make_function(&ModelPythonVisitor::extraFrames, bp::return_internal_reference<>()) )
.add_property("gravity",&ModelPythonVisitor::gravity,&ModelPythonVisitor::setGravity)
.def("BuildEmptyModel",&ModelPythonVisitor::maker_empty)
@@ -179,14 +175,6 @@ namespace se3
static std::vector<bool> & fix_hasVisual ( ModelHandler & m ) { return m->fix_hasVisual; }
static std::vector<std::string> & fix_bodyNames ( ModelHandler & m ) { return m->fix_bodyNames; }
- static Model::Index getFrameParent( ModelHandler & m, const std::string & name ) { return m->getFrameParent(name); }
- static SE3 getJointToFrameTransform( ModelHandler & m, const std::string & name ) { return m->getJointToFrameTransform(name); }
- static void addExtraFrame( ModelHandler & m, const std::string & frameName, Index parent, const SE3_fx & placementWrtParent )
- {
- m->addFrame(frameName, parent, placementWrtParent);
- }
- static std::vector<Frame> & extraFrames (ModelHandler & m ) { return m->extra_frames;}
-
static Motion gravity( ModelHandler & m ) { return m->gravity; }
static void setGravity( ModelHandler & m,const Motion_fx & g ) { m->gravity = g; }
@@ -244,8 +232,6 @@ namespace se3
.def(bp::vector_indexing_suite< std::vector<double> >());
bp::class_< JointModelVector >("StdVec_JointModelVector")
.def(bp::vector_indexing_suite< JointModelVector, true >());
- bp::class_< std::vector<Frame> >("StdVec_FrameVector")
- .def(bp::vector_indexing_suite< std::vector<Frame> >());
bp::class_<ModelHandler>("Model",
"Articulated rigid body model (const)",
diff --git a/src/python/python.cpp b/src/python/python.cpp
index 36b4a99b3f0cc8f6ed6afa79b91006deb5a76625..eddca903957f9c6fcdbc6d933f3c858721a7c6bc 100644
--- a/src/python/python.cpp
+++ b/src/python/python.cpp
@@ -24,7 +24,6 @@
#include "pinocchio/python/joint-dense.hpp"
#include "pinocchio/python/joints-variant.hpp"
-#include "pinocchio/python/frame.hpp"
#include "pinocchio/python/model.hpp"
#include "pinocchio/python/data.hpp"
#include "pinocchio/python/algorithms.hpp"
@@ -66,7 +65,6 @@ namespace se3
}
void exposeModel()
{
- FramePythonVisitor::expose();
ModelPythonVisitor::expose();
DataPythonVisitor::expose();
#ifdef WITH_HPP_FCL
diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt
index f4cc3bed8b0b2544c70badce32668b3c7c3fdd82..ebbb3f509297291f7ef2c71dbe2224e0584f2cda 100644
--- a/unittest/CMakeLists.txt
+++ b/unittest/CMakeLists.txt
@@ -87,4 +87,3 @@ ENDIF(LUA5_1_FOUND)
ADD_UNIT_TEST(joints eigen3)
ADD_UNIT_TEST(compute-all-terms eigen3)
ADD_UNIT_TEST(energy eigen3)
-ADD_UNIT_TEST(extra-frames eigen3)
\ No newline at end of file
diff --git a/unittest/extra-frames.cpp b/unittest/extra-frames.cpp
deleted file mode 100644
index b757d506ee69e7629d36d47fb501b20cc23420af..0000000000000000000000000000000000000000
--- a/unittest/extra-frames.cpp
+++ /dev/null
@@ -1,83 +0,0 @@
-//
-// 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/>.
-
-#include "pinocchio/multibody/model.hpp"
-#include "pinocchio/algorithm/jacobian.hpp"
-#include "pinocchio/algorithm/extra-frames.hpp"
-#include "pinocchio/algorithm/rnea.hpp"
-#include "pinocchio/spatial/act-on-set.hpp"
-#include "pinocchio/multibody/parser/sample-models.hpp"
-#include "pinocchio/tools/timer.hpp"
-
-#include <iostream>
-
-#define BOOST_TEST_DYN_LINK
-#define BOOST_TEST_MODULE ExtraFramesTest
-#include <boost/test/unit_test.hpp>
-#include <boost/utility/binary.hpp>
-#include "pinocchio/tools/matrix-comparison.hpp"
-
-BOOST_AUTO_TEST_SUITE ( ExtraFramesTest)
-
-BOOST_AUTO_TEST_CASE ( test_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.nbody-1);
- const std::string & frame_name = std::string( model.getJointName(parent_idx)+ "_frame");
- model.addFrame(frame_name, parent_idx, SE3());
- se3::Data data(model);
-
- VectorXd q = VectorXd::Random(model.nq);
- extraFramesForwardKinematic(model, data, q);
-
-}
-
-
-BOOST_AUTO_TEST_CASE ( test_jacobian )
-{
- 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.nbody-1);
- const std::string & frame_name = std::string( model.getJointName(parent_idx)+ "_frame");
- model.addFrame(frame_name, parent_idx, SE3(1));
- se3::Data data(model);
-
- VectorXd q = VectorXd::Random(model.nq);
- extraFramesForwardKinematic(model, data, q);
-
- computeJacobians(model,data,q);
-
- MatrixXd expected(6,model.nv); expected.fill(0);
- MatrixXd Jef(6,model.nv); Jef.fill(0);
- Model::Index idx = model.getFrameId(frame_name);
-
- getExtraFrameJacobian<false>(model,data,idx,Jef);
-
- getJacobian<false>(model, data, parent_idx, expected);
-
- is_matrix_absolutely_closed(Jef, expected, 1e-12);
-}
-
-BOOST_AUTO_TEST_SUITE_END ()
-