diff --git a/benchmark/timings-geometry.cpp b/benchmark/timings-geometry.cpp
index 1fcfbb548c2174397a93ee5bbcc41f35f2917b2e..955d165566093dff9cd37dbd3c7fb7f84c82bd43 100644
--- a/benchmark/timings-geometry.cpp
+++ b/benchmark/timings-geometry.cpp
@@ -70,7 +70,8 @@ int main()
std::string romeo_meshDir = PINOCCHIO_SOURCE_DIR"/models/";
package_dirs.push_back(romeo_meshDir);
- se3::Model model = se3::urdf::buildModel(romeo_filename, se3::JointModelFreeFlyer());
+ se3::Model model;
+ se3::urdf::buildModel(romeo_filename, se3::JointModelFreeFlyer(),model);
se3::GeometryModel geom_model; se3::urdf::buildGeom(model, romeo_filename, COLLISION, geom_model, package_dirs);
#ifdef WITH_HPP_FCL
geom_model.addAllCollisionPairs();
diff --git a/benchmark/timings.cpp b/benchmark/timings.cpp
index 55107238d6486981a4fc714c62cdbc344d601c14..d52031052ae1deffc9547c0842318bf09f5f71af 100644
--- a/benchmark/timings.cpp
+++ b/benchmark/timings.cpp
@@ -59,7 +59,7 @@ int main(int argc, const char ** argv)
else if( filename == "H2" )
se3::buildModels::humanoid2d(model);
else
- model = se3::urdf::buildModel(filename,JointModelFreeFlyer());
+ se3::urdf::buildModel(filename,JointModelFreeFlyer(),model);
std::cout << "nq = " << model.nq << std::endl;
se3::Data data(model);
diff --git a/src/multibody/joint/joint-prismatic-unaligned.hpp b/src/multibody/joint/joint-prismatic-unaligned.hpp
index 0be8e5522736996632dcec087318f8aeaa5b620d..01668ed0d8b2b87397c991fcab8560a82478abdf 100644
--- a/src/multibody/joint/joint-prismatic-unaligned.hpp
+++ b/src/multibody/joint/joint-prismatic-unaligned.hpp
@@ -341,10 +341,7 @@ namespace se3
{
const double & q = qs[idx_q()];
- /* It should not be necessary to copy axis in jdata, however a current bug
- * in the fusion visitor prevents a proper access to jmodel::axis. A
- * by-pass is to access to a copy of it in jdata. */
- data.M.translation() = data.S.axis * q;
+ data.M.translation() = axis * q;
}
void calc(JointDataDerived & data,
@@ -354,17 +351,14 @@ namespace se3
const Scalar & q = qs[idx_q()];
const Scalar & v = vs[idx_v()];
- /* It should not be necessary to copy axis in jdata, however a current bug
- * in the fusion visitor prevents a proper access to jmodel::axis. A
- * by-pass is to access to a copy of it in jdata. */
- data.M.translation() = data.S.axis * q;
+ data.M.translation() = axis * q;
data.v.v = v;
}
void calc_aba(JointDataDerived & data, Inertia::Matrix6 & I, const bool update_I) const
{
- data.U = I.block<6,3> (0,Inertia::LINEAR) * data.S.axis;
- data.Dinv[0] = 1./data.S.axis.dot(data.U.segment <3> (Inertia::LINEAR));
+ data.U = I.block<6,3> (0,Inertia::LINEAR) * axis;
+ data.Dinv[0] = 1./axis.dot(data.U.segment <3> (Inertia::LINEAR));
data.UDinv = data.U * data.Dinv;
if (update_I)
diff --git a/src/multibody/joint/joint-revolute-unaligned.hpp b/src/multibody/joint/joint-revolute-unaligned.hpp
index 6dd3817ea8699b29ead71a3fa6a5d8ed9b5a2464..2e122a07d88c1ec7e5ef1625319d66a3e3923cc3 100644
--- a/src/multibody/joint/joint-revolute-unaligned.hpp
+++ b/src/multibody/joint/joint-revolute-unaligned.hpp
@@ -289,7 +289,6 @@ namespace se3
Bias_t c;
F_t F;
- Eigen::AngleAxisd angleaxis;
// [ABA] specific data
U_t U;
@@ -298,13 +297,11 @@ namespace se3
JointDataRevoluteUnaligned()
: M(1),S(Eigen::Vector3d::Constant(NAN)),v(Eigen::Vector3d::Constant(NAN),NAN)
- , angleaxis( NAN,Eigen::Vector3d::Constant(NAN))
, U(), Dinv(), UDinv()
{}
JointDataRevoluteUnaligned(const Motion::Vector3 & axis)
: M(1),S(axis),v(axis,NAN)
- , angleaxis(NAN,axis)
, U(), Dinv(), UDinv()
{}
@@ -342,12 +339,8 @@ namespace se3
const Eigen::VectorXd & qs ) const
{
const double & q = qs[idx_q()];
-
- /* It should not be necessary to copy axis in jdata, however a current bug
- * in the fusion visitor prevents a proper access to jmodel::axis. A
- * by-pass is to access to a copy of it in jdata. */
- data.angleaxis.angle() = q;
- data.M.rotation(data.angleaxis.toRotationMatrix());
+
+ data.M.rotation(Eigen::AngleAxisd(q, axis).toRotationMatrix());
}
void calc( JointDataDerived& data,
@@ -357,18 +350,15 @@ namespace se3
const double & q = qs[idx_q()];
const double & v = vs[idx_v()];
- /* It should not be necessary to copy axis in jdata, however a current bug
- * in the fusion visitor prevents a proper access to jmodel::axis. A
- * by-pass is to access to a copy of it in jdata. */
- data.angleaxis.angle() = q;
- data.M.rotation(data.angleaxis.toRotationMatrix());
+ data.M.rotation(Eigen::AngleAxisd(q, axis).toRotationMatrix());
+
data.v.w = v;
}
void calc_aba(JointDataDerived & data, Inertia::Matrix6 & I, const bool update_I) const
{
- data.U = I.block<6,3> (0,Inertia::ANGULAR) * data.angleaxis.axis();
- data.Dinv[0] = 1./data.angleaxis.axis().dot(data.U.segment <3> (Inertia::ANGULAR));
+ data.U = I.block<6,3> (0,Inertia::ANGULAR) * axis;
+ data.Dinv[0] = 1./axis.dot(data.U.segment <3> (Inertia::ANGULAR));
data.UDinv = data.U * data.Dinv;
if (update_I)
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index 3140567b81435d8d16389e2f9e64e128b7ae89bc..ab4da7163323e059825e816a509807646b927a4d 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -292,42 +292,7 @@ namespace se3
///
bool existFrame (const std::string & name, const FrameType& type = (FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR )) const;
- ///
- /// \brief Get the name of a frame given by its index.
- ///
- /// \param[in] index Index of the frame.
- ///
- /// \return The name of the frame.
- ///
- PINOCCHIO_DEPRECATED const std::string & getFrameName (const FrameIndex index) const;
-
- ///
- /// \brief Get the index of the joint supporting the frame given by its index.
- ///
- /// \param[in] index Index of the frame.
- ///
- /// \return
- ///
- PINOCCHIO_DEPRECATED JointIndex getFrameParent(const FrameIndex index) const;
- ///
- /// \brief Get the type of the frame given by its index.
- ///
- /// \param[in] index Index of the frame.
- ///
- /// \return
- ///
- PINOCCHIO_DEPRECATED FrameType getFrameType(const FrameIndex index) const;
-
- ///
- /// \brief Return the relative placement between a frame and its supporting joint.
- ///
- /// \param[in] index Index of the frame.
- ///
- /// \return The frame placement regarding the supporing joint.
- ///
- PINOCCHIO_DEPRECATED const SE3 & getFramePlacement(const FrameIndex index) const;
-
///
/// \brief Adds a frame to the kinematic tree.
///
diff --git a/src/multibody/model.hxx b/src/multibody/model.hxx
index 8b7fa5fa136b92fd54645a3a7cf6c2b503bf8fa4..2b82f17892cf2a7ed37b465fde4af0a073b1759f 100644
--- a/src/multibody/model.hxx
+++ b/src/multibody/model.hxx
@@ -149,7 +149,7 @@ namespace se3
inline const std::string& Model::getBodyName (const Model::JointIndex index) const
{
assert( index < (Model::Index)nbody );
- return getFrameName(index);
+ return frames[index].name;
}
inline Model::JointIndex Model::getJointId (const std::string & name) const
@@ -190,25 +190,6 @@ namespace se3
details::FilterFrame (name, type)) != frames.end();
}
- inline const std::string & Model::getFrameName ( const FrameIndex index ) const
- {
- return frames[index].name;
- }
-
- inline Model::JointIndex Model::getFrameParent( const FrameIndex index ) const
- {
- return frames[index].parent;
- }
-
- inline FrameType Model::getFrameType( const FrameIndex index ) const
- {
- return frames[index].type;
- }
-
- inline const SE3 & Model::getFramePlacement( const FrameIndex index ) const
- {
- return frames[index].placement;
- }
inline int Model::addFrame ( const Frame & frame )
{
diff --git a/src/parsers/urdf.hpp b/src/parsers/urdf.hpp
index ea026794355f2a7862a0d5b68849138201776c10..01955134ebd97098a9db84e3a034d2672a3bac9e 100644
--- a/src/parsers/urdf.hpp
+++ b/src/parsers/urdf.hpp
@@ -58,21 +58,6 @@ namespace se3
Model & model,
const bool verbose = false) throw (std::invalid_argument);
- ///
- /// \brief Build the model from a URDF file with a particular joint as root of the model tree.
- ///
- /// \param[in] filemane The URDF complete file path.
- /// \param[in] rootJoint The joint at the root of the model tree.
- /// \param[in] verbose Print parsing info.
- ///
- /// \return The se3::Model of the URDF file.
- ///
- PINOCCHIO_DEPRECATED
- inline Model buildModel (const std::string & filename,
- const JointModelVariant & rootJoint,
- const bool verbose = false)
- throw (std::invalid_argument)
- { Model m; return buildModel(filename,rootJoint,m,verbose); }
///
/// \brief Build the model from a URDF file with a fixed joint as root of the model tree.
@@ -86,20 +71,6 @@ namespace se3
Model & model,
const bool verbose = false) throw (std::invalid_argument);
- ///
- /// \brief Build the model from a URDF file with a fixed joint as root of the model tree.
- ///
- /// \param[in] filemane The URDF complete file path.
- /// \param[in] verbose Print parsing info.
- ///
- /// \return The se3::Model of the URDF file.
- ///
- PINOCCHIO_DEPRECATED
- inline Model buildModel (const std::string & filename,
- const bool verbose = false)
- throw (std::invalid_argument)
- { Model m; return buildModel(filename,m,verbose); }
-
/**
* @brief Build The GeometryModel from a URDF file. Search for meshes
@@ -128,30 +99,6 @@ namespace se3
const std::vector<std::string> & packageDirs = std::vector<std::string> ())
throw (std::invalid_argument);
- /**
- * @brief Inline call to the previous method (deprecated).
- *
- * @param[in] model The model of the robot, built with
- * urdf::buildModel
- * @param[in] filename The URDF complete (absolute) file path
- * @param[in] packageDirs A vector containing the different directories
- * where to search for models and meshes, typically
- * obtained from calling se3::rosPaths()
- *
- *@param[in] type The type of objects that must be loaded ( can be VISUAL or COLLISION)
- *
- * @return The GeometryModel associated to the urdf file and the given Model.
- *
- * \warning If hpp-fcl has not been found during compilation, COLLISION types can not be loaded
- */
- PINOCCHIO_DEPRECATED
- inline GeometryModel buildGeom(const Model & model,
- const std::string & filename,
- const GeometryType type,
- const std::vector<std::string> & packageDirs = std::vector<std::string> ())
- throw (std::invalid_argument)
- { GeometryModel g; return buildGeom (model,filename,type,g,packageDirs); }
-
} // namespace urdf
} // namespace se3
diff --git a/unittest/urdf.cpp b/unittest/urdf.cpp
index 5f260b62db6885f03fe4fb38e84f1df8238cd563..a63dc41952d78dcf5f40dc9d68a45c352dea2d05 100644
--- a/unittest/urdf.cpp
+++ b/unittest/urdf.cpp
@@ -34,7 +34,8 @@ BOOST_AUTO_TEST_CASE ( buildModel )
#ifndef NDEBUG
std::cout << "Parse filename \"" << filename << "\"" << std::endl;
#endif
- se3::Model model = se3::urdf::buildModel(filename);
+ se3::Model model;
+ se3::urdf::buildModel(filename, model);
}
BOOST_AUTO_TEST_SUITE_END()