From 3aaefd1387a72f475cef1a6f82d42ac63f4fca74 Mon Sep 17 00:00:00 2001
From: Antonio El Khoury <antonio.elkhoury@wandercraft.eu>
Date: Mon, 12 Oct 2015 14:14:09 +0200
Subject: [PATCH] [C++][Bug] Avoid confusion between joint and body methods in
se3::Model.
* se3::Model::getBodyId was used to retrieve joint id in parsers.
* Implement new methods to retrieve joint ids.
* Fix body methods.
---
src/multibody/model.hpp | 33 ++++-
src/multibody/parser/sample-models.hpp | 168 ++++++++++++++++---------
src/multibody/parser/urdf.hpp | 3 +-
unittest/joints.cpp | 8 +-
4 files changed, 142 insertions(+), 70 deletions(-)
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index b18d03b11..d860403f1 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -1,5 +1,6 @@
//
// Copyright (c) 2015 CNRS
+// Copyright (c) 2015 Wandercraft
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
@@ -102,6 +103,9 @@ namespace se3
Index getBodyId( const std::string & name ) const;
bool existBodyName( const std::string & name ) const;
const std::string& getBodyName( Index index ) const;
+ Index getJointId( const std::string & name ) const;
+ bool existJointName( const std::string & name ) const;
+ const std::string& getJointName( Index index ) const;
};
class Data
@@ -276,23 +280,40 @@ namespace se3
inline Model::Index Model::getBodyId( const std::string & name ) const
{
std::vector<std::string>::iterator::difference_type
- res = std::find(names.begin(),names.end(),name) - names.begin();
+ 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 do not exist" );
return Model::Index(res);
}
inline bool Model::existBodyName( const std::string & name ) const
{
- std::vector<std::string>::iterator::difference_type
- res = std::find(names.begin(),names.end(),name) - names.begin();
- return (res>=0)&&(res<nbody);
+ return (bodyNames.end() != std::find(bodyNames.begin(),bodyNames.end(),name));
}
-
+
inline const std::string& Model::getBodyName( Model::Index index ) const
{
assert( index < (Model::Index)nbody );
+ return bodyNames[index];
+ }
+
+ inline Model::Index Model::getJointId( const std::string & name ) const
+ {
+ std::vector<std::string>::iterator::difference_type
+ res = std::find(names.begin(),names.end(),name) - names.begin();
+ assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
+ assert( (res>=0)&&(res<joints.size())&&"The joint name you asked do not exist" );
+ return Model::Index(res);
+ }
+ inline bool Model::existJointName( const std::string & name ) const
+ {
+ return (names.end() != std::find(names.begin(),names.end(),name));
+ }
+
+ inline const std::string& Model::getJointName( Model::Index index ) const
+ {
+ assert( index < (Model::Index)joints.size() );
return names[index];
- }
+ }
inline Data::Data( const Model& ref )
:model(ref)
diff --git a/src/multibody/parser/sample-models.hpp b/src/multibody/parser/sample-models.hpp
index 6506b5a8d..7f76e7284 100644
--- a/src/multibody/parser/sample-models.hpp
+++ b/src/multibody/parser/sample-models.hpp
@@ -1,5 +1,6 @@
//
// Copyright (c) 2015 CNRS
+// Copyright (c) 2015 Wandercraft
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
@@ -25,71 +26,118 @@ namespace se3
namespace buildModels
{
- void humanoid2d( Model& model)
+ void humanoid2d( Model& model)
{
- model.addBody(model.getBodyId("universe"),JointModelRX(),SE3::Identity(),Inertia::Random(),"ff1");
- model.addBody(model.getBodyId("ff1"),JointModelRY(),SE3::Identity(),Inertia::Random(),"root");
-
- model.addBody(model.getBodyId("root"),JointModelRZ(),SE3::Random(),Inertia::Random(),"lleg1");
- model.addBody(model.getBodyId("lleg1"),JointModelRY(),SE3::Random(),Inertia::Random(),"lleg2");
-
- model.addBody(model.getBodyId("root"),JointModelRZ(),SE3::Random(),Inertia::Random(),"rleg1");
- model.addBody(model.getBodyId("rleg1"),JointModelRY(),SE3::Random(),Inertia::Random(),"rleg2");
-
- model.addBody(model.getBodyId("root"),JointModelRY(),SE3::Random(),Inertia::Random(),"torso1");
- model.addBody(model.getBodyId("torso1"),JointModelRZ(),SE3::Random(),Inertia::Random(),"chest");
-
- model.addBody(model.getBodyId("chest"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm1");
- model.addBody(model.getBodyId("rarm1"),JointModelRZ(),SE3::Random(),Inertia::Random(),"rarm2");
-
- model.addBody(model.getBodyId("chest"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm1");
- model.addBody(model.getBodyId("larm1"),JointModelRZ(),SE3::Random(),Inertia::Random(),"larm2");
+ model.addBody(model.getBodyId("universe"),JointModelRX(),SE3::Identity(),Inertia::Random(),
+ "ff1_joint", "ff1_body");
+ model.addBody(model.getBodyId("ff1_body"),JointModelRY(),SE3::Identity(),Inertia::Random(),
+ "root_joint", "root_body");
+
+ model.addBody(model.getBodyId("root_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
+ "lleg1_joint", "lleg1_body");
+ model.addBody(model.getBodyId("lleg1_body"),JointModelRY(),SE3::Random(),Inertia::Random(),
+ "lleg2_joint", "lleg2_body");
+
+ model.addBody(model.getBodyId("root_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
+ "rleg1_joint", "rleg1_body");
+ model.addBody(model.getBodyId("rleg1_body"),JointModelRY(),SE3::Random(),Inertia::Random(),
+ "rleg2_joint", "rleg2_body");
+
+ model.addBody(model.getBodyId("root_body"),JointModelRY(),SE3::Random(),Inertia::Random(),
+ "torso1_joint", "torso1_body");
+ model.addBody(model.getBodyId("torso1_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
+ "chest_joint", "chest_body");
+
+ model.addBody(model.getBodyId("chest_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "rarm1_joint", "rarm1_body");
+ model.addBody(model.getBodyId("rarm1_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
+ "rarm2_joint", "rarm2_body");
+
+ model.addBody(model.getBodyId("chest_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "larm1_joint", "larm1_body");
+ model.addBody(model.getBodyId("larm1_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
+ "larm2_joint", "larm2_body");
}
void humanoidSimple( Model& model, bool usingFF = true)
- {
- if(! usingFF )
- {
- model.addBody(model.getBodyId("universe"),JointModelRX(),SE3::Identity(),Inertia::Random(),"ff1");
- model.addBody(model.getBodyId("ff1"),JointModelRY(),SE3::Identity(),Inertia::Random(),"ff2");
- model.addBody(model.getBodyId("ff2"),JointModelRZ(),SE3::Identity(),Inertia::Random(),"ff3");
- model.addBody(model.getBodyId("ff3"),JointModelRZ(),SE3::Random(),Inertia::Random(),"ff4");
- model.addBody(model.getBodyId("ff4"),JointModelRY(),SE3::Identity(),Inertia::Random(),"ff5");
- model.addBody(model.getBodyId("ff5"),JointModelRX(),SE3::Identity(),Inertia::Random(),"root");
- }
+ {
+ if(! usingFF )
+ {
+ model.addBody(model.getBodyId("universe"),JointModelRX(),SE3::Identity(),Inertia::Random(),
+ "ff1_joint", "ff1_body");
+ model.addBody(model.getBodyId("ff1_body"),JointModelRY(),SE3::Identity(),Inertia::Random(),
+ "ff2_joint", "ff2_body");
+ model.addBody(model.getBodyId("ff2_body"),JointModelRZ(),SE3::Identity(),Inertia::Random(),
+ "ff3_joint", "ff3_body");
+ model.addBody(model.getBodyId("ff3_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
+ "ff4_joint", "ff4_body");
+ model.addBody(model.getBodyId("ff4_body"),JointModelRY(),SE3::Identity(),Inertia::Random(),
+ "ff5_joint", "ff5_body");
+ model.addBody(model.getBodyId("ff5_body"),JointModelRX(),SE3::Identity(),Inertia::Random(),
+ "root_joint", "root_body");
+ }
else
- model.addBody(model.getBodyId("universe"),JointModelFreeFlyer(),SE3::Identity(),Inertia::Random(),"root");
-
- model.addBody(model.getBodyId("root"),JointModelRX(),SE3::Random(),Inertia::Random(),"lleg1");
- model.addBody(model.getBodyId("lleg1"),JointModelRX(),SE3::Random(),Inertia::Random(),"lleg2");
- model.addBody(model.getBodyId("lleg2"),JointModelRX(),SE3::Random(),Inertia::Random(),"lleg3");
- model.addBody(model.getBodyId("lleg3"),JointModelRX(),SE3::Random(),Inertia::Random(),"lleg4");
- model.addBody(model.getBodyId("lleg4"),JointModelRX(),SE3::Random(),Inertia::Random(),"lleg5");
- model.addBody(model.getBodyId("lleg5"),JointModelRX(),SE3::Random(),Inertia::Random(),"lleg6");
-
- model.addBody(model.getBodyId("root"),JointModelRX(),SE3::Random(),Inertia::Random(),"rleg1");
- model.addBody(model.getBodyId("rleg1"),JointModelRX(),SE3::Random(),Inertia::Random(),"rleg2");
- model.addBody(model.getBodyId("rleg2"),JointModelRX(),SE3::Random(),Inertia::Random(),"rleg3");
- model.addBody(model.getBodyId("rleg3"),JointModelRX(),SE3::Random(),Inertia::Random(),"rleg4");
- model.addBody(model.getBodyId("rleg4"),JointModelRX(),SE3::Random(),Inertia::Random(),"rleg5");
- model.addBody(model.getBodyId("rleg5"),JointModelRX(),SE3::Random(),Inertia::Random(),"rleg6");
-
- model.addBody(model.getBodyId("root"),JointModelRX(),SE3::Random(),Inertia::Random(),"torso1");
- model.addBody(model.getBodyId("torso1"),JointModelRX(),SE3::Random(),Inertia::Random(),"chest");
-
- model.addBody(model.getBodyId("chest"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm1");
- model.addBody(model.getBodyId("rarm1"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm2");
- model.addBody(model.getBodyId("rarm2"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm3");
- model.addBody(model.getBodyId("rarm3"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm4");
- model.addBody(model.getBodyId("rarm4"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm5");
- model.addBody(model.getBodyId("rarm5"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm6");
-
- model.addBody(model.getBodyId("chest"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm1");
- model.addBody(model.getBodyId("larm1"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm2");
- model.addBody(model.getBodyId("larm2"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm3");
- model.addBody(model.getBodyId("larm3"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm4");
- model.addBody(model.getBodyId("larm4"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm5");
- model.addBody(model.getBodyId("larm5"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm6");
+ {
+ model.addBody(model.getBodyId("universe"),JointModelFreeFlyer(),SE3::Identity(),
+ Inertia::Random(),"root_joint", "root_body");
+ }
+
+ model.addBody(model.getBodyId("root_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "lleg1_joint", "lleg1_body");
+ model.addBody(model.getBodyId("lleg1_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "lleg2_joint", "lleg2_body");
+ model.addBody(model.getBodyId("lleg2_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "lleg3_joint", "lleg3_body");
+ model.addBody(model.getBodyId("lleg3_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "lleg4_joint", "lleg4_body");
+ model.addBody(model.getBodyId("lleg4_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "lleg5_joint", "lleg5_body");
+ model.addBody(model.getBodyId("lleg5_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "lleg6_joint", "lleg6_body");
+
+ model.addBody(model.getBodyId("root_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "rleg1_joint", "rleg1_body");
+ model.addBody(model.getBodyId("rleg1_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "rleg2_joint", "rleg2_body");
+ model.addBody(model.getBodyId("rleg2_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "rleg3_joint", "rleg3_body");
+ model.addBody(model.getBodyId("rleg3_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "rleg4_joint", "rleg4_body");
+ model.addBody(model.getBodyId("rleg4_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "rleg5_joint", "rleg5_body");
+ model.addBody(model.getBodyId("rleg5_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "rleg6_joint", "rleg6_body");
+
+ model.addBody(model.getBodyId("root_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "torso1_joint", "torso1_body");
+ model.addBody(model.getBodyId("torso1_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "chest_joint", "chest_body");
+
+ model.addBody(model.getBodyId("chest_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "rarm1_joint", "rarm1_body");
+ model.addBody(model.getBodyId("rarm1_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "rarm2_joint", "rarm2_body");
+ model.addBody(model.getBodyId("rarm2_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "rarm3_joint", "rarm3_body");
+ model.addBody(model.getBodyId("rarm3_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "rarm4_joint", "rarm4_body");
+ model.addBody(model.getBodyId("rarm4_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "rarm5_joint", "rarm5_body");
+ model.addBody(model.getBodyId("rarm5_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "rarm6_joint", "rarm6_body");
+
+ model.addBody(model.getBodyId("chest_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "larm1_joint", "larm1_body");
+ model.addBody(model.getBodyId("larm1_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "larm2_joint", "larm2_body");
+ model.addBody(model.getBodyId("larm2_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "larm3_joint", "larm3_body");
+ model.addBody(model.getBodyId("larm3_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "larm4_joint", "larm4_body");
+ model.addBody(model.getBodyId("larm4_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "larm5_joint", "larm5_body");
+ model.addBody(model.getBodyId("larm5_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ "larm6_joint", "larm6_body");
}
} // namespace buildModels
diff --git a/src/multibody/parser/urdf.hpp b/src/multibody/parser/urdf.hpp
index 725fb5109..a9ef9f613 100644
--- a/src/multibody/parser/urdf.hpp
+++ b/src/multibody/parser/urdf.hpp
@@ -1,5 +1,6 @@
//
// Copyright (c) 2015 CNRS
+// Copyright (c) 2015 Wandercraft
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
@@ -100,7 +101,7 @@ namespace se3
Model::Index parent
= (link->getParent()->parent_joint==NULL) ?
(freeFlyer ? 1 : 0)
- : model.getBodyId( link->getParent()->parent_joint->name );
+ : model.getJointId( link->getParent()->parent_joint->name );
//std::cout << joint->name << " === " << parent << std::endl;
const SE3 & jointPlacement = placementOffset*convertFromUrdf(joint->parent_to_joint_origin_transform);
diff --git a/unittest/joints.cpp b/unittest/joints.cpp
index f7ec4a27d..3d103dfd4 100644
--- a/unittest/joints.cpp
+++ b/unittest/joints.cpp
@@ -1,5 +1,6 @@
//
// Copyright (c) 2015 CNRS
+// Copyright (c) 2015 Wandercraft
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
@@ -997,10 +998,11 @@ BOOST_AUTO_TEST_CASE ( test_merge_body )
SE3 liMi(Matrix3::Identity(),Vector3(1.0, 1.0, 0.0));
//SE3 liMi(Matrix3::Identity(),Vector3d::Zero());
- model.addBody (model.getBodyId("universe"), JointModelRX (), SE3::Identity (), inertiaRoot, "root");
- model.mergeFixedBody(model.getBodyId("root"), liMi, inertiaFixedBodyAtJoint);
+ model.addBody (model.getBodyId("universe"), JointModelRX (), SE3::Identity (), inertiaRoot,
+ "root_joint", "root_body");
+ model.mergeFixedBody(model.getBodyId("root_body"), liMi, inertiaFixedBodyAtJoint);
- Inertia mergedInertia(model.inertias[(size_t)(model.getBodyId("root"))]);
+ Inertia mergedInertia(model.inertias[(size_t)(model.getBodyId("root_body"))]);
double expected_mass=2;
Eigen::Vector3d expected_com(Eigen::Vector3d::Zero());expected_com << 1.125, 0.5, 0.;
--
GitLab