Skip to content
Snippets Groups Projects
Commit 3b7906d9 authored by Nicolas Mansard's avatar Nicolas Mansard Committed by GitHub
Browse files

Merge pull request #324 from fvalenza/topic/hpp-devel 

removing deprecated methods + change unaligned joints with boost fusion fix
parents c6fbc151 dd6627ad
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
benchmark/timings-geometry.cpp
+ 2
− 1
View file @ 3b7906d9
......@@ -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();
......
benchmark/timings.cpp
+ 1
− 1
View file @ 3b7906d9
......@@ -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);
......
src/multibody/joint/joint-prismatic-unaligned.hpp
+ 4
− 10
View file @ 3b7906d9
......@@ -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)
......
src/multibody/joint/joint-revolute-unaligned.hpp
+ 6
− 16
View file @ 3b7906d9
......@@ -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)
......
src/multibody/model.hpp
+ 0
− 35
View file @ 3b7906d9
......@@ -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.
///
......
src/multibody/model.hxx
+ 1
− 20
View file @ 3b7906d9
......@@ -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 )
{
......
src/parsers/urdf.hpp
+ 0
− 53
View file @ 3b7906d9
......@@ -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
......
unittest/urdf.cpp
+ 2
− 1
View file @ 3b7906d9
......@@ -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()
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