diff --git a/CMakeLists.txt b/CMakeLists.txt
index fdfa254a8af06fe498c333785777ff966048d377..55f00d742fad6752985f3b4b2cdb458aa59d661b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -118,6 +118,7 @@ SET(${PROJECT_NAME}_MATH_HEADERS
math/fwd.hpp
math/sincos.hpp
math/quaternion.hpp
+ math/matrix.hpp
)
SET(${PROJECT_NAME}_TOOLS_HEADERS
@@ -156,6 +157,7 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
multibody/joint/joint.hpp
multibody/joint/joint-basic-visitors.hpp
multibody/joint/joint-basic-visitors.hxx
+ multibody/joint/joint-composite.hpp
)
SET(${PROJECT_NAME}_MULTIBODY_HEADERS
diff --git a/benchmark/timings-geometry.cpp b/benchmark/timings-geometry.cpp
index c34ccea678c75405a19981f24624730fdcbe12df..955d165566093dff9cd37dbd3c7fb7f84c82bd43 100644
--- a/benchmark/timings-geometry.cpp
+++ b/benchmark/timings-geometry.cpp
@@ -70,8 +70,9 @@ 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::GeometryModel geom_model = se3::urdf::buildGeom(model, romeo_filename, package_dirs, COLLISION);
+ 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();
#endif // WITH_HPP_FCL
@@ -113,7 +114,7 @@ int main()
updateGeometryPlacements(model,data,geom_model,geom_data,qs_romeo[_smooth]);
for (std::vector<se3::CollisionPair>::iterator it = geom_model.collisionPairs.begin(); it != geom_model.collisionPairs.end(); ++it)
{
- geom_data.computeCollision(*it);
+ computeCollision(geom_model,geom_data,std::size_t(it-geom_model.collisionPairs.begin()));
}
}
double collideTime = timer.toc(StackTicToc::US)/NBT - (update_col_time + geom_time);
@@ -124,7 +125,7 @@ int main()
timer.tic();
SMOOTH(NBT)
{
- computeCollisions(model,data,geom_model,geom_data,qs_romeo[_smooth], true);
+ computeCollisions(geom_model,geom_data, true);
}
double is_colliding_time = timer.toc(StackTicToc::US)/NBT - (update_col_time + geom_time);
std::cout << "Collision Test : robot in collision? = \t" << is_colliding_time
@@ -203,7 +204,7 @@ hpp::model::HumanoidRobotPtr_t humanoidRobot =
timer.tic();
SMOOTH(NBT)
{
- computeCollisions(geom_data, true);
+ computeCollisions(geom_model, geom_data, true);
}
double is_romeo_colliding_time_pino = timer.toc(StackTicToc::US)/NBT;
std::cout << "Pinocchio - Collision Test : robot in collision? (G) = \t" << is_romeo_colliding_time_pino
@@ -244,7 +245,7 @@ hpp::model::HumanoidRobotPtr_t humanoidRobot =
timer.tic();
SMOOTH(NBD)
{
- computeDistances(geom_data);
+ computeDistances(geom_model, geom_data);
}
computeDistancesTime = timer.toc(StackTicToc::US)/NBD ;
std::cout << "Pinocchio - Compute distances (D) " << geom_model.collisionPairs.size() << " col pairs\t" << computeDistancesTime
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/bindings/python/algorithm/expose-geometry.cpp b/bindings/python/algorithm/expose-geometry.cpp
index dadf7c06c3b06f5778ed0f0eb6a2ac136034e6de..ff4fc77fad637a03a1566ef945938fbeea818609 100644
--- a/bindings/python/algorithm/expose-geometry.cpp
+++ b/bindings/python/algorithm/expose-geometry.cpp
@@ -36,11 +36,20 @@ namespace se3
return updateGeometryPlacements(*model, *data, *geom_model, *geom_data, q);
}
-#ifdef WITH_HPP_FCL
- static bool computeCollisions_proxy(GeometryDataHandler & data_geom,
+#ifdef WITH_HPP_FCL
+
+ static bool computeCollision_proxy(const GeometryModelHandler & model_geom,
+ GeometryDataHandler & data_geom,
+ const PairIndex & pairId)
+ {
+ return computeCollision(*model_geom, *data_geom, pairId);
+ }
+
+ static bool computeCollisions_proxy(const GeometryModelHandler & model_geom,
+ GeometryDataHandler & data_geom,
const bool stopAtFirstCollision)
{
- return computeCollisions(*data_geom, stopAtFirstCollision);
+ return computeCollisions(*model_geom, *data_geom, stopAtFirstCollision);
}
static bool computeGeometryAndCollisions_proxy(const ModelHandler & model,
@@ -53,20 +62,29 @@ namespace se3
return computeCollisions(*model,*data,*model_geom, *data_geom, q, stopAtFirstCollision);
}
- static void computeDistances_proxy(GeometryDataHandler & data_geom)
+ static fcl::DistanceResult computeDistance_proxy(const GeometryModelHandler & model_geom,
+ GeometryDataHandler & data_geom,
+ const PairIndex & pairId)
+ {
+ return computeDistance(*model_geom, *data_geom, pairId);
+ }
+
+ static std::size_t computeDistances_proxy(const GeometryModelHandler & model_geom,
+ GeometryDataHandler & data_geom)
{
- computeDistances(*data_geom);
+ return computeDistances(*model_geom, *data_geom);
}
- static void computeGeometryAndDistances_proxy(const ModelHandler & model,
- DataHandler & data,
- const GeometryModelHandler & model_geom,
- GeometryDataHandler & data_geom,
- const Eigen::VectorXd & q
- )
+ static std::size_t computeGeometryAndDistances_proxy(const ModelHandler & model,
+ DataHandler & data,
+ const GeometryModelHandler & model_geom,
+ GeometryDataHandler & data_geom,
+ const Eigen::VectorXd & q
+ )
{
- computeDistances(*model, *data, *model_geom, *data_geom, q);
+ return computeDistances<true>(*model, *data, *model_geom, *data_geom, q);
}
+
#endif // WITH_HPP_FCL
void exposeGeometryAlgo()
@@ -78,6 +96,12 @@ namespace se3
);
#ifdef WITH_HPP_FCL
+ bp::def("computeCollision", computeCollision_proxy,
+ bp::args("GoometryModel", "GeometryData", "pairIndex"),
+ "Check if the collision objects of a collision pair for a given Geometry Model and Data are in collision."
+ "The collision pair is given by the two index of the collision objects."
+ );
+
bp::def("computeCollisions",computeCollisions_proxy,
bp::args("GeometryData","bool"),
"Determine if collision pairs are effectively in collision."
@@ -89,9 +113,14 @@ namespace se3
"determine if all collision pairs are effectively in collision or not."
);
+ bp::def("computeDistance",computeDistance_proxy,
+ bp::args("GeometryModel","GeometryData", "pairIndex"),
+ "Compute the distance between the two geometry objects of a given collision pair for a GeometryModel and associated GeometryData."
+ );
+
bp::def("computeDistances",computeDistances_proxy,
- bp::args("GeometryData"),
- "Compute the distance between each collision pair."
+ bp::args("GeometryModel","GeometryData"),
+ "Compute the distance between each collision pair for a given GeometryModel and associated GeometryData."
);
bp::def("computeGeometryAndDistances",computeGeometryAndDistances_proxy,
diff --git a/bindings/python/algorithm/expose-joints.cpp b/bindings/python/algorithm/expose-joints.cpp
index 54023c1959ecf7d4d2bb294394a1df8a466fa9fd..0c4179b72b326d2c79db0e2857ce517b7a81fc6e 100644
--- a/bindings/python/algorithm/expose-joints.cpp
+++ b/bindings/python/algorithm/expose-joints.cpp
@@ -57,6 +57,21 @@ namespace se3
{
return randomConfiguration(*model, lowerPosLimit, upperPosLimit);
}
+
+ static void normalize_proxy(const ModelHandler & model,
+ VectorXd_fx & config)
+ {
+ Eigen::VectorXd q(config);
+ normalize(*model, q);
+ config = q;
+ }
+
+ static bool isSameConfiguration_proxy(const ModelHandler & model,
+ const VectorXd_fx & q1,
+ const VectorXd_fx & q2)
+ {
+ return isSameConfiguration(*model, q1, q2);
+ }
void exposeJointsAlgo()
{
@@ -88,6 +103,15 @@ namespace se3
"Joint lower limits (size Model::nq)",
"Joint upper limits (size Model::nq)"),
"Generate a random configuration ensuring provied joint limits are respected ");
+ bp::def("normalize",normalize_proxy,
+ bp::args("Model",
+ "Configuration q (size Model::nq)"),
+ "return the configuration normalized ");
+ bp::def("isSameConfiguration",isSameConfiguration_proxy,
+ bp::args("Model",
+ "Configuration q1 (size Model::nq)",
+ "Configuration q2 (size Model::nq)"),
+ "Return true if two configurations are equivalent");
}
} // namespace python
diff --git a/bindings/python/force.hpp b/bindings/python/force.hpp
index 52852ff0dadc4b0549fe127c1bb168d1331b1603..5e29e6cc7c71657a4e108ed345056f8ff91e30b4 100644
--- a/bindings/python/force.hpp
+++ b/bindings/python/force.hpp
@@ -85,6 +85,7 @@ namespace se3
.def("__add__",&ForcePythonVisitor::add)
.def("__sub__",&ForcePythonVisitor::subst)
+ .def("__neg__",&ForcePythonVisitor::neg)
.def(bp::self_ns::str(bp::self_ns::self))
.def(bp::self_ns::repr(bp::self_ns::self))
@@ -108,6 +109,7 @@ namespace se3
static Force_fx add(const Force_fx & f1, const Force_fx & f2) { return f1+f2; }
static Force_fx subst(const Force_fx & f1, const Force_fx & f2) { return f1-f2; }
+ static Force_fx neg(const Force_fx & f1) { return -f1; }
static void expose()
{
diff --git a/bindings/python/frame.hpp b/bindings/python/frame.hpp
index 4a1984655d6f62de1273da245ddd5c4512756d1c..c255ef3b349218c80d1c4c600f55af270675fec5 100644
--- a/bindings/python/frame.hpp
+++ b/bindings/python/frame.hpp
@@ -52,16 +52,20 @@ namespace se3
void visit(PyClass& cl) const
{
cl
- .def(bp::init< const std::string&,const JointIndex, const SE3_fx&,FrameType> ((bp::arg("name (string)"),bp::arg("parent (index)"), bp::arg("SE3 placement"), bp::arg("type (FrameType)")),
- "Initialize from name, parent id and placement wrt parent joint."))
+ .def(bp::init< const std::string&,const JointIndex, const FrameIndex, const SE3_fx&,FrameType> ((bp::arg("name (string)"),bp::arg("index of parent joint"), bp::args("index of parent frame"), bp::arg("SE3 placement"), bp::arg("type (FrameType)")),
+ "Initialize from name, parent joint id, parent frame id and placement wrt parent joint."))
.def_readwrite("name", &Frame::name, "name of the frame")
.def_readwrite("parent", &Frame::parent, "id of the parent joint")
+ .def_readwrite("previousFrame", &Frame::previousFrame, "id of the previous frame")
.add_property("placement",
&FramePythonVisitor::getPlacementWrtParentJoint,
&FramePythonVisitor::setPlacementWrtParentJoint,
"placement in the parent joint local frame")
.def_readwrite("type", &Frame::type, "type of the frame")
+
+ .def(bp::self_ns::str(bp::self_ns::self))
+ .def(bp::self_ns::repr(bp::self_ns::self))
;
}
diff --git a/bindings/python/geometry-data.hpp b/bindings/python/geometry-data.hpp
index e8b380f0d57c62ce643d4ce64cbbe18cd0f752bd..b95ec75734b8d569db046cb6dcc1443c77cd5801 100644
--- a/bindings/python/geometry-data.hpp
+++ b/bindings/python/geometry-data.hpp
@@ -28,6 +28,20 @@
#include "pinocchio/bindings/python/data.hpp"
#include "pinocchio/bindings/python/geometry-model.hpp"
+namespace fcl
+{
+ // This operator is defined here temporary, as it is needed by vector_indexing_suite
+ // It has also been defined in hpp-fcl in a pending pull request.
+ // Once it has been integrated in releases of hpp-fcl, please remove this operator
+ inline bool operator ==(const DistanceResult & dr1, const DistanceResult& dr2)
+ {
+ return dr1.min_distance == dr2.min_distance
+ && dr1.o1 == dr2.o1
+ && dr1.o2 == dr2.o2
+ && dr1.nearest_points[0] == dr2.nearest_points[0]
+ && dr1.nearest_points[1] == dr2.nearest_points[1];
+ }
+}
namespace se3
{
namespace python
@@ -96,18 +110,33 @@ namespace se3
bp::make_function(&GeometryDataPythonVisitor::oMg,
bp::return_internal_reference<>()),
"Vector of collision objects placement relative to the world.")
-#ifdef WITH_HPP_FCL
- .add_property("distance_results",
- bp::make_function(&GeometryDataPythonVisitor::distance_results,
+#ifdef WITH_HPP_FCL
+ .add_property("activeCollisionPairs",
+ bp::make_function(&GeometryDataPythonVisitor::activeCollisionPairs,
+ bp::return_internal_reference<>()))
+ .add_property("distanceRequest",
+ bp::make_function(&GeometryDataPythonVisitor::distanceRequest,
+ bp::return_internal_reference<>()),
+ "Defines what information should be computed by fcl for distances")
+ .add_property("distanceResults",
+ bp::make_function(&GeometryDataPythonVisitor::distanceResults,
bp::return_internal_reference<>()),
"Vector of distance results computed in ")
+ .add_property("CollisionRequest",
+ bp::make_function(&GeometryDataPythonVisitor::CollisionRequest,
+ bp::return_internal_reference<>()),
+ "Defines what information should be computed by fcl for collision tests")
.add_property("collision_results",
bp::make_function(&GeometryDataPythonVisitor::collision_results,
bp::return_internal_reference<>()) )
- .add_property("activeCollisionPairs",
- bp::make_function(&GeometryDataPythonVisitor::activeCollisionPairs,
- bp::return_internal_reference<>()))
+ .add_property("radius",
+ bp::make_function(&GeometryDataPythonVisitor::radius,
+ bp::return_internal_reference<>()),
+ "Vector of radius of bodies, ie distance of the further point of the geometry object from the joint center ")
+ .def("fillInnerOuterObjectMaps", &GeometryDataPythonVisitor::fillInnerOuterObjectMaps,
+ bp::args("GeometryModel"),
+ "Fill inner and outer objects maps")
.def("activateCollisionPair",&GeometryDataPythonVisitor::activateCollisionPair,
bp::args("pairIndex (int)"),
"Activate pair ID <pairIndex> in geomModel.collisionPairs."
@@ -116,23 +145,6 @@ namespace se3
bp::args("pairIndex (int)"),
"Deactivate pair ID <pairIndex> in geomModel.collisionPairs.")
- .def("computeCollision",&GeometryDataPythonVisitor::computeCollision,
- bp::args("co1 (index)","co2 (index)"),
- "Check if the two collision objects of a collision pair are in collision."
- "The collision pair is given by the two index of the collision objects.")
- .def("computeAllCollisions",&GeometryDataPythonVisitor::computeAllCollisions,
- "Same as computeCollision. It applies computeCollision to all collision pairs contained collision_pairs."
- "The results are stored in collision_results.")
- .def("isColliding",&GeometryDataPythonVisitor::isColliding,
- "Check if at least one of the collision pairs is in collision.")
-
- .def("computeDistance",&GeometryDataPythonVisitor::computeDistance,
- bp::args("co1 (index)","co2 (index)"),
- "Compute the distance result between two collision objects of a collision pair."
- "The collision pair is given by the two index of the collision objects.")
- .def("computeAllDistances",&GeometryDataPythonVisitor::computeAllDistances,
- "Same as computeDistance. It applies computeDistance to all collision pairs contained collision_pairs."
- "The results are stored in collision_distances.")
#endif // WITH_HPP_FCL
.def("__str__",&GeometryDataPythonVisitor::toString)
;
@@ -145,25 +157,21 @@ namespace se3
static std::vector<SE3> & oMg(GeometryDataHandler & m) { return m->oMg; }
#ifdef WITH_HPP_FCL
- static std::vector<DistanceResult> & distance_results( GeometryDataHandler & m ) { return m->distance_results; }
- static std::vector<CollisionResult> & collision_results( GeometryDataHandler & m ) { return m->collision_results; }
static std::vector<bool> & activeCollisionPairs(GeometryDataHandler & m) { return m->activeCollisionPairs; }
- static CollisionResult computeCollision(const GeometryDataHandler & m, const GeomIndex co1, const GeomIndex co2)
- {
- return m->computeCollision(CollisionPair(co1, co2));
- }
- static bool isColliding(const GeometryDataHandler & m) { return m->isColliding(); }
- static void computeAllCollisions(GeometryDataHandler & m) { m->computeAllCollisions(); }
+ static fcl::DistanceRequest & distanceRequest( GeometryDataHandler & m ) { return m->distanceRequest; }
+ static std::vector<fcl::DistanceResult> & distanceResults( GeometryDataHandler & m ) { return m->distanceResults; }
+ static fcl::CollisionRequest & CollisionRequest( GeometryDataHandler & m ) { return m->collisionRequest; }
+ static std::vector<fcl::CollisionResult> & collision_results( GeometryDataHandler & m ) { return m->collisionResults; }
+ static std::vector<double> & radius( GeometryDataHandler & m ) { return m->radius; }
+
+ static void fillInnerOuterObjectMaps(GeometryDataHandler & m, const GeometryModelHandler & model)
+ {m->fillInnerOuterObjectMaps(*model);}
+
static void activateCollisionPair(GeometryDataHandler & m,
Index pairID) { m->activateCollisionPair(pairID); }
static void deactivateCollisionPair(GeometryDataHandler & m,
Index pairID) { m->deactivateCollisionPair(pairID); }
- static DistanceResult computeDistance(const GeometryDataHandler & m, const GeomIndex co1, const GeomIndex co2)
- {
- return m->computeDistance(CollisionPair(co1, co2));
- }
- static void computeAllDistances(GeometryDataHandler & m) { m->computeAllDistances(); }
#endif // WITH_HPP_FCL
static std::string toString(const GeometryDataHandler& m)
@@ -173,11 +181,11 @@ namespace se3
static void expose()
{
#ifdef WITH_HPP_FCL
- bp::class_< std::vector<DistanceResult> >("StdVec_DistanceResult")
- .def(bp::vector_indexing_suite< std::vector<DistanceResult> >());
+ bp::class_< std::vector<fcl::DistanceResult> >("StdVec_DistanceResult")
+ .def(bp::vector_indexing_suite< std::vector<fcl::DistanceResult> >());
- bp::class_< std::vector<CollisionResult> >("StdVec_CollisionResult")
- .def(bp::vector_indexing_suite< std::vector<CollisionResult> >());
+ bp::class_< std::vector<fcl::CollisionResult> >("StdVec_CollisionResult")
+ .def(bp::vector_indexing_suite< std::vector<fcl::CollisionResult> >());
#endif // WITH_HPP_FCL
bp::class_<GeometryDataHandler>("GeometryData",
"Geometry data linked to a geometry model and data struct.",
diff --git a/bindings/python/geometry-model.hpp b/bindings/python/geometry-model.hpp
index 1dcc106a5d79ded02491bef4c205ec229b3eb7c9..7fa8a1ece64c6ce96ca1cf074b6d590ed1a07ce5 100644
--- a/bindings/python/geometry-model.hpp
+++ b/bindings/python/geometry-model.hpp
@@ -28,6 +28,8 @@
#include "pinocchio/multibody/geometry.hpp"
+#include "pinocchio/bindings/python/model.hpp"
+
namespace se3
{
namespace python
@@ -62,17 +64,19 @@ namespace se3
{
cl
.add_property("ngeoms", &GeometryModelPythonVisitor::ngeoms)
-
- .def("getGeometryId",&GeometryModelPythonVisitor::getGeometryId)
- .def("existGeometryName",&GeometryModelPythonVisitor::existGeometryName)
- .def("getGeometryName",&GeometryModelPythonVisitor::getGeometryName)
.add_property("geometryObjects",
bp::make_function(&GeometryModelPythonVisitor::geometryObjects,
bp::return_internal_reference<>()) )
+
+ .def("addGeometryObject", &GeometryModelPythonVisitor::addGeometryObject,
+ bp::args("GeometryObject", "Model", "bool"),
+ "Add a GeometryObject to a GeometryModel")
+ .def("getGeometryId",&GeometryModelPythonVisitor::getGeometryId)
+ .def("existGeometryName",&GeometryModelPythonVisitor::existGeometryName)
.def("__str__",&GeometryModelPythonVisitor::toString)
#ifdef WITH_HPP_FCL
- .add_property("collision_pairs",
- bp::make_function(&GeometryModelPythonVisitor::collision_pairs,
+ .add_property("collisionPairs",
+ bp::make_function(&GeometryModelPythonVisitor::collisionPairs,
bp::return_internal_reference<>()),
"Vector of collision pairs.")
.def("addCollisionPair",&GeometryModelPythonVisitor::addCollisionPair,
@@ -106,12 +110,14 @@ namespace se3
{ return gmodelPtr->getGeometryId(name); }
static bool existGeometryName(const GeometryModelHandler & gmodelPtr, const std::string & name)
{ return gmodelPtr->existGeometryName(name);}
- static std::string getGeometryName(const GeometryModelHandler & gmodelPtr, const GeomIndex index)
- { return gmodelPtr->getGeometryName(index);}
+
static std::vector<GeometryObject> & geometryObjects( GeometryModelHandler & m ) { return m->geometryObjects; }
+ static GeomIndex addGeometryObject( GeometryModelHandler & m, GeometryObject gobject, const ModelHandler & model, const bool autofillJointParent)
+ { return m-> addGeometryObject(gobject, *model, autofillJointParent); }
+
#ifdef WITH_HPP_FCL
- static std::vector<CollisionPair> & collision_pairs( GeometryModelHandler & m )
+ static std::vector<CollisionPair> & collisionPairs( GeometryModelHandler & m )
{ return m->collisionPairs; }
static void addCollisionPair (GeometryModelHandler & m, const GeomIndex co1, const GeomIndex co2)
@@ -146,7 +152,6 @@ namespace se3
bp::enum_<GeometryType>("GeometryType")
.value("VISUAL",VISUAL)
.value("COLLISION",COLLISION)
- .value("NONE",NONE)
;
bp::class_<GeometryModelHandler>("GeometryModel",
diff --git a/bindings/python/geometry-object.hpp b/bindings/python/geometry-object.hpp
index cd9506845e11acf9da5b8c0b4c3f5fdeb4796a15..5117a77a4d10a2bb2b456a99b94ef1411e30800f 100644
--- a/bindings/python/geometry-object.hpp
+++ b/bindings/python/geometry-object.hpp
@@ -53,11 +53,12 @@ namespace se3
{
cl
.def_readwrite("name", &GeometryObject::name, "Name of the GeometryObject")
- .def_readwrite("parent", &GeometryObject::parent, "Index of the parent joint")
+ .def_readwrite("parentJoint", &GeometryObject::parentJoint, "Index of the parent joint")
+ .def_readwrite("parentFrame", &GeometryObject::parentFrame, "Index of the parent frame")
.add_property("placement", &GeometryObjectPythonVisitor::getPlacementWrtParentJoint,
&GeometryObjectPythonVisitor::setPlacementWrtParentJoint,
"Position of geometry object in parent joint's frame")
- .def_readonly("mesh_path", &GeometryObject::mesh_path, "Absolute path to the mesh file")
+ .def_readonly("meshPath", &GeometryObject::meshPath, "Absolute path to the mesh file")
;
}
@@ -67,7 +68,7 @@ namespace se3
static void expose()
{
bp::class_<GeometryObject>("GeometryObject",
- "A wrapper on a collision geometry including its parent joint, placement in parent frame.\n\n",
+ "A wrapper on a collision geometry including its parent joint, parent frame, placement in parent joint's frame.\n\n",
bp::no_init
)
.def(GeometryObjectPythonVisitor())
diff --git a/bindings/python/joint.hpp b/bindings/python/joint.hpp
index 9014d720fdbbb2e9609aa0db29bc280911f6ccf6..094bb7f02102f11fa6268a549bd5a0c2e6b9e90d 100644
--- a/bindings/python/joint.hpp
+++ b/bindings/python/joint.hpp
@@ -20,7 +20,7 @@
#include <eigenpy/exception.hpp>
#include <eigenpy/eigenpy.hpp>
-#include "pinocchio/multibody/joint/joint.hpp"
+#include "pinocchio/multibody/joint/joint-composite.hpp"
namespace se3
{
diff --git a/bindings/python/model.hpp b/bindings/python/model.hpp
index 6e16e9686eca03af940726c8d487730b21d70cec..a9136c0cdfef877a4edadc75065ef01c4cad7a99 100644
--- a/bindings/python/model.hpp
+++ b/bindings/python/model.hpp
@@ -94,16 +94,15 @@ namespace se3
void visit(PyClass& cl) const
{
cl
- .def("getBodyId",&ModelPythonVisitor::getBodyId)
- .def("getJointId",&ModelPythonVisitor::getJointId)
- .def("createData",&ModelPythonVisitor::createData)
+ // Class Members
.def("__str__",&ModelPythonVisitor::toString)
.add_property("nq", &ModelPythonVisitor::nq)
.add_property("nv", &ModelPythonVisitor::nv)
- .add_property("njoint", &ModelPythonVisitor::njoint)
- .add_property("nbody", &ModelPythonVisitor::nbody)
+ .add_property("njoints", &ModelPythonVisitor::njoints)
+ .add_property("nbodies", &ModelPythonVisitor::nbodies)
+ .add_property("nframes", &ModelPythonVisitor::nframes)
.add_property("inertias",
bp::make_function(&ModelPythonVisitor::inertias,
bp::return_internal_reference<>()) )
@@ -116,32 +115,41 @@ namespace se3
.add_property("parents",
bp::make_function(&ModelPythonVisitor::parents,
bp::return_internal_reference<>()) )
- .add_property("subtrees",
- bp::make_function(&ModelPythonVisitor::subtrees,
- bp::return_internal_reference<>()), "Vector of subtrees. subtree[j] corresponds to the subtree supported by the joint j.")
.add_property("names",
bp::make_function(&ModelPythonVisitor::names,
bp::return_internal_reference<>()) )
-
- .def("addJoint",&ModelPythonVisitor::addJoint,bp::args("parent_id","joint_model","joint_placement","joint_name"),"Adds a joint to the kinematic tree. The joint is defined by its placement relative to its parent joint and its name.")
- .def("appendBodyToJoint",&ModelPythonVisitor::appendBodyToJoint,bp::args("joint_id","body_inertia","body_placement","body_name"),"Appends a body to the joint given by its index. The body is defined by its inertia, its relative placement regarding to the joint and its name.")
-
-
- .add_property("effortLimit", bp::make_function(&ModelPythonVisitor::effortLimit), "Joint max effort")
.add_property("neutralConfiguration", bp::make_function(&ModelPythonVisitor::neutralConfiguration), "Joint's neutral configurations")
+ .add_property("effortLimit", bp::make_function(&ModelPythonVisitor::effortLimit), "Joint max effort")
.add_property("velocityLimit", bp::make_function(&ModelPythonVisitor::velocityLimit), "Joint max velocity")
.add_property("lowerPositionLimit", bp::make_function(&ModelPythonVisitor::lowerPositionLimit), "Limit for joint lower position")
.add_property("upperPositionLimit", bp::make_function(&ModelPythonVisitor::upperPositionLimit), "Limit for joint upper position")
+
+ .add_property("frames", bp::make_function(&ModelPythonVisitor::frames, bp::return_internal_reference<>()),"Vector of frames contained in the model.")
- .def("getFrameParent", &ModelPythonVisitor::getFrameParent)
- .def("getFramePlacement", &ModelPythonVisitor::getFramePlacement)
- .def("addFrame",(bool (*)(ModelHandler&,const std::string &,const JointIndex, const SE3_fx &,const FrameType &)) &ModelPythonVisitor::addFrame,bp::args("name","parent_id","placement","type"),"Add a frame to the vector of frames. See also Frame for more details. Returns False if the frame already exists.")
- .def("addFrame",(bool (*)(ModelHandler&,const Frame &)) &ModelPythonVisitor::addFrame,bp::args("frame"),"Add a frame to the vector of frames.")
- .add_property("frames", bp::make_function(&ModelPythonVisitor::frames, bp::return_internal_reference<>()),"Vector of frames contained in the model.")
- .def("existFrame",&ModelPythonVisitor::existFrame,bp::args("name"),"Returns true if the frame given by its name exists inside the Model.")
- .def("getFrameId",&ModelPythonVisitor::getFrameId,bp::args("name"),"Returns the index of the frame given by its name. If the frame is not in the frames vector, it returns the current size of the frames vector.")
-
+ .add_property("subtrees",
+ bp::make_function(&ModelPythonVisitor::subtrees,
+ bp::return_internal_reference<>()), "Vector of subtrees. subtree[j] corresponds to the subtree supported by the joint j.")
+
.add_property("gravity",&ModelPythonVisitor::gravity,&ModelPythonVisitor::setGravity)
+
+ // Class Methods
+ .def("addJoint",&ModelPythonVisitor::addJoint,bp::args("parent_id","joint_model","joint_placement","joint_name"),"Adds a joint to the kinematic tree. The joint is defined by its placement relative to its parent joint and its name.")
+ // ADD addJoint with limits ? See boost::python overloading/default parameters
+ .def("addJointFrame", &ModelPythonVisitor::addJointFrame, bp::args("jointIndex", "frameIndex"), "add the joint at index jointIndex as a frame to the frame tree")
+ .def("appendBodyToJoint",&ModelPythonVisitor::appendBodyToJoint,bp::args("joint_id","body_inertia","body_placement"),"Appends a body to the joint given by its index. The body is defined by its inertia, its relative placement regarding to the joint and its name.")
+
+ .def("addBodyFrame", &ModelPythonVisitor::addBodyFrame, bp::args("body_name", "parentJoint", "body_plaement", "previous_frame(parent frame)"), "add a body to the frame tree")
+ .def("getBodyId",&ModelPythonVisitor::getBodyId, bp::args("name"), "Return the index of a frame of type BODY given by its name")
+ .def("existBodyName", &ModelPythonVisitor::existBodyName, bp::args("name"), "Check if a frame of type BODY exists, given its name")
+ .def("getJointId",&ModelPythonVisitor::getJointId, bp::args("name"), "Return the index of a joint given by its name")
+ .def("existJointName", &ModelPythonVisitor::existJointName, bp::args("name"), "Check if a joint given by its name exists")
+ .def("getFrameId",&ModelPythonVisitor::getFrameId,bp::args("name"),"Returns the index of the frame given by its name. If the frame is not in the frames vector, it returns the current size of the frames vector.")
+ .def("existFrame",&ModelPythonVisitor::existFrame,bp::args("name"),"Returns true if the frame given by its name exists inside the Model.")
+
+ .def("addFrame",(bool (*)(ModelHandler&,const std::string &,const JointIndex, const FrameIndex, const SE3_fx &,const FrameType &)) &ModelPythonVisitor::addFrame,bp::args("name","parent_id","placement","type"),"Add a frame to the vector of frames. See also Frame for more details. Returns False if the frame already exists.")
+ .def("addFrame",(bool (*)(ModelHandler&,const Frame &)) &ModelPythonVisitor::addFrame,bp::args("frame"),"Add a frame to the vector of frames.")
+
+ .def("createData",&ModelPythonVisitor::createData)
.def("BuildEmptyModel",&ModelPythonVisitor::maker_empty)
.staticmethod("BuildEmptyModel")
.def("BuildHumanoidSimple",&ModelPythonVisitor::maker_humanoidSimple)
@@ -149,24 +157,28 @@ namespace se3
;
}
- static Model::Index getBodyId( const ModelHandler & modelPtr, const std::string & name )
- { return modelPtr->getBodyId(name); }
- static Model::Index getJointId( const ModelHandler & modelPtr, const std::string & name )
- { return modelPtr->getJointId(name); }
- static boost::shared_ptr<Data> createData(const ModelHandler& m )
- { return boost::shared_ptr<Data>( new Data(*m) ); }
static int nq( ModelHandler & m ) { return m->nq; }
static int nv( ModelHandler & m ) { return m->nv; }
- static int njoint( ModelHandler & m ) { return m->njoint; }
- static int nbody( ModelHandler & m ) { return m->nbody; }
+ static int njoints( ModelHandler & m ) { return m->njoints; }
+ static int nbodies( ModelHandler & m ) { return m->nbodies; }
+ static int nframes( ModelHandler & m ) { return m->nframes; }
static std::vector<Inertia> & inertias( ModelHandler & m ) { return m->inertias; }
static std::vector<SE3> & jointPlacements( ModelHandler & m ) { return m->jointPlacements; }
static JointModelVector & joints( ModelHandler & m ) { return m->joints; }
static std::vector<Model::JointIndex> & parents( ModelHandler & m ) { return m->parents; }
static std::vector<std::string> & names ( ModelHandler & m ) { return m->names; }
+ static Eigen::VectorXd neutralConfiguration(ModelHandler & m) {return m->neutralConfiguration;}
+ static Eigen::VectorXd effortLimit(ModelHandler & m) {return m->effortLimit;}
+ static Eigen::VectorXd velocityLimit(ModelHandler & m) {return m->velocityLimit;}
+ static Eigen::VectorXd lowerPositionLimit(ModelHandler & m) {return m->lowerPositionLimit;}
+ static Eigen::VectorXd upperPositionLimit(ModelHandler & m) {return m->upperPositionLimit;}
+ static std::vector<Frame> & frames ( ModelHandler & m ) {return m->frames; }
static std::vector<Model::IndexVector> & subtrees(ModelHandler & m) { return m->subtrees; }
+ static Motion gravity( ModelHandler & m ) { return m->gravity; }
+ static void setGravity( ModelHandler & m,const Motion_fx & g ) { m->gravity = g; }
+
static JointIndex addJoint(ModelHandler & model,
JointIndex parent_id,
bp::object jmodel,
@@ -177,39 +189,51 @@ namespace se3
return boost::apply_visitor(addJointVisitor(model,parent_id,joint_placement,joint_name), jmodel_variant);
}
+ static int addJointFrame( ModelHandler & m, const JointIndex jointIndex, int frameIndex)
+ {
+ return m->addJointFrame(jointIndex, frameIndex);
+ }
+
static void appendBodyToJoint(ModelHandler & model,
const JointIndex joint_parent_id,
const Inertia_fx & inertia,
- const SE3_fx & body_placement,
- const std::string & body_name)
+ const SE3_fx & body_placement)
{
- model->appendBodyToJoint(joint_parent_id,inertia,body_placement,body_name);
+ model->appendBodyToJoint(joint_parent_id,inertia,body_placement);
}
- static Eigen::VectorXd neutralConfiguration(ModelHandler & m) {return m->neutralConfiguration;}
- static Eigen::VectorXd effortLimit(ModelHandler & m) {return m->effortLimit;}
- static Eigen::VectorXd velocityLimit(ModelHandler & m) {return m->velocityLimit;}
- static Eigen::VectorXd lowerPositionLimit(ModelHandler & m) {return m->lowerPositionLimit;}
- static Eigen::VectorXd upperPositionLimit(ModelHandler & m) {return m->upperPositionLimit;}
-
- static Model::JointIndex getFrameParent( ModelHandler & m, const std::string & name ) { return m->getFrameParent(name); }
- static SE3 getFramePlacement(ModelHandler & m, const std::string & name) { return m->getFramePlacement(name); }
-
- static bool addFrame(ModelHandler & m, const Frame & frame) { return m->addFrame(frame); }
- static bool addFrame( ModelHandler & m, const std::string & frameName, const JointIndex parent, const SE3_fx & placementWrtParent, const FrameType & type)
+ static bool addBodyFrame( ModelHandler & m, const std::string & bodyName, const JointIndex parentJoint, const SE3_fx & bodyPlacement, int previousFrame)
{
- return m->addFrame(frameName,parent,placementWrtParent,type);
+ return m->addBodyFrame(bodyName,parentJoint,bodyPlacement,previousFrame);
}
+
+ static Model::Index getBodyId( const ModelHandler & m, const std::string & name )
+ { return m->getBodyId(name); }
+
+ static bool existBodyName( const ModelHandler & m, const std::string & name )
+ { return m->existBodyName(name); }
+
+ static Model::Index getJointId( const ModelHandler & m, const std::string & name )
+ { return m->getJointId(name); }
+
+ static bool existJointName( const ModelHandler & m, const std::string & name )
+ { return m->existJointName(name); }
+
static Model::FrameIndex getFrameId(const ModelHandler & m, const std::string & frame_name)
{ return m->getFrameId(frame_name); }
static bool existFrame(const ModelHandler & m, const std::string & frame_name)
{ return m->existFrame(frame_name); }
+
+ static bool addFrame(ModelHandler & m, const Frame & frame) { return m->addFrame(frame); }
+ static bool addFrame( ModelHandler & m, const std::string & frameName, const JointIndex parentJoint, const FrameIndex parentFrame, const SE3_fx & placementWrtParent, const FrameType & type)
+ {
+ return m->addFrame(Frame(frameName,parentJoint,parentFrame,placementWrtParent,type));
+ }
- static std::vector<Frame> & frames (ModelHandler & m ) { return m->frames;}
- static Motion gravity( ModelHandler & m ) { return m->gravity; }
- static void setGravity( ModelHandler & m,const Motion_fx & g ) { m->gravity = g; }
+ static boost::shared_ptr<Data> createData(const ModelHandler& m )
+ { return boost::shared_ptr<Data>( new Data(*m) ); }
static ModelHandler maker_empty()
{
diff --git a/bindings/python/motion.hpp b/bindings/python/motion.hpp
index d0be176ce44f1ffef1edc1073bdf6276cf3d7a0f..b84af9dfeb75b87a2a330eba6d46788576df9bb3 100644
--- a/bindings/python/motion.hpp
+++ b/bindings/python/motion.hpp
@@ -91,6 +91,7 @@ namespace se3
.def("__add__",&MotionPythonVisitor::add)
.def("__sub__",&MotionPythonVisitor::subst)
+ .def("__neg__",&MotionPythonVisitor::neg)
.def(bp::self_ns::str(bp::self_ns::self))
.def("Random",&Motion_fx::Random)
@@ -113,6 +114,7 @@ namespace se3
static Motion_fx add( const Motion_fx& m1,const Motion_fx& m2 ) { return m1+m2; }
static Motion_fx subst( const Motion_fx& m1,const Motion_fx& m2 ) { return m1-m2; }
+ static Motion_fx neg(const Motion_fx & m1) { return -m1; }
static Motion_fx cross_motion( const Motion_fx& m1,const Motion_fx& m2 ) { return m1.cross(m2); }
static Force_fx cross_force( const Motion_fx& m,const Force_fx& f ) { return m.cross(f); }
diff --git a/bindings/python/robot_wrapper.py b/bindings/python/robot_wrapper.py
index 8e8b8156b6ce683ef01bdee096bbafa597844100..ebb0d943c64f99cd5ccb267e9eab5a08339b3e66 100644
--- a/bindings/python/robot_wrapper.py
+++ b/bindings/python/robot_wrapper.py
@@ -32,6 +32,7 @@ class RobotWrapper(object):
self.data = self.model.createData()
self.model_filename = filename
+ print dir(se3)
if "buildGeomFromUrdf" not in dir(se3):
self.collision_model = None
self.visual_model = None
diff --git a/models/simple_model.py b/models/simple_model.py
index 2151c0819af9bbe67481e0e0368346469965463b..6fbf0661b35411256671d8705226a305ab885e7f 100644
--- a/models/simple_model.py
+++ b/models/simple_model.py
@@ -86,7 +86,9 @@ class ModelWrapper(object):
self.display.viewer.gui.addSphere(body_name, dimensions, color(body_color))
body_inertia.lever = lever.translation
joint_id = self.model.addJoint(parent, joint_model, joint_placement, joint_name)
- self.model.appendBodyToJoint(joint_id, body_inertia, se3.SE3.Identity(), body_name)
+ self.model.appendBodyToJoint(joint_id, body_inertia, se3.SE3.Identity())
+ self.model.addJointFrame(joint_id, -1)
+ self.model.addBodyFrame(body_name, joint_id, se3.SE3.Identity(),-1)
self.visuals.append((body_name, joint_placement, lever))
self.data = self.model.createData()
if self.display:
diff --git a/python/bindings_frame.py b/python/bindings_frame.py
index 2643ced00d7e41279b25394c223e4323f9e52d7f..69fa1a417659ab2c140b1647ae9c061ea68f71f3 100644
--- a/python/bindings_frame.py
+++ b/python/bindings_frame.py
@@ -24,19 +24,19 @@ class TestFrameBindings(unittest.TestCase):
robot = RobotWrapper(romeo_model_path, hint_list, se3.JointModelFreeFlyer())
def test_type_get_set(self):
- f = self.robot.model.frames[2]
+ f = self.robot.model.frames[5]
self.assertTrue(f.type == se3.FrameType.JOINT)
f.type = se3.FrameType.BODY
self.assertTrue(f.type == se3.FrameType.BODY)
def test_name_get_set(self):
- f = self.robot.model.frames[2]
+ f = self.robot.model.frames[5]
self.assertTrue(f.name == 'LHipYaw')
f.name = 'new_hip_frame'
self.assertTrue(f.name == 'new_hip_frame')
def test_parent_get_set(self):
- f = self.robot.model.frames[2]
+ f = self.robot.model.frames[5]
self.assertTrue(f.parent == 2)
f.parent = 5
self.assertTrue(f.parent == 5)
diff --git a/python/bindings_geometry_object.py b/python/bindings_geometry_object.py
index c9868b6dd923225b08723d0354387aa0aef0a155..e64bc1783ec07fa2d3a9966f607277ae33e6e64e 100644
--- a/python/bindings_geometry_object.py
+++ b/python/bindings_geometry_object.py
@@ -32,9 +32,9 @@ class TestGeometryObjectBindings(unittest.TestCase):
def test_parent_get_set(self):
col = self.robot.collision_model.geometryObjects[1]
- self.assertTrue(col.parent == 4)
- col.parent = 5
- self.assertTrue(col.parent == 5)
+ self.assertTrue(col.parentJoint == 4)
+ col.parentJoint = 5
+ self.assertTrue(col.parentJoint == 5)
def test_placement_get_set(self):
m = se3.SE3(self.m3ones, self.v3zero)
@@ -47,7 +47,7 @@ class TestGeometryObjectBindings(unittest.TestCase):
def test_meshpath_get(self):
expected_mesh_path = os.path.join(self.pinocchio_models_dir,'meshes/romeo/collision/LHipPitch.dae')
col = self.robot.collision_model.geometryObjects[1]
- self.assertTrue(col.mesh_path == expected_mesh_path)
+ self.assertTrue(col.meshPath == expected_mesh_path)
if __name__ == '__main__':
unittest.main()
diff --git a/python/model.py b/python/model.py
index 280a4d2378c7576d150513fd78b1744f907d6b6d..4824c16029629e12e6fea54d42245e475ccfe764 100644
--- a/python/model.py
+++ b/python/model.py
@@ -10,14 +10,14 @@ class TestModel(TestCase):
def test_empty_model_sizes(self):
model = se3.Model.BuildEmptyModel()
- self.assertEqual(model.nbody, 1)
+ self.assertEqual(model.nbodies, 1)
self.assertEqual(model.nq, 0)
self.assertEqual(model.nv, 0)
def test_model(self):
model = self.model
nb = 28 # We should have 28 bodies, thus 27 joints, one of them a free-flyer.
- self.assertEqual(model.nbody, nb)
+ self.assertEqual(model.nbodies, nb)
self.assertEqual(model.nq, nb - 1 + 6)
self.assertEqual(model.nv, nb - 1 + 5)
@@ -46,11 +46,11 @@ class TestModel(TestCase):
q = zero(model.nq)
qdot = zero(model.nv)
qddot = zero(model.nv)
- for i in range(model.nbody):
+ for i in range(model.nbodies):
data.a[i] = se3.Motion.Zero()
se3.rnea(model, data, q, qdot, qddot)
- for i in range(model.nbody):
+ for i in range(model.nbodies):
self.assertApprox(data.v[i].np, zero(6))
self.assertApprox(data.a_gf[0].np, -model.gravity.np)
self.assertApprox(data.f[-1], model.inertias[-1] * data.a_gf[-1])
diff --git a/src/algorithm/aba.hxx b/src/algorithm/aba.hxx
index 6b49a33a4ac8db993b8704474a41b0cd6230b043..af0f5c413af8ee0ea2e069f23fee04355ea7aade 100644
--- a/src/algorithm/aba.hxx
+++ b/src/algorithm/aba.hxx
@@ -197,19 +197,19 @@ namespace se3
data.a[0] = -model.gravity;
data.u = tau;
- for(Model::Index i=1;i<(Model::Index)model.njoint;++i)
+ for(Model::Index i=1;i<(Model::Index)model.njoints;++i)
{
AbaForwardStep1::run(model.joints[i],data.joints[i],
AbaForwardStep1::ArgsType(model,data,q,v));
}
- for( Model::Index i=(Model::Index)model.njoint-1;i>0;--i )
+ for( Model::Index i=(Model::Index)model.njoints-1;i>0;--i )
{
AbaBackwardStep::run(model.joints[i],data.joints[i],
AbaBackwardStep::ArgsType(model,data));
}
- for(Model::Index i=1;i<(Model::Index)model.njoint;++i)
+ for(Model::Index i=1;i<(Model::Index)model.njoints;++i)
{
AbaForwardStep2::run(model.joints[i],data.joints[i],
AbaForwardStep2::ArgsType(model,data));
diff --git a/src/algorithm/center-of-mass.hxx b/src/algorithm/center-of-mass.hxx
index 680fd31ece03f922a94b91f48594551aa5929ad4..af008090177ebcd86cbd520af8feb0fb9f3e6945 100644
--- a/src/algorithm/center-of-mass.hxx
+++ b/src/algorithm/center-of-mass.hxx
@@ -36,7 +36,7 @@ namespace se3
if (updateKinematics)
forwardKinematics(model, data, q);
- for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoint);++i)
+ for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoints);++i)
{
const double mass = model.inertias[i].mass();
const SE3::Vector3 & lever = model.inertias[i].lever();
@@ -46,7 +46,7 @@ namespace se3
}
// Backward Step
- for(Model::JointIndex i=(Model::JointIndex)(model.njoint-1); i>0; --i)
+ for(Model::JointIndex i=(Model::JointIndex)(model.njoints-1); i>0; --i)
{
const Model::JointIndex & parent = model.parents[i];
@@ -84,7 +84,7 @@ namespace se3
if (updateKinematics)
forwardKinematics(model, data, q, v);
- for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoint);++i)
+ for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoints);++i)
{
const double mass = model.inertias[i].mass();
const SE3::Vector3 & lever = model.inertias[i].lever();
@@ -98,7 +98,7 @@ namespace se3
}
// Backward Step
- for(Model::JointIndex i=(Model::JointIndex)(model.njoint-1); i>0; --i)
+ for(Model::JointIndex i=(Model::JointIndex)(model.njoints-1); i>0; --i)
{
const Model::JointIndex & parent = model.parents[i];
@@ -142,7 +142,7 @@ namespace se3
if (updateKinematics)
forwardKinematics(model, data, q, v, a);
- for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoint);++i)
+ for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoints);++i)
{
const double mass = model.inertias[i].mass();
const SE3::Vector3 & lever = model.inertias[i].lever();
@@ -158,7 +158,7 @@ namespace se3
}
// Backward Step
- for(Model::JointIndex i=(Model::JointIndex)(model.njoint-1); i>0; --i)
+ for(Model::JointIndex i=(Model::JointIndex)(model.njoints-1); i>0; --i)
{
const Model::JointIndex & parent = model.parents[i];
@@ -225,14 +225,37 @@ namespace se3
ColBlock Jcols = jmodel.jointCols(data.J);
Jcols = data.oMi[i].act(jdata.S());
- if( JointModel::NV==1 )
+ if (JointModel::NV == -1)
+ {
+ if( jmodel.nv() ==1 )
+ {
data.Jcom.col(jmodel.idx_v())
= data.mass[i] * Jcols.template topLeftCorner<3,1>()
- data.com[i].cross(Jcols.template bottomLeftCorner<3,1>()) ;
+ }
+ else
+ {
+ jmodel.jointCols(data.Jcom)
+ = data.mass[i] * Jcols.template topRows<3>()
+ - skew(data.com[i]) * Jcols.template bottomRows<3>();
+ }
+ }
else
- jmodel.jointCols(data.Jcom)
- = data.mass[i] * Jcols.template topRows<3>()
- - skew(data.com[i]) * Jcols.template bottomRows<3>();
+ {
+ if( JointModel::NV ==1 )
+ {
+ data.Jcom.col(jmodel.idx_v())
+ = data.mass[i] * Jcols.template topLeftCorner<3,1>()
+ - data.com[i].cross(Jcols.template bottomLeftCorner<3,1>()) ;
+ }
+ else
+ {
+ jmodel.jointCols(data.Jcom)
+ = data.mass[i] * Jcols.template topRows<3>()
+ - skew(data.com[i]) * Jcols.template bottomRows<3>();
+ }
+ }
+
if(computeSubtreeComs)
data.com[i] /= data.mass[i];
@@ -253,7 +276,7 @@ namespace se3
if (updateKinematics)
forwardKinematics(model, data, q);
- for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoint);++i)
+ for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoints);++i)
{
const double mass = model.inertias[i].mass();
const SE3::Vector3 & lever = model.inertias[i].lever();
@@ -263,7 +286,7 @@ namespace se3
}
// Backward step
- for( Model::JointIndex i= (Model::JointIndex) (model.njoint-1);i>0;--i )
+ for( Model::JointIndex i= (Model::JointIndex) (model.njoints-1);i>0;--i )
{
JacobianCenterOfMassBackwardStep
::run(model.joints[i],data.joints[i],
diff --git a/src/algorithm/compute-all-terms.hpp b/src/algorithm/compute-all-terms.hpp
index eceab35543f10bf0a8ee28d374d7300b865ae7f5..66525c539c3b9661b2f1f9d2dd979840ec286626 100644
--- a/src/algorithm/compute-all-terms.hpp
+++ b/src/algorithm/compute-all-terms.hpp
@@ -211,13 +211,13 @@ namespace se3
data.com[0].setZero ();
data.vcom[0].setZero ();
- for(Model::JointIndex i=1;i<(Model::JointIndex) model.njoint;++i)
+ for(Model::JointIndex i=1;i<(Model::JointIndex) model.njoints;++i)
{
CATForwardStep::run(model.joints[i],data.joints[i],
CATForwardStep::ArgsType(model,data,q,v));
}
- for(Model::JointIndex i=(Model::JointIndex)(model.njoint-1);i>0;--i)
+ for(Model::JointIndex i=(Model::JointIndex)(model.njoints-1);i>0;--i)
{
CATBackwardStep::run(model.joints[i],data.joints[i],
CATBackwardStep::ArgsType(model,data));
diff --git a/src/algorithm/copy.hpp b/src/algorithm/copy.hpp
index c9bc510f8db12e6b44b398745ac50160ebe45dd3..c0d5b1e913b354317ccc2a43c90e3f8f40c04423 100644
--- a/src/algorithm/copy.hpp
+++ b/src/algorithm/copy.hpp
@@ -47,7 +47,7 @@ namespace se3
inline void
copy (const Model& model, const Data & origin, Data & dest )
{
- for( se3::JointIndex jid=1; int(jid)<model.njoint; ++ jid )
+ for( se3::JointIndex jid=1; int(jid)<model.njoints; ++ jid )
{
assert(LEVEL>=0);
diff --git a/src/algorithm/crba.hxx b/src/algorithm/crba.hxx
index c84190f7ff2d68ae473b9f51445d4b7f0e5b71a9..a3315af801524cc59f51647f541acb4b926cf847 100644
--- a/src/algorithm/crba.hxx
+++ b/src/algorithm/crba.hxx
@@ -102,13 +102,13 @@ namespace se3
crba(const Model & model, Data& data,
const Eigen::VectorXd & q)
{
- for( Model::JointIndex i=1;i<(Model::JointIndex)(model.njoint);++i )
+ for( Model::JointIndex i=1;i<(Model::JointIndex)(model.njoints);++i )
{
CrbaForwardStep::run(model.joints[i],data.joints[i],
CrbaForwardStep::ArgsType(model,data,q));
}
- for( Model::JointIndex i=(Model::JointIndex)(model.njoint-1);i>0;--i )
+ for( Model::JointIndex i=(Model::JointIndex)(model.njoints-1);i>0;--i )
{
CrbaBackwardStep::run(model.joints[i],data.joints[i],
CrbaBackwardStep::ArgsType(model,data));
@@ -161,7 +161,7 @@ namespace se3
const se3::Model & model,
se3::Data & data)
{
- typedef typename Data::Matrix6x::NColsBlockXpr<JointModel::NV>::Type ColsBlock;
+ typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
const Model::Index & parent = model.parents[i];
@@ -169,11 +169,33 @@ namespace se3
data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
jdata.U() = data.Ycrb[i] * jdata.S();
+
ColsBlock jF
- = data.Ag.middleCols <JointModel::NV> (jmodel.idx_v());
+ = data.Ag.middleCols <JointModel::NV> (jmodel.idx_v());
+
forceSet::se3Action(data.oMi[i],jdata.U(),jF);
}
+ static void algo(const se3::JointModelBase<JointModelComposite> & jmodel,
+ se3::JointDataBase<JointDataComposite> & jdata,
+ const se3::Model & model,
+ se3::Data & data)
+ {
+ typedef SizeDepType<JointModel::NV>::ColsReturn<Data::Matrix6x>::Type ColsBlock;
+
+ const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
+ const Model::Index & parent = model.parents[i];
+
+ data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
+
+ jdata.U() = data.Ycrb[i] * jdata.S();
+
+ ColsBlock jF
+ = data.Ag.middleCols(jmodel.idx_v(), jmodel.nv());
+
+ forceSet::se3Action(data.oMi[i],jdata.U(),jF);
+ }
+
}; // struct CcrbaBackwardStep
inline const Data::Matrix6x &
@@ -185,11 +207,11 @@ namespace se3
forwardKinematics(model, data, q);
data.Ycrb[0].setZero();
- for(Model::Index i=1;i<(Model::Index)(model.njoint);++i )
+ for(Model::Index i=1;i<(Model::Index)(model.njoints);++i )
data.Ycrb[i] = model.inertias[i];
- for(Model::Index i=(Model::Index)(model.njoint-1);i>0;--i)
+ for(Model::Index i=(Model::Index)(model.njoints-1);i>0;--i)
{
CcrbaBackwardStep::run(model.joints[i],data.joints[i],
CcrbaBackwardStep::ArgsType(model,data));
diff --git a/src/algorithm/energy.hpp b/src/algorithm/energy.hpp
index 6c6207e57f0645f5d3f410fe6194ec1dc0e7d980..c6287805a839aca3f2c6292de13ce629c2cd7adf 100644
--- a/src/algorithm/energy.hpp
+++ b/src/algorithm/energy.hpp
@@ -76,7 +76,7 @@ namespace se3
if (update_kinematics)
forwardKinematics(model,data,q,v);
- for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoint);++i)
+ for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoints);++i)
data.kinetic_energy += model.inertias[i].vtiv(data.v[i]);
data.kinetic_energy *= .5;
@@ -96,7 +96,7 @@ namespace se3
if (update_kinematics)
forwardKinematics(model,data,q);
- for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoint);++i)
+ for(Model::JointIndex i=1;i<(Model::JointIndex)(model.njoints);++i)
{
com_global = data.oMi[i].translation() + data.oMi[i].rotation() * model.inertias[i].lever();
data.potential_energy += model.inertias[i].mass() * com_global.dot(g);
diff --git a/src/algorithm/frames.hpp b/src/algorithm/frames.hpp
index 30fbc7c1573126c17019ce3f99d92777360c4d59..825251898c5f8b31e964282e90a24ee741f83b6e 100644
--- a/src/algorithm/frames.hpp
+++ b/src/algorithm/frames.hpp
@@ -84,7 +84,7 @@ namespace se3
Data & data
)
{
- for (Model::FrameIndex i=0; i < (Model::FrameIndex) model.nFrames; ++i)
+ for (Model::FrameIndex i=0; i < (Model::FrameIndex) model.nframes; ++i)
{
const Frame & frame = model.frames[i];
const Model::JointIndex & parent = frame.parent;
diff --git a/src/algorithm/geometry.hpp b/src/algorithm/geometry.hpp
index b99352e61b9855c389214859a24652986b70ee43..8612aef9b0fac64bb31539f5f48d82f2a0422ac7 100644
--- a/src/algorithm/geometry.hpp
+++ b/src/algorithm/geometry.hpp
@@ -27,20 +27,19 @@
namespace se3
{
-
///
/// \brief Apply a forward kinematics and update the placement of the geometry objects.
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
- /// \param[in] geom The geometry model containing the collision objects.
- /// \param[out] geom_data The geometry data containing the placements of the collision objects. See oMg field in GeometryData.
+ /// \param[in] geomModel The geometry model containing the collision objects.
+ /// \param[out] geomData The geometry data containing the placements of the collision objects. See oMg field in GeometryData.
/// \param[in] q The joint configuration vector (dim model.nq).
///
inline void updateGeometryPlacements(const Model & model,
Data & data,
- const GeometryModel & geom,
- GeometryData & geom_data,
+ const GeometryModel & geomModel,
+ GeometryData & geomData,
const Eigen::VectorXd & q
);
@@ -49,50 +48,115 @@ namespace se3
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
- /// \param[in] geom The geometry model containing the collision objects.
- /// \param[out] geom_data The geometry data containing the placements of the collision objects. See oMg field in GeometryData.
+ /// \param[in] geomModel The geometry model containing the collision objects.
+ /// \param[out] geomData The geometry data containing the placements of the collision objects. See oMg field in GeometryData.
///
inline void updateGeometryPlacements(const Model & model,
const Data & data,
- const GeometryModel & geom,
- GeometryData & geom_data
+ const GeometryModel & geomModel,
+ GeometryData & geomData
);
+
#ifdef WITH_HPP_FCL
+
+ ///
+ /// \brief Compute the collision status between a *SINGLE* collision pair.
+ /// The result is store in the collisionResults vector.
+ ///
+ /// \param[in] GeomModel the geometry model (const)
+ /// \param[out] GeomData the corresponding geometry data, where computations are done.
+ /// \param[in] pairId The collsion pair index in the GeometryModel.
+ ///
+ /// \return Return true is the collision objects are colliding.
+ /// \note The complete collision result is also available in geomData.collisionResults[pairId]
+ ///
+ bool computeCollision(const GeometryModel & geomModel,
+ GeometryData & geomData,
+ const PairIndex & pairId
+ );
+
+ /// Compute the forward kinematics, update the geometry placements and
+ /// calls computeCollision for every active pairs of GeometryData.
+ ///
+ /// \param[in] model robot model (const)
+ /// \param[out] data corresponding data (nonconst) where FK results are stored
+ /// \param[in] geomModel geometry model (const)
+ /// \param[out] geomData corresponding geometry data (nonconst) where distances are computed
+ /// \param[in] q robot configuration.
+ /// \param[in] stopAtFirstCollision if true, stop the loop on pairs after the first collision.
+ /// \return When ComputeShortest is true, the index of the collision pair which has the shortest distance.
+ /// When ComputeShortest is false, the number of collision pairs.
+ /// \warning if stopAtFirstcollision = true, then the collisions vector will
+ /// not be entirely fulfilled (of course).
+ /// \note A similar function is available without model, data and q, not recomputing the FK.
inline bool computeCollisions(const Model & model,
Data & data,
- const GeometryModel & model_geom,
- GeometryData & data_geom,
+ const GeometryModel & geomModel,
+ GeometryData & geomData,
const Eigen::VectorXd & q,
const bool stopAtFirstCollision = false
);
- inline bool computeCollisions(GeometryData & data_geom,
- const bool stopAtFirstCollision = false
- );
-
- /// Compute the distances of all collision pairs
///
- /// \param ComputeShortest default to true.
- /// \param data_geom
+ /// \brief Compute the minimal distance between collision objects of a *SINGLE* collison pair
+ ///
+ /// \param[in] GeomModel the geometry model (const)
+ /// \param[out] GeomData the corresponding geometry data, where computations are done.
+ /// \param[in] pairId The index of the collision pair in geom model.
+ ///
+ /// \return A reference on fcl struct containing the distance result, referring an element
+ /// of vector geomData::distanceResults.
+ /// \note The complete distance result is also available in geomData.distanceResults[pairId]
+ ///
+ fcl::DistanceResult & computeDistance(const GeometryModel & geomModel,
+ GeometryData & geomData,
+ const PairIndex & pairId
+ );
+
+ /// Compute the forward kinematics, update the geometry placements and
+ /// calls computeDistance for every active pairs of GeometryData.
+ ///
+ /// \param[in] ComputeShortest default to true.
+ /// \param[in][out] model: robot model (const)
+ /// \param[out] data: corresponding data (nonconst) where FK results are stored
+ /// \param[in] geomModel: geometry model (const)
+ /// \param[out] geomData: corresponding geometry data (nonconst) where distances are computed
+ /// \param[in] q: robot configuration.
/// \return When ComputeShortest is true, the index of the collision pair which has the shortest distance.
/// When ComputeShortest is false, the number of collision pairs.
- template <bool ComputeShortest>
- inline std::size_t computeDistances(GeometryData & data_geom);
-
- /// Compute the forward kinematics, update the goemetry placements and
- /// calls computeDistances(GeometryData&).
+ /// \note A similar function is available without model, data and q, not recomputing the FK.
template <bool ComputeShortest>
inline std::size_t computeDistances(const Model & model,
Data & data,
- const GeometryModel & model_geom,
- GeometryData & data_geom,
+ const GeometryModel & geomModel,
+ GeometryData & geomData,
const Eigen::VectorXd & q
);
+ /// Compute the radius of the geometry volumes attached to every joints.
+ /// \sa GeometryData::radius
inline void computeBodyRadius(const Model & model,
const GeometryModel & geomModel,
GeometryData & geomData);
#endif // WITH_HPP_FCL
+
+ /// Append geomModel2 to geomModel1
+ ///
+ /// The steps for appending are:
+ /// \li add GeometryObject of geomModel2 to geomModel1,
+ /// \li add the collision pairs of geomModel2 into geomModel1 (indexes are updated)
+ /// \li add all the collision pairs between geometry objects of geomModel1 and geomModel2.
+ /// It is possible to ommit both data (an additional function signature is available which makes
+ /// them optionnal), then inner/outer objects are not updated.
+ ///
+ /// \param[out] geomModel1 geometry model where the data is added
+ /// \param[in] geomModel2 geometry model from which new geometries are taken
+ /// \note Of course, the geomData corresponding to geomModel1 will not be valid anymore,
+ /// and should be updated (or more simply, re-created from the new setting of geomModel1).
+ /// \todo This function is not asserted in unittest.
+ inline void appendGeometryModel(GeometryModel & geomModel1,
+ GeometryData & geomData1);
+
} // namespace se3
/* --- Details -------------------------------------------------------------------- */
diff --git a/src/algorithm/geometry.hxx b/src/algorithm/geometry.hxx
index 3865d4e6147706fc2778452b595870e75356d949..da39583c5445f81d597d47915794c3e524b6c1ce 100644
--- a/src/algorithm/geometry.hxx
+++ b/src/algorithm/geometry.hxx
@@ -22,49 +22,75 @@
namespace se3
{
-
+ /* --- GEOMETRY PLACEMENTS -------------------------------------------------------- */
+ /* --- GEOMETRY PLACEMENTS -------------------------------------------------------- */
+ /* --- GEOMETRY PLACEMENTS -------------------------------------------------------- */
inline void updateGeometryPlacements(const Model & model,
Data & data,
- const GeometryModel & model_geom,
- GeometryData & data_geom,
+ const GeometryModel & geomModel,
+ GeometryData & geomData,
const Eigen::VectorXd & q
)
{
forwardKinematics(model, data, q);
- updateGeometryPlacements(model, data, model_geom, data_geom);
+ updateGeometryPlacements(model, data, geomModel, geomData);
}
inline void updateGeometryPlacements(const Model &,
const Data & data,
- const GeometryModel & model_geom,
- GeometryData & data_geom
+ const GeometryModel & geomModel,
+ GeometryData & geomData
)
{
- for (GeomIndex i=0; i < (GeomIndex) data_geom.model_geom.ngeoms; ++i)
+ for (GeomIndex i=0; i < (GeomIndex) geomModel.ngeoms; ++i)
{
- const Model::JointIndex & parent = model_geom.geometryObjects[i].parent;
- if (parent>0) data_geom.oMg[i] = (data.oMi[parent] * model_geom.geometryObjects[i].placement);
- else data_geom.oMg[i] = model_geom.geometryObjects[i].placement;
+ const Model::JointIndex & joint = geomModel.geometryObjects[i].parentJoint;
+ if (joint>0) geomData.oMg[i] = (data.oMi[joint] * geomModel.geometryObjects[i].placement);
+ else geomData.oMg[i] = geomModel.geometryObjects[i].placement;
#ifdef WITH_HPP_FCL
- data_geom.collisionObjects[i].setTransform( toFclTransform3f(data_geom.oMg[i]) );
+ geomData.collisionObjects[i].setTransform( toFclTransform3f(geomData.oMg[i]) );
#endif // WITH_HPP_FCL
}
}
#ifdef WITH_HPP_FCL
- inline bool computeCollisions(GeometryData & data_geom,
- const bool stopAtFirstCollision
- )
+
+ /* --- COLLISIONS ----------------------------------------------------------------- */
+ /* --- COLLISIONS ----------------------------------------------------------------- */
+ /* --- COLLISIONS ----------------------------------------------------------------- */
+
+ inline bool computeCollision(const GeometryModel & geomModel,
+ GeometryData & geomData,
+ const PairIndex& pairId)
+ {
+ assert( pairId < geomModel.collisionPairs.size() );
+ const CollisionPair & pair = geomModel.collisionPairs[pairId];
+
+ assert( pairId < geomData.collisionResults.size() );
+ assert( pair.first < geomData.collisionObjects.size() );
+ assert( pair.second < geomData.collisionObjects.size() );
+
+ fcl::CollisionResult& collisionResult = geomData.collisionResults[pairId];
+ collisionResult.clear();
+ fcl::collide (&geomData.collisionObjects[pair.first],
+ &geomData.collisionObjects[pair.second],
+ geomData.collisionRequest,
+ collisionResult);
+
+ return collisionResult.isCollision();
+ }
+
+ inline bool computeCollisions(const GeometryModel & geomModel,
+ GeometryData & geomData,
+ const bool stopAtFirstCollision = true)
{
bool isColliding = false;
- const GeometryModel & geomModel = data_geom.model_geom;
for (std::size_t cpt = 0; cpt < geomModel.collisionPairs.size(); ++cpt)
{
- if(data_geom.activeCollisionPairs[cpt])
+ if(geomData.activeCollisionPairs[cpt])
{
- data_geom.collision_results[cpt]
- = data_geom.computeCollision(geomModel.collisionPairs[cpt]);
- isColliding |= data_geom.collision_results[cpt].fcl_collision_result.isCollision();
+ computeCollision(geomModel,geomData,cpt);
+ isColliding |= geomData.collisionResults[cpt].isCollision();
if(isColliding && stopAtFirstCollision)
return true;
}
@@ -76,67 +102,97 @@ namespace se3
// WARNING, if stopAtFirstcollision = true, then the collisions vector will not be fulfilled.
inline bool computeCollisions(const Model & model,
Data & data,
- const GeometryModel & model_geom,
- GeometryData & data_geom,
+ const GeometryModel & geomModel,
+ GeometryData & geomData,
const Eigen::VectorXd & q,
const bool stopAtFirstCollision
)
{
- updateGeometryPlacements (model, data, model_geom, data_geom, q);
+ updateGeometryPlacements (model, data, geomModel, geomData, q);
- return computeCollisions(data_geom, stopAtFirstCollision);
+ return computeCollisions(geomModel,geomData, stopAtFirstCollision);
}
- // Required to have a default template argument on templated free function
- inline std::size_t computeDistances(GeometryData & data_geom)
+ /* --- DISTANCES ----------------------------------------------------------------- */
+ /* --- DISTANCES ----------------------------------------------------------------- */
+ /* --- DISTANCES ----------------------------------------------------------------- */
+
+ inline fcl::DistanceResult & computeDistance(const GeometryModel & geomModel,
+ GeometryData & geomData,
+ const PairIndex & pairId )
{
- return computeDistances<true>(data_geom);
+ assert( pairId < geomModel.collisionPairs.size() );
+ const CollisionPair & pair = geomModel.collisionPairs[pairId];
+
+ assert( pairId < geomData.distanceResults.size() );
+ assert( pair.first < geomData.collisionObjects.size() );
+ assert( pair.second < geomData.collisionObjects.size() );
+
+ geomData.distanceResults[pairId].clear();
+ fcl::distance ( &geomData.collisionObjects[pair.first],
+ &geomData.collisionObjects[pair.second],
+ geomData.distanceRequest,
+ geomData.distanceResults[pairId]);
+
+ return geomData.distanceResults[pairId];
}
+
template <bool COMPUTE_SHORTEST>
- inline std::size_t computeDistances(GeometryData & data_geom)
+ inline std::size_t computeDistances(const GeometryModel & geomModel,
+ GeometryData & geomData)
{
- const GeometryModel & geomModel = data_geom.model_geom;
std::size_t min_index = geomModel.collisionPairs.size();
double min_dist = std::numeric_limits<double>::infinity();
for (std::size_t cpt = 0; cpt < geomModel.collisionPairs.size(); ++cpt)
{
- if(data_geom.activeCollisionPairs[cpt])
+ if(geomData.activeCollisionPairs[cpt])
{
- data_geom.distance_results[cpt] = data_geom.computeDistance(geomModel.collisionPairs[cpt]);
- if (COMPUTE_SHORTEST && data_geom.distance_results[cpt].distance() < min_dist)
+ computeDistance(geomModel,geomData,cpt);
+ if (COMPUTE_SHORTEST && geomData.distanceResults[cpt].min_distance < min_dist)
{
min_index = cpt;
- min_dist = data_geom.distance_results[cpt].distance();
+ min_dist = geomData.distanceResults[cpt].min_distance;
}
}
}
return min_index;
}
+ // Required to have a default template argument on templated free function
+ inline std::size_t computeDistances(const GeometryModel& geomModel,
+ GeometryData & geomData)
+ {
+ return computeDistances<true>(geomModel,geomData);
+ }
+
// Required to have a default template argument on templated free function
inline std::size_t computeDistances(const Model & model,
Data & data,
- const GeometryModel & model_geom,
- GeometryData & data_geom,
+ const GeometryModel & geomModel,
+ GeometryData & geomData,
const Eigen::VectorXd & q
)
{
- return computeDistances<true>(model, data, model_geom, data_geom, q);
+ return computeDistances<true>(model, data, geomModel, geomData, q);
}
template <bool ComputeShortest>
inline std::size_t computeDistances(const Model & model,
Data & data,
- const GeometryModel & model_geom,
- GeometryData & data_geom,
+ const GeometryModel & geomModel,
+ GeometryData & geomData,
const Eigen::VectorXd & q
)
{
- updateGeometryPlacements (model, data, model_geom, data_geom, q);
- return computeDistances<ComputeShortest>(data_geom);
+ updateGeometryPlacements (model, data, geomModel, geomData, q);
+ return computeDistances<ComputeShortest>(geomModel,geomData);
}
+ /* --- RADIUS -------------------------------------------------------------------- */
+ /* --- RADIUS -------------------------------------------------------------------- */
+ /* --- RADIUS -------------------------------------------------------------------- */
+
/// Given p1..3 being either "min" or "max", return one of the corners of the
/// AABB cub of the FCL object.
#define SE3_GEOM_AABB(FCL,p1,p2,p3) \
@@ -156,10 +212,12 @@ namespace se3
BOOST_FOREACH(const GeometryObject & geom,geomModel.geometryObjects)
{
const boost::shared_ptr<const fcl::CollisionGeometry> & fcl
- = geom.collision_geometry;
+ = geom.fcl;
const SE3 & jMb = geom.placement; // placement in joint.
+ const Model::JointIndex & i = geom.parentJoint;
+ assert (i<geomData.radius.size());
- double radius = geomData.radius[geom.parent];
+ double radius = geomData.radius[i];
// The radius is simply the one of the 8 corners of the AABB cube, expressed
// in the joint frame, whose norm is the highest.
@@ -173,14 +231,48 @@ namespace se3
radius = std::max (jMb.act(SE3_GEOM_AABB(fcl,max,max,max)).squaredNorm(),radius);
// Don't forget to sqroot the squared norm before storing it.
- assert (geom.parent<geomData.radius.size());
- geomData.radius[geom.parent] = sqrt(radius);
+ geomData.radius[i] = sqrt(radius);
}
}
#undef SE3_GEOM_AABB
#endif // WITH_HPP_FCL
+ /* --- APPEND GEOMETRY MODEL ----------------------------------------------------------- */
+
+ inline void appendGeometryModel(GeometryModel & geomModel1,
+ const GeometryModel & geomModel2)
+ {
+ assert (geomModel1.ngeoms == geomModel1.geometryObjects.size());
+ Index nGeom1 = geomModel1.geometryObjects.size();
+ Index nColPairs1 = geomModel1.collisionPairs.size();
+ assert (geomModel2.ngeoms == geomModel2.geometryObjects.size());
+ Index nGeom2 = geomModel2.geometryObjects.size();
+ Index nColPairs2 = geomModel2.collisionPairs.size();
+
+ /// Append the geometry objects and geometry positions
+ geomModel1.geometryObjects.insert(geomModel1.geometryObjects.end(),
+ geomModel2.geometryObjects.begin(), geomModel2.geometryObjects.end());
+ geomModel1.ngeoms += nGeom2;
+
+ /// 1. copy the collision pairs and update geomData1 accordingly.
+ geomModel1.collisionPairs.reserve(nColPairs1 + nColPairs2 + nGeom1 * nGeom2);
+ for (Index i = 0; i < nColPairs2; ++i)
+ {
+ const CollisionPair& cp = geomModel2.collisionPairs[i];
+ geomModel1.collisionPairs.push_back(
+ CollisionPair (cp.first + nGeom1, cp.second + nGeom1)
+ );
+ }
+
+ /// 2. add the collision pairs between geomModel1 and geomModel2.
+ for (Index i = 0; i < nGeom1; ++i) {
+ for (Index j = 0; j < nGeom2; ++j) {
+ geomModel1.collisionPairs.push_back(CollisionPair(i, nGeom1 + j));
+ }
+ }
+ }
+
} // namespace se3
#endif // ifnded __se3_algo_geometry_hxx__
diff --git a/src/algorithm/jacobian.hxx b/src/algorithm/jacobian.hxx
index 396c54ec036ad4ea97acd7708726dd981b77282b..adae3233e437e02b84a5da1efb49bc0802cf7ca8 100644
--- a/src/algorithm/jacobian.hxx
+++ b/src/algorithm/jacobian.hxx
@@ -59,7 +59,7 @@ namespace se3
computeJacobians(const Model & model, Data & data,
const Eigen::VectorXd & q)
{
- for( Model::JointIndex i=1; i< (Model::JointIndex) model.njoint;++i )
+ for( Model::JointIndex i=1; i< (Model::JointIndex) model.njoints;++i )
{
JacobiansForwardStep::run(model.joints[i],data.joints[i],
JacobiansForwardStep::ArgsType(model,data,q));
diff --git a/src/algorithm/joint-configuration.hpp b/src/algorithm/joint-configuration.hpp
index a9de210cdf684197a68eacbdbb354b9c4f3ad348..d4a98f35e405fd52e411367811812ae45caa36d7 100644
--- a/src/algorithm/joint-configuration.hpp
+++ b/src/algorithm/joint-configuration.hpp
@@ -71,7 +71,7 @@ namespace se3
* @param[in] model Model we want to compute the distance
* @param[in] q0 Configuration 0 (size model.nq)
* @param[in] q1 Configuration 1 (size model.nq)
- * @return The corresponding distances for each joint (size model.njoint-1 = number of joints)
+ * @return The corresponding distances for each joint (size model.njoints-1 = number of joints)
*/
inline Eigen::VectorXd distance(const Model & model,
const Eigen::VectorXd & q0,
@@ -146,7 +146,6 @@ namespace se3
const Eigen::VectorXd & v,
Eigen::VectorXd & result)
{
-
jmodel.jointConfigSelector(result) = jmodel.integrate(q, v);
}
@@ -158,7 +157,7 @@ namespace se3
const Eigen::VectorXd & v)
{
Eigen::VectorXd integ(model.nq);
- for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
+ for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
{
IntegrateStep::run(model.joints[i],
IntegrateStep::ArgsType (q, v, integ)
@@ -197,7 +196,7 @@ namespace se3
const double u)
{
Eigen::VectorXd interp(model.nq);
- for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
+ for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
{
InterpolateStep::run(model.joints[i],
InterpolateStep::ArgsType (q0, q1, u, interp)
@@ -232,7 +231,7 @@ namespace se3
const Eigen::VectorXd & q1)
{
Eigen::VectorXd diff(model.nv);
- for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
+ for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
{
DifferentiateStep::run(model.joints[i],
DifferentiateStep::ArgsType (q0, q1, diff)
@@ -268,8 +267,8 @@ namespace se3
const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1)
{
- Eigen::VectorXd distances(model.njoint-1);
- for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
+ Eigen::VectorXd distances(model.njoints-1);
+ for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
{
DistanceStep::run(model.joints[i],
DistanceStep::ArgsType (i-1, q0, q1, distances)
@@ -305,7 +304,7 @@ namespace se3
randomConfiguration(const Model & model, const Eigen::VectorXd & lowerLimits, const Eigen::VectorXd & upperLimits)
{
Eigen::VectorXd q(model.nq);
- for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
+ for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
{
RandomConfiguration::run(model.joints[i],
RandomConfiguration::ArgsType ( q, lowerLimits, upperLimits)
@@ -338,7 +337,7 @@ namespace se3
normalize(const Model & model,
Eigen::VectorXd & q)
{
- for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
+ for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
{
NormalizeStep::run(model.joints[i],
NormalizeStep::ArgsType (q));
@@ -370,7 +369,7 @@ namespace se3
const Eigen::VectorXd & q2)
{
bool result = false;
- for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
+ for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
{
IsSameConfigurationStep::run(model.joints[i], IsSameConfigurationStep::ArgsType (result,q1,q2));
if( !result )
diff --git a/src/algorithm/kinematics.hxx b/src/algorithm/kinematics.hxx
index 51361b8124fbbd32d042499e16475779fe039e72..efb20a29fdb25e71fc3299af14b45aedab7742d1 100644
--- a/src/algorithm/kinematics.hxx
+++ b/src/algorithm/kinematics.hxx
@@ -44,7 +44,7 @@ namespace se3
inline void emptyForwardPass(const Model & model,
Data & data)
{
- for (Model::JointIndex i=1; i < (Model::JointIndex) model.njoint; ++i)
+ for (Model::JointIndex i=1; i < (Model::JointIndex) model.njoints; ++i)
{
emptyForwardStep::run(model.joints[i],
data.joints[i],
@@ -91,7 +91,7 @@ namespace se3
{
assert(q.size() == model.nq && "The configuration vector is not of right size");
- for (Model::JointIndex i=1; i < (Model::JointIndex) model.njoint; ++i)
+ for (Model::JointIndex i=1; i < (Model::JointIndex) model.njoints; ++i)
{
ForwardKinematicZeroStep::run(model.joints[i], data.joints[i],
ForwardKinematicZeroStep::ArgsType (model,data,q)
@@ -146,7 +146,7 @@ namespace se3
data.v[0].setZero();
- for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
+ for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
{
ForwardKinematicFirstStep::run(model.joints[i],data.joints[i],
ForwardKinematicFirstStep::ArgsType(model,data,q,v));
@@ -207,7 +207,7 @@ namespace se3
data.v[0].setZero();
data.a[0].setZero();
- for( Model::JointIndex i=1; i < (Model::JointIndex) model.njoint; ++i )
+ for( Model::JointIndex i=1; i < (Model::JointIndex) model.njoints; ++i )
{
ForwardKinematicSecondStep::run(model.joints[i],data.joints[i],
ForwardKinematicSecondStep::ArgsType(model,data,q,v,a));
diff --git a/src/algorithm/rnea.hxx b/src/algorithm/rnea.hxx
index b08303b380827dd8b707fb39161c484ff831ced0..f0eeaca52a6413b932737e31611c08596730a1a1 100644
--- a/src/algorithm/rnea.hxx
+++ b/src/algorithm/rnea.hxx
@@ -93,13 +93,13 @@ namespace se3
data.v[0].setZero();
data.a_gf[0] = -model.gravity;
- for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoint;++i )
+ for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i )
{
RneaForwardStep::run(model.joints[i],data.joints[i],
RneaForwardStep::ArgsType(model,data,q,v,a));
}
- for( Model::JointIndex i=(Model::JointIndex)model.njoint-1;i>0;--i )
+ for( Model::JointIndex i=(Model::JointIndex)model.njoints-1;i>0;--i )
{
RneaBackwardStep::run(model.joints[i],data.joints[i],
RneaBackwardStep::ArgsType(model,data));
@@ -174,13 +174,13 @@ namespace se3
data.v[0].setZero ();
data.a_gf[0] = -model.gravity;
- for( size_t i=1;i<(size_t) model.njoint;++i )
+ for( size_t i=1;i<(size_t) model.njoints;++i )
{
NLEForwardStep::run(model.joints[i],data.joints[i],
NLEForwardStep::ArgsType(model,data,q,v));
}
- for( size_t i=(size_t) (model.njoint-1);i>0;--i )
+ for( size_t i=(size_t) (model.njoints-1);i>0;--i )
{
NLEBackwardStep::run(model.joints[i],data.joints[i],
NLEBackwardStep::ArgsType(model,data));
diff --git a/src/math/matrix.hpp b/src/math/matrix.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..08e09bb417ac1806d3e708c5a54913d02b9f77de
--- /dev/null
+++ b/src/math/matrix.hpp
@@ -0,0 +1,34 @@
+//
+// 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 __math_matrix_hpp__
+#define __math_matrix_hpp__
+
+#include <Eigen/Dense>
+
+namespace se3
+{
+
+ template<typename Derived>
+ inline bool hasNaN(const Eigen::DenseBase<Derived> & m)
+ {
+ return !((m.derived().array()==m.derived().array()).all());
+ }
+
+
+}
+#endif //#ifndef __math_matrix_hpp__
diff --git a/src/math/quaternion.hpp b/src/math/quaternion.hpp
index 8f20cc57ba690ddb494ed5bd203869d11bc04bee..aa98d53a7b2e9a699591a80f966cab219141a8b2 100644
--- a/src/math/quaternion.hpp
+++ b/src/math/quaternion.hpp
@@ -61,5 +61,25 @@ namespace se3
return (q1.coeffs().isApprox(q2.coeffs()) || q1.coeffs().isApprox(-q2.coeffs()) );
}
+ /// Approximately normalize by applying the first order limited development
+ /// of the normalization function.
+ ///
+ /// Only additions and multiplications are required. Neither square root nor
+ /// division are used (except a division by 2).
+ ///
+ /// \warning \f$ ||q||^2 - 1 \f$ should already be close to zero.
+ ///
+ /// \note See
+ /// http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html#title3
+ /// to know the reason why the argument is const.
+ template <typename D> void
+ firstOrderNormalize(const Eigen::QuaternionBase<D> & q)
+ {
+ assert(std::fabs(q.norm() - 1) < 1e-2);
+ typedef typename D::Scalar Scalar;
+ const Scalar alpha = ((Scalar)3 - q.squaredNorm()) / 2;
+ const_cast <Eigen::QuaternionBase<D> &> (q).coeffs() *= alpha;
+ }
+
}
#endif //#ifndef __math_quaternion_hpp__
diff --git a/src/multibody/constraint.hpp b/src/multibody/constraint.hpp
index cbb1c72bbdf662abb23a117fc80225cff3a0cff3..450f3403fc218f836c096ea750825599f6ca9a5e 100644
--- a/src/multibody/constraint.hpp
+++ b/src/multibody/constraint.hpp
@@ -181,6 +181,11 @@ namespace se3
return (m.toActionMatrix()*S).eval();
}
+ DenseBase se3ActionInverse(const SE3 & m) const
+ {
+ return (m.inverse().toActionMatrix()*S).eval();
+ }
+
void disp_impl(std::ostream & os) const { os << "S =\n" << S << std::endl;}
protected:
diff --git a/src/multibody/fcl.hpp b/src/multibody/fcl.hpp
index 0ba8b92aca7f34c2f64071970ccf52889cd7aa10..1587e4b5a09b248ba39612c4803ed48e79431e75 100644
--- a/src/multibody/fcl.hpp
+++ b/src/multibody/fcl.hpp
@@ -30,7 +30,7 @@
#include <iostream>
#include <map>
-#include <list>
+#include <vector>
#include <utility>
#include <assert.h>
@@ -44,116 +44,16 @@ namespace se3
typedef std::pair<GeomIndex, GeomIndex> Base;
- ///
- /// \brief Default constructor of a collision pair from two collision object indexes.
- /// The indexes must be ordered such that co1 < co2. If not, the constructor reverts the indexes.
- ///
- /// \param[in] co1 Index of the first collision object
- /// \param[in] co2 Index of the second collision object
- ///
- CollisionPair(const GeomIndex co1, const GeomIndex co2) : Base(co1,co2)
- {
- assert(co1 != co2 && "The index of collision objects must not be equal.");
- }
-
- bool operator== (const CollisionPair& rhs) const
- {
- return (first == rhs.first && second == rhs.second)
- || (first == rhs.second && second == rhs.first);
- }
-
- void disp(std::ostream & os) const { os << "collision pair (" << first << "," << second << ")\n"; }
- friend std::ostream & operator << (std::ostream & os, const CollisionPair & X);
+ CollisionPair(const GeomIndex co1, const GeomIndex co2);
+ bool operator == (const CollisionPair& rhs) const;
+ void disp (std::ostream & os) const;
+ friend std::ostream & operator << (std::ostream & os,const CollisionPair & X);
}; // struct CollisionPair
- typedef std::vector<CollisionPair> CollisionPairsVector_t;
-
-#ifdef WITH_HPP_FCL
- /**
- * @brief Result of distance computation between two CollisionObjects
- */
- struct DistanceResult
- {
-
- DistanceResult() : fcl_distance_result(), object1(0), object2(0) {}
- DistanceResult(fcl::DistanceResult dist_fcl, const GeomIndex co1, const GeomIndex co2)
- : fcl_distance_result(dist_fcl), object1(co1), object2(co2)
- {}
-
-
- ///
- /// @brief Return the minimal distance between two geometry objects
- ///
- double distance () const;
-
- ///
- /// \brief Return the witness point on the inner object expressed in global frame.
- ///
- Eigen::Vector3d closestPointInner () const;
-
- ///
- /// \brief Return the witness point on the outer object expressed in global frame.
- ///
- Eigen::Vector3d closestPointOuter () const;
-
- bool operator == (const DistanceResult & other) const
- {
- return (distance() == other.distance()
- && closestPointInner() == other.closestPointInner()
- && closestPointOuter() == other.closestPointOuter()
- && object1 == other.object1
- && object2 == other.object2);
- }
-
- /// \brief The FCL result of the distance computation
- fcl::DistanceResult fcl_distance_result;
-
- /// \brief Index of the first colision object
- GeomIndex object1;
-
- /// \brief Index of the second colision object
- GeomIndex object2;
-
- }; // struct DistanceResult
-
-
- /**
- * @brief Result of collision computation between two CollisionObjects
- */
- struct CollisionResult
- {
- /**
- * @brief Default constrcutor of a CollisionResult
- *
- * @param[in] coll_fcl The FCL collision result
- * @param[in] co1 Index of the first geometry object involved in the computation
- * @param[in] co2 Index of the second geometry object involved in the computation
- */
- CollisionResult(fcl::CollisionResult coll_fcl, const GeomIndex co1, const GeomIndex co2)
- : fcl_collision_result(coll_fcl), object1(co1), object2(co2)
- {}
- CollisionResult() : fcl_collision_result(), object1(0), object2(0) {}
-
- bool operator == (const CollisionResult & other) const
- {
- return (fcl_collision_result == other.fcl_collision_result
- && object1 == other.object1
- && object2 == other.object2);
- }
-
- /// \brief The FCL result of the collision computation
- fcl::CollisionResult fcl_collision_result;
-
- /// \brief Index of the first collision object
- GeomIndex object1;
-
- /// \brief Index of the second collision object
- GeomIndex object2;
-
- }; // struct CollisionResult
+ typedef std::vector<CollisionPair> CollisionPairsVector_t;
-#else
+#ifndef WITH_HPP_FCL
namespace fcl
{
@@ -179,8 +79,7 @@ namespace se3
enum GeometryType
{
VISUAL,
- COLLISION,
- NONE
+ COLLISION
};
struct GeometryObject
@@ -189,44 +88,51 @@ struct GeometryObject
/// \brief Name of the geometry object
std::string name;
+ /// \brief Index of the parent frame
+ ///
+ /// Parent frame may be unset (set to -1) as it is mostly used as a documentation of the tree, or in third-party libraries.
+ /// The URDF parser of Pinocchio is setting it to the proper value according to the urdf link-joint tree.
+ /// In particular, anchor joints of URDF would cause parent frame to be different to joint frame.
+ FrameIndex parentFrame;
+
/// \brief Index of the parent joint
- JointIndex parent;
+ JointIndex parentJoint;
/// \brief The actual cloud of points representing the collision mesh of the object
- boost::shared_ptr<fcl::CollisionGeometry> collision_geometry;
+ boost::shared_ptr<fcl::CollisionGeometry> fcl;
- /// \brief Position of geometry object in parent joint's frame
+ /// \brief Position of geometry object in parent joint frame
SE3 placement;
/// \brief Absolute path to the mesh file
- std::string mesh_path;
-
+ std::string meshPath;
- GeometryObject(const std::string & name, const JointIndex parent, const boost::shared_ptr<fcl::CollisionGeometry> & collision,
- const SE3 & placement, const std::string & mesh_path)
+ GeometryObject(const std::string & name, const FrameIndex parentF,
+ const JointIndex parentJ, const boost::shared_ptr<fcl::CollisionGeometry> & collision,
+ const SE3 & placement, const std::string & meshPath)
: name(name)
- , parent(parent)
- , collision_geometry(collision)
+ , parentFrame(parentF)
+ , parentJoint(parentJ)
+ , fcl(collision)
, placement(placement)
- , mesh_path(mesh_path)
+ , meshPath(meshPath)
{}
GeometryObject & operator=(const GeometryObject & other)
{
- name = other.name;
- parent = other.parent;
- collision_geometry = other.collision_geometry;
- placement = other.placement;
- mesh_path = other.mesh_path;
+ name = other.name;
+ parentFrame = other.parentFrame;
+ parentJoint = other.parentJoint;
+ fcl = other.fcl;
+ placement = other.placement;
+ meshPath = other.meshPath;
return *this;
}
- friend std::ostream & operator<< (std::ostream & os, const GeometryObject & geom_object);
+ friend std::ostream & operator<< (std::ostream & os, const GeometryObject & geomObject);
};
-
-
} // namespace se3
/* --- Details -------------------------------------------------------------- */
diff --git a/src/multibody/fcl.hxx b/src/multibody/fcl.hxx
index 4a5a4070b43b794a77793c6c7c601d57e19888eb..95a7d26ac670d063b74b18465b533246a7d32744 100644
--- a/src/multibody/fcl.hxx
+++ b/src/multibody/fcl.hxx
@@ -25,6 +25,28 @@
namespace se3
{
+ ///
+ /// \brief Default constructor of a collision pair from two collision object indexes.
+ /// The indexes must be ordered such that co1 < co2. If not, the constructor reverts the indexes.
+ ///
+ /// \param[in] co1 Index of the first collision object
+ /// \param[in] co2 Index of the second collision object
+ ///
+ inline CollisionPair::CollisionPair(const GeomIndex co1, const GeomIndex co2)
+ : Base(co1,co2)
+ {
+ assert(co1 != co2 && "The index of collision objects must not be equal.");
+ }
+
+ inline bool CollisionPair::operator== (const CollisionPair& rhs) const
+ {
+ return (first == rhs.first && second == rhs.second)
+ || (first == rhs.second && second == rhs.first );
+ }
+
+ inline void CollisionPair::disp(std::ostream & os) const
+ { os << "collision pair (" << first << "," << second << ")\n"; }
+
inline std::ostream & operator << (std::ostream & os, const CollisionPair & X)
{
X.disp(os); return os;
@@ -33,22 +55,6 @@ namespace se3
#ifdef WITH_HPP_FCL
-
- inline double DistanceResult::distance () const
- {
- return fcl_distance_result.min_distance;
- }
-
- inline Eigen::Vector3d DistanceResult::closestPointInner () const
- {
- return toVector3d(fcl_distance_result.nearest_points [0]);
- }
-
- inline Eigen::Vector3d DistanceResult::closestPointOuter () const
- {
- return toVector3d(fcl_distance_result.nearest_points [1]);
- }
-
inline bool operator == (const fcl::CollisionObject & lhs, const fcl::CollisionObject & rhs)
{
return lhs.collisionGeometry() == rhs.collisionGeometry()
@@ -57,25 +63,25 @@ namespace se3
}
#endif // WITH_HPP_FCL
-
inline bool operator==(const GeometryObject & lhs, const GeometryObject & rhs)
{
- return ( lhs.name == rhs.name
- && lhs.parent == rhs.parent
- && lhs.collision_geometry == rhs.collision_geometry
- && lhs.placement == rhs.placement
- && lhs.mesh_path == rhs.mesh_path
+ return ( lhs.name == rhs.name
+ && lhs.parentFrame == rhs.parentFrame
+ && lhs.parentJoint == rhs.parentJoint
+ && lhs.fcl == rhs.fcl
+ && lhs.placement == rhs.placement
+ && lhs.meshPath == rhs.meshPath
);
}
inline std::ostream & operator<< (std::ostream & os, const GeometryObject & geom_object)
{
os << "Name: \t \n" << geom_object.name << "\n"
- << "Parent ID: \t \n" << geom_object.parent << "\n"
- // << "collision object: \t \n" << geom_object.collision_geometry << "\n"
+ << "Parent frame ID: \t \n" << geom_object.parentFrame << "\n"
+ << "Parent joint ID: \t \n" << geom_object.parentJoint << "\n"
<< "Position in parent frame: \t \n" << geom_object.placement << "\n"
- << "Absolute path to mesh file: \t \n" << geom_object.mesh_path << "\n"
+ << "Absolute path to mesh file: \t \n" << geom_object.meshPath << "\n"
<< std::endl;
return os;
}
diff --git a/src/multibody/frame.hpp b/src/multibody/frame.hpp
index 6634f40cab536814685657cfc515593cc8c47ed6..c440277efc64bdc7f9c4cd352512347967a04bc6 100644
--- a/src/multibody/frame.hpp
+++ b/src/multibody/frame.hpp
@@ -33,11 +33,11 @@ namespace se3
///
enum FrameType
{
- OP_FRAME, // operational frame type
- JOINT, // joint frame type
- FIXED_JOINT, // fixed joint frame type
- BODY, // body frame type
- SENSOR // sensor frame type
+ OP_FRAME = 0x1 << 0, // operational frame type
+ JOINT = 0x1 << 1, // joint frame type
+ FIXED_JOINT = 0x1 << 2, // fixed joint frame type
+ BODY = 0x1 << 3, // body frame type
+ SENSOR = 0x1 << 4 // sensor frame type
};
///
@@ -58,12 +58,14 @@ namespace se3
///
/// \param[in] name Name of the frame.
/// \param[in] parent Index of the parent joint in the kinematic tree.
+ /// \param[in] previousFrame Index of the parent frame in the kinematic tree.
/// \param[in] placement Placement of the frame wrt the parent joint frame.
/// \param[in] type The type of the frame, see the enum FrameType
///
- Frame(const std::string & name, const JointIndex parent, const SE3 & frame_placement, const FrameType type)
+ Frame(const std::string & name, const JointIndex parent, const FrameIndex previousFrame, const SE3 & frame_placement, const FrameType type)
: name(name)
, parent(parent)
+ , previousFrame(previousFrame)
, placement(frame_placement)
, type(type)
{}
@@ -76,6 +78,7 @@ namespace se3
bool operator == (const Frame & other) const
{
return name == other.name && parent == other.parent
+ && previousFrame == other.previousFrame
&& placement == other.placement
&& type == other.type ;
}
@@ -86,14 +89,24 @@ namespace se3
/// \brief Index of the parent joint.
JointIndex parent;
+ /// \brief Index of the previous frame.
+ FrameIndex previousFrame;
+
/// \brief Placement of the frame wrt the parent joint.
SE3 placement;
/// \brief Type of the frame
FrameType type;
-
+
}; // struct Frame
+ inline std::ostream & operator << (std::ostream& os, const Frame & f)
+ {
+ os << "Frame name:" << f.name << "paired to (parent joint/ previous frame)" << "(" <<f.parent << "/" << f.previousFrame << ")"<< std::endl;
+ os << "with relative placement wrt parent joint:\n" << f.placement << std::endl;
+ return os;
+ }
+
} // namespace se3
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Frame)
diff --git a/src/multibody/fwd.hpp b/src/multibody/fwd.hpp
index 5d9a3032c6cbe81412d2753a54a07e8c3327583e..29da7ab5e6aa2e8c5e64cc87d75ac58f4d37fef6 100644
--- a/src/multibody/fwd.hpp
+++ b/src/multibody/fwd.hpp
@@ -18,12 +18,15 @@
#ifndef __se3_multibody_fwd_hpp__
#define __se3_multibody_fwd_hpp__
+# include <cstddef> // std::size_t
+
namespace se3
{
typedef std::size_t Index;
typedef Index JointIndex;
typedef Index GeomIndex;
typedef Index FrameIndex;
+ typedef Index PairIndex;
struct Frame;
struct Model;
diff --git a/src/multibody/geometry.hpp b/src/multibody/geometry.hpp
index 0a2aedd22e0825d6aa540216f9ea49a63cf43f4a..e9b00f5578b4b1246ec3e8ddf2c79d3af9c5f39b 100644
--- a/src/multibody/geometry.hpp
+++ b/src/multibody/geometry.hpp
@@ -37,9 +37,6 @@ namespace se3
struct GeometryModel
{
- typedef std::vector<GeomIndex> GeomIndexList;
-
-
/// \brief The number of GeometryObjects
Index ngeoms;
@@ -48,22 +45,12 @@ namespace se3
///
/// \brief Vector of collision pairs.
///
- CollisionPairsVector_t collisionPairs;
+ std::vector<CollisionPair> collisionPairs;
- /// \brief A list of associated collision GeometryObjects to a given joint Id.
- /// Inner objects can be seen as geometry objects that directly move when the associated joint moves
- std::map < JointIndex, GeomIndexList > innerObjects;
-
- /// \brief A list of associated collision GeometryObjects to a given joint Id
- /// Outer objects can be seen as geometry objects that may often be obstacles to the Inner objects of given joint
- std::map < JointIndex, GeomIndexList > outerObjects;
-
GeometryModel()
: ngeoms(0)
, geometryObjects()
, collisionPairs()
- , innerObjects()
- , outerObjects()
{
const std::size_t num_max_collision_pairs = (ngeoms * (ngeoms-1))/2;
collisionPairs.reserve(num_max_collision_pairs);
@@ -74,19 +61,16 @@ namespace se3
/**
* @brief Add a geometry object to a GeometryModel
*
- * @param[in] parent Index of the parent joint
- * @param[in] co The actual fcl CollisionGeometry
- * @param[in] placement The relative placement regarding to the parent frame
- * @param[in] geom_name The name of the Geometry Object
- * @param[in] mesh_path The absolute path to the mesh
+ * @param[in] object Object
+ * @param[in] model Corresponding model, used to assert the attributes of object.
+ * @param[in] autofillJointParent if true, set jointParent from frameParent.
*
* @return The index of the new added GeometryObject in geometryObjects
+ * @note object is a nonconst copy to ease the insertion code.
*/
- inline GeomIndex addGeometryObject(const JointIndex parent, const boost::shared_ptr<fcl::CollisionGeometry> & co,
- const SE3 & placement, const std::string & geom_name = "",
- const std::string & mesh_path = "") throw(std::invalid_argument);
-
-
+ inline GeomIndex addGeometryObject(GeometryObject object,
+ const Model & model,
+ const bool autofillJointParent = false);
/**
* @brief Return the index of a GeometryObject given by its name.
@@ -115,7 +99,7 @@ namespace se3
*
* @return Name of the GeometryObject
*/
- const std::string & getGeometryName(const GeomIndex index) const;
+ PINOCCHIO_DEPRECATED const std::string & getGeometryName(const GeomIndex index) const;
#ifdef WITH_HPP_FCL
///
@@ -129,6 +113,9 @@ namespace se3
///
/// \brief Add all possible collision pairs.
///
+ /// \note Collision pairs between geometries of having the same parent joint
+ /// are not added.
+ ///
void addAllCollisionPairs();
///
@@ -159,39 +146,15 @@ namespace se3
///
/// \return The index of the CollisionPair in collisionPairs.
///
- Index findCollisionPair (const CollisionPair & pair) const;
+ PairIndex findCollisionPair (const CollisionPair & pair) const;
- /// \brief Display on std::cout the list of pairs (is it really useful?).
- void displayCollisionPairs() const;
#endif // WITH_HPP_FCL
- /**
- * @brief Associate a GeometryObject of type COLLISION to a joint's inner objects list
- *
- * @param[in] joint Index of the joint
- * @param[in] inner_object Index of the GeometryObject that will be an inner object
- */
- void addInnerObject(const JointIndex joint, const GeomIndex inner_object);
-
- /**
- * @brief Associate a GeometryObject of type COLLISION to a joint's outer objects list
- *
- * @param[in] joint Index of the joint
- * @param[in] inner_object Index of the GeometryObject that will be an outer object
- */
- void addOutterObject(const JointIndex joint, const GeomIndex outer_object);
friend std::ostream& operator<<(std::ostream & os, const GeometryModel & model_geom);
}; // struct GeometryModel
struct GeometryData
{
-
- ///
- /// \brief A const reference to the model storing all the geometries.
- /// See class GeometryModel.
- ///
- const GeometryModel & model_geom;
-
///
/// \brief Vector gathering the SE3 placements of the geometry objects relative to the world.
/// See updateGeometryPlacements to update the placements.
@@ -200,6 +163,7 @@ namespace se3
/// for fcl (collision) computation. The copy is done in collisionObjects[i]->setTransform(.)
///
std::vector<se3::SE3> oMg;
+
#ifdef WITH_HPP_FCL
///
/// \brief Collision objects (ie a fcl placed geometry).
@@ -214,88 +178,90 @@ namespace se3
///
std::vector<bool> activeCollisionPairs;
+ ///
+ /// \brief Defines what information should be computed by distance computation.
+ ///
+ fcl::DistanceRequest distanceRequest;
+
///
/// \brief Vector gathering the result of the distance computation for all the collision pairs.
///
- std::vector <DistanceResult> distance_results;
+ std::vector <fcl::DistanceResult> distanceResults;
+ ///
+ /// \brief Defines what information should be computed by collision test.
+ ///
+ fcl::CollisionRequest collisionRequest;
+
///
/// \brief Vector gathering the result of the collision computation for all the collision pairs.
///
- std::vector <CollisionResult> collision_results;
+ std::vector <fcl::CollisionResult> collisionResults;
///
/// \brief Radius of the bodies, i.e. distance of the further point of the geometry model
/// attached to the body from the joint center.
///
std::vector<double> radius;
- GeometryData(const GeometryModel & modelGeom)
- : model_geom(modelGeom)
- , oMg(model_geom.ngeoms)
- , activeCollisionPairs(modelGeom.collisionPairs.size(), true)
- , distance_results(modelGeom.collisionPairs.size())
- , collision_results(modelGeom.collisionPairs.size())
- , radius()
-
- {
- collisionObjects.reserve(modelGeom.geometryObjects.size());
- BOOST_FOREACH( const GeometryObject & geom, modelGeom.geometryObjects)
- { collisionObjects.push_back
- (fcl::CollisionObject(geom.collision_geometry)); }
- }
-#else
- GeometryData(const GeometryModel & modelGeom)
- : model_geom(modelGeom)
- , oMg(model_geom.ngeoms)
- {}
-#endif // WITH_HPP_FCL
-
-
- ~GeometryData() {};
-#ifdef WITH_HPP_FCL
- void activateCollisionPair(const Index pairId,const bool flag=true);
- void deactivateCollisionPair(const Index pairId);
///
- /// \brief Compute the collision status between two collision objects of a given collision pair.
- ///
- /// \param[in] pair The collsion pair.
+ /// \brief index of the collision pair
///
- /// \return Return true is the collision objects are colliding.
- ///
- CollisionResult computeCollision(const CollisionPair & pair) const;
-
+ /// It is used by some method to return additional information. For instance,
+ /// the algo computeCollisions() sets it to the first colliding pair.
///
- /// \brief Compute the collision result of all the collision pairs according to
- /// the current placements of the geometires stored in GeometryData::oMg.
- /// The results are stored in the vector GeometryData::collision_results.
- ///
- void computeAllCollisions();
-
- ///
- /// \brief Check if at least one of the collision pairs has its two collision objects in collision.
- ///
- bool isColliding() const;
+ PairIndex collisionPairIndex;
+ typedef std::vector<GeomIndex> GeomIndexList;
+
+ /// \brief Map over vector GeomModel::geometryObjects, indexed by joints.
+ ///
+ /// The map lists the collision GeometryObjects associated to a given joint Id.
+ /// Inner objects can be seen as geometry objects that directly move when the associated joint moves
+ std::map < JointIndex, GeomIndexList > innerObjects;
+
+ /// \brief A list of associated collision GeometryObjects to a given joint Id
///
- /// \brief Compute the minimal distance between collision objects of a collison pair
- ///
- /// \param[in] pair The collsion pair.
+ /// Outer objects can be seen as geometry objects that may often be
+ /// obstacles to the Inner objects of given joint
+ std::map < JointIndex, GeomIndexList > outerObjects;
+#endif // WITH_HPP_FCL
+
+ GeometryData(const GeometryModel & geomModel);
+ ~GeometryData() {};
+
+#ifdef WITH_HPP_FCL
+
+ /// Fill both innerObjects and outerObjects maps, from vectors collisionObjects and
+ /// collisionPairs.
///
- /// \return An fcl struct containing the distance result.
+ /// This simply corresponds to storing in a re-arranged manner the information stored
+ /// in geomModel.geometryObjects and geomModel.collisionPairs.
+ /// \param[in] GeomModel the geometry model (const)
///
- DistanceResult computeDistance(const CollisionPair & pair) const;
-
+ /// \warning Outer objects are not duplicated (i.e. if a is in outerObjects[b], then
+ /// b is not in outerObjects[a]).
+ void fillInnerOuterObjectMaps(const GeometryModel & geomModel);
+
+ /// Activate a collision pair, for which collisions and distances would now be computed.
///
- /// \brief Compute the distance result for all collision pairs according to
- /// the current placements of the geometries stored in GeometryData::oMg.
- /// The results are stored in the vector GeometryData::distance_results.
+ /// A collision (resp distance) between to geometries of GeomModel::geometryObjects
+ /// is computed *iff* the corresponding pair has been added in GeomModel::collisionPairs *AND*
+ /// it is active, i.e. the corresponding boolean in GeomData::activePairs is true. The second
+ /// condition can be used to temporarily remove a pair without touching the model, in a versatile
+ /// manner.
+ /// \param[in] pairId the index of the pair in GeomModel::collisionPairs vector.
+ /// \param[in] new value of the activation boolean (true by default).
+ void activateCollisionPair(const PairIndex pairId,const bool flag=true);
+
+ /// Deactivate a collision pair.
///
- void computeAllDistances();
-
- void resetDistances();
+ /// Calls indeed GeomData::activateCollisionPair(pairId,false)
+ /// \sa activateCollisionPair
+ void deactivateCollisionPair(const PairIndex pairId);
+
#endif //WITH_HPP_FCL
- friend std::ostream & operator<<(std::ostream & os, const GeometryData & data_geom);
+ friend std::ostream & operator<<(std::ostream & os, const GeometryData & geomData);
}; // struct GeometryData
diff --git a/src/multibody/geometry.hxx b/src/multibody/geometry.hxx
index 759575dcd146ff71f24c8ce11d1a1efce769a083..c1a6eff85d2e26793dd88c5fbc47fe7e47c90e0c 100644
--- a/src/multibody/geometry.hxx
+++ b/src/multibody/geometry.hxx
@@ -31,23 +31,50 @@
namespace se3
{
+ inline GeometryData::GeometryData(const GeometryModel & modelGeom)
+ : oMg(modelGeom.ngeoms)
+
+#ifdef WITH_HPP_FCL
+ , activeCollisionPairs(modelGeom.collisionPairs.size(), true)
+ , distanceRequest (true, 0, 0, fcl::GST_INDEP)
+ , distanceResults(modelGeom.collisionPairs.size())
+ , collisionRequest (1, false, false, 1, false, true, fcl::GST_INDEP)
+ , collisionResults(modelGeom.collisionPairs.size())
+ , radius()
+ , collisionPairIndex(-1)
+ , innerObjects()
+ , outerObjects()
+ {
+ collisionObjects.reserve(modelGeom.geometryObjects.size());
+ BOOST_FOREACH( const GeometryObject & geom, modelGeom.geometryObjects)
+ { collisionObjects.push_back
+ (fcl::CollisionObject(geom.fcl)); }
+ fillInnerOuterObjectMaps(modelGeom);
+ }
+#else
+ {}
+#endif // WITH_HPP_FCL
- inline GeomIndex GeometryModel::addGeometryObject(const JointIndex parent,
- const boost::shared_ptr<fcl::CollisionGeometry> & co,
- const SE3 & placement,
- const std::string & geom_name,
- const std::string & mesh_path) throw(std::invalid_argument)
+ inline GeomIndex GeometryModel::addGeometryObject(GeometryObject object,
+ const Model & model,
+ const bool autofillJointParent)
{
+ // TODO reenable when relevant: assert( (object.parentFrame != -1) || (object.parentJoint != -1) );
+
+ if( autofillJointParent )
+ // TODO: this might be automatically done for some default value of parentJoint (eg ==-1)
+ object.parentJoint = model.frames[object.parentFrame].parent;
- Index idx = (Index) (ngeoms ++);
+ assert( //TODO: reenable when relevant (object.parentFrame == -1) ||
+ (model.frames[object.parentFrame].type == se3::BODY) );
+ assert( //TODO: reenable when relevant (object.parentFrame == -1) ||
+ (model.frames[object.parentFrame].parent == object.parentJoint) );
- geometryObjects.push_back(GeometryObject( geom_name, parent, co,
- placement, mesh_path));
- addInnerObject(parent, idx);
+ GeomIndex idx = (GeomIndex) (ngeoms ++);
+ geometryObjects.push_back(object);
return idx;
}
-
inline GeomIndex GeometryModel::getGeometryId(const std::string & name) const
{
@@ -75,50 +102,76 @@ namespace se3
}
- inline void GeometryModel::addInnerObject(const JointIndex joint_id, const GeomIndex inner_object)
- {
- if (std::find(innerObjects[joint_id].begin(),
- innerObjects[joint_id].end(),
- inner_object) == innerObjects[joint_id].end())
- innerObjects[joint_id].push_back(inner_object);
- else
- std::cout << "inner object already added" << std::endl;
- }
-
- inline void GeometryModel::addOutterObject (const JointIndex joint, const GeomIndex outer_object)
- {
- if (std::find(outerObjects[joint].begin(),
- outerObjects[joint].end(),
- outer_object) == outerObjects[joint].end())
- outerObjects[joint].push_back(outer_object);
- else
- std::cout << "outer object already added" << std::endl;
+ /**
+ * @brief Associate a GeometryObject of type COLLISION to a joint's inner objects list
+ *
+ * @param[in] joint Index of the joint
+ * @param[in] inner_object Index of the GeometryObject that will be an inner object
+ */
+ // inline void GeometryModel::addInnerObject(const JointIndex joint_id, const GeomIndex inner_object)
+ // {
+ // if (std::find(innerObjects[joint_id].begin(),
+ // innerObjects[joint_id].end(),
+ // inner_object) == innerObjects[joint_id].end())
+ // innerObjects[joint_id].push_back(inner_object);
+ // else
+ // std::cout << "inner object already added" << std::endl;
+ // }
+
+ /**
+ * @brief Associate a GeometryObject of type COLLISION to a joint's outer objects list
+ *
+ * @param[in] joint Index of the joint
+ * @param[in] inner_object Index of the GeometryObject that will be an outer object
+ */
+ // inline void GeometryModel::addOutterObject (const JointIndex joint, const GeomIndex outer_object)
+ // {
+ // if (std::find(outerObjects[joint].begin(),
+ // outerObjects[joint].end(),
+ // outer_object) == outerObjects[joint].end())
+ // outerObjects[joint].push_back(outer_object);
+ // else
+ // std::cout << "outer object already added" << std::endl;
+ // }
+
+ inline void GeometryData::fillInnerOuterObjectMaps(const GeometryModel & geomModel)
+ {
+ innerObjects.clear();
+ outerObjects.clear();
+
+ for( GeomIndex gid = 0; gid<geomModel.geometryObjects.size(); gid++)
+ innerObjects[geomModel.geometryObjects[gid].parentJoint].push_back(gid);
+
+ BOOST_FOREACH( const CollisionPair & pair, geomModel.collisionPairs )
+ {
+ outerObjects[geomModel.geometryObjects[pair.first].parentJoint].push_back(pair.second);
+ }
}
- inline std::ostream & operator<< (std::ostream & os, const GeometryModel & model_geom)
+ inline std::ostream & operator<< (std::ostream & os, const GeometryModel & geomModel)
{
- os << "Nb geometry objects = " << model_geom.ngeoms << std::endl;
+ os << "Nb geometry objects = " << geomModel.ngeoms << std::endl;
- for(Index i=0;i<(Index)(model_geom.ngeoms);++i)
+ for(GeomIndex i=0;i<(GeomIndex)(geomModel.ngeoms);++i)
{
- os << model_geom.geometryObjects[i] <<std::endl;
+ os << geomModel.geometryObjects[i] <<std::endl;
}
return os;
}
- inline std::ostream & operator<< (std::ostream & os, const GeometryData & data_geom)
+ inline std::ostream & operator<< (std::ostream & os, const GeometryData & geomData)
{
#ifdef WITH_HPP_FCL
- os << "Nb collision pairs = " << data_geom.activeCollisionPairs.size() << std::endl;
+ os << "Nb collision pairs = " << geomData.activeCollisionPairs.size() << std::endl;
- for(Index i=0;i<(Index)(data_geom.activeCollisionPairs.size());++i)
+ for(PairIndex i=0;i<(PairIndex)(geomData.activeCollisionPairs.size());++i)
{
- os << "collision object in position " << data_geom.model_geom.collisionPairs[i] << std::endl;
+ os << "Pairs " << i << (geomData.activeCollisionPairs[i]?"active":"unactive") << std::endl;
}
#else
os << "WARNING** Without fcl, no collision computations are possible. Only Positions can be computed" << std::endl;
- os << "Nb of geometry objects = " << data_geom.oMg.size() << std::endl;
+ os << "Nb of geometry objects = " << geomData.oMg.size() << std::endl;
#endif
return os;
@@ -135,9 +188,16 @@ namespace se3
inline void GeometryModel::addAllCollisionPairs()
{
removeAllCollisionPairs();
- for (Index i = 0; i < ngeoms; ++i)
- for (Index j = i+1; j < ngeoms; ++j)
- addCollisionPair(CollisionPair(i,j));
+ for (GeomIndex i = 0; i < ngeoms; ++i)
+ {
+ const JointIndex& joint_i = geometryObjects[i].parentJoint;
+ for (GeomIndex j = i+1; j < ngeoms; ++j)
+ {
+ const JointIndex& joint_j = geometryObjects[j].parentJoint;
+ if (joint_i != joint_j)
+ addCollisionPair(CollisionPair(i,j));
+ }
+ }
}
inline void GeometryModel::removeCollisionPair (const CollisionPair & pair)
@@ -159,100 +219,27 @@ namespace se3
pair) != collisionPairs.end());
}
- inline Index GeometryModel::findCollisionPair (const CollisionPair & pair) const
+ inline PairIndex GeometryModel::findCollisionPair (const CollisionPair & pair) const
{
CollisionPairsVector_t::const_iterator it = std::find(collisionPairs.begin(),
collisionPairs.end(),
pair);
- return (Index) distance(collisionPairs.begin(), it);
- }
-
- inline void GeometryModel::displayCollisionPairs() const
- {
- for (CollisionPairsVector_t::const_iterator it = collisionPairs.begin();
- it != collisionPairs.end(); ++it)
- {
- std::cout << it-> first << "\t" << it->second << std::endl;
- }
+ return (PairIndex) std::distance(collisionPairs.begin(), it);
}
- inline void GeometryData::activateCollisionPair(const Index pairId,const bool flag)
+ inline void GeometryData::activateCollisionPair(const PairIndex pairId,const bool flag)
{
- assert( activeCollisionPairs.size() == model_geom.collisionPairs.size() );
assert( pairId < activeCollisionPairs.size() );
activeCollisionPairs[pairId] = flag;
}
- inline void GeometryData::deactivateCollisionPair(const Index pairId)
+ inline void GeometryData::deactivateCollisionPair(const PairIndex pairId)
{
- assert( activeCollisionPairs.size() == model_geom.collisionPairs.size() );
assert( pairId < activeCollisionPairs.size() );
activeCollisionPairs[pairId] = false;
}
- inline CollisionResult GeometryData::computeCollision(const CollisionPair & pair) const
- {
- const Index & co1 = pair.first; assert(co1<collisionObjects.size());
- const Index & co2 = pair.second; assert(co2<collisionObjects.size());
-
- fcl::CollisionRequest collisionRequest (1, false, false, 1, false, true, fcl::GST_INDEP);
- fcl::CollisionResult collisionResult;
-
- fcl::collide (&collisionObjects[co1],&collisionObjects[co2],
- collisionRequest, collisionResult);
-
- return CollisionResult (collisionResult, co1, co2);
- }
-
- inline void GeometryData::computeAllCollisions()
- {
- assert( activeCollisionPairs.size() == model_geom.collisionPairs.size() );
- assert( collision_results .size() == model_geom.collisionPairs.size() );
- for(size_t i = 0; i<model_geom.collisionPairs.size(); ++i)
- {
- if(activeCollisionPairs[i])
- { collision_results[i] = computeCollision(model_geom.collisionPairs[i]); }
- }
- }
-
- inline bool GeometryData::isColliding() const
- {
- for(size_t i = 0; i<model_geom.collisionPairs.size(); ++i)
- {
- if (activeCollisionPairs[i]
- && computeCollision(model_geom.collisionPairs[i]).fcl_collision_result.isCollision())
- return true;
- }
- return false;
- }
-
- inline DistanceResult GeometryData::computeDistance(const CollisionPair & pair) const
- {
- const Index & co1 = pair.first; assert(co1<collisionObjects.size());
- const Index & co2 = pair.second; assert(co2<collisionObjects.size());
-
- fcl::DistanceRequest distanceRequest (true, 0, 0, fcl::GST_INDEP);
- fcl::DistanceResult result;
- fcl::distance ( &collisionObjects[co1],&collisionObjects[co2],
- distanceRequest, result);
-
- return DistanceResult (result, co1, co2);
- }
-
- inline void GeometryData::computeAllDistances ()
- {
- for(size_t i = 0; i<activeCollisionPairs.size(); ++i)
- {
- if (activeCollisionPairs[i])
- distance_results[i] = computeDistance(model_geom.collisionPairs[i]);
- }
- }
-
- inline void GeometryData::resetDistances()
- {
- std::fill(distance_results.begin(), distance_results.end(), DistanceResult( fcl::DistanceResult(), 0, 0) );
- }
#endif //WITH_HPP_FCL
} // namespace se3
diff --git a/src/multibody/joint/joint-base.hpp b/src/multibody/joint/joint-base.hpp
index d5f2a83a410114d933c13dffb516465cc6d28005..8950273c4eb5ab47da295cfb514b894c1fb7b13a 100644
--- a/src/multibody/joint/joint-base.hpp
+++ b/src/multibody/joint/joint-base.hpp
@@ -399,7 +399,9 @@ namespace se3
int idx_v() const { return i_v; }
JointIndex id() const { return i_id; }
- void setIndexes(JointIndex id,int q,int v) { i_id = id, i_q = q; i_v = v; }
+ void setIndexes(JointIndex id, int q, int v) { derived().setIndexes_impl(id, q, v); }
+
+ void setIndexes_impl(JointIndex id,int q,int v) { i_id = id, i_q = q; i_v = v; }
void disp(std::ostream & os) const
{
diff --git a/src/multibody/joint/joint-basic-visitors.hpp b/src/multibody/joint/joint-basic-visitors.hpp
index 9aa612990297fa4b1dc0716b4d73823922386e28..bcaf830478692801d41ae4b37941a6221ce5ab8f 100644
--- a/src/multibody/joint/joint-basic-visitors.hpp
+++ b/src/multibody/joint/joint-basic-visitors.hpp
@@ -19,9 +19,9 @@
#define __se3_joint_basic_visitors_hpp__
#include <Eigen/StdVector>
-#include <boost/variant.hpp>
#include "pinocchio/multibody/joint/joint-variant.hpp"
+
namespace se3
{
@@ -108,6 +108,16 @@ namespace se3
const double u);
+ /**
+ * @brief Visit a JointModelVariant through JointRandomVisitor to
+ * generate a random configuration vector
+ *
+ * @param[in] jmodel The JointModelVariant
+ *
+ * @return The joint randomconfiguration
+ */
+ inline Eigen::VectorXd random(const JointModelVariant & jmodel);
+
/**
* @brief Visit a JointModelVariant through JointRandomConfigurationVisitor to
* generate a configuration vector uniformly sampled among provided limits
@@ -343,7 +353,8 @@ namespace se3
/* --- Details -------------------------------------------------------------------- */
-#include "pinocchio/multibody/joint/joint-basic-visitors.hxx"
+// Included later
+// #include "pinocchio/multibody/joint/joint-basic-visitors.hxx"
#endif // ifndef __se3_joint_basic_visitors_hpp__
diff --git a/src/multibody/joint/joint-basic-visitors.hxx b/src/multibody/joint/joint-basic-visitors.hxx
index ecb549b4ea5f511dc97d3660356501e682d1acca..a155bb2d21d3c8ef9df270381c24e3abaf5a7316 100644
--- a/src/multibody/joint/joint-basic-visitors.hxx
+++ b/src/multibody/joint/joint-basic-visitors.hxx
@@ -19,7 +19,8 @@
#define __se3_joint_basic_visitors_hxx__
#include "pinocchio/assert.hpp"
-#include "pinocchio/multibody/joint/joint-variant.hpp"
+#include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
+#include "pinocchio/multibody/joint/joint-composite.hpp"
#include "pinocchio/multibody/visitor.hpp"
namespace se3
@@ -192,6 +193,25 @@ namespace se3
return JointInterpolateVisitor::run(jmodel, q0, q1, u);
}
+ /**
+ * @brief JointRandomVisitor visitor
+ */
+ class JointRandomVisitor: public boost::static_visitor<Eigen::VectorXd>
+ {
+ public:
+
+ template<typename D>
+ Eigen::VectorXd operator()(const JointModelBase<D> & jmodel) const
+ { return jmodel.random(); }
+
+ static Eigen::VectorXd run(const JointModelVariant & jmodel)
+ { return boost::apply_visitor( JointRandomVisitor(), jmodel ); }
+ };
+ inline Eigen::VectorXd random(const JointModelVariant & jmodel)
+ {
+ return JointRandomVisitor::run(jmodel);
+ }
+
/**
* @brief JointRandomConfigurationVisitor visitor
*/
@@ -348,8 +368,8 @@ namespace se3
{
public:
template<typename D>
- int operator()(const JointModelBase<D> & ) const
- { return D::NV; }
+ int operator()(const JointModelBase<D> & jmodel) const
+ { return jmodel.nv(); }
static int run( const JointModelVariant & jmodel)
{ return boost::apply_visitor( JointNvVisitor(), jmodel ); }
@@ -364,8 +384,8 @@ namespace se3
{
public:
template<typename D>
- int operator()(const JointModelBase<D> & ) const
- { return D::NQ; }
+ int operator()(const JointModelBase<D> & jmodel) const
+ { return jmodel.nq(); }
static int run( const JointModelVariant & jmodel)
{ return boost::apply_visitor( JointNqVisitor(), jmodel ); }
diff --git a/src/multibody/joint/joint-composite.hpp b/src/multibody/joint/joint-composite.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..01ed3d89d7b62f3741ba6ec239c96fc421d4ea3f
--- /dev/null
+++ b/src/multibody/joint/joint-composite.hpp
@@ -0,0 +1,481 @@
+//
+// Copyright (c) 2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terljMj 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_joint_composite_hpp__
+#define __se3_joint_composite_hpp__
+
+#include "pinocchio/assert.hpp"
+#include "pinocchio/multibody/joint/joint.hpp"
+
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::SE3)
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Motion)
+
+namespace se3
+{
+
+ struct JointComposite;
+ struct JointModelComposite;
+ struct JointDataComposite;
+
+ template<>
+ struct traits<JointComposite>
+ {
+
+ enum {
+ NQ = Eigen::Dynamic,
+ NV = Eigen::Dynamic
+ };
+ typedef double Scalar;
+ typedef JointDataComposite JointDataDerived;
+ typedef JointModelComposite JointModelDerived;
+ typedef ConstraintXd Constraint_t;
+ typedef SE3 Transformation_t;
+ typedef Motion Motion_t;
+ typedef Motion Bias_t;
+
+ typedef Eigen::Matrix<double,6,Eigen::Dynamic> F_t;
+ // [ABA]
+ typedef Eigen::Matrix<double,6,Eigen::Dynamic> U_t;
+ typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> D_t;
+ typedef Eigen::Matrix<double,6,Eigen::Dynamic> UD_t;
+
+ typedef Eigen::Matrix<double,Eigen::Dynamic,1> ConfigVector_t;
+ typedef Eigen::Matrix<double,Eigen::Dynamic,1> TangentVector_t;
+ };
+ template<> struct traits<JointDataComposite> { typedef JointComposite JointDerived; };
+ template<> struct traits<JointModelComposite> { typedef JointComposite JointDerived; };
+
+ struct JointDataComposite : public JointDataBase<JointDataComposite>
+ {
+ typedef JointComposite Joint;
+ SE3_JOINT_TYPEDEF;
+
+ JointDataVector joints;
+ int nq_composite,nv_composite;
+
+ Constraint_t S;
+ std::vector<Transformation_t> ljMj;
+ Transformation_t M;
+ Motion_t v;
+ Bias_t c;
+
+
+ // // [ABA] specific data
+ U_t U;
+ D_t Dinv;
+ UD_t UDinv;
+
+
+ // JointDataComposite() {} // can become necessary if we want a vector of JointDataComposite ?
+ JointDataComposite( JointDataVector & joints, int nq, int nv )
+ : joints(joints)
+ , nq_composite(nq)
+ , nv_composite(nv)
+ , S(Eigen::MatrixXd::Zero(6, nv_composite))
+ , ljMj(joints.size())
+ , M(Transformation_t::Identity())
+ , v(Motion_t::Zero())
+ , c(Bias_t::Zero())
+ , U(Eigen::MatrixXd::Zero(6, nv_composite))
+ , Dinv(Eigen::MatrixXd::Zero(nv_composite, nv_composite))
+ , UDinv(Eigen::MatrixXd::Zero(6, nv_composite))
+ {}
+
+ };
+
+ struct JointModelComposite : public JointModelBase<JointModelComposite>
+ {
+ typedef JointComposite Joint;
+ SE3_JOINT_TYPEDEF;
+ SE3_JOINT_USE_INDEXES;
+ using JointModelBase<JointModelComposite>::id;
+ using JointModelBase<JointModelComposite>::setIndexes;
+
+ std::size_t max_joints;
+ JointModelVector joints;
+ int nq_composite,nv_composite;
+
+ // Same as JointModelComposite(1)
+ JointModelComposite() : max_joints(1)
+ , joints(0)
+ , nq_composite(0)
+ , nv_composite(0)
+ {}
+ JointModelComposite(std::size_t max_number_of_joints) : max_joints(max_number_of_joints)
+ , joints(0)
+ , nq_composite(0)
+ , nv_composite(0)
+ {}
+
+ template <typename D1>
+ JointModelComposite(const JointModelBase<D1> & jmodel1) : max_joints(1)
+ , joints(0)
+ , nq_composite(jmodel1.nq())
+ , nv_composite(jmodel1.nv())
+ {
+ joints.push_back(JointModel(jmodel1.derived()));
+ }
+
+ template <typename D1, typename D2 >
+ JointModelComposite(const JointModelBase<D1> & jmodel1, const JointModelBase<D2> & jmodel2)
+ : max_joints(2)
+ , joints(0)
+ , nq_composite(jmodel1.nq() + jmodel2.nq())
+ , nv_composite(jmodel1.nv() + jmodel2.nv())
+ {
+ joints.push_back(JointModel(jmodel1.derived()));
+ joints.push_back(JointModel(jmodel2.derived()));
+ }
+
+ template <typename D1, typename D2, typename D3 >
+ JointModelComposite(const JointModelBase<D1> & jmodel1,
+ const JointModelBase<D2> & jmodel2,
+ const JointModelBase<D3> & jmodel3)
+ : max_joints(3)
+ , joints(0)
+ , nq_composite(jmodel1.nq() + jmodel2.nq() + jmodel3.nq())
+ , nv_composite(jmodel1.nv() + jmodel2.nv() + jmodel3.nv())
+ {
+ joints.push_back(JointModel(jmodel1.derived()));
+ joints.push_back(JointModel(jmodel2.derived()));
+ joints.push_back(JointModel(jmodel3.derived()));
+ }
+
+ // JointModelComposite( const JointModelVector & models ) : max_joints(models.size()) , joints(models) {}
+
+ template < typename D>
+ std::size_t addJointModel(const JointModelBase<D> & jmodel)
+ {
+ std::size_t nb_joints = joints.size();
+ if (!isFull())
+ {
+ joints.push_back(JointModel(jmodel.derived()));
+ nq_composite += jmodel.nq();
+ nv_composite += jmodel.nv();
+ nb_joints = joints.size();
+ }
+ return nb_joints;
+ }
+
+ bool isFull() const
+ {
+ return joints.size() == max_joints ;
+ }
+
+ bool isEmpty() const
+ {
+ return joints.size() <= 0 ;
+ }
+ JointDataDerived createData() const
+ {
+ JointDataVector res;
+ for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
+ {
+ res.push_back(::se3::createData(*i));
+ }
+ return JointDataDerived(res, nq_composite, nv_composite);
+ }
+
+
+ void calc (JointDataDerived & data,
+ const Eigen::VectorXd & qs) const
+ {
+ data.M.setIdentity();
+ for (JointDataVector::iterator i = data.joints.begin(); i != data.joints.end(); ++i)
+ {
+ JointDataVector::iterator::difference_type index = i - data.joints.begin();
+ calc_zero_order(joints[(std::size_t)index], *i, qs);
+ data.ljMj[(std::size_t)index] = joint_transform(*i);
+ data.M = data.M * data.ljMj[(std::size_t)index];
+ }
+ }
+
+ void calc (JointDataDerived & data,
+ const Eigen::VectorXd & qs,
+ const Eigen::VectorXd & vs ) const
+ {
+ data.M.setIdentity();
+ data.v.setZero();
+ data.c.setZero();
+ data.S.matrix().setZero();
+ for (JointDataVector::iterator i = data.joints.begin(); i != data.joints.end(); ++i)
+ {
+ JointDataVector::iterator::difference_type index = i - data.joints.begin();
+ calc_first_order(joints[(std::size_t)index], *i, qs, vs);
+ data.ljMj[(std::size_t)index] = joint_transform(*i);
+ data.M = data.M * data.ljMj[(std::size_t)index];
+ data.v = motion(*i);
+ if (i == data.joints.begin())
+ {}
+ else
+ {
+ data.v += data.ljMj[(std::size_t)index].actInv(motion(*(i-1)));
+ }
+ }
+
+ data.c = bias(data.joints[joints.size()-1]);
+ int start_col = nv_composite;
+ int sub_constraint_dimension = (int)constraint_xd(data.joints[joints.size()-1]).matrix().cols();
+ start_col -= sub_constraint_dimension;
+ data.S.matrix().middleCols(start_col,sub_constraint_dimension) = constraint_xd(data.joints[joints.size()-1]).matrix();
+
+ SE3 iMcomposite(SE3::Identity());
+ for (JointDataVector::reverse_iterator i = data.joints.rbegin()+1; i != data.joints.rend(); ++i)
+ {
+ sub_constraint_dimension = (int)constraint_xd(*i).matrix().cols();
+ start_col -= sub_constraint_dimension;
+
+ iMcomposite = joint_transform(*(i+0)) * iMcomposite;
+ data.S.matrix().middleCols(start_col,sub_constraint_dimension) = iMcomposite.actInv(constraint_xd(*(i+0)));
+
+ Motion acceleration_d_entrainement_coriolis = Motion::Zero(); // TODO: compute
+ data.c += iMcomposite.actInv(bias(*i)) + acceleration_d_entrainement_coriolis;
+ }
+ }
+
+
+ void calc_aba(JointDataDerived & data, Inertia::Matrix6 & I, const bool update_I) const
+ {
+ // calc has been called previously in abaforwardstep1 so data.M, data.v are up to date
+ data.U.setZero();
+ data.Dinv.setZero();
+ data.UDinv.setZero();
+ for (JointDataVector::iterator i = data.joints.begin(); i != data.joints.end(); ++i)
+ {
+ JointDataVector::iterator::difference_type index = i - data.joints.begin();
+ ::se3::calc_aba(joints[(std::size_t)index], *i, I, false);
+ }
+ data.U = I * data.S;
+ Eigen::MatrixXd tmp = data.S.matrix().transpose() * I * data.S.matrix();
+ data.Dinv = tmp.inverse();
+ data.UDinv = data.U * data.Dinv;
+
+ if (update_I)
+ I -= data.UDinv * data.U.transpose();
+ }
+
+ Scalar finiteDifferenceIncrement() const
+ {
+ using std::sqrt;
+ return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
+ }
+
+ ConfigVector_t integrate_impl(const Eigen::VectorXd & qs,const Eigen::VectorXd & vs) const
+ {
+ ConfigVector_t result(Eigen::VectorXd::Zero(nq_composite));
+ for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
+ {
+ result.segment(::se3::idx_q(*i),::se3::nq(*i)) += ::se3::integrate(*i,qs,vs);
+ }
+ return result;
+ }
+
+ ConfigVector_t interpolate_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1, const double u) const
+ {
+ ConfigVector_t result(Eigen::VectorXd::Zero(nq_composite));
+ for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
+ {
+ result.segment(::se3::idx_q(*i),::se3::nq(*i)) += ::se3::interpolate(*i,q0,q1,u);
+ }
+ return result;
+ }
+
+ ConfigVector_t random_impl() const
+ {
+ ConfigVector_t result(Eigen::VectorXd::Zero(nq_composite));
+ for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
+ {
+ result.segment(::se3::idx_q(*i),::se3::nq(*i)) += ::se3::random(*i);
+ }
+ return result;
+ }
+
+ ConfigVector_t randomConfiguration_impl(const ConfigVector_t & lower_pos_limit, const ConfigVector_t & upper_pos_limit ) const throw (std::runtime_error)
+ {
+ ConfigVector_t result(Eigen::VectorXd::Zero(nq_composite));
+ for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
+ {
+ result.segment(::se3::idx_q(*i),::se3::nq(*i)) +=
+ ::se3::randomConfiguration(*i,
+ lower_pos_limit.segment(::se3::idx_q(*i), ::se3::nq(*i)),
+ upper_pos_limit.segment(::se3::idx_q(*i), ::se3::nq(*i))
+ );
+ }
+ return result;
+ }
+
+ TangentVector_t difference_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
+ {
+ TangentVector_t result(Eigen::VectorXd::Zero(nv_composite));
+ for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
+ {
+ result.segment(::se3::idx_v(*i),::se3::nv(*i)) += ::se3::difference(*i,q0,q1);
+ }
+ return result;
+ }
+
+ double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
+ {
+ return difference_impl(q0,q1).norm();
+ }
+
+ void normalize_impl(Eigen::VectorXd & q) const
+ {
+ for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
+ {
+ ::se3::normalize(*i,q);
+ }
+ }
+
+ ConfigVector_t neutralConfiguration_impl() const
+ {
+ ConfigVector_t result(Eigen::VectorXd::Zero(nq_composite));
+ for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
+ {
+ result.segment(::se3::idx_q(*i),::se3::nq(*i)) += ::se3::neutralConfiguration(*i);
+ }
+ return result;
+ }
+
+ bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & = Eigen::NumTraits<Scalar>::dummy_precision()) const
+ {
+ for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
+ {
+ if ( !::se3::isSameConfiguration(*i, q1, q2) )
+ return false;
+ }
+ return true;
+ }
+
+ int nv_impl() const { return nv_composite; }
+ int nq_impl() const { return nq_composite; }
+
+
+ /**
+ * @brief Update the indexes of subjoints in the stack
+ */
+ void updateComponentsIndexes()
+ {
+ int current_idx_q, current_idx_v;
+ int next_idx_q = idx_q();
+ int next_idx_v = idx_v();
+
+ for (JointModelVector::iterator i = joints.begin(); i != joints.end(); ++i)
+ {
+ current_idx_q = next_idx_q;
+ current_idx_v = next_idx_v;
+ ::se3::setIndexes(*i,id(),current_idx_q, current_idx_v);
+ next_idx_q = current_idx_q + ::se3::nq(*i);
+ next_idx_v = current_idx_v + ::se3::nv(*i);
+ }
+ }
+
+ void setIndexes_impl(JointIndex id, int q, int v)
+ {
+ JointModelBase<JointModelComposite>::setIndexes_impl(id, q, v);
+ updateComponentsIndexes();
+ }
+
+ static std::string classname() { return std::string("JointModelComposite"); }
+ std::string shortname() const { return classname(); }
+
+ template <class D>
+ bool operator == (const JointModelBase<D> &) const
+ {
+ return false;
+ }
+
+ bool operator == (const JointModelBase<JointModelComposite> & jmodel) const
+ {
+ return jmodel.id() == id()
+ && jmodel.idx_q() == idx_q()
+ && jmodel.idx_v() == idx_v();
+ }
+
+ // see http://en.cppreference.com/w/cpp/language/copy_assignment#Copy-and-swap_idiom
+ void swap(JointModelComposite & other) {
+ max_joints = other.max_joints;
+ nq_composite = other.nq_composite;
+ nv_composite = other.nv_composite;
+
+ joints.swap(other.joints);
+ }
+
+ JointModelComposite& operator=(JointModelComposite other)
+ {
+ swap(other);
+ return *this;
+ }
+
+ template<typename D>
+ typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
+ jointConfigSelector(const Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq_composite); }
+ template<typename D>
+ typename SizeDepType<NQ>::template SegmentReturn<D>::Type
+ jointConfigSelector( Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq_composite); }
+
+ template<typename D>
+ typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
+ jointVelocitySelector(const Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_composite); }
+ template<typename D>
+ typename SizeDepType<NV>::template SegmentReturn<D>::Type
+ jointVelocitySelector( Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_composite); }
+
+ template<typename D>
+ typename SizeDepType<NV>::template ColsReturn<D>::ConstType
+ jointCols(const Eigen::MatrixBase<D>& A) const { return A.segment(i_v,nv_composite); }
+ template<typename D>
+ typename SizeDepType<NV>::template ColsReturn<D>::Type
+ jointCols(Eigen::MatrixBase<D>& A) const { return A.segment(i_v,nv_composite); }
+
+ template<typename D>
+ typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
+ jointConfigSelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq_composite); }
+ template<typename D>
+ typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
+ jointConfigSelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq_composite); }
+ template<typename D>
+ typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
+ jointVelocitySelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_composite); }
+ template<typename D>
+ typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
+ jointVelocitySelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_composite); }
+
+ template<typename D>
+ typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::ConstType
+ jointCols_impl(const Eigen::MatrixBase<D>& A) const { return A.middleCols(i_v,nv_composite); }
+ template<typename D>
+ typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::Type
+ jointCols_impl(Eigen::MatrixBase<D>& A) const { return A.middleCols(i_v,nv_composite); }
+
+ };
+
+
+ inline std::ostream & operator << (std::ostream & os, const JointModelComposite & jmodel)
+ {
+ os << "JointModelComposite containing following models:\n" ;
+ for (JointModelVector::const_iterator i = jmodel.joints.begin(); i != jmodel.joints.end(); ++i)
+ {
+ os << shortname(*i) << std::endl;
+ }
+ return os;
+ }
+
+} // namespace se3
+
+
+#endif // ifndef __se3_joint_composite_hpp__
diff --git a/src/multibody/joint/joint-free-flyer.hpp b/src/multibody/joint/joint-free-flyer.hpp
index da0228de63b4079f40b013f9c90402dce0995b79..f5f9e444c2442cf95e27ebb7c0de4037810829a6 100644
--- a/src/multibody/joint/joint-free-flyer.hpp
+++ b/src/multibody/joint/joint-free-flyer.hpp
@@ -21,6 +21,7 @@
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
+#include "pinocchio/multibody/joint/joint-dense.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/spatial/explog.hpp"
#include "pinocchio/math/fwd.hpp"
@@ -207,11 +208,11 @@ namespace se3
template<typename V>
inline void forwardKinematics(Transformation_t & M, const Eigen::MatrixBase<V> & q_joint) const
{
+ EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,V);
using std::sqrt;
typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
- typename Eigen::MatrixBase<V>::template ConstFixedSegmentReturnType<NQ>::Type & q = q_joint.template segment<NQ> (idx_q ());
- ConstQuaternionMap_t quat(q.template tail<4>().data());
+ ConstQuaternionMap_t quat(q_joint.template tail<4>().data());
assert(std::fabs(quat.coeffs().norm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon()));
M.rotation(quat.matrix());
@@ -274,12 +275,17 @@ namespace se3
res.head<3>() = M1.translation();
QuaternionMap_t res_quat(res.tail<4>().data());
res_quat = M1.rotation();
+ // Norm of qs might be epsilon-different to 1, so M1.rotation might be epsilon-different to a rotation matrix.
+ // It is then safer to re-normalized after converting M1.rotation to quaternion.
+ firstOrderNormalize(res_quat);
return res;
}
ConfigVector_t interpolate_impl(const Eigen::VectorXd & q0, const Eigen::VectorXd & q1, const double u) const
{
+ typedef Eigen::Map<Motion_t::Quaternion_t> QuaternionMap_t;
+
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_0 = q0.segment<NQ> (idx_q ());
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
@@ -287,8 +293,21 @@ namespace se3
else if( u == 1.) return q_1;
else
{
+ // TODO: If integrate takes an arguments (ConfigVector_t, TangentVector_t), then we can merely do:
+ // TangentVector_t nu(u*difference(q0, q1));
+ // return integrate(q0, nu);
+
TangentVector_t nu(u*difference(q0, q1));
- return integrate(q0, nu);
+ Transformation_t M0; forwardKinematics(M0,q_0);
+ Transformation_t M1(M0*exp6(Motion_t(nu)));
+
+ ConfigVector_t res;
+ res.head<3>() = M1.translation();
+ QuaternionMap_t res_quat(res.tail<4>().data());
+ res_quat = M1.rotation();
+ firstOrderNormalize(res_quat);
+
+ return res;
}
}
@@ -337,8 +356,8 @@ namespace se3
TangentVector_t difference_impl(const Eigen::VectorXd & q0, const Eigen::VectorXd & q1) const
{
- Transformation_t M0(Transformation_t::Identity()); forwardKinematics(M0, q0);
- Transformation_t M1(Transformation_t::Identity()); forwardKinematics(M1, q1);
+ Transformation_t M0(Transformation_t::Identity()); forwardKinematics(M0, q0.segment<NQ> (idx_q ()));
+ Transformation_t M1(Transformation_t::Identity()); forwardKinematics(M1, q1.segment<NQ> (idx_q ()));
return se3::log6(M0.inverse()*M1);
}
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-prismatic.hpp b/src/multibody/joint/joint-prismatic.hpp
index 10a806addf1085e313137f8611a58e2a669bfab1..fc6d001d82387774b74a88434ef2573c2c37a96e 100644
--- a/src/multibody/joint/joint-prismatic.hpp
+++ b/src/multibody/joint/joint-prismatic.hpp
@@ -482,7 +482,7 @@ namespace se3
const Scalar & q_0 = q0[idx_q()];
const Scalar & q_1 = q1[idx_q()];
- ConfigVector_t result;
+ TangentVector_t result;
result << (q_1 - q_0);
return result;
}
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/joint/joint-revolute-unbounded.hpp b/src/multibody/joint/joint-revolute-unbounded.hpp
index 69089ef9caf9a555c7da4cf428f7766ba7a21fc3..259ad52583577bb8f4702c6bdc6eb75aa754ffa6 100644
--- a/src/multibody/joint/joint-revolute-unbounded.hpp
+++ b/src/multibody/joint/joint-revolute-unbounded.hpp
@@ -18,6 +18,7 @@
#ifndef __se3_joint_revolute_unbounded_hpp__
#define __se3_joint_revolute_unbounded_hpp__
+#include "pinocchio/math/fwd.hpp"
#include "pinocchio/math/sincos.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
@@ -157,10 +158,11 @@ namespace se3
double cosOmega,sinOmega; SINCOS(omega, &sinOmega, &cosOmega);
// TODO check the cost of atan2 vs SINCOS
- ConfigVector_t result;
- result <<
- cosOmega * ca - sinOmega * sa,
- sinOmega * ca + cosOmega * sa;
+ ConfigVector_t result (cosOmega * ca - sinOmega * sa,
+ sinOmega * ca + cosOmega * sa);
+ const double norm2 = q.squaredNorm();
+ result *= (3 - norm2) / 2;
+
return result;
}
diff --git a/src/multibody/joint/joint-spherical.hpp b/src/multibody/joint/joint-spherical.hpp
index f299cf1bb9f13518afaf8f5c5e7093b068e9d766..fdbf086c5a3d9e412cf443f081ee93ad368423f4 100644
--- a/src/multibody/joint/joint-spherical.hpp
+++ b/src/multibody/joint/joint-spherical.hpp
@@ -271,10 +271,10 @@ namespace se3
template<typename V>
inline void forwardKinematics(Transformation_t & M, const Eigen::MatrixBase<V> & q_joint) const
{
+ EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,V);
using std::sqrt;
- typename Eigen::MatrixBase<V>::template ConstFixedSegmentReturnType<NQ>::Type & q = q_joint.template segment<NQ> (idx_q ());
- ConstQuaternionMap_t quat(q.data());
+ ConstQuaternionMap_t quat(q_joint.derived().data());
assert(std::fabs(quat.coeffs().norm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon()));
M.rotation(quat.matrix());
@@ -335,6 +335,7 @@ namespace se3
Motion_t::Quaternion_t pOmega(se3::exp3(q_dot));
Motion_t::Quaternion_t quaternion_result(q*pOmega);
+ firstOrderNormalize(quaternion_result);
return quaternion_result.coeffs();
}
@@ -385,8 +386,8 @@ namespace se3
TangentVector_t difference_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
- Transformation_t M0; forwardKinematics(M0, q0);
- Transformation_t M1; forwardKinematics(M1, q1);
+ Transformation_t M0; forwardKinematics(M0, q0.segment<NQ>(idx_q()));
+ Transformation_t M1; forwardKinematics(M1, q1.segment<NQ>(idx_q()));
return se3::log3((M0.rotation().transpose()*M1.rotation()).eval());
diff --git a/src/multibody/joint/joint-variant.hpp b/src/multibody/joint/joint-variant.hpp
index c748da1819a5db60a405ea0deb6ddff08572f821..b1e2b43c278cd683229d7d33b618e4bdd9a67f26 100644
--- a/src/multibody/joint/joint-variant.hpp
+++ b/src/multibody/joint/joint-variant.hpp
@@ -31,16 +31,33 @@
#include "pinocchio/multibody/joint/joint-spherical.hpp"
#include "pinocchio/multibody/joint/joint-translation.hpp"
-#include <Eigen/StdVector>
#include <boost/variant.hpp>
+#include <boost/variant/recursive_wrapper.hpp>
namespace se3
{
enum { MAX_JOINT_NV = 6 };
- typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelSpherical, JointModelSphericalZYX, JointModelPX, JointModelPY, JointModelPZ, JointModelPrismaticUnaligned, JointModelFreeFlyer, JointModelPlanar, JointModelTranslation, JointModelDense<-1,-1>, JointModelRUBX, JointModelRUBY, JointModelRUBZ > JointModelVariant;
- typedef boost::variant< JointDataRX, JointDataRY, JointDataRZ, JointDataRevoluteUnaligned, JointDataSpherical, JointDataSphericalZYX, JointDataPX, JointDataPY, JointDataPZ, JointDataPrismaticUnaligned, JointDataFreeFlyer, JointDataPlanar, JointDataTranslation, JointDataDense<-1,-1>, JointDataRUBX, JointDataRUBY, JointDataRUBZ > JointDataVariant;
-
+ struct JointComposite;
+ struct JointModelComposite;
+ struct JointDataComposite;
+
+ // The JointModelComposite contains several JointModel (which are JointModelVariant). Hence there is a circular
+ // dependency between JointModelComposite and JointModelVariant that can be resolved with the use of boost::recursive_variant
+ // For more details, see http://www.boost.org/doc/libs/1_58_0/doc/html/variant/tutorial.html#variant.tutorial.recursive
+ //
+ // The same applies for JointDataComposite
+ typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelSpherical,
+ JointModelSphericalZYX, JointModelPX, JointModelPY, JointModelPZ,
+ JointModelPrismaticUnaligned, JointModelFreeFlyer, JointModelPlanar, JointModelTranslation,
+ JointModelDense<-1,-1> ,JointModelRUBX, JointModelRUBY, JointModelRUBZ,
+ boost::recursive_wrapper<JointModelComposite> >JointModelVariant;
+ typedef boost::variant< JointDataRX, JointDataRY, JointDataRZ, JointDataRevoluteUnaligned, JointDataSpherical,
+ JointDataSphericalZYX, JointDataPX, JointDataPY, JointDataPZ, JointDataPrismaticUnaligned,
+ JointDataFreeFlyer, JointDataPlanar, JointDataTranslation, JointDataDense<-1,-1>,
+ JointDataRUBX, JointDataRUBY, JointDataRUBZ,
+ boost::recursive_wrapper<JointDataComposite> > JointDataVariant;
+
} // namespace se3
#endif // ifndef __se3_joint_variant_hpp__
diff --git a/src/multibody/joint/joint.hpp b/src/multibody/joint/joint.hpp
index 84f2637d437996b16e871c3b46baad7acdfb6a2a..34b8f2074ece0f9719556c9bec3234c805e89a94 100644
--- a/src/multibody/joint/joint.hpp
+++ b/src/multibody/joint/joint.hpp
@@ -20,7 +20,7 @@
#include "pinocchio/assert.hpp"
#include "pinocchio/multibody/joint/joint-variant.hpp"
-#include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
+#include "pinocchio/multibody/joint/joint-basic-visitors.hxx"
namespace se3
{
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index 8e38896c6bccaafe4278f5ca088f422822008077..a11b2218cb91df1c205c8b7b4499d5b9213c7721 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -26,16 +26,15 @@
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/multibody/frame.hpp"
-#include "pinocchio/multibody/joint/joint.hpp"
+#include "pinocchio/multibody/joint/joint-composite.hpp"
#include "pinocchio/deprecated.hh"
+#include "pinocchio/tools/string-generator.hpp"
#include <iostream>
#include <Eigen/Cholesky>
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::SE3)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Inertia)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Force)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Motion)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,6,Eigen::Dynamic>)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,6,6>)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::SE3::Vector3)
@@ -58,13 +57,13 @@ namespace se3
int nv;
/// \brief Number of joints.
- int njoint;
+ int njoints;
/// \brief Number of bodies.
- int nbody;
+ int nbodies;
/// \brief Number of operational frames.
- int nFrames;
+ int nframes;
/// \brief Spatial inertias of the body <i> expressed in the supporting joint frame <i>.
std::vector<Inertia> inertias;
@@ -112,23 +111,62 @@ namespace se3
Model()
: nq(0)
, nv(0)
- , njoint(1)
- , nbody(1)
- , nFrames(0)
+ , njoints(1)
+ , nbodies(1)
+ , nframes(0)
, inertias(1)
- , jointPlacements(1)
+ , jointPlacements(1, SE3::Identity())
, joints(1)
- , parents(1)
+ , parents(1, 0)
, names(1)
, subtrees(1)
, gravity( gravity981,Eigen::Vector3d::Zero() )
{
names[0] = "universe"; // Should be "universe joint (trivial)"
+ // FIXME Should the universe joint be a FIXED_JOINT even if it is
+ // in the list of joints ? See comment in definition of
+ // Model::addJointFrame and Model::addBodyFrame
+ addFrame(Frame("universe", 0, 0, SE3::Identity(), FIXED_JOINT));
+ // Inertia of universe has no sense.
+ inertias[0].mass() = std::numeric_limits<double>::quiet_NaN();
+ inertias[0].lever().fill (std::numeric_limits<double>::quiet_NaN());
+ inertias[0].inertia().fill (std::numeric_limits<double>::quiet_NaN());
}
~Model() {} // std::cout << "Destroy model" << std::endl; }
///
- /// \brief Add a joint to the kinematic tree.
+ /// \brief Add a joint to the kinematic tree with given bounds.
+ ///
+ /// \remark This method does not add a Frame of same name to the vector of frames.
+ /// Use Model::addJointFrame.
+ /// \remark The inertia supported by the joint is set to Zero.
+ ///
+ /// \tparam JointModelDerived The type of the joint model.
+ ///
+ /// \param[in] parent Index of the parent joint.
+ /// \param[in] joint_model The joint model.
+ /// \param[in] joint_placement Placement of the joint inside its parent joint.
+ /// \param[in] joint_name Name of the joint. If empty, the name is random.
+ /// \param[in] max_effort Maximal joint torque.
+ /// \param[in] max_velocity Maximal joint velocity.
+ /// \param[in] min_config Lower joint configuration.
+ /// \param[in] max_config Upper joint configuration.
+ ///
+ /// \return The index of the new joint.
+ ///
+ /// \sa Model::appendBodyToJoint, Model::addJointFrame
+ ///
+ template<typename JointModelDerived>
+ JointIndex addJoint(const JointIndex parent, const JointModelBase<JointModelDerived> & joint_model, const SE3 & joint_placement,
+ const Eigen::VectorXd & max_effort,
+ const Eigen::VectorXd & max_velocity,
+ const Eigen::VectorXd & min_config,
+ const Eigen::VectorXd & max_config,
+ const std::string & joint_name = ""
+ );
+
+ ///
+ /// \brief Add a joint to the kinematic tree with infinite bounds.
///
/// \remark This method also adds a Frame of same name to the vector of frames.
/// \remark The inertia supported by the joint is set to Zero.
@@ -139,10 +177,6 @@ namespace se3
/// \param[in] joint_model The joint model.
/// \param[in] joint_placement Placement of the joint inside its parent joint.
/// \param[in] joint_name Name of the joint. If empty, the name is random.
- /// \param[in] max_effort Maximal joint torque. (Default set to infinity).
- /// \param[in] max_velocity Maximal joint velocity. (Default set to infinity).
- /// \param[in] min_config Lower joint configuration. (Default set to infinity).
- /// \param[in] max_config Upper joint configuration. (Default set to infinity).
///
/// \return The index of the new joint.
///
@@ -150,28 +184,48 @@ namespace se3
///
template<typename JointModelDerived>
JointIndex addJoint(const JointIndex parent, const JointModelBase<JointModelDerived> & joint_model, const SE3 & joint_placement,
- const std::string & joint_name = "",
- const Eigen::VectorXd & max_effort = Eigen::VectorXd::Constant(JointModelDerived::NV,std::numeric_limits<double>::max()),
- const Eigen::VectorXd & max_velocity = Eigen::VectorXd::Constant(JointModelDerived::NV,std::numeric_limits<double>::max()),
- const Eigen::VectorXd & min_config = Eigen::VectorXd::Constant(JointModelDerived::NQ,std::numeric_limits<double>::min()),
- const Eigen::VectorXd & max_config = Eigen::VectorXd::Constant(JointModelDerived::NQ,std::numeric_limits<double>::max())
+ const std::string & joint_name = ""
);
///
- /// \brief Append a body to a given joint of the kinematic tree.
+ /// \brief Add a joint to the frame tree.
///
- /// \remark This method also adds a Frame of same name to the vector of frames.
+ /// \param[in] jointIndex Index of the joint.
+ /// \param[in] frameIndex Index of the parent frame. If negative,
+ /// the parent frame is the frame of the parent joint.
+ ///
+ /// \return The index of the new frame or -1 in case of error.
+ ///
+ int addJointFrame (const JointIndex& jointIndex,
+ int frameIndex = -1);
+
+ ///
+ /// \brief Append a body to a given joint of the kinematic tree.
///
/// \param[in] joint_index Index of the supporting joint.
/// \param[in] Y Spatial inertia of the body.
/// \param[in] body_placement The relative placement of the body regarding to the parent joint. Set default to the Identity placement.
- /// \param[in] body_name Name of the body. If empty, the name is random.
///
/// \sa Model::addJoint
///
void appendBodyToJoint(const JointIndex joint_index, const Inertia & Y,
- const SE3 & body_placement = SE3::Identity(),
- const std::string & body_name = "");
+ const SE3 & body_placement = SE3::Identity());
+
+ ///
+ /// \brief Add a body to the frame tree.
+ ///
+ /// \param[in] body_name Name of the body.
+ /// \param[in] parentJoint Index of the parent joint.
+ /// \param[in] body_placement The relative placement of the body regarding to the parent joint. Set default to the Identity placement.
+ /// \param[in] previousFrame Index of the parent frame. If negative,
+ /// the parent frame is the frame of the parent joint.
+ ///
+ /// \return The index of the new frame or -1 in case of error.
+ ///
+ int addBodyFrame (const std::string & body_name,
+ const JointIndex & parentJoint,
+ const SE3 & body_placement = SE3::Identity(),
+ int previousFrame = -1);
///
/// \brief Return the index of a body given by its name.
@@ -195,14 +249,6 @@ namespace se3
///
bool existBodyName(const std::string & name) const;
- ///
- /// \brief Get the name of a body given by its index.
- ///
- /// \param[in] index Index of the body.
- ///
- /// \return Name of the body.
- ///
- const std::string & getBodyName(const JointIndex index) const;
///
/// \brief Return the index of a joint given by its name.
@@ -232,7 +278,7 @@ namespace se3
///
/// \return Name of the joint.
///
- const std::string & getJointName(const JointIndex index) const;
+ PINOCCHIO_DEPRECATED const std::string & getJointName(const JointIndex index) const;
///
/// \brief Returns the index of a frame given by its name.
@@ -243,103 +289,32 @@ namespace se3
/// (for example to get the id of a parent frame).
///
/// \param[in] name Name of the frame.
+ /// \param[in] type Type of the frame.
///
/// \return Index of the frame.
///
- FrameIndex getFrameId (const std::string & name) const;
+ FrameIndex getFrameId (const std::string & name, const FrameType& type = (FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR )) const;
///
/// \brief Checks if a frame given by its name exists.
///
/// \param[in] name Name of the frame.
+ /// \param[in] type Type of the frame.
///
/// \return Returns true if the frame exists.
///
- bool existFrame (const std::string & name) 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 name.
- ///
- /// \param[in] name Name of the frame.
- ///
- /// \return
- ///
- PINOCCHIO_DEPRECATED JointIndex getFrameParent(const std::string & name) const;
+ bool existFrame (const std::string & name, const FrameType& type = (FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR )) 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] name Name of the frame.
- ///
- /// \return
- ///
- PINOCCHIO_DEPRECATED FrameType getFrameType(const std::string & name) 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] name Name of the frame.
- ///
- /// \return The frame placement regarding the supporing joint.
- ///
- PINOCCHIO_DEPRECATED const SE3 & getFramePlacement(const std::string & name) 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.
///
/// \param[in] frame The frame to add to the kinematic tree.
///
- /// \return Returns true if the frame has been successfully added.
- ///
- bool addFrame(const Frame & frame);
-
- ///
- /// \brief Creates and adds a frame to the kinematic tree.
- ///
- /// \param[in] name Name of the frame.
- /// \param[in] parent Index of the supporting joint.
- /// \param[in] placement Placement of the frame regarding to the joint frame.
- /// \param[in] type The type of the frame
- ///
- /// \return Returns true if the frame has been successfully added.
+ /// \return Returns the index of the frame if it has been successfully added,
+ /// -1 otherwise.
///
- PINOCCHIO_DEPRECATED bool addFrame(const std::string & name, const JointIndex parent, const SE3 & placement, const FrameType type = OP_FRAME);
+ int addFrame(const Frame & frame);
/// Check the validity of the attributes of Model with respect to the specification of some
/// algorithms.
@@ -530,7 +505,7 @@ namespace se3
///
/// \param[in] model The model structure of the rigid body system.
///
- Data (const Model & model);
+ explicit Data (const Model & model);
private:
void computeLastChild(const Model& model);
diff --git a/src/multibody/model.hxx b/src/multibody/model.hxx
index cee1244d40aa2ff1b7125739c832d8783357cff7..8b21b59a1c91f307feda2860778161c1352fa726 100644
--- a/src/multibody/model.hxx
+++ b/src/multibody/model.hxx
@@ -23,15 +23,28 @@
#include "pinocchio/tools/string-generator.hpp"
#include <boost/bind.hpp>
+#include <boost/utility.hpp>
/// @cond DEV
namespace se3
{
+ namespace details
+ {
+ struct FilterFrame {
+ const std::string& name;
+ const FrameType & typeMask;
+ FilterFrame (const std::string& name, const FrameType& typeMask)
+ : name (name), typeMask (typeMask) {}
+ bool operator() (const Frame& frame) const
+ { return (typeMask & frame.type) && (name == frame.name); }
+ };
+ }
+
inline std::ostream& operator<< (std::ostream & os, const Model & model)
{
- os << "Nb joints = " << model.njoint << " (nq="<< model.nq<<",nv="<<model.nv<<")" << std::endl;
- for(Model::Index i=0;i<(Model::Index)(model.njoint);++i)
+ os << "Nb joints = " << model.njoints << " (nq="<< model.nq<<",nv="<<model.nv<<")" << std::endl;
+ for(Model::Index i=0;i<(Model::Index)(model.njoints);++i)
{
os << " Joint "<< model.names[i] << ": parent=" << model.parents[i] << std::endl;
}
@@ -43,24 +56,24 @@ namespace se3
Model::JointIndex Model::addJoint(const Model::JointIndex parent,
const JointModelBase<JointModelDerived> & joint_model,
const SE3 & joint_placement,
- const std::string & joint_name,
const Eigen::VectorXd & max_effort,
const Eigen::VectorXd & max_velocity,
const Eigen::VectorXd & min_config,
- const Eigen::VectorXd & max_config
+ const Eigen::VectorXd & max_config,
+ const std::string & joint_name
)
{
typedef JointModelDerived D;
- assert( (njoint==(int)joints.size())&&(njoint==(int)inertias.size())
- &&(njoint==(int)parents.size())&&(njoint==(int)jointPlacements.size()) );
+ assert( (njoints==(int)joints.size())&&(njoints==(int)inertias.size())
+ &&(njoints==(int)parents.size())&&(njoints==(int)jointPlacements.size()) );
assert((joint_model.nq()>=0) && (joint_model.nv()>=0));
-
+
assert(max_effort.size() == joint_model.nv()
&& max_velocity.size() == joint_model.nv()
&& min_config.size() == joint_model.nq()
&& max_config.size() == joint_model.nq());
- Model::JointIndex idx = (Model::JointIndex) (njoint++);
+ Model::JointIndex idx = (Model::JointIndex) (njoints++);
joints .push_back(JointModel(joint_model.derived()));
boost::get<JointModelDerived>(joints.back()).setIndexes(idx,nq,nv);
@@ -71,18 +84,18 @@ namespace se3
names .push_back((joint_name!="")?joint_name:randomStringGenerator(8));
nq += joint_model.nq();
nv += joint_model.nv();
-
- effortLimit.conservativeResize(nv);effortLimit.bottomRows<D::NV>() = max_effort;
- velocityLimit.conservativeResize(nv);velocityLimit.bottomRows<D::NV>() = max_velocity;
- lowerPositionLimit.conservativeResize(nq);lowerPositionLimit.bottomRows<D::NQ>() = min_config;
- upperPositionLimit.conservativeResize(nq);upperPositionLimit.bottomRows<D::NQ>() = max_config;
+
+ // Optimal efficiency here would be using the static-dim bottomRows, while specifying the dimension in argument in the case where D::NV is Eigen::Dynamic.
+ // However, this option is not compiling in Travis (why?).
+ // As efficiency of Model::addJoint is not critical, the dynamic bottomRows is used here.
+ effortLimit.conservativeResize(nv);effortLimit.bottomRows(joint_model.nv()) = max_effort;
+ velocityLimit.conservativeResize(nv);velocityLimit.bottomRows(joint_model.nv()) = max_velocity;
+ lowerPositionLimit.conservativeResize(nq);lowerPositionLimit.bottomRows(joint_model.nq()) = min_config;
+ upperPositionLimit.conservativeResize(nq);upperPositionLimit.bottomRows(joint_model.nq()) = max_config;
neutralConfiguration.conservativeResize(nq);
neutralConfiguration.tail(joint_model.nq()) = joint_model.neutralConfiguration();
- // Add a the joint frame attached to itself to the frame vector - redundant information but useful.
- addFrame(names[idx],idx,SE3::Identity(),JOINT);
-
// Init and add joint index to its parent subtrees.
subtrees.push_back(IndexVector(1));
subtrees[idx][0] = idx;
@@ -90,33 +103,71 @@ namespace se3
return idx;
}
+ template<typename JointModelDerived>
+ Model::JointIndex Model::addJoint(const Model::JointIndex parent,
+ const JointModelBase<JointModelDerived> & joint_model,
+ const SE3 & joint_placement,
+ const std::string & joint_name
+ )
+ {
+ typedef JointModelDerived D;
+ Eigen::VectorXd max_effort, max_velocity, min_config, max_config;
+
+ max_effort = Eigen::VectorXd::Constant(joint_model.nv(), std::numeric_limits<double>::max());
+ max_velocity = Eigen::VectorXd::Constant(joint_model.nv(), std::numeric_limits<double>::max());
+ min_config = Eigen::VectorXd::Constant(joint_model.nq(), std::numeric_limits<double>::max());
+ max_config = Eigen::VectorXd::Constant(joint_model.nq(), std::numeric_limits<double>::max());
+
+ return addJoint(parent, joint_model, joint_placement, max_effort, max_velocity, min_config, max_config, joint_name);
+ }
+
+ inline int Model::addJointFrame (const JointIndex& jidx,
+ int fidx)
+ {
+ if (fidx < 0) {
+ // FIXED_JOINT is required because the parent can be the universe and its
+ // type is FIXED_JOINT
+ fidx = getFrameId(names[parents[jidx]], (FrameType)(JOINT | FIXED_JOINT));
+ }
+ assert(fidx < frames.size() && "Frame index out of bound");
+ // Add a the joint frame attached to itself to the frame vector - redundant information but useful.
+ return addFrame(Frame(names[jidx],jidx,fidx,SE3::Identity(),JOINT));
+ }
+
+
inline void Model::appendBodyToJoint(const Model::JointIndex joint_index,
const Inertia & Y,
- const SE3 & body_placement,
- const std::string & body_name)
+ const SE3 & body_placement)
{
const Inertia & iYf = Y.se3Action(body_placement);
inertias[joint_index] += iYf;
+ nbodies++;
+ }
- addFrame((body_name!="")?body_name:randomStringGenerator(8), joint_index, body_placement, BODY);
- nbody++;
+ inline int Model::addBodyFrame (const std::string & body_name,
+ const JointIndex & parentJoint,
+ const SE3 & body_placement,
+ int previousFrame)
+ {
+ if (previousFrame < 0) {
+ // FIXED_JOINT is required because the parent can be the universe and its
+ // type is FIXED_JOINT
+ previousFrame = getFrameId(names[parentJoint], (FrameType)(JOINT | FIXED_JOINT));
+ }
+ assert(previousFrame < frames.size() && "Frame index out of bound");
+ return addFrame(Frame(body_name, parentJoint, previousFrame, body_placement, BODY));
}
inline Model::JointIndex Model::getBodyId (const std::string & name) const
{
- return getFrameId(name);
+ return getFrameId(name, BODY);
}
inline bool Model::existBodyName (const std::string & name) const
{
- return existFrame(name);
+ return existFrame(name, BODY);
}
- inline const std::string& Model::getBodyName (const Model::JointIndex index) const
- {
- assert( index < (Model::Index)nbody );
- return getFrameName(index);
- }
inline Model::JointIndex Model::getJointId (const std::string & name) const
{
@@ -138,97 +189,38 @@ namespace se3
return names[index];
}
- inline Model::FrameIndex Model::getFrameId ( const std::string & name ) const
+ inline Model::FrameIndex Model::getFrameId ( const std::string & name, const FrameType & type ) const
{
std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
, frames.end()
- , boost::bind(&Frame::name, _1) == name
+ , details::FilterFrame (name, type)
);
+ assert (it != frames.end() && "Frame not found");
+ assert ((std::find_if( boost::next(it), frames.end(), details::FilterFrame (name, type)) == frames.end())
+ && "Several frames match the filter");
return Model::FrameIndex(it - frames.begin());
}
- inline bool Model::existFrame ( const std::string & name ) const
+ inline bool Model::existFrame ( const std::string & name, const FrameType & type) const
{
- return std::find_if( frames.begin(), frames.end(), boost::bind(&Frame::name, _1) == name) != frames.end();
+ return std::find_if( frames.begin(), frames.end(),
+ 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 std::string & name ) const
+ inline int Model::addFrame ( const Frame & frame )
{
- assert(existFrame(name) && "The Frame you requested does not exist");
- std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
- , frames.end()
- , boost::bind(&Frame::name, _1) == name
- );
-
- std::vector<Frame>::iterator::difference_type it_diff = it - frames.begin();
- return getFrameParent(Model::JointIndex(it_diff));
- }
-
- inline Model::JointIndex Model::getFrameParent( const FrameIndex index ) const
- {
- return frames[index].parent;
- }
-
- inline FrameType Model::getFrameType( const std::string & name ) const
- {
- assert(existFrame(name) && "The Frame you requested does not exist");
- std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
- , frames.end()
- , boost::bind(&Frame::name, _1) == name
- );
-
- std::vector<Frame>::iterator::difference_type it_diff = it - frames.begin();
- return getFrameType(Model::JointIndex(it_diff));
- }
-
- inline FrameType Model::getFrameType( const FrameIndex index ) const
- {
- return frames[index].type;
- }
-
- inline const SE3 & Model::getFramePlacement( const std::string & name) const
- {
- assert(existFrame(name) && "The Frame you requested does not exist");
- std::vector<Frame>::const_iterator it = std::find_if( frames.begin()
- , frames.end()
- , boost::bind(&Frame::name, _1) == name
- );
-
- std::vector<Frame>::iterator::difference_type it_diff = it - frames.begin();
- return getFramePlacement(Model::Index(it_diff));
- }
-
- inline const SE3 & Model::getFramePlacement( const FrameIndex index ) const
- {
- return frames[index].placement;
- }
-
- inline bool Model::addFrame ( const Frame & frame )
- {
- if( !existFrame(frame.name) )
+ if( !existFrame(frame.name, frame.type) )
{
frames.push_back(frame);
- nFrames++;
- return true;
+ nframes++;
+ return nframes - 1;
}
else
{
- return false;
+ return -1;
}
}
-
- inline bool Model::addFrame ( const std::string & name, JointIndex index, const SE3 & placement, const FrameType type)
- {
- if( !existFrame(name) )
- return addFrame(Frame(name, index, placement, type));
- else
- return false;
- }
inline void Model::addJointIndexToParentSubtrees(const JointIndex joint_id)
{
@@ -239,35 +231,35 @@ namespace se3
inline Data::Data (const Model & model)
:joints(0)
- ,a((std::size_t)model.njoint)
- ,a_gf((std::size_t)model.njoint)
- ,v((std::size_t)model.njoint)
- ,f((std::size_t)model.njoint)
- ,oMi((std::size_t)model.njoint)
- ,liMi((std::size_t)model.njoint)
+ ,a((std::size_t)model.njoints)
+ ,a_gf((std::size_t)model.njoints)
+ ,v((std::size_t)model.njoints)
+ ,f((std::size_t)model.njoints)
+ ,oMi((std::size_t)model.njoints)
+ ,liMi((std::size_t)model.njoints)
,tau(model.nv)
,nle(model.nv)
- ,oMf((std::size_t)model.nFrames)
- ,Ycrb((std::size_t)model.njoint)
+ ,oMf((std::size_t)model.nframes)
+ ,Ycrb((std::size_t)model.njoints)
,M(model.nv,model.nv)
,ddq(model.nv)
- ,Yaba((std::size_t)model.njoint)
+ ,Yaba((std::size_t)model.njoints)
,u(model.nv)
,Ag(6,model.nv)
- ,Fcrb((std::size_t)model.njoint)
- ,lastChild((std::size_t)model.njoint)
- ,nvSubtree((std::size_t)model.njoint)
+ ,Fcrb((std::size_t)model.njoints)
+ ,lastChild((std::size_t)model.njoints)
+ ,nvSubtree((std::size_t)model.njoints)
,U(model.nv,model.nv)
,D(model.nv)
,tmp(model.nv)
,parents_fromRow((std::size_t)model.nv)
,nvSubtree_fromRow((std::size_t)model.nv)
,J(6,model.nv)
- ,iMf((std::size_t)model.njoint)
- ,com((std::size_t)model.njoint)
- ,vcom((std::size_t)model.njoint)
- ,acom((std::size_t)model.njoint)
- ,mass((std::size_t)model.njoint)
+ ,iMf((std::size_t)model.njoints)
+ ,com((std::size_t)model.njoints)
+ ,vcom((std::size_t)model.njoints)
+ ,acom((std::size_t)model.njoints)
+ ,mass((std::size_t)model.njoints)
,Jcom(3,model.nv)
,JMinvJt()
,llt_JMinvJt()
@@ -278,12 +270,12 @@ namespace se3
,impulse_c()
{
/* Create data strcture associated to the joints */
- for(Model::Index i=0;i<(Model::JointIndex)(model.njoint);++i)
+ for(Model::Index i=0;i<(Model::JointIndex)(model.njoints);++i)
joints.push_back(CreateJointData::run(model.joints[i]));
/* Init for CRBA */
M.fill(0);
- for(Model::Index i=0;i<(Model::Index)(model.njoint);++i ) { Fcrb[i].resize(6,model.nv); }
+ for(Model::Index i=0;i<(Model::Index)(model.njoints);++i ) { Fcrb[i].resize(6,model.nv); }
computeLastChild(model);
/* Init for Cholesky */
@@ -308,7 +300,7 @@ namespace se3
{
typedef Model::Index Index;
std::fill(lastChild.begin(),lastChild.end(),-1);
- for( int i=model.njoint-1;i>=0;--i )
+ for( int i=model.njoints-1;i>=0;--i )
{
if(lastChild[(Index)i] == -1) lastChild[(Index)i] = i;
const Index & parent = model.parents[(Index)i];
@@ -322,7 +314,7 @@ namespace se3
inline void Data::computeParents_fromRow (const Model & model)
{
- for( Model::Index joint=1;joint<(Model::Index)(model.njoint);joint++)
+ for( Model::Index joint=1;joint<(Model::Index)(model.njoints);joint++)
{
const Model::Index & parent = model.parents[joint];
const int nvj = nv (model.joints[joint]);
diff --git a/src/parsers/lua.cpp b/src/parsers/lua.cpp
index 48dbcbda30c27a51fb58ca20db15cf3ee2ca6d57..085ba645fddc3d3632d166ff50a8346c287bf25b 100644
--- a/src/parsers/lua.cpp
+++ b/src/parsers/lua.cpp
@@ -142,8 +142,10 @@ namespace se3
idx = model.addJoint(parent_id,jmodel,
joint_placement,joint_name);
- model.appendBodyToJoint(idx,Y,SE3::Identity(),body_name);
-
+ model.addJointFrame(idx);
+ model.appendBodyToJoint(idx,Y);
+ model.addBodyFrame(body_name, idx);
+
return idx;
}
@@ -295,9 +297,13 @@ namespace se3
}
else if (joint_type == "JointTypeFixed")
{
- model.appendBodyToJoint(parent_id, Y, global_placement, "");
+ model.appendBodyToJoint(parent_id, Y, global_placement);
+ // TODO Why not
+ // model.addBodyFrame(body_name, parent_id, global_placement);
+ // ???
+ model.addBodyFrame(randomStringGenerator(8), parent_id, global_placement);
- joint_id = (Model::JointIndex)model.njoint;
+ joint_id = (Model::JointIndex)model.njoints;
fixed_body_table_id_map[body_name] = parent_id;
fixed_placement_map[body_name] = global_placement;
diff --git a/src/parsers/sample-models.cpp b/src/parsers/sample-models.cpp
index c46a64f310d684baf2f44318bf5a6a86f7e4a493..9ebf0496bcffeb804076c03088d7b09f1cc11387 100644
--- a/src/parsers/sample-models.cpp
+++ b/src/parsers/sample-models.cpp
@@ -22,112 +22,92 @@ namespace se3
{
namespace buildModels
{
+ 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 std::string & name,
+ const SE3 placement = SE3::Random(),
+ bool setRandomLimits = true)
{
typedef typename JointModel::ConfigVector_t CV;
typedef typename JointModel::TangentVector_t TV;
Model::JointIndex idx;
- idx = model.addJoint(model.getJointId(parent_name),joint,
- SE3::Random(),name + "_joint",
+ if (setRandomLimits)
+ idx = model.addJoint(model.getJointId(parent_name),joint,
+ SE3::Random(),
TV::Random() + TV::Constant(1),
TV::Random() + TV::Constant(1),
CV::Random() - CV::Constant(1),
- CV::Random() + CV::Constant(1));
+ CV::Random() + CV::Constant(1),
+ name + "_joint");
+ else
+ idx = model.addJoint(model.getJointId(parent_name),joint,
+ placement, name + "_joint");
+
+ model.addJointFrame(idx);
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),name + "_body");
+ model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
+ model.addBodyFrame(name + "_body", idx);
}
void humanoid2d(Model & model)
{
- Model::JointIndex idx;
-
// root
- idx = model.addJoint(model.getJointId("universe"),JointModelRX(),SE3::Identity(),"ff1_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"ff1_body");
-
- idx = model.addJoint(model.getJointId("ff1_joint"),JointModelRY(),SE3::Identity(),"root_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"root_body");
+ addJointAndBody(model, JointModelRX(), "universe", "ff1", Id, false);
+ addJointAndBody(model, JointModelRY(), "ff1_joint", "root", Id, false);
// lleg
- idx = model.addJoint(model.getJointId("root_joint"),JointModelRZ(),SE3::Identity(),"lleg1_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"lleg1_body");
-
- idx = model.addJoint(model.getJointId("lleg1_joint"),JointModelRY(),SE3::Identity(),"lleg2_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"lleg2_body");
+ addJointAndBody(model, JointModelRZ(), "root_joint", "lleg1", Id, false);
+ addJointAndBody(model, JointModelRY(), "lleg1_joint", "lleg2", Id, false);
// rlgg
- idx = model.addJoint(model.getJointId("root_joint"),JointModelRZ(),SE3::Identity(),"rleg1_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"rleg1_body");
-
- idx = model.addJoint(model.getJointId("rleg1_joint"),JointModelRY(),SE3::Identity(),"rleg2_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"rleg2_body");
+ addJointAndBody(model, JointModelRZ(), "root_joint", "rleg1", Id, false);
+ addJointAndBody(model, JointModelRY(), "lleg1_joint", "rleg2", Id, false);
// torso
- idx = model.addJoint(model.getJointId("root_joint"),JointModelRY(),SE3::Identity(),"torso1_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"torso1_body");
-
- idx = model.addJoint(model.getJointId("torso1_joint"),JointModelRZ(),SE3::Identity(),"chest_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"chest_body");
+ addJointAndBody(model, JointModelRY(), "root_joint", "torso1", Id, false);
+ addJointAndBody(model, JointModelRZ(), "torso1_joint", "chest", Id, false);
// rarm
- idx = model.addJoint(model.getJointId("chest_joint"),JointModelRX(),SE3::Identity(),"rarm1_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"rarm1_body");
-
- idx = model.addJoint(model.getJointId("rarm1_joint"),JointModelRZ(),SE3::Identity(),"rarm2_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"rarm2_body");
+ addJointAndBody(model, JointModelRX(), "chest_joint", "rarm1", Id, false);
+ addJointAndBody(model, JointModelRZ(), "rarm1_joint", "rarm2", Id, false);
// larm
- idx = model.addJoint(model.getJointId("chest_joint"),JointModelRX(),SE3::Identity(),"larm1_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"larm1_body");
-
- idx = model.addJoint(model.getJointId("larm1_joint"),JointModelRZ(),SE3::Identity(),"larm2_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"larm2_body");
+ addJointAndBody(model, JointModelRX(), "root_joint", "larm1", Id, false);
+ addJointAndBody(model, JointModelRZ(), "larm1_joint", "larm2", Id, false);
}
void humanoidSimple(Model & model, bool usingFF)
{
- Model::JointIndex idx;
-
// root
if(! usingFF )
{
- idx = model.addJoint(model.getJointId("universe"),JointModelRX(),SE3::Identity(),"ff1_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"ff1_body");
-
- idx = model.addJoint(model.getJointId("ff1_joint"),JointModelRY(),SE3::Identity(),"ff2_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"ff2_body");
-
- idx = model.addJoint(model.getJointId("ff2_joint"),JointModelRZ(),SE3::Identity(),"ff3_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"ff3_body");
-
- idx = model.addJoint(model.getJointId("ff3_joint"),JointModelRZ(),SE3::Random(),"ff4_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"ff4_body");
-
- idx = model.addJoint(model.getJointId("ff4_joint"),JointModelRY(),SE3::Identity(),"ff5_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"ff5_body");
-
- idx = model.addJoint(model.getJointId("ff5_joint"),JointModelRX(),SE3::Identity(),"root_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"root_body");
+ addJointAndBody(model, JointModelRX(), "universe", "ff1", Id, false);
+ addJointAndBody(model, JointModelRY(), "ff1_joint", "ff2", Id, false);
+ addJointAndBody(model, JointModelRZ(), "ff2_joint", "ff3", Id, false);
+ addJointAndBody(model, JointModelRZ(), "ff3_joint", "ff4", Id, false);
+ addJointAndBody(model, JointModelRY(), "ff4_joint", "ff5", Id, false);
+ addJointAndBody(model, JointModelRX(), "ff5_joint", "root", Id, false);
}
else
{
- typedef JointModelFreeFlyer::ConfigVector_t CV;
- typedef JointModelFreeFlyer::TangentVector_t TV;
+ // typedef JointModelFreeFlyer::ConfigVector_t CV;
+ // typedef JointModelFreeFlyer::TangentVector_t TV;
- idx = model.addJoint(model.getJointId("universe"),JointModelFreeFlyer(),
- SE3::Identity(),"root_joint",
- TV::Zero(), 1e3 * (TV::Random() + TV::Constant(1.)),
- 1e3 * (CV::Random() - CV::Constant(1)),
- 1e3 * (CV::Random() + CV::Constant(1)));
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"root_body");
+ addJointAndBody(model, JointModelFreeFlyer(), "universe", "root", Id, false);
+ // idx = model.addJoint(model.getJointId("universe"),JointModelFreeFlyer(),
+ // SE3::Identity(),"root_joint",
+ // TV::Zero(), 1e3 * (TV::Random() + TV::Constant(1.)),
+ // 1e3 * (CV::Random() - CV::Constant(1)),
+ // 1e3 * (CV::Random() + CV::Constant(1)));
+ // model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"root_body");
}
// lleg
diff --git a/src/parsers/srdf.hpp b/src/parsers/srdf.hpp
index 6c03878f20636e2365a6b7006b34d93872cf411a..e6cfd0202cef6fcabd5373bc27322a674c55188b 100644
--- a/src/parsers/srdf.hpp
+++ b/src/parsers/srdf.hpp
@@ -39,13 +39,13 @@ namespace se3
/// It throws if the SRDF file is incorrect.
///
/// \param[in] model Model of the kinematic tree.
- /// \param[in] model_geom Model of the geometries.
+ /// \param[in] geomModel Model of the geometries.
/// \param[out] data_geom Data containing the active collision pairs.
/// \param[in] filename The complete path to the SRDF file.
/// \param[in] verbose Verbosity mode (print removed collision pairs and undefined link inside the model).
///
inline void removeCollisionPairsFromSrdf(const Model& model,
- GeometryModel & model_geom,
+ GeometryModel & geomModel,
const std::string & filename,
const bool verbose) throw (std::invalid_argument)
{
@@ -90,24 +90,38 @@ namespace se3
const Model::JointIndex joint_id1 = model.frames[frame_id1].parent;
const Model::JointIndex frame_id2 = model.getBodyId(link2);
const Model::JointIndex joint_id2 = model.frames[frame_id2].parent;
-
- typedef GeometryModel::GeomIndexList GeomIndexList;
- const GeomIndexList & innerObject1 = model_geom.innerObjects.at(joint_id1);
- const GeomIndexList & innerObject2 = model_geom.innerObjects.at(joint_id2);
-
- for(GeomIndexList::const_iterator it1 = innerObject1.begin();
- it1 != innerObject1.end();
- ++it1)
+
+ // Malformed SRDF
+ if (frame_id1 == frame_id2)
{
- for(GeomIndexList::const_iterator it2 = innerObject2.begin();
- it2 != innerObject2.end();
- ++it2)
- {
- model_geom.removeCollisionPair(CollisionPair(*it1, *it2));
- if(verbose)
- std::cout << "Remove collision pair (" << joint_id1 << "," << joint_id2 << ")" << std::endl;
+ if (verbose)
+ std::cout << "Cannot disable collision between " << link1 << " and " << link2 << std::endl;
+ continue;
+ }
+
+ typedef std::vector<CollisionPair> CollisionPairs_t;
+ bool didRemove = false;
+ for(CollisionPairs_t::iterator _colPair = geomModel.collisionPairs.begin();
+ _colPair != geomModel.collisionPairs.end(); ) {
+ const CollisionPair& colPair (*_colPair);
+ bool remove =
+ (
+ (geomModel.geometryObjects[colPair.first ].parentFrame == frame_id1)
+ && (geomModel.geometryObjects[colPair.second].parentFrame == frame_id2)
+ ) || (
+ (geomModel.geometryObjects[colPair.second].parentFrame == frame_id1)
+ && (geomModel.geometryObjects[colPair.first ].parentFrame == frame_id2)
+ );
+
+ if (remove) {
+ _colPair = geomModel.collisionPairs.erase(_colPair);
+ didRemove = true;
+ } else {
+ ++_colPair;
}
}
+ if(didRemove && verbose)
+ std::cout << "Remove collision pair (" << link1 << "," << link2 << ")" << std::endl;
}
} // BOOST_FOREACH
diff --git a/src/parsers/urdf.hpp b/src/parsers/urdf.hpp
index 4eb54c72ffe211b1a60bfb912d3fa3190e05e629..01955134ebd97098a9db84e3a034d2672a3bac9e 100644
--- a/src/parsers/urdf.hpp
+++ b/src/parsers/urdf.hpp
@@ -48,31 +48,16 @@ namespace se3
/// the model given as reference argument.
///
/// \param[in] filemane The URDF complete file path.
- /// \param[in] root_joint The joint at the root of the model tree.
+ /// \param[in] rootJoint The joint at the root of the model tree.
/// \param[in] verbose Print parsing info.
/// \param[out] model Reference model where to put the parsed information.
/// \return Return the reference on argument model for convenience.
///
Model& buildModel (const std::string & filename,
- const JointModelVariant & root_joint,
+ const JointModelVariant & rootJoint,
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] root_joint 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 & root_joint,
- const bool verbose = false)
- throw (std::invalid_argument)
- { Model m; return buildModel(filename,root_joint,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
@@ -109,7 +80,7 @@ namespace se3
* @param[in] model The model of the robot, built with
* urdf::buildModel
* @param[in] filename The URDF complete (absolute) file path
- * @param[in] package_dirs A vector containing the different directories
+ * @param[in] packageDirs A vector containing the different directories
* where to search for models and meshes, typically
* obtained from calling se3::rosPaths()
*
@@ -125,33 +96,9 @@ namespace se3
const std::string & filename,
const GeometryType type,
GeometryModel & geomModel,
- const std::vector<std::string> & package_dirs = std::vector<std::string> ())
+ 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] package_dirs 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, or NONE)
- *
- * @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 std::vector<std::string> & package_dirs = std::vector<std::string> (),
- const GeometryType type = NONE)
- throw (std::invalid_argument)
- { GeometryModel g; return buildGeom (model,filename,type,g,package_dirs); }
-
} // namespace urdf
} // namespace se3
diff --git a/src/parsers/urdf/geometry.cpp b/src/parsers/urdf/geometry.cpp
index 91abef569daf1a44fc804a11d4993816def0b8fb..96fd20a6c8cdcac30e8bc9dec7de310ff8127b14 100644
--- a/src/parsers/urdf/geometry.cpp
+++ b/src/parsers/urdf/geometry.cpp
@@ -48,13 +48,13 @@ namespace se3
*
* @param[in] urdf_geometry A shared pointer on the input urdf Geometry
* @param[in] package_dirs A vector containing the different directories where to search for packages
- * @param[out] mesh_path The Absolute path of the mesh currently read
+ * @param[out] meshPath The Absolute path of the mesh currently read
*
* @return A shared pointer on the he geometry converted as a fcl::CollisionGeometry
*/
boost::shared_ptr<fcl::CollisionGeometry> retrieveCollisionGeometry(const boost::shared_ptr< ::urdf::Geometry> urdf_geometry,
const std::vector<std::string> & package_dirs,
- std::string & mesh_path)
+ std::string & meshPath)
{
boost::shared_ptr<fcl::CollisionGeometry> geometry;
@@ -64,7 +64,7 @@ namespace se3
boost::shared_ptr < ::urdf::Mesh> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Mesh> (urdf_geometry);
std::string collisionFilename = collisionGeometry->filename;
- mesh_path = retrieveResourcePath(collisionFilename, package_dirs);
+ meshPath = retrieveResourcePath(collisionFilename, package_dirs);
fcl::Vec3f scale = fcl::Vec3f(collisionGeometry->scale.x,
collisionGeometry->scale.y,
@@ -74,7 +74,7 @@ namespace se3
// Create FCL mesh by parsing Collada file.
PolyhedronPtrType polyhedron (new PolyhedronType);
- fcl::loadPolyhedronFromResource (mesh_path, scale, polyhedron);
+ fcl::loadPolyhedronFromResource (meshPath, scale, polyhedron);
geometry = polyhedron;
}
@@ -82,7 +82,7 @@ namespace se3
// Use FCL capsules for cylinders
else if (urdf_geometry->type == ::urdf::Geometry::CYLINDER)
{
- mesh_path = "CYLINDER";
+ meshPath = "CYLINDER";
boost::shared_ptr < ::urdf::Cylinder> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Cylinder> (urdf_geometry);
double radius = collisionGeometry->radius;
@@ -94,7 +94,7 @@ namespace se3
// Handle the case where collision geometry is a box.
else if (urdf_geometry->type == ::urdf::Geometry::BOX)
{
- mesh_path = "BOX";
+ meshPath = "BOX";
boost::shared_ptr < ::urdf::Box> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Box> (urdf_geometry);
double x = collisionGeometry->dim.x;
@@ -106,7 +106,7 @@ namespace se3
// Handle the case where collision geometry is a sphere.
else if (urdf_geometry->type == ::urdf::Geometry::SPHERE)
{
- mesh_path = "SPHERE";
+ meshPath = "SPHERE";
boost::shared_ptr < ::urdf::Sphere> collisionGeometry = boost::dynamic_pointer_cast< ::urdf::Sphere> (urdf_geometry);
double radius = collisionGeometry->radius;
@@ -181,60 +181,63 @@ namespace se3
template<typename T>
inline void addLinkGeometryToGeomModel(::urdf::LinkConstPtr link,
const Model & model,
- GeometryModel & geom_model,
+ GeometryModel & geomModel,
const std::vector<std::string> & package_dirs) throw (std::invalid_argument)
{
if(getLinkGeometry<T>(link))
{
- std::string mesh_path = "";
+ std::string meshPath = "";
std::string link_name = link->name;
- assert(link->getParent()!=NULL);
- if (link->getParent() == NULL)
- {
- const std::string exception_message (link->name + " - joint information missing.");
- throw std::invalid_argument(exception_message);
- }
std::vector< boost::shared_ptr< T > > geometries_array = getLinkGeometryArray<T>(link);
+ if (!model.existFrame(link_name, BODY))
+ throw std::invalid_argument("No link " + link_name + " in model");
+ FrameIndex frame_id = model.getFrameId(link_name, BODY);
+ SE3 body_placement = model.frames[frame_id].placement;
+ assert(model.frames[frame_id].type == BODY);
+
std::size_t objectId = 0;
for (typename std::vector< boost::shared_ptr< T > >::const_iterator i = geometries_array.begin();i != geometries_array.end(); ++i)
{
+ meshPath.clear();
#ifdef WITH_HPP_FCL
- const boost::shared_ptr<fcl::CollisionGeometry> geometry = retrieveCollisionGeometry((*i)->geometry, package_dirs, mesh_path);
+ const boost::shared_ptr<fcl::CollisionGeometry> geometry = retrieveCollisionGeometry((*i)->geometry, package_dirs, meshPath);
#else
boost::shared_ptr < ::urdf::Mesh> urdf_mesh = boost::dynamic_pointer_cast< ::urdf::Mesh> ((*i)->geometry);
- if (urdf_mesh) mesh_path = retrieveResourcePath(urdf_mesh->filename, package_dirs);
+ if (urdf_mesh) meshPath = retrieveResourcePath(urdf_mesh->filename, package_dirs);
const boost::shared_ptr<fcl::CollisionGeometry> geometry(new fcl::CollisionGeometry());
#endif // WITH_HPP_FCL
- SE3 geomPlacement = convertFromUrdf((*i)->origin);
+ SE3 geomPlacement = body_placement * convertFromUrdf((*i)->origin);
std::ostringstream geometry_object_suffix;
geometry_object_suffix << "_" << objectId;
const std::string & geometry_object_name = std::string(link_name + geometry_object_suffix.str());
- geom_model.addGeometryObject(model.getFrameParent(link_name), geometry, geomPlacement, geometry_object_name, mesh_path);
- ++objectId;
+ geomModel.addGeometryObject(GeometryObject(geometry_object_name,
+ frame_id, model.frames[frame_id].parent,
+ geometry,
+ geomPlacement, meshPath),
+ model);
+ ++objectId;
}
}
}
-
-
/**
* @brief Recursive procedure for reading the URDF tree, looking for geometries
* This function fill the geometric model whith geometry objects retrieved from the URDF tree
*
* @param[in] link The current URDF link
* @param model The model to which is the GeometryModel associated
- * @param model_geom The GeometryModel where the Collision Objects must be added
+ * @param geomModel The GeometryModel where the Collision Objects must be added
* @param[in] package_dirs A vector containing the different directories where to search for packages
* @param[in] type The type of objects that must be loaded ( can be VISUAL or COLLISION)
*/
void parseTreeForGeom(::urdf::LinkConstPtr link,
const Model & model,
- GeometryModel & geom_model,
+ GeometryModel & geomModel,
const std::vector<std::string> & package_dirs,
const GeometryType type) throw (std::invalid_argument)
{
@@ -242,10 +245,10 @@ namespace se3
switch(type)
{
case COLLISION:
- addLinkGeometryToGeomModel< ::urdf::Collision >(link, model, geom_model, package_dirs);
+ addLinkGeometryToGeomModel< ::urdf::Collision >(link, model, geomModel, package_dirs);
break;
case VISUAL:
- addLinkGeometryToGeomModel< ::urdf::Visual >(link, model, geom_model, package_dirs);
+ addLinkGeometryToGeomModel< ::urdf::Visual >(link, model, geomModel, package_dirs);
break;
default:
break;
@@ -253,7 +256,7 @@ namespace se3
BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
{
- parseTreeForGeom(child, model, geom_model, package_dirs,type);
+ parseTreeForGeom(child, model, geomModel, package_dirs,type);
}
}
@@ -265,16 +268,10 @@ namespace se3
GeometryModel& buildGeom(const Model & model,
const std::string & filename,
const GeometryType type,
- GeometryModel & model_geom,
+ GeometryModel & geomModel,
const std::vector<std::string> & package_dirs)
throw(std::invalid_argument)
{
- if (type == NONE)
- {
- const std::string exception_message ("You must specify if you want to load VISUAL or COLLISION meshes");
- throw std::invalid_argument(exception_message);
- }
-
std::vector<std::string> hint_directories(package_dirs);
// Append the ROS_PACKAGE_PATH
@@ -287,8 +284,8 @@ namespace se3
}
::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDFFile (filename);
- details::parseTreeForGeom(urdfTree->getRoot(), model, model_geom, hint_directories,type);
- return model_geom;
+ details::parseTreeForGeom(urdfTree->getRoot(), model, geomModel, hint_directories,type);
+ return geomModel;
}
} // namespace urdf
diff --git a/src/parsers/urdf/model.cpp b/src/parsers/urdf/model.cpp
index 47bcb230488148187027f8572a43e40f3be69593..d7799c7c3995ab80d659e4059f0ac62ec83fe19d 100644
--- a/src/parsers/urdf/model.cpp
+++ b/src/parsers/urdf/model.cpp
@@ -16,6 +16,7 @@
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
+#include "pinocchio/math/matrix.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/urdf/utils.hpp"
#include "pinocchio/multibody/model.hpp"
@@ -33,41 +34,121 @@ namespace se3
{
namespace details
{
+ const FrameType JOINT_OR_FIXED_JOINT = (FrameType) (JOINT | FIXED_JOINT);
+
+ FrameIndex getParentJointFrame(::urdf::LinkConstPtr link, Model & model)
+ {
+ assert(link!=NULL && link->getParent()!=NULL);
+
+ FrameIndex id;
+ if (link->getParent()->parent_joint==NULL) {
+ if (model.existFrame("root_joint", JOINT_OR_FIXED_JOINT))
+ id = model.getFrameId ("root_joint", JOINT_OR_FIXED_JOINT);
+ else
+ id = 0;
+ } else {
+ if (model.existFrame(link->getParent()->parent_joint->name, JOINT_OR_FIXED_JOINT))
+ id = model.getFrameId (link->getParent()->parent_joint->name, JOINT_OR_FIXED_JOINT);
+ else
+ throw std::invalid_argument ("Model does not have any joints named "
+ + link->getParent()->parent_joint->name);
+ }
+
+ Frame& f = model.frames[id];
+ if (f.type == JOINT || f.type == FIXED_JOINT)
+ return id;
+ throw std::invalid_argument ("Parent frame is not a JOINT neither a FIXED_JOINT");
+ }
+
+ void appendBodyToJoint(Model& model, const FrameIndex fid,
+ const boost::shared_ptr< ::urdf::Inertial> Y,
+ const SE3 & placement,
+ const std::string & body_name)
+ {
+ const Frame& frame = model.frames[fid];
+ const SE3& p = frame.placement * placement;
+ if (frame.parent > 0
+ && Y != NULL
+ && Y->mass > Eigen::NumTraits<double>::epsilon()) {
+ model.appendBodyToJoint(frame.parent, convertFromUrdf(*Y), p);
+ }
+ model.addBodyFrame(body_name, frame.parent, p, (int)fid);
+ // Reference to model.frames[fid] can has changed because the vector
+ // may have been reallocated.
+ if (model.frames[fid].parent > 0) {
+ assert (!hasNaN(model.inertias[model.frames[fid].parent].lever())
+ && !hasNaN(model.inertias[model.frames[fid].parent].inertia().data()));
+ }
+ }
+
///
/// \brief Shortcut for adding a joint and directly append a body to it.
///
template<typename JointModel>
- void addJointAndBody(Model & model, const JointModelBase<JointModel> & jmodel, const Model::JointIndex parent_id,
+ void addJointAndBody(Model & model, const JointModelBase<JointModel> & jmodel, const FrameIndex& parentFrameId,
const SE3 & joint_placement, const std::string & joint_name,
- const Inertia & Y, const std::string & body_name,
+ const boost::shared_ptr< ::urdf::Inertial> Y,
+ const std::string & body_name,
const typename JointModel::TangentVector_t & max_effort = JointModel::TangentVector_t::Constant(std::numeric_limits<double>::max()),
const typename JointModel::TangentVector_t & max_velocity = JointModel::TangentVector_t::Constant(std::numeric_limits<double>::max()),
const typename JointModel::ConfigVector_t & min_config = JointModel::ConfigVector_t::Constant(-std::numeric_limits<double>::min()),
const typename JointModel::ConfigVector_t & max_config = JointModel::ConfigVector_t::Constant(std::numeric_limits<double>::max()))
{
Model::JointIndex idx;
+ Frame& frame = model.frames[parentFrameId];
- idx = model.addJoint(parent_id,jmodel,
- joint_placement,joint_name,
- max_effort,max_velocity,
- min_config,max_config);
-
- model.appendBodyToJoint(idx,Y,SE3::Identity(),body_name);
+ idx = model.addJoint(frame.parent,jmodel,
+ frame.placement * joint_placement,
+ max_effort,max_velocity,min_config,max_config,
+ joint_name);
+ FrameIndex jointFrameId = (FrameIndex) model.addJointFrame(idx, (int)parentFrameId); // C-style cast to remove polluting compilation warning. This is Bad practice. See issue #323 (rework indexes)
+ appendBodyToJoint(model, jointFrameId, Y, SE3::Identity(), body_name);
}
///
/// \brief Handle the case of JointModelDense which is dynamic.
///
- void addJointAndBody(Model & model, const JointModelBase< JointModelDense<-1,-1> > & jmodel, const Model::JointIndex parent_id,
+ void addJointAndBody(Model & , const JointModelBase< JointModelDense<-1,-1> > & , const FrameIndex& ,
+ const SE3 & , const std::string & ,
+ const boost::shared_ptr< ::urdf::Inertial> ,
+ const std::string & )
+ {
+ assert(false && "Cannot add a joint of type JointModelDense");
+ }
+
+ ///
+ /// \brief Shortcut for adding a fixed joint and directly append a body to it.
+ ///
+ void addFixedJointAndBody(Model & model, const FrameIndex& parentFrameId,
+ const SE3 & joint_placement, const std::string & joint_name,
+ const boost::shared_ptr< ::urdf::Inertial> Y,
+ const std::string & body_name)
+ {
+ Model::JointIndex idx;
+ Frame& frame = model.frames[parentFrameId];
+
+ int fid = model.addFrame(
+ Frame (joint_name, frame.parent, parentFrameId,
+ frame.placement * joint_placement, FIXED_JOINT)
+ );
+ if (fid < 0)
+ throw std::invalid_argument ("Fixed joint " + joint_name + " could not be added.");
+ appendBodyToJoint(model, (FrameIndex)fid, Y, SE3::Identity(), body_name);
+ }
+
+ ///
+ /// \brief Handle the case of JointModelComposite which is dynamic.
+ ///
+ void addJointAndBody(Model & model, const JointModelBase< JointModelComposite > & jmodel, const Model::JointIndex parent_id,
const SE3 & joint_placement, const std::string & joint_name,
- const Inertia & Y, const std::string & body_name)
+ const boost::shared_ptr< ::urdf::Inertial> Y, const std::string & body_name)
{
Model::JointIndex idx;
idx = model.addJoint(parent_id,jmodel,
joint_placement,joint_name);
- model.appendBodyToJoint(idx,Y,SE3::Identity(),body_name);
+ appendBodyToJoint(model,idx,Y,SE3::Identity(),body_name);
}
///
@@ -76,17 +157,13 @@ namespace se3
///
/// \param[in] link The current URDF link.
/// \param[in] model The model where the link must be added.
- /// \param[in] placementOffset The relative placement of the link relative to the closer non fixed joint in the tree.
///
- void parseTree(::urdf::LinkConstPtr link, Model & model, const SE3 & placementOffset, bool verbose) throw (std::invalid_argument)
+ void parseTree(::urdf::LinkConstPtr link, Model & model, bool verbose) throw (std::invalid_argument)
{
// Parent joint of the current body
::urdf::JointConstPtr joint = link->parent_joint;
- // OffSet of the next link. In case we encounter a fixed joint, we need to propagate the offset placement of its attached body to next joint.
- SE3 nextPlacementOffset = SE3::Identity();
-
if(joint != NULL) // if the link is not the root of the tree
{
assert(link->getParent()!=NULL);
@@ -96,21 +173,12 @@ namespace se3
const std::string & parent_link_name = link->getParent()->name;
std::ostringstream joint_info;
- // check if inertial information is provided
- if (!link->inertial && joint->type != ::urdf::Joint::FIXED)
- {
- const std::string exception_message (link->name + " - spatial inertial information missing.");
- throw std::invalid_argument(exception_message);
- }
-
- Model::JointIndex parent_joint_id = (link->getParent()->parent_joint==NULL) ? (model.existJointName("root_joint") ? model.getJointId("root_joint") : 0) :
- model.getJointId( link->getParent()->parent_joint->name );
+ FrameIndex parentFrameId = getParentJointFrame(link, model);
// Transformation from the parent link to the joint origin
- const SE3 & jointPlacement = placementOffset*convertFromUrdf(joint->parent_to_joint_origin_transform);
-
- const Inertia & Y = (link->inertial)?convertFromUrdf(*link->inertial):Inertia::Zero();
+ const SE3 jointPlacement = convertFromUrdf(joint->parent_to_joint_origin_transform);
+ const boost::shared_ptr< ::urdf::Inertial> Y = link->inertial;
switch(joint->type)
{
@@ -118,7 +186,7 @@ namespace se3
{
joint_info << "joint FreeFlyer";
addJointAndBody(model,JointModelFreeFlyer(),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name);
break;
@@ -153,7 +221,7 @@ namespace se3
{
joint_info << " along X";
addJointAndBody(model,JointModelRX(),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -163,7 +231,7 @@ namespace se3
{
joint_info << " along Y";
addJointAndBody(model,JointModelRY(),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -173,7 +241,7 @@ namespace se3
{
joint_info << " along Z";
addJointAndBody(model,JointModelRZ(),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -186,7 +254,7 @@ namespace se3
joint_info << " unaligned along (" << joint_axis.transpose() << ")";
addJointAndBody(model,JointModelRevoluteUnaligned(joint_axis.normalized()),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -228,7 +296,7 @@ namespace se3
{
joint_info << " along X";
addJointAndBody(model,JointModelRX(),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -238,7 +306,7 @@ namespace se3
{
joint_info << " along Y";
addJointAndBody(model,JointModelRY(),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -248,7 +316,7 @@ namespace se3
{
joint_info << " along Z";
addJointAndBody(model,JointModelRZ(),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -261,7 +329,7 @@ namespace se3
joint_info << " unaligned along (" << joint_axis.transpose() << ")";
addJointAndBody(model,JointModelRevoluteUnaligned(joint_axis.normalized()),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -302,7 +370,7 @@ namespace se3
{
joint_info << " along X";
addJointAndBody(model,JointModelPX(),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -312,7 +380,7 @@ namespace se3
{
joint_info << " along Y";
addJointAndBody(model,JointModelPY(),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -322,7 +390,7 @@ namespace se3
{
joint_info << " along Z";
addJointAndBody(model,JointModelPZ(),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -335,7 +403,7 @@ namespace se3
joint_info << " unaligned along (" << joint_axis.transpose() << ")";
addJointAndBody(model,JointModelPrismaticUnaligned(joint_axis.normalized()),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -370,7 +438,7 @@ namespace se3
}
addJointAndBody(model,JointModelPlanar(),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -388,23 +456,9 @@ namespace se3
// -add fixed body in model to display it in gepetto-viewer
joint_info << "fixed joint";
- if (link->inertial)
- model.appendBodyToJoint(parent_joint_id, Y, jointPlacement, link->name); //Modify the parent inertia in the model
- else
- model.addFrame(link->name, parent_joint_id, nextPlacementOffset, BODY);
-
- //transformation of the current placement offset
- nextPlacementOffset = jointPlacement;
-
- // Add a frame in the model to keep trace of this fixed joint
- model.addFrame(joint->name, parent_joint_id, nextPlacementOffset, FIXED_JOINT);
-
- //for the children of the current link, set their parent to be
- //the the parent of the current link.
- BOOST_FOREACH(::urdf::LinkPtr child_link, link->child_links)
- {
- child_link->setParent(link->getParent() );
- }
+ addFixedJointAndBody(model, parentFrameId, jointPlacement,
+ joint_name, Y, link_name);
+
break;
}
default:
@@ -417,14 +471,15 @@ namespace se3
if (verbose)
{
+ const Inertia YY = (Y==NULL) ? Inertia::Zero() : convertFromUrdf(*Y);
std::cout << "Adding Body" << std::endl;
std::cout << "\"" << link_name << "\" connected to " << "\"" << parent_link_name << "\" throw joint " << "\"" << joint_name << "\"" << std::endl;
std::cout << "joint type: " << joint_info << std::endl;
std::cout << "joint placement:\n" << jointPlacement;
std::cout << "body info: " << std::endl;
- std::cout << " " << "mass: " << Y.mass() << std::endl;
- std::cout << " " << "lever: " << Y.lever().transpose() << std::endl;
- std::cout << " " << "inertia elements (Ixx,Iyx,Iyy,Izx,Izy,Izz): " << Y.inertia().data().transpose() << std::endl << std::endl;
+ std::cout << " " << "mass: " << YY.mass() << std::endl;
+ std::cout << " " << "lever: " << YY.lever().transpose() << std::endl;
+ std::cout << " " << "inertia elements (Ixx,Iyx,Iyy,Izx,Izy,Izz): " << YY.inertia().data().transpose() << std::endl << std::endl;
}
}
else if (link->getParent() != NULL)
@@ -436,7 +491,7 @@ namespace se3
BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
{
- parseTree(child, model, nextPlacementOffset, verbose);
+ parseTree(child, model, verbose);
}
}
@@ -450,66 +505,17 @@ namespace se3
///
void parseRootTree (::urdf::LinkConstPtr root_link, Model & model, const bool verbose) throw (std::invalid_argument)
{
- // If the root link has no inertial info, it may be because it is a base_link.
- // In this case, we look for its child links which indeed contribute to the dynamics, they are not fixed to the universe.
- // TODO: it may be necessary to compute joint placement variable instead of setting it to SE3::Identity()
- if (!root_link->inertial)
- {
- typedef std::vector< ::urdf::LinkPtr > LinkSharedPtrVector_t;
- LinkSharedPtrVector_t movable_child_links;
- LinkSharedPtrVector_t direct_child_links(root_link->child_links);
- LinkSharedPtrVector_t next_direct_child_links; // next child to visit
- LinkSharedPtrVector_t pathologic_child_links; // children which have inertial info but rigidly attached to the world
- do
- {
- next_direct_child_links.clear();
- BOOST_FOREACH(::urdf::LinkPtr child, direct_child_links)
- {
- if (child->parent_joint->type != ::urdf::Joint::FIXED)
- movable_child_links.push_back(child);
- else
- {
- if (child->inertial)
- pathologic_child_links.push_back(child);
- next_direct_child_links.insert (next_direct_child_links.end(), child->child_links.begin(), child->child_links.end());
- }
-
- }
- direct_child_links = next_direct_child_links;
- }
- while (!direct_child_links.empty());
-
- if (!pathologic_child_links.empty())
- {
- std::cout << "[INFO] The links:" << std::endl;
- for (LinkSharedPtrVector_t::iterator it = pathologic_child_links.begin();
- it < pathologic_child_links.end(); ++it)
- {
- std::cout << " - " << (*it)->name << std::endl;
- }
- std::cout << "are fixed regarding to the universe (base_link) and convey inertial info. They won't affect the dynamics of the output model. Maybe, a root joint is missing connecting this links to the universe." << std::endl;
-
- }
-
- BOOST_FOREACH(::urdf::LinkPtr child, movable_child_links)
- {
- child->getParent()->parent_joint->name = model.names[0];
- parseTree(child, model, SE3::Identity(), verbose);
- }
-
- }
- else // Otherwise, we have to rase an exception because the first link will no be added to the model.
- // It seems a root joint is missing.
+ appendBodyToJoint(model, 0, root_link->inertial, SE3::Identity(), root_link->name);
+
+ BOOST_FOREACH(::urdf::LinkPtr child, root_link->child_links)
{
- std::cout << "[INFO] The root link " << root_link->name << " of the model tree contains inertial information. It seems that a root joint is missing connecting this root link to the universe. The root link won't affect the dynamics of the model." << std::endl;
-
- BOOST_FOREACH(::urdf::LinkPtr child, root_link->child_links)
- {
- parseTree(child, model, SE3::Identity(), verbose);
- }
+ parseTree(child, model, verbose);
}
-
-
+
+ // Inertias might be zero due to the URDF file. This is not a bug. However, it would lead to some
+ // algorithms of Pinocchio not to work.
+ // TODO: add an algorithm or a method in model to check the validity of the value wrt to the algorithms the user want to use.
+ // See also #306 on GitHub.
}
///
@@ -524,48 +530,16 @@ namespace se3
template <typename D>
void parseRootTree (::urdf::LinkConstPtr root_link, Model & model, const JointModelBase<D> & root_joint, const bool verbose) throw (std::invalid_argument)
{
- // If the root link has no inertial info (because it is a base_link for example),
- // we have to merge its inertial info with all its children connected to it with fixed joints
- if (!root_link->inertial)
- {
- // If the root link has only one child
- if (root_link->child_links.size() == 1)
- {
- ::urdf::LinkPtr child_link = root_link->child_links[0];
- assert(child_link->inertial != NULL && "Inertial information missing for parsing the root link.");
- const Inertia & Y = convertFromUrdf(*child_link->inertial);
- addJointAndBody(model,root_joint,
- 0,SE3::Identity(),"root_joint",
- Y,child_link->name);
-
- // Change the name of the parent joint in the URDF tree
- child_link->parent_joint->name = "root_joint";
-
- BOOST_FOREACH(::urdf::LinkConstPtr child, child_link->child_links)
- {
- parseTree(child, model, SE3::Identity(), verbose);
- }
- }
- else
- {
- const std::string exception_message (root_link->name + " has no inertial information and has more than one child link. It corresponds to a disjoint tree.");
- throw std::invalid_argument(exception_message);
- }
-
- }
- else // otherwise, it is a plain body with inertial info. It processes as usual.
+ addJointAndBody(model,root_joint,
+ 0,SE3::Identity(),"root_joint",
+ root_link->inertial,root_link->name);
+
+ BOOST_FOREACH(::urdf::LinkPtr child, root_link->child_links)
{
- const Inertia & Y = convertFromUrdf(*root_link->inertial);
- addJointAndBody(model,root_joint,
- 0,SE3::Identity(),"root_joint",
- Y,root_link->name);
-
- BOOST_FOREACH(::urdf::LinkPtr child, root_link->child_links)
- {
- parseTree(child, model, SE3::Identity(), verbose);
- }
+ parseTree(child, model, verbose);
}
+ // FIXME: See fixme in previous parseRootTree definition
}
} // namespace details
diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt
index 023d1b3db9153233922030630e9554e9a3d798a4..caaf613985a090492a6a6476227d2a7b82c3161c 100644
--- a/unittest/CMakeLists.txt
+++ b/unittest/CMakeLists.txt
@@ -115,3 +115,5 @@ ADD_UNIT_TEST(explog eigen3)
ADD_UNIT_TEST(finite-differences eigen3)
ADD_UNIT_TEST(visitor eigen3)
ADD_UNIT_TEST(algo-check eigen3)
+ADD_UNIT_TEST(joint-composite eigen3)
+
diff --git a/unittest/compute-all-terms.cpp b/unittest/compute-all-terms.cpp
index 368db7155c4f9ca2a3edc2c3f93aa564192642dd..9f8c9cb9c38dffd790600170549a5a9ccf6c6b90 100644
--- a/unittest/compute-all-terms.cpp
+++ b/unittest/compute-all-terms.cpp
@@ -73,7 +73,7 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
BOOST_CHECK (data.J.isApprox(data_other.J, 1e-12));
BOOST_CHECK (data.Jcom.isApprox(data_other.Jcom, 1e-12));
- for (int k=0; k<model.njoint; ++k)
+ for (int k=0; k<model.njoints; ++k)
{
BOOST_CHECK (data.com[(size_t)k].isApprox(data_other.com[(size_t)k], 1e-12));
BOOST_CHECK (data.vcom[(size_t)k].isApprox(data_other.vcom[(size_t)k], 1e-12));
@@ -103,7 +103,7 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
BOOST_CHECK (data.J.isApprox(data_other.J, 1e-12));
BOOST_CHECK (data.Jcom.isApprox(data_other.Jcom, 1e-12));
- for (int k=0; k<model.njoint; ++k)
+ for (int k=0; k<model.njoints; ++k)
{
BOOST_CHECK (data.com[(size_t)k].isApprox(data_other.com[(size_t)k], 1e-12));
BOOST_CHECK (data.vcom[(size_t)k].isApprox(data_other.vcom[(size_t)k], 1e-12));
@@ -134,7 +134,7 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
BOOST_CHECK (data.J.isApprox(data_other.J, 1e-12));
BOOST_CHECK (data.Jcom.isApprox(data_other.Jcom, 1e-12));
- for (int k=0; k<model.njoint; ++k)
+ for (int k=0; k<model.njoints; ++k)
{
BOOST_CHECK (data.com[(size_t)k].isApprox(data_other.com[(size_t)k], 1e-12));
BOOST_CHECK (data.vcom[(size_t)k].isApprox(data_other.vcom[(size_t)k], 1e-12));
@@ -165,7 +165,7 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
BOOST_CHECK (data.J.isApprox(data_other.J, 1e-12));
BOOST_CHECK (data.Jcom.isApprox(data_other.Jcom, 1e-12));
- for (int k=0; k<model.njoint; ++k)
+ for (int k=0; k<model.njoints; ++k)
{
BOOST_CHECK (data.com[(size_t)k].isApprox(data_other.com[(size_t)k], 1e-12));
BOOST_CHECK (data.vcom[(size_t)k].isApprox(data_other.vcom[(size_t)k], 1e-12));
diff --git a/unittest/finite-differences.cpp b/unittest/finite-differences.cpp
index 23fb794562d399c1e1d00f5f8622cbc7cd6e208b..deaf7b9b2ae02c7071c0bacb6d75a7ad39610305 100644
--- a/unittest/finite-differences.cpp
+++ b/unittest/finite-differences.cpp
@@ -129,6 +129,49 @@ void FiniteDiffJoint::init<JointModelPrismaticUnaligned>(JointModelBase<JointMod
template<>
void FiniteDiffJoint::operator()< JointModelDense<-1,-1> > (JointModelBase< JointModelDense<-1,-1> > & /*jmodel*/) const {}
+template<>
+void FiniteDiffJoint::operator()< JointModelComposite > (JointModelBase<JointModelComposite> & ) const
+{
+ typedef typename JointModel::ConfigVector_t CV;
+ typedef typename JointModel::TangentVector_t TV;
+
+ se3::JointModelComposite jmodel((se3::JointModelRX())/*, (se3::JointModelRY())*/);
+ jmodel.setIndexes(0,0,0);
+ jmodel.updateComponentsIndexes();
+
+ se3::JointModelComposite::JointDataDerived jdata = jmodel.createData();
+
+ CV q = jmodel.random();
+ jmodel.calc(jdata,q);
+ SE3 M_ref(jdata.M);
+
+ CV q_int;
+ TV v(Eigen::VectorXd::Random(jmodel.nv())); v.setZero();
+ double eps = 1e-4;
+
+ assert(q.size() == jmodel.nq()&& "nq false");
+ assert(v.size() == jmodel.nv()&& "nv false");
+ Eigen::MatrixXd S(6,jmodel.nv()), S_ref(ConstraintXd(jdata.S).matrix());
+
+ eps = jmodel.finiteDifferenceIncrement();
+ for(int k=0;k<jmodel.nv();++k)
+ {
+ v[k] = eps;
+ q_int = jmodel.integrate(q,v);
+ jmodel.calc(jdata,q_int);
+ SE3 M_int = jdata.M;
+
+ S.col(k) = log6(M_ref.inverse()*M_int).toVector();
+ S.col(k) /= eps;
+
+ v[k] = 0.;
+ }
+
+ std::cout << "S\n" << S << std::endl;
+ std::cout << "S_ref\n" << S_ref << std::endl;
+ // BOOST_CHECK(S.isApprox(S_ref,eps*1e1)); //@TODO Uncomment to test once JointComposite maths are ok
+}
+
BOOST_AUTO_TEST_SUITE(FiniteDifferences)
BOOST_AUTO_TEST_CASE(increment)
@@ -165,7 +208,7 @@ BOOST_AUTO_TEST_CASE (test_jacobian_vs_finit_diff)
q.segment<4>(3).normalize();
computeJacobians(model,data,q);
- Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoint-1);
+ Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-1);
Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
getJacobian<false>(model,data,idx,Jrh);
diff --git a/unittest/frames.cpp b/unittest/frames.cpp
index 1b305cf0d40c9e4100427f9c5fa8b16a92461d6d..7b08529aeed99103f18a769ebd4e797b6d36353d 100644
--- a/unittest/frames.cpp
+++ b/unittest/frames.cpp
@@ -39,10 +39,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
se3::Model model;
se3::buildModels::humanoidSimple(model);
- Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoint-1);
- const std::string & frame_name = std::string( model.getJointName(parent_idx)+ "_frame");
+ Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
+ const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
const SE3 & framePlacement = SE3::Random();
- model.addFrame(frame_name, parent_idx, framePlacement);
+ model.addFrame(Frame (frame_name, parent_idx, -1, framePlacement, OP_FRAME));
se3::Data data(model);
VectorXd q = VectorXd::Ones(model.nq);
@@ -61,10 +61,10 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
Model model;
buildModels::humanoidSimple(model);
- Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoint-1);
- const std::string & frame_name = std::string( model.getJointName(parent_idx)+ "_frame");
+ Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
+ const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
const SE3 & framePlacement = SE3::Random();
- model.addFrame(frame_name, parent_idx, framePlacement);
+ model.addFrame(Frame (frame_name, parent_idx, -1, framePlacement, OP_FRAME));
se3::Data data(model);
VectorXd q = VectorXd::Ones(model.nq);
diff --git a/unittest/geom.cpp b/unittest/geom.cpp
index 434cd996f2c8b947e156926e92f5a5cc92bd4b59..3ed7e473c4c09c0e238e41cf266458c618fcff2d 100644
--- a/unittest/geom.cpp
+++ b/unittest/geom.cpp
@@ -48,7 +48,8 @@ typedef std::map <std::string, se3::SE3> JointPositionsMap_t;
typedef std::map <std::string, se3::SE3> GeometryPositionsMap_t;
typedef std::map <std::pair < std::string , std::string >, fcl::DistanceResult > PairDistanceMap_t;
JointPositionsMap_t fillPinocchioJointPositions(const se3::Model& model, const se3::Data & data);
-GeometryPositionsMap_t fillPinocchioGeometryPositions(const se3::GeometryData & data_geom);
+GeometryPositionsMap_t fillPinocchioGeometryPositions(const se3::GeometryModel & geomModel,
+ const se3::GeometryData & geomData);
#ifdef WITH_HPP_MODEL_URDF
JointPositionsMap_t fillHppJointPositions(const hpp::model::HumanoidRobotPtr_t robot);
GeometryPositionsMap_t fillHppGeometryPositions(const hpp::model::HumanoidRobotPtr_t robot);
@@ -145,53 +146,68 @@ BOOST_AUTO_TEST_CASE ( simple_boxes )
{
using namespace se3;
Model model;
- GeometryModel model_geom;
+ GeometryModel geomModel;
Model::JointIndex idx;
idx = model.addJoint(model.getJointId("universe"),JointModelPlanar(),SE3::Identity(),"planar1_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"planar1_body");
+ model.addJointFrame(idx);
+ model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
+ model.addBodyFrame("planar1_body", idx, SE3::Identity());
idx = model.addJoint(model.getJointId("universe"),JointModelPlanar(),SE3::Identity(),"planar2_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"planar2_body");
+ model.addJointFrame(idx);
+ model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
+ model.addBodyFrame("planar2_body", idx, SE3::Identity());
boost::shared_ptr<fcl::Box> sample(new fcl::Box(1));
- model_geom.addGeometryObject(model.getJointId("planar1_joint"),sample, SE3::Identity(), "ff1_collision_object", "");
+ geomModel.addGeometryObject(GeometryObject("ff1_collision_object",
+ model.getBodyId("planar1_body"),0,
+ sample,SE3::Identity(), ""),
+ model,true);
boost::shared_ptr<fcl::Box> sample2(new fcl::Box(1));
- model_geom.addGeometryObject(model.getJointId("planar2_joint"),sample2, SE3::Identity(), "ff2_collision_object", "");
+ geomModel.addGeometryObject(GeometryObject("ff2_collision_object",
+ model.getBodyId("planar2_body"),0,
+ sample2,SE3::Identity(), ""),
+ model,true);
+ geomModel.addAllCollisionPairs();
se3::Data data(model);
- se3::GeometryData data_geom(model_geom);
+ se3::GeometryData geomData(geomModel);
+
+ BOOST_CHECK(CollisionPair(0,1) == geomModel.collisionPairs[0]);
std::cout << "------ Model ------ " << std::endl;
std::cout << model;
std::cout << "------ Geom ------ " << std::endl;
- std::cout << model_geom;
+ std::cout << geomModel;
std::cout << "------ DataGeom ------ " << std::endl;
- std::cout << data_geom;
- BOOST_CHECK(data_geom.computeCollision(CollisionPair(0,1)).fcl_collision_result.isCollision() == true);
+ std::cout << geomData;
Eigen::VectorXd q(model.nq);
+ q << 0, 0, 0,
+ 0, 0, 0 ;
+
+ se3::updateGeometryPlacements(model, data, geomModel, geomData, q);
+ BOOST_CHECK(computeCollision(geomModel,geomData,0) == true);
+
q << 2, 0, 0,
0, 0, 0 ;
- se3::updateGeometryPlacements(model, data, model_geom, data_geom, q);
- std::cout << data_geom;
- BOOST_CHECK(data_geom.computeCollision(CollisionPair(0,1)).fcl_collision_result.isCollision() == false);
+ se3::updateGeometryPlacements(model, data, geomModel, geomData, q);
+ BOOST_CHECK(computeCollision(geomModel,geomData,0) == false);
q << 0.99, 0, 0,
0, 0, 0 ;
- se3::updateGeometryPlacements(model, data, model_geom, data_geom, q);
- std::cout << data_geom;
- BOOST_CHECK(data_geom.computeCollision(CollisionPair(0,1)).fcl_collision_result.isCollision() == true);
+ se3::updateGeometryPlacements(model, data, geomModel, geomData, q);
+ BOOST_CHECK(computeCollision(geomModel,geomData,0) == true);
q << 1.01, 0, 0,
0, 0, 0 ;
- se3::updateGeometryPlacements(model, data, model_geom, data_geom, q);
- std::cout << data_geom;
- BOOST_CHECK(data_geom.computeCollision(CollisionPair(0,1)).fcl_collision_result.isCollision() == false);
+ se3::updateGeometryPlacements(model, data, geomModel, geomData, q);
+ BOOST_CHECK(computeCollision(geomModel,geomData,0) == false);
}
BOOST_AUTO_TEST_CASE ( loading_model )
@@ -203,44 +219,48 @@ BOOST_AUTO_TEST_CASE ( loading_model )
std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
- std::vector < std::string > package_dirs;
+ std::vector < std::string > packageDirs;
std::string meshDir = PINOCCHIO_SOURCE_DIR"/models/";
- package_dirs.push_back(meshDir);
+ packageDirs.push_back(meshDir);
Model model;
se3::urdf::buildModel(filename, se3::JointModelFreeFlyer(),model);
- GeometryModel geometry_model = se3::urdf::buildGeom(model, filename, package_dirs, se3::COLLISION);
+ GeometryModel geomModel;
+ se3::urdf::buildGeom(model, filename, se3::COLLISION, geomModel, packageDirs );
+ geomModel.addAllCollisionPairs();
Data data(model);
- GeometryData geometry_data(geometry_model);
+ GeometryData geomData(geomModel);
+ fcl::CollisionResult result;
Eigen::VectorXd q(model.nq);
q << 0, 0, 0.840252, 0, 0, 0, 1, 0, 0, -0.3490658, 0.6981317, -0.3490658, 0, 0, 0, -0.3490658,
0.6981317, -0.3490658, 0, 0, 1.5, 0.6, -0.5, -1.05, -0.4, -0.3, -0.2, 0, 0, 0, 0,
1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2 ;
- se3::updateGeometryPlacements(model, data, geometry_model, geometry_data, q);
- BOOST_CHECK(geometry_data.computeCollision(CollisionPair(1,10)).fcl_collision_result.isCollision() == false);
+ se3::updateGeometryPlacements(model, data, geomModel, geomData, q);
+ se3::Index idx = geomModel.findCollisionPair(CollisionPair(1,10));
+ BOOST_CHECK(computeCollision(geomModel,geomData,idx) == false);
}
#if defined(WITH_URDFDOM) && defined(WITH_HPP_FCL)
BOOST_AUTO_TEST_CASE (radius)
{
+ std::vector < std::string > packageDirs;
#ifdef ROMEO_DESCRIPTION_MODEL_DIR
std::string filename = ROMEO_DESCRIPTION_MODEL_DIR"/romeo_description/urdf/romeo_small.urdf";
- std::vector < std::string > package_dirs;
- package_dirs.push_back(ROMEO_DESCRIPTION_MODEL_DIR);
+ packageDirs.push_back(ROMEO_DESCRIPTION_MODEL_DIR);
#else
std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
- std::vector < std::string > package_dirs;
std::string meshDir = PINOCCHIO_SOURCE_DIR"/models/";
- package_dirs.push_back(meshDir);
+ packageDirs.push_back(meshDir);
#endif // ROMEO_DESCRIPTION_MODEL_DIR
se3::Model model;
se3::urdf::buildModel(filename, se3::JointModelFreeFlyer(),model);
- se3::GeometryModel geom = se3::urdf::buildGeom(model, filename, package_dirs, se3::COLLISION);
+ se3::GeometryModel geom;
+ se3::urdf::buildGeom(model, filename, se3::COLLISION, geom, packageDirs);
Data data(model);
GeometryData geomData(geom);
@@ -276,7 +296,8 @@ BOOST_AUTO_TEST_CASE (radius)
std::string bodyName = body->name();
if(bodyName != "base_link")
{
- double radius_pino = geomData.radius[model.getFrameParent(bodyName)];
+ FrameIndex fid = model.getFrameId(bodyName, BODY);
+ double radius_pino = geomData.radius[model.frames[fid].parent];
BOOST_CHECK_MESSAGE(radius_hpp - radius_pino < 1e-6, "Radius of body " << bodyName << " are not equals between hpp and pinocchio");
}
@@ -296,9 +317,6 @@ BOOST_AUTO_TEST_CASE ( romeo_joints_meshes_positions )
using hpp::model::Device;
using hpp::model::Joint;
using hpp::model::Body;
- typedef hpp::model::ObjectVector_t ObjectVector_t;
- typedef hpp::model::JointVector_t JointVector_t;
- typedef std::vector<double> vector_t;
@@ -306,18 +324,25 @@ BOOST_AUTO_TEST_CASE ( romeo_joints_meshes_positions )
/// ********************************* ///
// Building the model in pinocchio and compute kinematics/geometry for configuration q_pino
+ std::vector < std::string > packageDirs;
+#ifdef ROMEO_DESCRIPTION_MODEL_DIR
std::string filename = ROMEO_DESCRIPTION_MODEL_DIR"/romeo_description/urdf/romeo_small.urdf";
- std::vector < std::string > package_dirs;
- package_dirs.push_back(ROMEO_DESCRIPTION_MODEL_DIR);
+ packageDirs.push_back(ROMEO_DESCRIPTION_MODEL_DIR);
+#else
+ std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
+ std::string meshDir = PINOCCHIO_SOURCE_DIR"/models/";
+ packageDirs.push_back(meshDir);
+#endif // ROMEO_DESCRIPTION_MODEL_DIR
- se3::Model model;
+ Model model;
se3::urdf::buildModel(filename, se3::JointModelFreeFlyer(),model);
- se3::GeometryModel geom = se3::urdf::buildGeom(model, filename, package_dirs, se3::COLLISION);
+ se3::GeometryModel geom;
+ se3::urdf::buildGeom(model, filename, se3::COLLISION, geom, packageDirs);
std::cout << model << std::endl;
Data data(model);
- GeometryData data_geom(geom);
+ GeometryData geomData(geom);
// Configuration to be tested
@@ -332,7 +357,7 @@ BOOST_AUTO_TEST_CASE ( romeo_joints_meshes_positions )
BOOST_CHECK_MESSAGE(q_pino.size() == model.nq , "wrong config size" );
- se3::updateGeometryPlacements(model, data, geom, data_geom, q_pino);
+ se3::updateGeometryPlacements(model, data, geom, geomData, q_pino);
/// ************* HPP ************* ///
@@ -357,7 +382,7 @@ BOOST_AUTO_TEST_CASE ( romeo_joints_meshes_positions )
// retrieve all joint and geometry objects positions
JointPositionsMap_t joints_pin = fillPinocchioJointPositions(model, data);
JointPositionsMap_t joints_hpp = fillHppJointPositions(humanoidRobot);
- GeometryPositionsMap_t geom_pin = fillPinocchioGeometryPositions(data_geom);
+ GeometryPositionsMap_t geom_pin = fillPinocchioGeometryPositions(geom, geomData);
GeometryPositionsMap_t geom_hpp = fillHppGeometryPositions(humanoidRobot);
@@ -403,28 +428,32 @@ BOOST_AUTO_TEST_CASE ( hrp2_mesh_distance)
using hpp::model::Device;
using hpp::model::Joint;
using hpp::model::Body;
- typedef hpp::model::ObjectVector_t ObjectVector_t;
- typedef hpp::model::JointVector_t JointVector_t;
- typedef std::vector<double> vector_t;
-
/// ********** Pinocchio ********** ///
/// ********************************* ///
// Building the model in pinocchio and compute kinematics/geometry for configuration q_pino
+ std::vector < std::string > packageDirs;
+#ifdef ROMEO_DESCRIPTION_MODEL_DIR
std::string filename = ROMEO_DESCRIPTION_MODEL_DIR"/romeo_description/urdf/romeo_small.urdf";
- std::vector < std::string > package_dirs;
- package_dirs.push_back(ROMEO_DESCRIPTION_MODEL_DIR);
+ packageDirs.push_back(ROMEO_DESCRIPTION_MODEL_DIR);
+#else
+ std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
+ std::string meshDir = PINOCCHIO_SOURCE_DIR"/models/";
+ packageDirs.push_back(meshDir);
+#endif // ROMEO_DESCRIPTION_MODEL_DIR
- se3::Model model;
+ Model model;
se3::urdf::buildModel(filename, se3::JointModelFreeFlyer(),model);
- se3::GeometryModel geom = se3::urdf::buildGeom(model, filename, package_dirs, se3::COLLISION);
+ se3::GeometryModel geom;
+ se3::urdf::buildGeom(model, filename, se3::COLLISION, geom, packageDirs);
+ geom.addAllCollisionPairs();
std::cout << model << std::endl;
Data data(model);
- GeometryData data_geom(geom);
+ GeometryData geomData(geom);
// Configuration to be tested
@@ -439,7 +468,7 @@ BOOST_AUTO_TEST_CASE ( hrp2_mesh_distance)
BOOST_CHECK_MESSAGE(q_pino.size() == model.nq , "wrong config size");
- se3::updateGeometryPlacements(model, data, geom, data_geom, q_pino);
+ se3::updateGeometryPlacements(model, data, geom, geomData, q_pino);
/// ************* HPP ************* ///
@@ -482,12 +511,14 @@ BOOST_AUTO_TEST_CASE ( hrp2_mesh_distance)
std::cout << "comparison between " << body1 << " and " << body2 << std::endl;
+ se3::CollisionPair pair (geom.getGeometryId(body1),
+ geom.getGeometryId(body2));
+ BOOST_REQUIRE (geom.existCollisionPair(pair));
- se3::DistanceResult dist_pin
- = data_geom.computeDistance( CollisionPair(geom.getGeometryId(body1),
- geom.getGeometryId(body2)) );
+ fcl::DistanceResult dist_pin
+ = se3::computeDistance( geom, geomData, geom.findCollisionPair(pair));
- Distance_t distance_pin(dist_pin.fcl_distance_result);
+ Distance_t distance_pin(dist_pin);
distance_hpp.checkClose(distance_pin);
}
}
@@ -500,19 +531,20 @@ BOOST_AUTO_TEST_SUITE_END ()
JointPositionsMap_t fillPinocchioJointPositions(const se3::Model& model, const se3::Data & data)
{
JointPositionsMap_t result;
- for (se3::Model::Index i = 0; i < (se3::Model::Index)model.njoint; ++i)
+ for (se3::Model::Index i = 0; i < (se3::Model::Index)model.njoints; ++i)
{
- result[model.getJointName(i)] = data.oMi[i];
+ result[model.names[i]] = data.oMi[i];
}
return result;
}
-GeometryPositionsMap_t fillPinocchioGeometryPositions(const se3::GeometryData & data_geom)
+GeometryPositionsMap_t fillPinocchioGeometryPositions(const se3::GeometryModel & geomModel,
+ const se3::GeometryData & geomData)
{
GeometryPositionsMap_t result;
- for (std::size_t i = 0; i < data_geom.model_geom.ngeoms ; ++i)
+ for (std::size_t i = 0; i < geomModel.ngeoms ; ++i)
{
- result[data_geom.model_geom.getGeometryName(i)] = data_geom.oMg[i];
+ result[geomModel.getGeometryName(i)] = geomData.oMg[i];
}
return result;
}
diff --git a/unittest/jacobian.cpp b/unittest/jacobian.cpp
index 9c6e4b855610a773d2fc08bfd04613e8c79e98c0..c664eed8c5ef903a746c0893310a730da178c201 100644
--- a/unittest/jacobian.cpp
+++ b/unittest/jacobian.cpp
@@ -43,7 +43,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
VectorXd q = VectorXd::Zero(model.nq);
computeJacobians(model,data,q);
- Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoint-1);
+ Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-1);
Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
getJacobian<false>(model,data,idx,Jrh);
@@ -110,7 +110,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
if( flag >> 1 & 1 )
{
computeJacobians(model,data,q);
- Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.njoint-1);
+ Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.njoints-1);
Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
timer.tic();
@@ -125,7 +125,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
if( flag >> 2 & 1 )
{
computeJacobians(model,data,q);
- Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.njoint-1);
+ Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.njoints-1);
Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
timer.tic();
@@ -140,7 +140,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
if( flag >> 3 & 1 )
{
computeJacobians(model,data,q);
- Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.njoint-1);
+ Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.njoints-1);
Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
timer.tic();
diff --git a/unittest/joint-composite.cpp b/unittest/joint-composite.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ef342624c57d04a13a3e4f07510deb88f3619b56
--- /dev/null
+++ b/unittest/joint-composite.cpp
@@ -0,0 +1,318 @@
+//
+// 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/>.
+
+#include "pinocchio/tools/timer.hpp"
+
+// #include "pinocchio/multibody/joint/joint-accessor.hpp"
+#include "pinocchio/multibody/joint/joint-composite.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/algorithm/aba.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
+#include "pinocchio/algorithm/compute-all-terms.hpp"
+#include "pinocchio/algorithm/jacobian.hpp"
+
+#include <iostream>
+#include <cmath>
+
+#define BOOST_TEST_DYN_LINK
+#define BOOST_TEST_MODULE JointCompositeTest
+#include <boost/test/unit_test.hpp>
+#include <boost/utility/binary.hpp>
+
+
+template <typename T>
+void test_joint_methods (T & jmodel)
+{
+ using namespace se3;
+
+ typename T::JointDataDerived jdata = jmodel.createData();
+
+ JointModelComposite jmodel_composite(jmodel);
+ jmodel_composite.setIndexes(jmodel.id(), jmodel.idx_q(), jmodel.idx_v());
+ jmodel_composite.updateComponentsIndexes();
+
+ JointDataComposite jdata_composite = jmodel_composite.createData();
+
+ Eigen::VectorXd q1(Eigen::VectorXd::Random (jmodel.nq()));jmodel_composite.normalize(q1);
+ Eigen::VectorXd q1_dot(Eigen::VectorXd::Random (jmodel.nv()));
+ Eigen::VectorXd q2(Eigen::VectorXd::Random (jmodel.nq()));jmodel_composite.normalize(q2);
+ double u = 0.3;
+ se3::Inertia::Matrix6 Ia(se3::Inertia::Random().matrix());
+ bool update_I = false;
+
+ jmodel.calc(jdata, q1, q1_dot);
+ jmodel_composite.calc(jdata_composite, q1, q1_dot);
+
+
+ std::string error_prefix("Joint Model Composite on " + jmodel.shortname());
+
+ BOOST_CHECK_MESSAGE(jmodel.nq() == jmodel_composite.nq() ,std::string(error_prefix + " - nq "));
+ BOOST_CHECK_MESSAGE(jmodel.nv() == jmodel_composite.nv() ,std::string(error_prefix + " - nv "));
+
+ BOOST_CHECK_MESSAGE(jmodel.idx_q() == jmodel_composite.idx_q() ,std::string(error_prefix + " - Idx_q "));
+ BOOST_CHECK_MESSAGE(jmodel.idx_v() == jmodel_composite.idx_v() ,std::string(error_prefix + " - Idx_v "));
+ BOOST_CHECK_MESSAGE(jmodel.id() == jmodel_composite.id() ,std::string(error_prefix + " - JointId "));
+
+ BOOST_CHECK_MESSAGE(jmodel.integrate(q1,q1_dot).isApprox(jmodel_composite.integrate(q1,q1_dot)) ,std::string(error_prefix + " - integrate "));
+ BOOST_CHECK_MESSAGE(jmodel.interpolate(q1,q2,u).isApprox(jmodel_composite.interpolate(q1,q2,u)) ,std::string(error_prefix + " - interpolate "));
+ BOOST_CHECK_MESSAGE(jmodel.randomConfiguration( -1 * Eigen::VectorXd::Ones(jmodel.nq()),
+ Eigen::VectorXd::Ones(jmodel.nq())).size()
+ == jmodel_composite.randomConfiguration(-1 * Eigen::VectorXd::Ones(jmodel.nq()),
+ Eigen::VectorXd::Ones(jmodel.nq())).size()
+ ,std::string(error_prefix + " - RandomConfiguration dimensions "));
+
+ BOOST_CHECK_MESSAGE(jmodel.difference(q1,q2).isApprox(jmodel_composite.difference(q1,q2)) ,std::string(error_prefix + " - difference "));
+ BOOST_CHECK_MESSAGE(fabs(jmodel.distance(q1,q2) - jmodel_composite.distance(q1,q2)) <= 1e-6 ,std::string(error_prefix + " - distance "));
+
+ BOOST_CHECK_MESSAGE(((ConstraintXd)jdata.S).matrix().isApprox((jdata_composite.S.matrix())),std::string(error_prefix + " - ConstraintXd "));
+ BOOST_CHECK_MESSAGE(jdata.M == jdata_composite.M, std::string(error_prefix + " - Joint transforms ")); // == or isApprox ?
+ BOOST_CHECK_MESSAGE((Motion)jdata.v == jdata_composite.v, std::string(error_prefix + " - Joint motions "));
+ BOOST_CHECK_MESSAGE((Motion)jdata.c == jdata_composite.c, std::string(error_prefix + " - Joint bias "));
+
+ jmodel.calc_aba(jdata, Ia, update_I);
+ jmodel_composite.calc_aba(jdata_composite, Ia, update_I);
+
+ BOOST_CHECK_MESSAGE((jdata.U).isApprox(jdata_composite.U), std::string(error_prefix + " - Joint U inertia matrix decomposition "));
+ BOOST_CHECK_MESSAGE((jdata.Dinv).isApprox(jdata_composite.Dinv), std::string(error_prefix + " - Joint DInv inertia matrix decomposition "));
+ BOOST_CHECK_MESSAGE((jdata.UDinv).isApprox(jdata_composite.UDinv), std::string(error_prefix + " - Joint UDInv inertia matrix decomposition "));
+
+
+}
+
+struct TestJointComposite{
+
+ template <typename T>
+ void operator()(const T ) const
+ {
+ T jmodel;
+ jmodel.setIndexes(0,0,0);
+
+ test_joint_methods(jmodel);
+ }
+
+ template <int NQ, int NV>
+ void operator()(const se3::JointModelDense<NQ,NV> & ) const
+ {
+ // Not yet correctly implemented, test has no meaning for the moment
+ }
+
+ void operator()(const se3::JointModelComposite & ) const
+ {
+ se3::JointModelComposite jmodel_composite_rx(2);
+ jmodel_composite_rx.addJointModel(se3::JointModelRX());
+ jmodel_composite_rx.addJointModel(se3::JointModelRY());
+ jmodel_composite_rx.setIndexes(1,0,0);
+ jmodel_composite_rx.updateComponentsIndexes();
+
+ test_joint_methods(jmodel_composite_rx);
+
+ }
+
+ void operator()(const se3::JointModelRevoluteUnaligned & ) const
+ {
+ se3::JointModelRevoluteUnaligned jmodel(1.5, 1., 0.);
+ jmodel.setIndexes(0,0,0);
+
+ test_joint_methods(jmodel);
+ }
+
+ void operator()(const se3::JointModelPrismaticUnaligned & ) const
+ {
+ se3::JointModelPrismaticUnaligned jmodel(1.5, 1., 0.);
+ jmodel.setIndexes(0,0,0);
+
+ test_joint_methods(jmodel);
+ }
+
+};
+
+
+
+
+BOOST_AUTO_TEST_SUITE ( JointCompositeTest)
+
+// Test that a composite joint can contain any type of joint
+BOOST_AUTO_TEST_CASE ( test_all_joints )
+{
+ using namespace Eigen;
+ using namespace se3;
+
+ boost::mpl::for_each<JointModelVariant::types>(TestJointComposite());
+
+}
+
+
+
+BOOST_AUTO_TEST_CASE ( test_recursive_variant)
+{
+ // functional test. Test if one can create a composite joint containing composite joint
+
+ using namespace Eigen;
+ using namespace se3;
+
+ /// Create joint composite with two joints,
+ JointModelComposite jmodel_composite_two_rx(2);
+ jmodel_composite_two_rx.addJointModel(JointModelRX());
+ jmodel_composite_two_rx.addJointModel(JointModelRY());
+
+ /// Create Joint composite with three joints, and add a composite in it, to test the recursive_wrapper
+ JointModelComposite jmodel_composite_recursive(3);
+ jmodel_composite_recursive.addJointModel(JointModelFreeFlyer());
+ jmodel_composite_recursive.addJointModel(JointModelPlanar());
+ jmodel_composite_recursive.addJointModel(jmodel_composite_two_rx);
+
+}
+
+
+BOOST_AUTO_TEST_CASE (TestCopyComposite)
+{
+
+ using namespace Eigen;
+ using namespace se3;
+
+ JointModelComposite jmodel_composite_planar(3);
+ jmodel_composite_planar.addJointModel(JointModelPX());
+ jmodel_composite_planar.addJointModel(JointModelPY());
+ jmodel_composite_planar.addJointModel(JointModelRZ());
+ jmodel_composite_planar.setIndexes(1,0,0);
+ jmodel_composite_planar.updateComponentsIndexes();
+
+
+ JointDataComposite jdata_composite_planar = jmodel_composite_planar.createData();
+
+ Eigen::VectorXd q1(Eigen::VectorXd::Random(3));
+ Eigen::VectorXd q1_dot(Eigen::VectorXd::Random(3));
+
+ JointModelComposite model_copy = jmodel_composite_planar;
+ JointDataComposite data_copy = model_copy.createData();
+
+ BOOST_CHECK_MESSAGE( model_copy.max_joints == jmodel_composite_planar.max_joints, "Test Copy Composite, max_joints are differents");
+ BOOST_CHECK_MESSAGE( model_copy.nq() == jmodel_composite_planar.nq(), "Test Copy Composite, nq are differents");
+ BOOST_CHECK_MESSAGE( model_copy.nv() == jmodel_composite_planar.nv(), "Test Copy Composite, nv are differents");
+
+ BOOST_CHECK_MESSAGE( model_copy.max_joints == jmodel_composite_planar.max_joints, "Test Copy Composite, max_joints are differents");
+
+ jmodel_composite_planar.calc(jdata_composite_planar,q1, q1_dot);
+ model_copy.calc(data_copy,q1, q1_dot);
+
+}
+
+
+BOOST_AUTO_TEST_CASE ( test_R3xSO3)
+{
+ std::cout << " Testing R3xSO3 vs jointcomposite<R3 - SO3>" << std::endl;
+ using namespace Eigen;
+ using namespace se3;
+
+ Model model_composite;
+ Model model_zero_mass;
+ Model model_ff;
+
+
+
+ Inertia body_inertia(Inertia::Random());
+ SE3 placement(SE3::Identity());
+
+ model_zero_mass.addJoint(model_zero_mass.getJointId("universe"),JointModelTranslation(), placement, "R3_joint");
+ model_zero_mass.addJoint(model_zero_mass.getJointId("R3_joint"), JointModelSpherical(), SE3::Identity(), "SO3_joint");
+ model_zero_mass.appendBodyToJoint(model_zero_mass.getJointId("SO3_joint"), body_inertia, SE3::Identity());
+
+ JointModelComposite jmodel_composite(2);
+ jmodel_composite.addJointModel(JointModelTranslation());
+ jmodel_composite.addJointModel(JointModelSpherical());
+
+ model_composite.addJoint(model_composite.getJointId("universe"),jmodel_composite, placement, "composite_R3xSO3_joint");
+ model_composite.appendBodyToJoint(model_composite.getJointId("composite_R3xSO3_joint"), body_inertia, SE3::Identity());
+
+ model_ff.addJoint(model_ff.getJointId("universe"),JointModelFreeFlyer(), placement, "ff_joint");
+ model_ff.appendBodyToJoint(model_ff.getJointId("ff_joint"), body_inertia, SE3::Identity());
+
+ BOOST_CHECK_MESSAGE(model_composite.nq == model_zero_mass.nq ,"Model with R3 - SO3 vs composite <R3xSO3> - dimensions nq are not equal");
+ BOOST_CHECK_MESSAGE(model_composite.nq == model_zero_mass.nq ,"Model with R3 - SO3 vs composite <R3xSO3> - dimensions nv are not equal");
+
+
+ Data data_zero_mass(model_zero_mass);
+ Data data_composite(model_composite);
+ Data data_ff(model_ff);
+
+ Eigen::VectorXd q(Eigen::VectorXd::Random(model_zero_mass.nq));normalize(model_zero_mass,q);
+ Eigen::VectorXd q_dot(Eigen::VectorXd::Random(model_zero_mass.nv));
+ Eigen::VectorXd q_ddot(Eigen::VectorXd::Random(model_zero_mass.nv));
+ Eigen::VectorXd q1(Eigen::VectorXd::Random(model_zero_mass.nq));normalize(model_zero_mass,q1);
+ Eigen::VectorXd q2(Eigen::VectorXd::Random(model_zero_mass.nq));normalize(model_zero_mass,q2);
+ Eigen::VectorXd tau(Eigen::VectorXd::Random(model_zero_mass.nq));
+ double u = 0.3;
+
+
+
+ aba(model_composite,data_composite, q,q_dot, tau);
+ centerOfMass(model_composite, data_composite,q,q_dot,q_ddot,true,false);
+ forwardKinematics(model_composite,data_composite, q, q_dot, q_ddot);
+ computeAllTerms(model_zero_mass,data_zero_mass,q,q_dot);
+
+ forwardKinematics(model_zero_mass, data_zero_mass, q, q_dot, q_ddot);
+ computeAllTerms(model_composite,data_composite,q,q_dot);
+
+
+ Model::Index index_joint_R3xSO3 = (Model::Index) model_zero_mass.njoints-1;
+ Model::Index index_joint_composite = (Model::Index) model_composite.njoints-1;
+
+
+ BOOST_CHECK_MESSAGE(data_composite.oMi[index_joint_composite]
+ .isApprox(data_zero_mass.oMi[index_joint_R3xSO3]) , "composite<R3xSO3> vs R3-SO3 - oMi last joint not equal");
+
+
+ BOOST_CHECK_MESSAGE(data_composite.v[index_joint_composite]
+ == data_zero_mass.v[index_joint_R3xSO3] , "composite<R3xSO3> vs R3-SO3 - v last joint not equal");
+
+ // BOOST_CHECK_MESSAGE(data_composite.a[index_joint_composite] //@TODO Uncommente to test once JointComposite maths are ok
+ // == data_zero_mass.a[index_joint_R3xSO3] , "composite planar joint vs PxPyRz - a last joint not equal");
+
+ // BOOST_CHECK_MESSAGE(data_composite.f[index_joint_composite] //@TODO Uncommente to test once JointComposite maths are ok
+ // == data_zero_mass.f[index_joint_R3xSO3] , "composite planar joint vs PxPyRz - f last joint not equal");
+
+ BOOST_CHECK_MESSAGE(data_composite.com[index_joint_composite]
+ .isApprox(data_zero_mass.com[index_joint_R3xSO3]) , "composite<R3xSO3> vs R3-SO3 - com last joint not equal");
+
+ BOOST_CHECK_MESSAGE(data_composite.vcom[index_joint_composite]
+ .isApprox(data_zero_mass.vcom[index_joint_R3xSO3]) , "composite<R3xSO3> vs R3-SO3 - vcom last joint not equal");
+
+ BOOST_CHECK_MESSAGE(data_composite.mass[index_joint_composite]
+ == data_zero_mass.mass[index_joint_R3xSO3] , "composite<R3xSO3> vs R3-SO3 - mass last joint not equal");
+
+ BOOST_CHECK_MESSAGE(data_composite.kinetic_energy
+ == data_zero_mass.kinetic_energy , "composite<R3xSO3> vs R3-SO3 - kinetic energy not equal");
+
+ BOOST_CHECK_MESSAGE(data_composite.potential_energy
+ == data_zero_mass.potential_energy , "composite<R3xSO3> vs R3-SO3 - potential energy not equal");
+
+ // BOOST_CHECK_MESSAGE(data_composite.nle //@TODO Uncommente to test once JointComposite maths are ok
+ // .isApprox(data_zero_mass.nle) , "composite planar joint vs PxPyRz - nle not equal");
+
+ // BOOST_CHECK_MESSAGE(data_composite.M //@TODO Uncommente to test once JointComposite maths are ok
+ // .isApprox(data_zero_mass.M) , "composite planar joint vs PxPyRz - Mass Matrix not equal");
+
+
+ BOOST_CHECK_MESSAGE(integrate(model_composite, q,q_dot).isApprox(integrate(model_zero_mass ,q,q_dot)) ,std::string(" composite<R3xSO3> vs R3-SO3 - integrate model error "));
+ BOOST_CHECK_MESSAGE(interpolate(model_composite, q1,q2,u).isApprox(interpolate(model_zero_mass ,q1,q2,u)) ,std::string(" composite<R3xSO3> vs R3-SO3 - interpolate model error "));
+ BOOST_CHECK_MESSAGE(differentiate(model_composite, q1,q2).isApprox(differentiate(model_zero_mass ,q1,q2)) ,std::string(" composite<R3xSO3> vs R3-SO3 - differentiate model error "));
+ BOOST_CHECK_MESSAGE(fabs(distance(model_composite, q1,q2).norm() - distance(model_zero_mass ,q1,q2).norm()) <= 1e-6 ,std::string(" composite<R3xSO3> vs R3-SO3 - distance model error "));
+
+}
+
+BOOST_AUTO_TEST_SUITE_END ()
+
diff --git a/unittest/joint-configurations.cpp b/unittest/joint-configurations.cpp
index d6ba8a1632488bd801c51b38d91f5f8761fda78b..c46942b603220332564a8c07fc29cfd854ea6257 100644
--- a/unittest/joint-configurations.cpp
+++ b/unittest/joint-configurations.cpp
@@ -47,14 +47,15 @@ void addJointAndBody(Model & model, const JointModelBase<D> & jmodel, const Mode
typedef typename D::TangentVector_t TV;
typedef typename D::ConfigVector_t CV;
- idx = model.addJoint(parent_id,jmodel,joint_placement,name + "_joint",
+ idx = model.addJoint(parent_id,jmodel,joint_placement,
TV::Zero(),
1e3 * (TV::Random() + TV::Constant(1)),
1e3 * (CV::Random() - CV::Constant(1)),
- 1e3 * (CV::Random() + CV::Constant(1))
+ 1e3 * (CV::Random() + CV::Constant(1)),
+ name + "_joint"
);
- model.appendBodyToJoint(idx,Y,SE3::Identity(),name + "_body");
+ model.appendBodyToJoint(idx,Y,SE3::Identity());
}
void buildModel(Model & model)
@@ -101,7 +102,7 @@ struct TestIntegrationJoint
SE3 M1 = jdata.M;
SE3 M1_exp = M0*exp6(v0);
- BOOST_CHECK(M1.isApprox(M1_exp));
+ BOOST_CHECK_MESSAGE(M1.isApprox(M1_exp), std::string("Error when integrating1 " + jmodel.shortname()));
qdot *= -1;
@@ -114,7 +115,7 @@ struct TestIntegrationJoint
M1 = jdata.M;
M1_exp = M0*exp6(v0);
- BOOST_CHECK(M1.isApprox(M1_exp));
+ BOOST_CHECK_MESSAGE(M1.isApprox(M1_exp), std::string("Error when integrating2 " + jmodel.shortname()));
}
};
@@ -125,6 +126,35 @@ void TestIntegrationJoint::operator()< JointModelDense<-1,-1> >(JointModelBase<
template<>
void TestIntegrationJoint::operator()< JointModelSphericalZYX >(JointModelBase< JointModelSphericalZYX > & /*jmodel*/) {}
+template<>
+void TestIntegrationJoint::operator()< JointModelComposite >(JointModelBase< JointModelComposite > & /*jmodel*/)
+{
+ se3::JointModelComposite jmodel((se3::JointModelRX()), (se3::JointModelRY()));
+ jmodel.setIndexes(1,0,0);
+ jmodel.updateComponentsIndexes();
+
+ se3::JointModelComposite::JointDataDerived jdata = jmodel.createData();
+
+ typedef typename JointModel::ConfigVector_t CV;
+ typedef typename JointModel::TangentVector_t TV;
+ typedef typename JointModel::Transformation_t SE3;
+
+ CV q0 = jmodel.random();
+ TV qdot(Eigen::VectorXd::Random(jmodel.nv()));
+
+ jmodel.calc(jdata,q0,qdot);
+ SE3 M0 = jdata.M;
+ Motion v0 = jdata.v;
+
+ CV q1 = jmodel.integrate(q0,qdot);
+ jmodel.calc(jdata,q1);
+ SE3 M1 = jdata.M;
+
+ SE3 M1_exp = M0*exp6(v0);
+ // The computations in JointModelComposite::calc() may be wrong, this results cannot be tested yet.
+ // BOOST_CHECK_MESSAGE(M1.isApprox(M1_exp), std::string("Error when integrating " + jmodel.shortname()));
+}
+
template<>
void TestIntegrationJoint::init<JointModelRevoluteUnaligned>(JointModelBase<JointModelRevoluteUnaligned> & jmodel)
{
@@ -186,48 +216,9 @@ struct TestDifferentiationJoint
TV qdot = jmodel.difference(q0,q1);
- BOOST_CHECK_MESSAGE( jmodel.integrate(q0, qdot).isApprox(q1), std::string("Error in difference for joint " + jmodel.shortname()));
+ BOOST_CHECK_MESSAGE( jmodel.isSameConfiguration(jmodel.integrate(q0, qdot), q1), std::string("Error in difference for joint " + jmodel.shortname()));
- BOOST_CHECK_MESSAGE( jmodel.integrate(q1, -qdot).isApprox(q0), std::string("Error in difference for joint " + jmodel.shortname()));
-
- }
-
- void operator()(JointModelBase<JointModelFreeFlyer> & jmodel)
- {
- init(jmodel);
- typedef JointModelFreeFlyer::ConfigVector_t CV;
- typedef JointModelFreeFlyer::TangentVector_t TV;
- typedef JointModelFreeFlyer::Transformation_t SE3;
-
- jmodel.setIndexes(0,0,0);
-
- CV q0 = jmodel.random();
- CV q1 = jmodel.random();
-
- TV qdot = jmodel.difference(q0,q1);
-
- BOOST_CHECK_MESSAGE( jmodel.integrate(q0, qdot).head<3>().isApprox(q1.head<3>()), std::string("Error in difference for joint " + jmodel.shortname()));
- BOOST_CHECK_MESSAGE( defineSameRotation(Eigen::Quaterniond(jmodel.integrate(q0, qdot).tail<4>()), Eigen::Quaterniond(q1.tail<4>()))
- , std::string("Error in difference for joint " + jmodel.shortname()));
-
- }
-
- void operator()(JointModelBase<JointModelSpherical> & jmodel)
- {
- init(jmodel);
- typedef JointModelSpherical::ConfigVector_t CV;
- typedef JointModelSpherical::TangentVector_t TV;
- typedef JointModelSpherical::Transformation_t SE3;
-
- jmodel.setIndexes(0,0,0);
-
- CV q0 = jmodel.random();
- CV q1 = jmodel.random();
-
- TV qdot = jmodel.difference(q0,q1);
-
- BOOST_CHECK_MESSAGE( defineSameRotation(Eigen::Quaterniond(jmodel.integrate(q0, qdot)), Eigen::Quaterniond(q1))
- , std::string("Error in difference for joint " + jmodel.shortname()));
+ BOOST_CHECK_MESSAGE( jmodel.isSameConfiguration(jmodel.integrate(q1, -qdot), q0), std::string("Error in difference for joint " + jmodel.shortname()));
}
@@ -291,85 +282,21 @@ struct TestInterpolationJoint
double u = 0;
- BOOST_CHECK_MESSAGE( jmodel.interpolate(q0, q1,u).isApprox(q0)
+ BOOST_CHECK_MESSAGE(jmodel.isSameConfiguration(jmodel.interpolate(q0, q1,u),q0)
, std::string("Error in interpolation with u = 0 for joint " + jmodel.shortname()));
u = 0.3;
- BOOST_CHECK_MESSAGE( jmodel.interpolate(jmodel.interpolate(q0, q1,u), q1, 1).isApprox(q1)
+ BOOST_CHECK_MESSAGE(jmodel.isSameConfiguration(jmodel.interpolate(jmodel.interpolate(q0, q1,u), q1, 1),q1)
, std::string("Error in double interpolation for joint " + jmodel.shortname()));
u = 1;
- BOOST_CHECK_MESSAGE( jmodel.interpolate(q0, q1,u).isApprox(q1)
+ BOOST_CHECK_MESSAGE(jmodel.isSameConfiguration(jmodel.interpolate(q0, q1,u),q1)
, std::string("Error in interpolation with u = 1 for joint " + jmodel.shortname()));
}
- void operator()(JointModelBase<JointModelFreeFlyer> & jmodel)
- {
- init(jmodel);
- typedef JointModelFreeFlyer::ConfigVector_t CV;
- typedef JointModelFreeFlyer::TangentVector_t TV;
- typedef JointModelFreeFlyer::Transformation_t SE3;
-
- jmodel.setIndexes(0,0,0);
-
- CV q0 = jmodel.random();
- CV q1 = jmodel.random();
-
- double u = 0;
-
- BOOST_CHECK_MESSAGE( jmodel.interpolate(q0, q1,u).head<3>().isApprox(q0.head<3>())
- , std::string("Error in interpolation with u = 0 for joint " + jmodel.shortname()));
- BOOST_CHECK_MESSAGE( defineSameRotation(Eigen::Quaterniond(jmodel.interpolate(q0, q1,u).tail<4>()), Eigen::Quaterniond(q0.tail<4>()) )
- , std::string("Error in interpolation with u = 0 for joint " + jmodel.shortname()));
-
- u = 0.3;
-
- BOOST_CHECK_MESSAGE( jmodel.interpolate(jmodel.interpolate(q0, q1,u), q1, 1).head<3>().isApprox(q1.head<3>())
- , std::string("Error in double interpolation for joint " + jmodel.shortname()));
- BOOST_CHECK_MESSAGE( defineSameRotation(Eigen::Quaterniond(jmodel.interpolate(jmodel.interpolate(q0, q1,u), q1, 1).tail<4>()), Eigen::Quaterniond(q1.tail<4>()) )
- , std::string("Error in double interpolation for joint " + jmodel.shortname()));
-
- u = 1;
-
- BOOST_CHECK_MESSAGE( jmodel.interpolate(q0, q1,u).head<3>().isApprox(q1.head<3>())
- , std::string("Error in interpolation with u = 1 for joint " + jmodel.shortname()));
- BOOST_CHECK_MESSAGE( defineSameRotation(Eigen::Quaterniond(jmodel.interpolate(q0, q1,u).tail<4>()), Eigen::Quaterniond(q1.tail<4>()) )
- , std::string("Error in interpolation with u = 1 for joint " + jmodel.shortname()));
-
- }
-
-
-
- void operator()(JointModelBase<JointModelSpherical> & jmodel)
- {
- init(jmodel);
- typedef JointModelSpherical::ConfigVector_t CV;
- typedef JointModelSpherical::TangentVector_t TV;
- typedef JointModelSpherical::Transformation_t SE3;
-
- jmodel.setIndexes(0,0,0);
-
- CV q0 = jmodel.random();
- CV q1 = jmodel.random();
-
- double u = 0;
-
- BOOST_CHECK_MESSAGE( defineSameRotation(Eigen::Quaterniond(jmodel.interpolate(q0, q1,u)), Eigen::Quaterniond(q0))
- , std::string("Error in interpolation with u = 0 for joint " + jmodel.shortname()));
-
- u = 0.3;
-
- BOOST_CHECK_MESSAGE( defineSameRotation(Eigen::Quaterniond(jmodel.interpolate(jmodel.interpolate(q0, q1,u), q1, 1)), Eigen::Quaterniond(q1) )
- , std::string("Error in double interpolation for joint " + jmodel.shortname()));
-
- u = 1;
-
- BOOST_CHECK_MESSAGE( defineSameRotation(Eigen::Quaterniond(jmodel.interpolate(q0, q1,u)), Eigen::Quaterniond(q1))
- , std::string("Error in interpolation with u = 1 for joint " + jmodel.shortname()));
- }
};
diff --git a/unittest/joint.cpp b/unittest/joint.cpp
index a191c8d7f91a49fbae4a87420f2a4892bc7ae40e..162c9fd39ca98be8af2a41c797345ad608adef85 100644
--- a/unittest/joint.cpp
+++ b/unittest/joint.cpp
@@ -15,6 +15,7 @@
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
+#include "pinocchio/multibody/joint/joint-composite.hpp"
#include "pinocchio/multibody/joint/joint.hpp"
#define BOOST_TEST_DYN_LINK
@@ -25,6 +26,7 @@
template <typename T>
void test_joint_methods (T & jmodel, typename T::JointDataDerived & jdata)
{
+ std::cout << "Testing Joint over " << jmodel.shortname() << std::endl;
Eigen::VectorXd q1(Eigen::VectorXd::Random (jmodel.nq()));
Eigen::VectorXd q1_dot(Eigen::VectorXd::Random (jmodel.nv()));
Eigen::VectorXd q2(Eigen::VectorXd::Random (jmodel.nq()));
@@ -32,8 +34,9 @@ void test_joint_methods (T & jmodel, typename T::JointDataDerived & jdata)
se3::Inertia::Matrix6 Ia(se3::Inertia::Random().matrix());
bool update_I = false;
- q1 = jmodel.random();
- q2 = jmodel.random();
+ q1 = jmodel.random();
+ q2 = jmodel.random();
+
jmodel.calc(jdata, q1, q1_dot);
jmodel.calc_aba(jdata, Ia, update_I);
@@ -42,7 +45,6 @@ void test_joint_methods (T & jmodel, typename T::JointDataDerived & jdata)
se3::JointModel jma(jmodel);
se3::JointData jda(jdata);
-
jma.calc(jda, q1, q1_dot);
jma.calc_aba(jda, Ia, update_I);
@@ -93,6 +95,21 @@ struct TestJoint{
// JointModelDense will be removed soon
}
+ void operator()(const se3::JointModelComposite & ) const
+ {
+ // se3::JointModelComposite jmodel(2);
+ // jmodel.addJointModel(se3::JointModelRX());
+ // jmodel.addJointModel(se3::JointModelRY());
+
+ se3::JointModelComposite jmodel((se3::JointModelRX()), (se3::JointModelRY()));
+ jmodel.setIndexes(0,0,0);
+ jmodel.updateComponentsIndexes();
+
+ se3::JointModelComposite::JointDataDerived jdata = jmodel.createData();
+
+ test_joint_methods(jmodel, jdata);
+ }
+
void operator()(const se3::JointModelRevoluteUnaligned & ) const
{
se3::JointModelRevoluteUnaligned jmodel(1.5, 1., 0.);
diff --git a/unittest/model.cpp b/unittest/model.cpp
index 341a6aeb2e997b69852924c87d2ea2259a18a7c4..eac27fef59898e187fe406627a3f70cf6fb7c219 100644
--- a/unittest/model.cpp
+++ b/unittest/model.cpp
@@ -34,7 +34,7 @@ BOOST_AUTO_TEST_CASE(test_model_subtree)
buildModels::humanoidSimple(model);
Model::JointIndex idx_larm1 = model.getJointId("larm1_joint");
- BOOST_CHECK(idx_larm1<model.njoint);
+ BOOST_CHECK(idx_larm1<model.njoints);
Model::IndexVector subtree = model.subtrees[idx_larm1];
BOOST_CHECK(subtree.size()==6);
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()
diff --git a/unittest/visitor.cpp b/unittest/visitor.cpp
index bd4dfea3739ce54662cbf8cb48273a5d7d474b5d..30afec1d16005941541203aced37380934185444 100644
--- a/unittest/visitor.cpp
+++ b/unittest/visitor.cpp
@@ -17,7 +17,7 @@
#include <iostream>
-#include <pinocchio/multibody/joint/joint.hpp>
+#include <pinocchio/multibody/joint/joint-composite.hpp>
#include <pinocchio/multibody/model.hpp>
#include "pinocchio/multibody/visitor.hpp"
@@ -85,7 +85,7 @@ BOOST_AUTO_TEST_CASE ( test_runal )
model.addJoint(0,se3::JointModelRevoluteUnaligned(0,0,1),se3::SE3::Random());
Data data(model);
- for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoint;++i )
+ for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoints;++i )
{
SimpleVisitor::run(model.joints[i],data.joints[i],
SimpleVisitor::ArgsType(model,data,i));