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

[C++] bodies are now a subpart of the Frames : some Frames are of type BODY...

[C++] bodies are now a subpart of the Frames : some Frames are of type BODY and when referring to getBodyId/Name in Model it will look in its frames
parent 98a5132c
......@@ -75,24 +75,15 @@ namespace se3
/// \brief Placement (SE3) of the input of joint <i> regarding to the parent joint output <li>.
std::vector<SE3> jointPlacements;
/// \brief Placement (SE3) of the body <i> regarding to the parent joint output <li>.
std::vector<SE3> bodyPlacements;
/// \brief Model of joint <i>.
JointModelVector joints;
/// \brief Joint parent of joint <i>, denoted <li> (li==parents[i]).
std::vector<JointIndex> parents;
/// \brief Joint parent of body <i>
std::vector<JointIndex> bodyParents;
/// \brief Name of joint <i>
std::vector<std::string> names;
/// \brief Name of the body attached to the output of joint <i>.
std::vector<std::string> bodyNames;
/// \brief Vector of maximal joint torques
Eigen::VectorXd effortLimit;
/// \brief Vector of maximal joint velocities
......@@ -122,13 +113,10 @@ namespace se3
, jointPlacements(1)
, joints(1)
, parents(1)
, bodyParents(1)
, names(1)
, bodyNames(1)
, gravity( gravity981,Eigen::Vector3d::Zero() )
{
names[0] = "universe"; // Should be "universe joint (trivial)"
bodyNames[0] = "universe";
}
~Model() {} // std::cout << "Destroy model" << std::endl; }
......@@ -319,6 +307,24 @@ namespace se3
/// \return
///
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
///
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
///
FrameType getFrameType(const FrameIndex index) const;
///
/// \brief Return the relative placement between a frame and its supporting joint.
......
......@@ -140,9 +140,6 @@ namespace se3
inertias[parent] += iYf;
addFrame((bodyName!="")?bodyName:random(8), parent, bodyPlacement, BODY);
bodyParents.push_back(parent);
bodyPlacements.push_back(bodyPlacement);
bodyNames.push_back( (bodyName!="")?bodyName:random(8));
nbody ++;
}
......@@ -153,22 +150,18 @@ namespace se3
inline Model::JointIndex Model::getBodyId (const std::string & name) const
{
std::vector<std::string>::iterator::difference_type
res = std::find(bodyNames.begin(),bodyNames.end(),name) - bodyNames.begin();
assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
assert( (res>=0)&&(res<nbody) && "The body name you asked does not exist" );
return Model::JointIndex(res);
return getFrameId(name);
}
inline bool Model::existBodyName (const std::string & name) const
{
return (bodyNames.end() != std::find(bodyNames.begin(),bodyNames.end(),name));
return existFrame(name);
}
inline const std::string& Model::getBodyName (const Model::JointIndex index) const
{
assert( index < (Model::Index)nbody );
return bodyNames[index];
return getFrameName(index);
}
inline Model::JointIndex Model::getJointId (const std::string & name) const
......@@ -227,6 +220,23 @@ namespace se3
return operational_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( operational_frames.begin()
, operational_frames.end()
, boost::bind(&Frame::name, _1) == name
);
std::vector<Frame>::iterator::difference_type it_diff = it - operational_frames.begin();
return getFrameType(Model::JointIndex(it_diff));
}
inline FrameType Model::getFrameType( const FrameIndex index ) const
{
return operational_frames[index].type;
}
inline const SE3 & Model::getFramePlacement( const std::string & name) const
{
assert(existFrame(name) && "The Frame you requested does not exist");
......
......@@ -29,34 +29,34 @@ namespace se3
void humanoid2d(Model& model)
{
model.addJointAndBody(model.getBodyId("universe"),JointModelRX(),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getJointId("universe"),JointModelRX(),SE3::Identity(),Inertia::Random(),
"ff1_joint", "ff1_body");
model.addJointAndBody(model.getBodyId("ff1_body"),JointModelRY(),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getJointId("ff1_joint"),JointModelRY(),SE3::Identity(),Inertia::Random(),
"root_joint", "root_body");
model.addJointAndBody(model.getBodyId("root_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("root_joint"),JointModelRZ(),SE3::Random(),Inertia::Random(),
"lleg1_joint", "lleg1_body");
model.addJointAndBody(model.getBodyId("lleg1_body"),JointModelRY(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("lleg1_joint"),JointModelRY(),SE3::Random(),Inertia::Random(),
"lleg2_joint", "lleg2_body");
model.addJointAndBody(model.getBodyId("root_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("root_joint"),JointModelRZ(),SE3::Random(),Inertia::Random(),
"rleg1_joint", "rleg1_body");
model.addJointAndBody(model.getBodyId("rleg1_body"),JointModelRY(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("rleg1_joint"),JointModelRY(),SE3::Random(),Inertia::Random(),
"rleg2_joint", "rleg2_body");
model.addJointAndBody(model.getBodyId("root_body"),JointModelRY(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("root_joint"),JointModelRY(),SE3::Random(),Inertia::Random(),
"torso1_joint", "torso1_body");
model.addJointAndBody(model.getBodyId("torso1_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("torso1_joint"),JointModelRZ(),SE3::Random(),Inertia::Random(),
"chest_joint", "chest_body");
model.addJointAndBody(model.getBodyId("chest_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("chest_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
"rarm1_joint", "rarm1_body");
model.addJointAndBody(model.getBodyId("rarm1_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("rarm1_joint"),JointModelRZ(),SE3::Random(),Inertia::Random(),
"rarm2_joint", "rarm2_body");
model.addJointAndBody(model.getBodyId("chest_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("chest_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
"larm1_joint", "larm1_body");
model.addJointAndBody(model.getBodyId("larm1_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("larm1_joint"),JointModelRZ(),SE3::Random(),Inertia::Random(),
"larm2_joint", "larm2_body");
}
......@@ -64,22 +64,22 @@ namespace se3
{
if(! usingFF )
{
model.addJointAndBody(model.getBodyId("universe"),JointModelRX(),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getJointId("universe"),JointModelRX(),SE3::Identity(),Inertia::Random(),
"ff1_joint", "ff1_body");
model.addJointAndBody(model.getBodyId("ff1_body"),JointModelRY(),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getJointId("ff1_joint"),JointModelRY(),SE3::Identity(),Inertia::Random(),
"ff2_joint", "ff2_body");
model.addJointAndBody(model.getBodyId("ff2_body"),JointModelRZ(),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getJointId("ff2_joint"),JointModelRZ(),SE3::Identity(),Inertia::Random(),
"ff3_joint", "ff3_body");
model.addJointAndBody(model.getBodyId("ff3_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("ff3_joint"),JointModelRZ(),SE3::Random(),Inertia::Random(),
"ff4_joint", "ff4_body");
model.addJointAndBody(model.getBodyId("ff4_body"),JointModelRY(),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getJointId("ff4_joint"),JointModelRY(),SE3::Identity(),Inertia::Random(),
"ff5_joint", "ff5_body");
model.addJointAndBody(model.getBodyId("ff5_body"),JointModelRX(),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getJointId("ff5_joint"),JointModelRX(),SE3::Identity(),Inertia::Random(),
"root_joint", "root_body");
}
else
{
model.addJointAndBody(model.getBodyId("universe"),JointModelFreeFlyer(),
model.addJointAndBody(model.getJointId("universe"),JointModelFreeFlyer(),
SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Zero(6), 1e3 * (Eigen::VectorXd::Random(6).array() + 1),
1e3 * (Eigen::VectorXd::Random(7).array() - 1),
......@@ -87,111 +87,111 @@ namespace se3
"root_joint", "root_body");
}
model.addJointAndBody(model.getBodyId("root_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("root_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"lleg1_joint", "lleg1_body");
model.addJointAndBody(model.getBodyId("lleg1_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("lleg1_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"lleg2_joint", "lleg2_body");
model.addJointAndBody(model.getBodyId("lleg2_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("lleg2_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"lleg3_joint", "lleg3_body");
model.addJointAndBody(model.getBodyId("lleg3_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("lleg3_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"lleg4_joint", "lleg4_body");
model.addJointAndBody(model.getBodyId("lleg4_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("lleg4_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"lleg5_joint", "lleg5_body");
model.addJointAndBody(model.getBodyId("lleg5_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("lleg5_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"lleg6_joint", "lleg6_body");
model.addJointAndBody(model.getBodyId("root_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("root_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rleg1_joint", "rleg1_body");
model.addJointAndBody(model.getBodyId("rleg1_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("rleg1_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rleg2_joint", "rleg2_body");
model.addJointAndBody(model.getBodyId("rleg2_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("rleg2_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rleg3_joint", "rleg3_body");
model.addJointAndBody(model.getBodyId("rleg3_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("rleg3_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rleg4_joint", "rleg4_body");
model.addJointAndBody(model.getBodyId("rleg4_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("rleg4_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rleg5_joint", "rleg5_body");
model.addJointAndBody(model.getBodyId("rleg5_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("rleg5_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rleg6_joint", "rleg6_body");
model.addJointAndBody(model.getBodyId("root_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("root_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"torso1_joint", "torso1_body");
model.addJointAndBody(model.getBodyId("torso1_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("torso1_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"chest_joint", "chest_body");
model.addJointAndBody(model.getBodyId("chest_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("chest_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rarm1_joint", "rarm1_body");
model.addJointAndBody(model.getBodyId("rarm1_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("rarm1_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rarm2_joint", "rarm2_body");
model.addJointAndBody(model.getBodyId("rarm2_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("rarm2_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rarm3_joint", "rarm3_body");
model.addJointAndBody(model.getBodyId("rarm3_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("rarm3_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rarm4_joint", "rarm4_body");
model.addJointAndBody(model.getBodyId("rarm4_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("rarm4_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rarm5_joint", "rarm5_body");
model.addJointAndBody(model.getBodyId("rarm5_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("rarm5_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rarm6_joint", "rarm6_body");
model.addJointAndBody(model.getBodyId("chest_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("chest_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"larm1_joint", "larm1_body");
model.addJointAndBody(model.getBodyId("larm1_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("larm1_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"larm2_joint", "larm2_body");
model.addJointAndBody(model.getBodyId("larm2_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("larm2_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"larm3_joint", "larm3_body");
model.addJointAndBody(model.getBodyId("larm3_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("larm3_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"larm4_joint", "larm4_body");
model.addJointAndBody(model.getBodyId("larm4_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("larm4_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"larm5_joint", "larm5_body");
model.addJointAndBody(model.getBodyId("larm5_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getJointId("larm5_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"larm6_joint", "larm6_body");
......
......@@ -125,7 +125,7 @@ namespace se3
fcl::CollisionObject collision_object = retrieveCollisionGeometry((*i)->geometry, package_dirs, mesh_path);
SE3 geomPlacement = convertFromUrdf((*i)->origin);
std::string collision_object_name = link_name;
geom_model.addCollisionObject(model.bodyParents[model.getBodyId(link_name)], collision_object, geomPlacement, collision_object_name, mesh_path);
geom_model.addCollisionObject(model.getFrameParent(link_name), collision_object, geomPlacement, collision_object_name, mesh_path);
}
} // if(link->collision)
......@@ -144,7 +144,7 @@ namespace se3
fcl::CollisionObject visual_object = retrieveCollisionGeometry((*i)->geometry, package_dirs, mesh_path);
SE3 geomPlacement = convertFromUrdf((*i)->origin);
std::string visual_object_name = link_name;
geom_model.addVisualObject(model.bodyParents[model.getBodyId(link_name)], visual_object, geomPlacement, visual_object_name, mesh_path);
geom_model.addVisualObject(model.getFrameParent(link_name), visual_object, geomPlacement, visual_object_name, mesh_path);
}
} // if(link->visual)
......
......@@ -371,7 +371,7 @@ namespace se3
// Add a frame in the model to keep trace of this fixed joint
model.addFrame(joint->name, parent_joint_id, nextPlacementOffset);
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.
......
......@@ -110,24 +110,15 @@ 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<>()) )
.add_property("parents",
bp::make_function(&ModelPythonVisitor::parents,
bp::return_internal_reference<>()) )
.add_property("bodyParents",
bp::make_function(&ModelPythonVisitor::bodyParents,
bp::return_internal_reference<>()) )
.add_property("names",
bp::make_function(&ModelPythonVisitor::names,
bp::return_internal_reference<>()) )
.add_property("bodyNames",
bp::make_function(&ModelPythonVisitor::bodyNames,
bp::return_internal_reference<>()) )
.def("addJointAndBody",&ModelPythonVisitor::addJointAndBodyToModel)
......@@ -162,12 +153,9 @@ namespace se3
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<Model::JointIndex> & bodyParents( ModelHandler & m ) { return m->bodyParents; }
static std::vector<std::string> & names ( ModelHandler & m ) { return m->names; }
static std::vector<std::string> & bodyNames ( ModelHandler & m ) { return m->bodyNames; }
static Model::Index addJointAndBodyToModel(ModelHandler & modelPtr,
Model::JointIndex idx, bp::object joint,
......
......@@ -49,15 +49,15 @@ BOOST_AUTO_TEST_CASE ( test_FD )
VectorXd v = VectorXd::Ones(model.nv);
VectorXd tau = VectorXd::Zero(model.nv);
const std::string RF = "rleg6_body";
const std::string LF = "lleg6_body";
const std::string RF = "rleg6_joint";
const std::string LF = "lleg6_joint";
Data::Matrix6x J_RF (6, model.nv);
J_RF.setZero();
getJacobian <true> (model, data, model.getBodyId(RF), J_RF);
getJacobian <true> (model, data, model.getJointId(RF), J_RF);
Data::Matrix6x J_LF (6, model.nv);
J_LF.setZero();
getJacobian <true> (model, data, model.getBodyId(LF), J_LF);
getJacobian <true> (model, data, model.getJointId(LF), J_LF);
Eigen::MatrixXd J (12, model.nv);
J.setZero();
......@@ -111,15 +111,15 @@ BOOST_AUTO_TEST_CASE ( test_ID )
VectorXd v_before = VectorXd::Ones(model.nv);
const std::string RF = "rleg6_body";
const std::string LF = "lleg6_body";
const std::string RF = "rleg6_joint";
const std::string LF = "lleg6_joint";
Data::Matrix6x J_RF (6, model.nv);
J_RF.setZero();
getJacobian <true> (model, data, model.getBodyId(RF), J_RF);
getJacobian <true> (model, data, model.getJointId(RF), J_RF);
Data::Matrix6x J_LF (6, model.nv);
J_LF.setZero();
getJacobian <true> (model, data, model.getBodyId(LF), J_LF);
getJacobian <true> (model, data, model.getJointId(LF), J_LF);
Eigen::MatrixXd J (12, model.nv);
J.setZero();
......@@ -183,13 +183,13 @@ BOOST_AUTO_TEST_CASE (timings_fd_llt)
VectorXd v = VectorXd::Ones(model.nv);
VectorXd tau = VectorXd::Zero(model.nv);
const std::string RF = "rleg6_body";
const std::string LF = "lleg6_body";
const std::string RF = "rleg6_joint";
const std::string LF = "lleg6_joint";
Data::Matrix6x J_RF (6, model.nv);
getJacobian <true> (model, data, model.getBodyId(RF), J_RF);
getJacobian <true> (model, data, model.getJointId(RF), J_RF);
Data::Matrix6x J_LF (6, model.nv);
getJacobian <true> (model, data, model.getBodyId(LF), J_LF);
getJacobian <true> (model, data, model.getJointId(LF), J_LF);
Eigen::MatrixXd J (12, model.nv);
J.topRows<6> () = J_RF;
......
......@@ -149,9 +149,9 @@ BOOST_AUTO_TEST_CASE ( simple_boxes )
using namespace se3;
model.addJointAndBody(model.getBodyId("universe"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getJointId("universe"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
"planar1_joint", "planar1_body");
model.addJointAndBody(model.getBodyId("universe"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getJointId("universe"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
"planar2_joint", "planar2_body");
boost::shared_ptr<fcl::Box> Sample(new fcl::Box(1));
......
......@@ -43,7 +43,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
VectorXd q = VectorXd::Zero(model.nq);
computeJacobians(model,data,q);
Model::Index idx = model.existBodyName("rarm2")?model.getBodyId("rarm2"):(Model::Index)(model.nbody-1);
Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.nbody-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.existBodyName("rarm6")?model.getBodyId("rarm6"):(Model::Index)(model.nbody-1);
Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.nbody-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.existBodyName("rarm6")?model.getBodyId("rarm6"):(Model::Index)(model.nbody-1);
Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.nbody-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.existBodyName("rarm6")?model.getBodyId("rarm6"):(Model::Index)(model.nbody-1);
Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.nbody-1);
Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
timer.tic();
......
......@@ -70,23 +70,23 @@ BOOST_AUTO_TEST_CASE ( integration_test )
using namespace se3;
model.addJointAndBody(model.getBodyId("universe"),JointModelFreeFlyer(),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getJointId("universe"),JointModelFreeFlyer(),SE3::Identity(),Inertia::Random(),
"freeflyer_joint", "freeflyer_body");
model.addJointAndBody(model.getBodyId("freeflyer_body"),JointModelSpherical(),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getJointId("freeflyer_joint"),JointModelSpherical(),SE3::Identity(),Inertia::Random(),
"spherical_joint", "spherical_body");
model.addJointAndBody(model.getBodyId("spherical_body"),JointModelRX(),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getJointId("spherical_joint"),JointModelRX(),SE3::Identity(),Inertia::Random(),
"revolute_joint", "revolute_body");
model.addJointAndBody(model.getBodyId("revolute_body"),JointModelPX(),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getJointId("revolute_joint"),JointModelPX(),SE3::Identity(),Inertia::Random(),
"px_joint", "px_body");
model.addJointAndBody(model.getBodyId("px_body"),JointModelPrismaticUnaligned(Eigen::Vector3d(1,0,0)),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getJointId("px_joint"),JointModelPrismaticUnaligned(Eigen::Vector3d(1,0,0)),SE3::Identity(),Inertia::Random(),
"pu_joint", "pu_body");
model.addJointAndBody(model.getBodyId("pu_body"),JointModelRevoluteUnaligned(Eigen::Vector3d(0,0,1)),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getJointId("pu_joint"),JointModelRevoluteUnaligned(Eigen::Vector3d(0,0,1)),SE3::Identity(),Inertia::Random(),
"ru_joint", "ru_body");
model.addJointAndBody(model.getBodyId("ru_body"),JointModelSphericalZYX(),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getJointId("ru_joint"),JointModelSphericalZYX(),SE3::Identity(),Inertia::Random(),
"sphericalZYX_joint", "sphericalZYX_body");
model.addJointAndBody(model.getBodyId("sphericalZYX_body"),JointModelTranslation(),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getJointId("sphericalZYX_joint"),JointModelTranslation(),SE3::Identity(),Inertia::Random(),
"translation_joint", "translation_body");
model.addJointAndBody(model.getBodyId("translation_body"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getJointId("translation_joint"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
"planar_joint", "planar_body");
se3::Data data(model);
......@@ -122,23 +122,23 @@ BOOST_AUTO_TEST_CASE ( interpolation_test )