Commit 14b6d3be authored by jcarpent's avatar jcarpent
Browse files

Merge hpp-devel into devel

parents b2891091 9a134cee
......@@ -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
......
......@@ -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
......
......@@ -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);
......
......@@ -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,
......
......@@ -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
......
......@@ -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()
{
......
......@@ -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))
;
}
......
......@@ -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.",
......
......@@ -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",
......
......@@ -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())
......
......@@ -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
{
......
......@@ -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;}