Commit bc584d52 authored by Valenza Florian's avatar Valenza Florian
Browse files

[C++][Python] Removed fixbodies in Model. Were a way to store visuals merged...

[C++][Python] Removed fixbodies in Model. Were a way to store visuals merged with Joints. Now this information is in GeometryModel (visuals) + updated python bindings
parent d2296209
......@@ -7,7 +7,7 @@ from bindings_SE3 import TestSE3Bindings
from bindings_force import TestForceBindings
from bindings_motion import TestMotionBindings
from bindings_inertia import TestInertiaBindings
from bindings_frame import TestFrameBindings # No geometry module yet on travis
from bindings_frame import TestFrameBindings
from bindings_geometry_object import TestGeometryObjectBindings # Python Class RobotWrapper needs geometry module to not raise Exception
from explog import TestExpLog # noqa
from model import TestModel # noqa
......
......@@ -65,8 +65,6 @@ namespace se3
/// \brief Number of bodies (= number of joints + 1).
int nbody;
/// \brief Number of fixed-bodies (= number of fixed-joints).
int nFixBody;
/// \brief Number of operational frames.
int nOperationalFrames;
......@@ -105,21 +103,6 @@ namespace se3
/// \brief Upper joint configuration limit
Eigen::VectorXd upperPositionLimit;
/// \brief True iff body <i> has a visual mesh.
std::vector<bool> hasVisual;
/// \brief Fixed-body relative placement (wrt last moving parent)
std::vector<SE3> fix_lmpMi;
/// \brief Fixed-body index of the last moving parent
std::vector<Model::JointIndex> fix_lastMovingParent;
/// \brief True iff fixed-body <i> has a visual mesh.
std::vector<bool> fix_hasVisual;
/// \brief Name of fixed-joint <i>.
std::vector<std::string> fix_bodyNames;
/// \brief Vector of operational frames registered on the model.
std::vector<Frame> operational_frames;
......@@ -134,7 +117,6 @@ namespace se3
, nv(0)
, njoint(1)
, nbody(1)
, nFixBody(0)
, nOperationalFrames(0)
, inertias(1)
, jointPlacements(1)
......@@ -142,7 +124,6 @@ namespace se3
, parents(1)
, names(1)
, bodyNames(1)
, hasVisual(1)
, gravity( gravity981,Eigen::Vector3d::Zero() )
{
names[0] = "universe"; // Should be "universe joint (trivial)"
......@@ -238,21 +219,6 @@ namespace se3
void appendBodyToJoint (const JointIndex parent, const SE3 & placement, const Inertia & Y,
const std::string & bodyName = "");
/// Need modifications
/// \brief Add a body to the kinematic tree.
///
/// \param[in] fix_lastMovingParent Index of the closest movable parent joint.
/// \param[in] placementFromLastMoving Placement regarding to the closest movable parent joint.
/// \param[in] jointName Name of the joint.
/// \param[in] visual Inform if the current body has a visual or not.
///
/// \return The index of the new added joint.
///
JointIndex addFixedBody(JointIndex fix_lastMovingParent,
const SE3 & placementFromLastMoving,
const std::string & jointName = "",
bool visual=false);
///
/// \brief Return the index of a body given by its name.
......
......@@ -39,8 +39,7 @@ namespace se3
os << "Nb bodies = " << model.nbody << " (nq="<< model.nq<<",nv="<<model.nv<<")" << std::endl;
for(Model::Index i=0;i<(Model::Index)(model.nbody);++i)
{
os << " Joint "<<model.names[i] << ": parent=" << model.parents[i]
<< ( model.hasVisual[i] ? " (has visual) " : "(doesnt have visual)" ) << std::endl;
os << " Joint "<<model.names[i] << ": parent=" << model.parents[i] << std::endl;
}
return os;
......@@ -145,20 +144,7 @@ namespace se3
}
inline Model::JointIndex Model::addFixedBody (JointIndex lastMovingParent,
const SE3 & placementFromLastMoving,
const std::string & bodyName,
bool visual)
{
Model::JointIndex idx = (Model::JointIndex) (nFixBody++);
fix_lastMovingParent.push_back(lastMovingParent);
fix_lmpMi .push_back(placementFromLastMoving);
fix_hasVisual .push_back(visual);
fix_bodyNames .push_back( (bodyName!="")?bodyName:random(8));
return idx;
}
inline Model::JointIndex Model::getBodyId (const std::string & name) const
......
......@@ -230,7 +230,6 @@ namespace se3
Model::JointIndex body_id;
bool is_body_fixed = false;
if (joint_type == "JointTypeRevoluteX")
{
body_id = model.addJointAndBody (parent_id, JointModelRX (), global_placement, Y, joint_name, body_name);
......@@ -271,11 +270,11 @@ namespace se3
{
model.appendBodyToJoint(parent_id, global_placement, Y, "");
body_id = model.nbody;
fixed_body_table_id_map[body_name] = parent_id;
fixed_placement_map[body_name] = global_placement;
body_id = model.addFixedBody(parent_id, global_placement, body_name, false);
is_body_fixed = true;
}
else
{
......@@ -288,10 +287,7 @@ namespace se3
if (verbose) {
std::cout << "==== Added Body ====" << std::endl;
std::cout << " body name : " << body_name << std::endl;
if (! is_body_fixed)
std::cout << " body id : " << body_id << std::endl;
else
std::cout << " fixed body id : " << body_id << std::endl;
std::cout << " body id : " << body_id << std::endl;
std::cout << " closest parent id : " << parent_id << std::endl;
std::cout << " joint frame:\n" << joint_placement << std::endl;
std::cout << " joint type : " << joint_type << std::endl;
......
......@@ -369,8 +369,6 @@ namespace se3
//transformation of the current placement offset
nextPlacementOffset = jointPlacement;
//add the fixed Body in the model for the viewer
model.addFixedBody(parent_joint_id, nextPlacementOffset, link->name);
// Add a frame in the model to keep trace of this fixed joint
model.addFrame(joint->name, parent_joint_id, nextPlacementOffset);
......
......@@ -49,7 +49,7 @@ namespace se3
typedef eigenpy::UnalignedEquivalent<SE3>::type SE3_fx;
typedef eigenpy::UnalignedEquivalent<Inertia>::type Inertia_fx;
struct add_body_visitor : public boost::static_visitor<Model::Index>
struct add_joint_and_body_visitor : public boost::static_visitor<Model::Index>
{
ModelHandler & _model;
Model::JointIndex & _index_parent;
......@@ -57,24 +57,22 @@ namespace se3
const Inertia_fx & _inertia;
const std::string & _jName;
const std::string & _bName;
bool _visual;
add_body_visitor( ModelHandler & model,
add_joint_and_body_visitor( ModelHandler & model,
Model::JointIndex & idx, const SE3_fx & placement,
const Inertia_fx & Y, const std::string & jointName,
const std::string & bodyName, bool visual)
const std::string & bodyName)
: _model(model)
, _index_parent(idx)
, _placement(placement)
, _inertia(Y)
, _jName(jointName)
, _bName(bodyName)
, _visual(visual)
{}
template <typename T> Model::Index operator()( T & operand ) const
{
return _model->addJointAndBody(_index_parent, operand, _placement, _inertia, _jName, _bName, _visual);
return _model->addJointAndBody(_index_parent, operand, _placement, _inertia, _jName, _bName);
}
};
......@@ -104,6 +102,7 @@ namespace se3
.add_property("nq", &ModelPythonVisitor::nq)
.add_property("nv", &ModelPythonVisitor::nv)
.add_property("njoint", &ModelPythonVisitor::njoint)
.add_property("nbody", &ModelPythonVisitor::nbody)
.add_property("inertias",
bp::make_function(&ModelPythonVisitor::inertias,
......@@ -111,6 +110,9 @@ namespace se3
.add_property("jointPlacements",
bp::make_function(&ModelPythonVisitor::jointPlacements,
bp::return_internal_reference<>()) )
.add_property("bodyPlacements",
bp::make_function(&ModelPythonVisitor::bodyPlacements,
bp::return_internal_reference<>()) )
.add_property("joints",
bp::make_function(&ModelPythonVisitor::joints,
bp::return_internal_reference<>()) )
......@@ -123,17 +125,8 @@ namespace se3
.add_property("bodyNames",
bp::make_function(&ModelPythonVisitor::bodyNames,
bp::return_internal_reference<>()) )
.add_property("hasVisual",
bp::make_function(&ModelPythonVisitor::hasVisual,
bp::return_internal_reference<>()) )
.def("addJointAndBody",&ModelPythonVisitor::addJointToModel)
.def("addJointAndBody",&ModelPythonVisitor::addJointAndBodyToModel)
.add_property("nFixBody", &ModelPythonVisitor::nFixBody)
.add_property("fix_lmpMi", bp::make_function(&ModelPythonVisitor::fix_lmpMi, bp::return_internal_reference<>()) )
.add_property("fix_lastMovingParent",bp::make_function(&ModelPythonVisitor::fix_lastMovingParent,bp::return_internal_reference<>()) )
.add_property("fix_hasVisual", bp::make_function(&ModelPythonVisitor::fix_hasVisual, bp::return_internal_reference<>()) )
.add_property("fix_bodyNames", bp::make_function(&ModelPythonVisitor::fix_bodyNames, bp::return_internal_reference<>()) )
.add_property("effortLimit", bp::make_function(&ModelPythonVisitor::effortLimit), "Joint max effort")
.add_property("velocityLimit", bp::make_function(&ModelPythonVisitor::velocityLimit), "Joint max velocity")
......@@ -162,32 +155,27 @@ namespace se3
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 std::vector<Inertia> & inertias( ModelHandler & m ) { return m->inertias; }
static std::vector<SE3> & jointPlacements( ModelHandler & m ) { return m->jointPlacements; }
static std::vector<SE3> & bodyPlacements( ModelHandler & m ) { return m->bodyPlacements; }
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 std::vector<std::string> & bodyNames ( ModelHandler & m ) { return m->bodyNames; }
static std::vector<bool> & hasVisual ( ModelHandler & m ) { return m->hasVisual; }
static Model::Index addJointToModel(ModelHandler & modelPtr,
static Model::Index addJointAndBodyToModel(ModelHandler & modelPtr,
Model::JointIndex idx, bp::object joint,
const SE3_fx & placement,
const Inertia_fx & Y,
const std::string & jointName,
const std::string & bodyName,
bool visual=false)
const std::string & bodyName)
{
JointModelVariant variant = bp::extract<JointModelVariant> (joint);
return boost::apply_visitor(add_body_visitor(modelPtr, idx, placement, Y, jointName, bodyName, visual), variant);
return boost::apply_visitor(add_joint_and_body_visitor(modelPtr, idx, placement, Y, jointName, bodyName), variant);
}
static int nFixBody( ModelHandler & m ) { return m->nFixBody; }
static std::vector<SE3> & fix_lmpMi ( ModelHandler & m ) { return m->fix_lmpMi; }
static std::vector<Model::JointIndex> & fix_lastMovingParent( ModelHandler & m ) { return m->fix_lastMovingParent; }
static std::vector<bool> & fix_hasVisual ( ModelHandler & m ) { return m->fix_hasVisual; }
static std::vector<std::string> & fix_bodyNames ( ModelHandler & m ) { return m->fix_bodyNames; }
static Eigen::VectorXd effortLimit(ModelHandler & m) {return m->effortLimit;}
static Eigen::VectorXd velocityLimit(ModelHandler & m) {return m->velocityLimit;}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment