Unverified Commit abc02ef6 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #568 from jcarpent/devel

Fix compatibility when HPP-FCL is missing
parents 11fcf9b8 05657211
......@@ -21,7 +21,6 @@
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/bindings/python/multibody/data.hpp"
#include "pinocchio/bindings/python/multibody/geometry-model.hpp"
#include "pinocchio/bindings/python/multibody/geometry-data.hpp"
namespace se3
{
......@@ -51,12 +50,14 @@ namespace se3
return model;
}
#ifdef WITH_HPP_FCL
static GeometryModel buildSampleGeometryModelManipulator(const Model& model)
{
GeometryModel geom;
buildModels::manipulatorGeometries(model,geom);
return geom;
}
#endif
static Model buildSampleModelHumanoid()
{
......@@ -72,12 +73,14 @@ namespace se3
return model;
}
#ifdef WITH_HPP_FCL
static GeometryModel buildSampleGeometryModelHumanoid(const Model& model)
{
GeometryModel geom;
buildModels::humanoidGeometries(model,geom);
return geom;
}
#endif
/* --- Expose --------------------------------------------------------- */
static void expose();
......@@ -101,11 +104,13 @@ namespace se3
"Generate a (hard-coded) model of a simple manipulator."
);
#ifdef WITH_HPP_FCL
bp::def("buildSampleGeometryModelManipulator",
static_cast <GeometryModel (*) (const Model&)> (&SampleModelsPythonVisitor::buildSampleGeometryModelManipulator),
bp::args("Model (model)"),
"Generate a (hard-coded) geometry model of a simple manipulator."
);
#endif
bp::def("buildSampleModelHumanoid",
static_cast <Model (*) ()> (&SampleModelsPythonVisitor::buildSampleModelHumanoid),
......@@ -118,11 +123,13 @@ namespace se3
"Generate a (hard-coded) model of a simple humanoid."
);
#ifdef WITH_HPP_FCL
bp::def("buildSampleGeometryModelHumanoid",
static_cast <GeometryModel (*) (const Model&)> (&SampleModelsPythonVisitor::buildSampleGeometryModelHumanoid),
bp::args("Model (model)"),
"Generate a (hard-coded) geometry model of a simple humanoid."
);
#endif
}
......
......@@ -24,13 +24,12 @@ namespace se3
{
const SE3 Id = SE3::Identity();
template<typename JointModel>
static void addJointAndBody(Model & model,
const JointModelBase<JointModel> & joint,
const std::string & parent_name,
const std::string & name,
const SE3 placement = SE3::Random(),
const SE3 & placement = SE3::Random(),
bool setRandomLimits = true)
{
typedef typename JointModel::ConfigVector_t CV;
......@@ -137,7 +136,7 @@ namespace se3
static void addManipulator(Model & model,
Model::JointIndex rootJoint = 0,
const SE3 & Mroot = SE3::Identity(),
const std::string& pre = "")
const std::string & pre = "")
{
typedef JointModelRX::ConfigVector_t CV;
typedef JointModelRX::TangentVector_t TV;
......@@ -184,12 +183,13 @@ namespace se3
}
#ifdef WITH_HPP_FCL
/* Add a 6DOF manipulator shoulder-elbow-wrist geometries to an existing model.
* <model> is the the kinematic chain, constant.
* <geom> is the geometry model where the new geoms are added.
* <pre> is the prefix (string) before every name in the model.
*/
static void addManipulatorGeometries(const Model& model,
static void addManipulatorGeometries(const Model & model,
GeometryModel & geom,
const std::string & pre = "")
{
......@@ -229,11 +229,14 @@ namespace se3
SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0.1)) );
geom.addGeometryObject(effectorArm,model,true);
}
#endif
void manipulator(Model& model) { addManipulator(model); }
void manipulatorGeometries(const Model& model, GeometryModel & geom)
void manipulator(Model & model) { addManipulator(model); }
#ifdef WITH_HPP_FCL
void manipulatorGeometries(const Model & model, GeometryModel & geom)
{ addManipulatorGeometries(model,geom); }
#endif
static Eigen::Matrix3d rotate(const double angle, const Eigen::Vector3d & axis)
{ return Eigen::AngleAxisd(angle,axis).toRotationMatrix(); }
......@@ -305,7 +308,8 @@ namespace se3
addManipulator(model,chest,SE3(rotate(M_PI,Vector3d::UnitX()),Vector3d(0, 0.3, 1.)),"larm");
}
void humanoidGeometries(const Model& model, GeometryModel & geom)
#ifdef WITH_HPP_FCL
void humanoidGeometries(const Model & model, GeometryModel & geom)
{
addManipulatorGeometries(model,geom,"rleg");
addManipulatorGeometries(model,geom,"lleg");
......@@ -330,6 +334,7 @@ namespace se3
SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0.5)) );
geom.addGeometryObject(chestArm,model,true);
}
#endif
} // namespace buildModels
......
//
// Copyright (c) 2015-2016 CNRS
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
......@@ -30,14 +30,17 @@ namespace se3
*
* \param model: model, typically given empty, where the kinematic chain is added.
*/
void manipulator(Model& model);
void manipulator(Model & model);
#ifdef WITH_HPP_FCL
/** \brief Create the geometries on top of the kinematic model created by manipulator function.
*
* \param model, const, kinematic chain typically produced by the function manipulator(model).
* \warning this method is expecting specific namings of the kinematic chain, use it with care
* not using after manipulator(model).
*/
void manipulatorGeometries(const Model& model, GeometryModel & geom);
void manipulatorGeometries(const Model & model, GeometryModel & geom);
#endif
/** \brief Create a 28-DOF kinematic chain of a floating humanoid robot.
*
......@@ -51,14 +54,17 @@ namespace se3
* \param usingFF: if True, implement the chain with a plain JointModelFreeFloating; if False,
* uses a composite joint. This changes the size of the configuration space (35 vs 34).
*/
void humanoid(Model& model,bool usingFF=true);
void humanoid(Model & model, bool usingFF=true);
#ifdef WITH_HPP_FCL
/** \brief Create the geometries on top of the kinematic model created by humanoid function.
*
* \param model, const, kinematic chain typically produced by the function humanoid(model).
* \warning this method is expecting specific namings of the kinematic chain, use it with care
* not using after humanoid(model).
*/
void humanoidGeometries(const Model& model, GeometryModel & geom);
void humanoidGeometries(const Model & model, GeometryModel & geom);
#endif
/** \brief Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
*
......@@ -71,19 +77,19 @@ namespace se3
* uses a composite joint translation + roll-pitch-yaw.
* This changes the size of the configuration space (33 vs 32).
*/
void humanoidRandom(Model& model, bool usingFF = true);
void humanoidRandom(Model & model, bool usingFF = true);
/** \brief Create a random humanoid tree with 2d limbs.
* \ deprecated This function has been replaced by the non-random se3::humanoid function.
*/
PINOCCHIO_DEPRECATED
void humanoid2d(Model& model);
void humanoid2d(Model & model);
/** \brief Alias of humanoidRandom, for compatibility reasons.
* \deprecated use se3::humanoid or se3::humanoidRandom instead.
*/
PINOCCHIO_DEPRECATED
inline void humanoidSimple(Model& model, bool usingFF = true) { humanoidRandom(model,usingFF); }
inline void humanoidSimple(Model & model, bool usingFF = true) { humanoidRandom(model,usingFF); }
} // namespace buildModels
} // namespace se3
......
......@@ -174,13 +174,15 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
const SE3 & framePlacement = SE3::Random();
model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
BOOST_CHECK(model.existFrame(frame_name));
se3::Data data(model);
se3::Data data_ref(model);
VectorXd q = VectorXd::Ones(model.nq);
q.middleRows<4> (3).normalize();
VectorXd q_dot = VectorXd::Ones(model.nv);
model.lowerPositionLimit.head<7>().fill(-1.);
model.upperPositionLimit.head<7>().fill( 1.);
VectorXd q = randomConfiguration(model);
VectorXd v = VectorXd::Ones(model.nv);
/// In local frame
Model::Index idx = model.getFrameId(frame_name);
......@@ -188,11 +190,14 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));
Data::Matrix6x Jjj(6,model.nv); Jjj.fill(0);
Data::Matrix6x Jff(6,model.nv); Jff.fill(0);
getFrameJacobian<LOCAL>(model,data,idx,Jff);
computeJointJacobians(model,data,q);
updateFramePlacement(model, data, idx);
getFrameJacobian<LOCAL>(model, data, idx, Jff);
computeJointJacobians(model,data_ref,q);
getJointJacobian<LOCAL>(model, data_ref, parent_idx, Jjj);
Motion nu_frame = Motion(Jff*q_dot);
Motion nu_joint = Motion(Jjj*q_dot);
Motion nu_frame = Motion(Jff*v);
Motion nu_joint = Motion(Jjj*v);
const SE3::ActionMatrix_t jXf = frame.placement.toActionMatrix();
Data::Matrix6x Jjj_from_frame(jXf * Jff);
......
......@@ -16,8 +16,6 @@
// <http://www.gnu.org/licenses/>.
#include <iostream>
#include <fstream>
#include <streambuf>
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
......@@ -27,8 +25,6 @@
#include <boost/test/unit_test.hpp>
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
BOOST_AUTO_TEST_CASE ( build_model_sample_humanoid_random )
......@@ -54,9 +50,11 @@ BOOST_AUTO_TEST_CASE ( build_model_sample_manipulator )
BOOST_CHECK(model.nq == 6);
BOOST_CHECK(model.nv == 6);
#ifdef WITH_HPP_FCL
se3::Data data(model);
se3::GeometryModel geom;
se3::buildModels::manipulatorGeometries(model,geom);
#endif
}
BOOST_AUTO_TEST_CASE ( build_model_sample_humanoid )
......@@ -68,6 +66,7 @@ BOOST_AUTO_TEST_CASE ( build_model_sample_humanoid )
BOOST_CHECK(model.nq == 35);
BOOST_CHECK(model.nv == 34);
#ifdef WITH_HPP_FCL
se3::GeometryModel geom;
se3::buildModels::humanoidGeometries(model,geom);
se3::GeometryData geomdata(geom);
......@@ -75,11 +74,10 @@ BOOST_AUTO_TEST_CASE ( build_model_sample_humanoid )
Eigen::VectorXd q = se3::neutral(model);
se3::forwardKinematics(model,data,q);
se3::updateGeometryPlacements(model,data,geom,geomdata,q);
#endif
/* We might want to check here the joint namings, and validate the
* direct geometry with respect to reference values. */
}
BOOST_AUTO_TEST_SUITE_END()
Markdown is supported
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