Skip to content
GitLab
Commit 1e03ac79 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Reorganize headers to reduce dependency to pinocchio

parent da4ac8b0
Hide whitespace changes
Inline Side-by-side
include/hpp/core/configuration-shooter/uniform.hh
View file @ 1e03ac79
......@@ -21,8 +21,6 @@
# include <sstream>
# include <pinocchio/algorithm/joint-configuration.hpp>
# include <hpp/pinocchio/device.hh>
# include <hpp/core/configuration-shooter.hh>
......
src/config-projector.cc
View file @ 1e03ac79
......@@ -25,14 +25,12 @@
#include <hpp/util/debug.hh>
#include <hpp/util/timer.hh>
#include <pinocchio/multibody/liegroup/liegroup.hpp>
#include <pinocchio/multibody/model.hpp>
#include <hpp/pinocchio/configuration.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/extra-config-space.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/liegroup.hh>
#include <hpp/constraints/differentiable-function.hh>
#include <hpp/constraints/solver/by-substitution.hh>
......
src/configuration-shooter/uniform.cc
View file @ 1e03ac79
......@@ -18,6 +18,8 @@
# include <hpp/core/configuration-shooter/uniform.hh>
# include <pinocchio/algorithm/joint-configuration.hpp>
namespace hpp {
namespace core {
namespace configurationShooter {
......
src/continuous-collision-checking/dichotomy.cc
View file @ 1e03ac79
......@@ -19,7 +19,6 @@
#include <hpp/core/continuous-collision-checking/dichotomy.hh>
#include <deque>
#include <pinocchio/multibody/geometry.hpp>
#include <hpp/util/debug.hh>
#include <hpp/core/collision-path-validation-report.hh>
#include <hpp/core/straight-path.hh>
......
src/continuous-collision-checking/path-velocity.hh
View file @ 1e03ac79
......@@ -22,10 +22,6 @@
# include <boost/mpl/vector.hpp>
# include <boost/mpl/for_each.hpp>
# include <pinocchio/multibody/geometry.hpp>
# include <hpp/core/path/spline.hh>
namespace hpp {
namespace core {
namespace continuousCollisionChecking {
......
src/continuous-collision-checking/progressive.cc
View file @ 1e03ac79
......@@ -19,7 +19,6 @@
#include <hpp/core/continuous-collision-checking/progressive.hh>
#include <limits>
#include <pinocchio/multibody/geometry.hpp>
#include <hpp/util/debug.hh>
#include <hpp/core/collision-path-validation-report.hh>
#include <hpp/core/straight-path.hh>
......
src/distance-between-objects.cc
View file @ 1e03ac79
......@@ -20,8 +20,6 @@
#include <hpp/fcl/distance.h>
#include <pinocchio/multibody/geometry.hpp>
#include <hpp/pinocchio/collision-object.hh>
#include <hpp/pinocchio/body.hh>
#include <hpp/pinocchio/device.hh>
......
src/explicit-relative-transformation.cc
View file @ 1e03ac79
......@@ -16,7 +16,6 @@
#include <hpp/core/explicit-relative-transformation.hh>
#include <pinocchio/spatial/explog.hpp>
#include <pinocchio/spatial/skew.hpp>
#include <hpp/pinocchio/device.hh>
......
src/path-optimization/partial-shortcut.cc
View file @ 1e03ac79
......@@ -18,7 +18,6 @@
#include <hpp/util/debug.hh>
#include <pinocchio/multibody/joint/joint.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <hpp/pinocchio/joint.hh>
......
src/path-optimization/simple-time-parameterization.cc
View file @ 1e03ac79
......@@ -18,7 +18,6 @@
#include <limits>
#include <pinocchio/multibody/model.hpp>
#include <hpp/pinocchio/configuration.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/liegroup.hh>
......
src/reeds-shepp-path.cc
View file @ 1e03ac79
......@@ -22,8 +22,6 @@
#include <hpp/util/debug.hh>
#include <pinocchio/spatial/se3.hpp>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/liegroup.hh>
......
src/steering-method/car-like.cc
View file @ 1e03ac79
......@@ -14,12 +14,14 @@
// received a copy of the GNU Lesser General Public License along with
// hpp-core. If not, see <http://www.gnu.org/licenses/>.
#include <hpp/core/steering-method/car-like.hh>
#include <pinocchio/multibody/joint/joint-generic.hpp>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/core/problem.hh>
#include <hpp/core/steering-method/car-like.hh>
#include <pinocchio/spatial/se3.hpp>
#include <pinocchio/multibody/joint/joint.hpp>
namespace hpp {
namespace core {
......
tests/configuration-shooters.cc
View file @ 1e03ac79
......@@ -20,8 +20,6 @@
#include <hpp/core/configuration-shooter/uniform.hh>
#include <hpp/core/configuration-shooter/gaussian.hh>
#include <pinocchio/multibody/model.hpp>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/configuration.hh>
#include <hpp/pinocchio/simple-device.hh>
......@@ -48,9 +46,6 @@ void basic_test (CS_t cs, DevicePtr_t robot)
BOOST_AUTO_TEST_CASE (uniform)
{
DevicePtr_t robot = pin_test::makeDevice(pin_test::HumanoidSimple);
robot->model().lowerPositionLimit.head<3>().setConstant(-1);
robot->model().upperPositionLimit.head<3>().setConstant( 1);
UniformPtr_t cs = Uniform::create (robot);
basic_test (cs, robot);
......
tests/explicit-relative-transformation.cc
View file @ 1e03ac79
......@@ -26,14 +26,12 @@
#include <hpp/util/timer.hh>
#include <hpp/core/fwd.hh>
#include <hpp/core/configuration-shooter/uniform.hh>
#include <hpp/constraints/explicit/relative-pose.hh>
#include <hpp/constraints/matrix-view.hh>
#include <hpp/constraints/differentiable-function.hh>
#include <hpp/constraints/generic-transformation.hh>
#include <pinocchio/spatial/skew.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <hpp/pinocchio/simple-device.hh>
#include <hpp/pinocchio/urdf/util.hh>
#include <hpp/pinocchio/joint.hh>
......@@ -82,6 +80,7 @@ BOOST_AUTO_TEST_CASE (two_freeflyers)
{
DevicePtr_t robot = createRobot();
BOOST_REQUIRE (robot);
configurationShooter::UniformPtr_t cs = configurationShooter::Uniform::create(robot);
JointPtr_t object2 = robot->getJointByName("obj2/root_joint");
JointPtr_t object1 = robot->getJointByName("obj1/root_joint");
......@@ -95,7 +94,7 @@ BOOST_AUTO_TEST_CASE (two_freeflyers)
DifferentiableFunctionPtr_t ert (enm->explicitFunction ());
Configuration_t q = robot->currentConfiguration (),
qrand = se3::randomConfiguration(robot->model()),
qrand = *cs->shoot(),
qout = qrand;
// Check the output value
......@@ -146,7 +145,7 @@ BOOST_AUTO_TEST_CASE (two_freeflyers)
}
// Second at random configurations
for (size_type i=0; i<NB_RANDOM_CONF; ++i) {
q0 = se3::randomConfiguration(robot->model());
q0 = *cs->shoot();
enm->function ().jacobian (J, q0);
std::cout << "J=" << std::endl << J << std::endl;
std::cout << "q0=" << q0.transpose () << std::endl;
......@@ -173,6 +172,7 @@ BOOST_AUTO_TEST_CASE (two_frames_on_freeflyer)
{
DevicePtr_t robot = createRobot();
BOOST_REQUIRE (robot);
configurationShooter::UniformPtr_t cs = configurationShooter::Uniform::create(robot);
JointPtr_t object2 = robot->getJointByName("obj2/root_joint");
JointPtr_t object1 = robot->getJointByName("obj1/root_joint");
......@@ -189,7 +189,7 @@ BOOST_AUTO_TEST_CASE (two_frames_on_freeflyer)
DifferentiableFunctionPtr_t ert (enm->explicitFunction ());
Configuration_t q = robot->currentConfiguration (),
qrand = se3::randomConfiguration(robot->model()),
qrand = *cs->shoot(),
qout = qrand;
// Check the output value
......@@ -240,7 +240,7 @@ BOOST_AUTO_TEST_CASE (two_frames_on_freeflyer)
}
// Second at random configurations
for (size_type i=0; i<NB_RANDOM_CONF; ++i) {
q0 = se3::randomConfiguration(robot->model());
q0 = *cs->shoot();
enm->function ().jacobian (J, q0);
std::cout << "J=" << std::endl << J << std::endl;
std::cout << "q0=" << q0.transpose () << std::endl;
......@@ -267,6 +267,7 @@ BOOST_AUTO_TEST_CASE (compare_to_relative_transform)
{
DevicePtr_t robot = createRobot();
BOOST_REQUIRE (robot);
configurationShooter::UniformPtr_t cs = configurationShooter::Uniform::create(robot);
JointPtr_t object2 = robot->getJointByName("obj2/root_joint");
JointPtr_t object1 = robot->getJointByName("obj1/root_joint");
......@@ -287,7 +288,7 @@ BOOST_AUTO_TEST_CASE (compare_to_relative_transform)
object1, object2, M1inO1, M2inO2);
Configuration_t q = robot->currentConfiguration (),
qrand = se3::randomConfiguration(robot->model()),
qrand = *cs->shoot(),
qout = qrand;
// Check the output value
......@@ -334,7 +335,7 @@ BOOST_AUTO_TEST_CASE (compare_to_relative_transform)
// Second at random configurations
for (size_type i=0; i<NB_RANDOM_CONF; ++i) {
q0 = se3::randomConfiguration(robot->model());
q0 = *cs->shoot();
irt->value (v0, q0);
irt->jacobian (J0, q0);
......
tests/path-projectors.cc
View file @ 1e03ac79
......@@ -32,17 +32,14 @@
#define HPP_ENABLE_BENCHMARK 1
#include <hpp/util/timer.hh>
#include <pinocchio/multibody/joint/joint-variant.hpp>
#include <pinocchio/multibody/geometry.hpp>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/configuration.hh>
#include <hpp/pinocchio/urdf/util.hh>
#include <hpp/constraints/differentiable-function.hh>
#include <hpp/constraints/implicit.hh>
#include <hpp/core/straight-path.hh>
#include <hpp/core/config-projector.hh>
#include <hpp/constraints/implicit.hh>
#include <hpp/core/constraint-set.hh>
#include <hpp/core/problem.hh>
#include <hpp/core/interpolated-path.hh>
......@@ -56,66 +53,34 @@
#include <hpp/core/path-projector/recursive-hermite.hh>
using hpp::pinocchio::Device;
using hpp::pinocchio::DevicePtr_t;
using hpp::pinocchio::JointPtr_t;
using hpp::constraints::Implicit;
using namespace hpp::core;
using namespace hpp::pinocchio;
using ::se3::JointModelPX;
using ::se3::JointModelPY;
using ::se3::JointModelPZ;
using ::se3::JointIndex;
DevicePtr_t createRobot ()
{
DevicePtr_t robot = Device::create ("test");
const std::string& name = robot->name ();
ModelPtr_t m = ModelPtr_t(new ::se3::Model());
GeomModelPtr_t gm = GeomModelPtr_t(new ::se3::GeometryModel());
robot->setModel(m);
robot->setGeomModel(gm);
Transform3f mat; mat.setIdentity ();
std::string jointName = name + "_x";
std::string urdf ("<robot name='test'>"
"<link name='link1'/>"
"<link name='link2'/>"
"<link name='link3'/>"
"<joint name='tx' type='prismatic'>"
"<parent link='link1'/>"
"<child link='link2'/>"
"<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
"</joint>"
"<joint name='ty' type='prismatic'>"
"<axis xyz='0 1 0'/>"
"<parent link='link2'/>"
"<child link='link3'/>"
"<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
"</joint>"
"</robot>"
);
JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
JointModelPX::TangentVector_t max_velocity = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
JointModelPX::ConfigVector_t lower_position = JointModelPY::ConfigVector_t::Constant(JointModelPX::NQ,-4);
JointModelPX::ConfigVector_t upper_position = JointModelPY::ConfigVector_t::Constant(JointModelPX::NQ,4);
JointIndex idX = robot->model().addJoint(0,JointModelPX(), mat, jointName, max_effort,max_velocity,lower_position,upper_position);
JointModelPY::TangentVector_t max_effortY = JointModelPY::TangentVector_t::Constant(JointModelPY::NV,std::numeric_limits<double>::max());
JointModelPY::TangentVector_t max_velocityY = JointModelPY::TangentVector_t::Constant(JointModelPY::NV,std::numeric_limits<double>::max());
JointModelPY::ConfigVector_t lower_positionY = JointModelPY::ConfigVector_t::Constant(JointModelPY::NQ,-4);
JointModelPY::ConfigVector_t upper_positionY = JointModelPY::ConfigVector_t::Constant(JointModelPY::NQ,4);
std::string jointNameY = name + "_y";
robot->model().addJoint(idX,JointModelPY(), mat,jointNameY,max_effortY,max_velocityY,lower_positionY,upper_positionY);
robot->createData();
robot->createGeomData();
return robot;
/*
// Translation along x
joint = objectFactory.createJointTranslation2 (mat);
joint->name (jointName);
joint->isBounded (0, 1);
joint->lowerBound (0, -4);
joint->upperBound (0, +4);
joint->isBounded (1, 1);
joint->lowerBound (1, -4);
joint->upperBound (1, +4);
robot->rootJoint (joint);
DevicePtr_t robot = Device::create ("test");
urdf::loadModelFromString (robot, 0, "", "anchor", urdf, "");
return robot;
*/
}
ConstraintSetPtr_t createConstraints (DevicePtr_t r)
......
tests/paths.cc
View file @ 1e03ac79
......@@ -26,122 +26,61 @@
// because the original timers are already included by
// the unit test framework
// #include <boost/timer.hh>
#include <hpp/core/straight-path.hh>
#include <hpp/core/subchain-path.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/configuration.hh>
#include <hpp/pinocchio/urdf/util.hh>
#include <hpp/core/problem.hh>
#include <hpp/core/path.hh>
#include <hpp/core/straight-path.hh>
#include <hpp/core/subchain-path.hh>
#include <pinocchio/multibody/joint/joint-variant.hpp>
#include <pinocchio/multibody/geometry.hpp>
#define TOSTR( x ) static_cast< std::ostringstream & >( ( std::ostringstream() << x ) ).str()
using hpp::pinocchio::Device;
using hpp::pinocchio::DevicePtr_t;
using hpp::pinocchio::JointPtr_t;
using namespace hpp::core;
using namespace hpp::pinocchio;
using ::se3::JointModelPX;
using ::se3::JointModelPY;
using ::se3::JointModelPZ;
using ::se3::JointIndex;
DevicePtr_t createRobot ()
{
DevicePtr_t robot = Device::create ("test");
const std::string& name = robot->name ();
ModelPtr_t m = ModelPtr_t(new ::se3::Model());
GeomModelPtr_t gm = GeomModelPtr_t(new ::se3::GeometryModel());
robot->setModel(m);
robot->setGeomModel(gm);
Transform3f mat; mat.setIdentity ();
std::string jointName = name + "_x";
JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
JointModelPX::TangentVector_t max_velocity = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
JointModelPX::ConfigVector_t lower_position; lower_position << -4;
JointModelPX::ConfigVector_t upper_position; upper_position << 4;
robot->model().addJoint(0,JointModelPX(), mat,jointName,max_effort,max_velocity,lower_position,upper_position);
robot->createData();
robot->createGeomData();
std::string urdf ("<robot name='test'>"
"<link name='link0'/>"
"<joint name='joint0' type='prismatic'>"
"<parent link='link0'/>"
"<child link='link1'/>"
"<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
"</joint>"
"<link name='link1'/>"
"</robot>"
);
DevicePtr_t robot = Device::create ("test");
urdf::loadModelFromString (robot, 0, "", "anchor", urdf, "");
return robot;
/* DevicePtr_t robot = Device::create ("test");
const std::string& name = robot->name ();
fcl::Transform3f mat; mat.setIdentity ();
JointPtr_t joint;
std::string jointName = name + "_x";
// Translation along x
fcl::Matrix3f permutation;
joint = objectFactory.createJointTranslation (mat);
joint->name (jointName);
joint->isBounded (0, 1);
joint->lowerBound (0, -4);
joint->upperBound (0, +4);
robot->rootJoint (joint);
return robot;*/
}
DevicePtr_t createRobot2 ()
{
DevicePtr_t robot = Device::create ("test");
const std::string& name = robot->name ();
ModelPtr_t m = ModelPtr_t(new ::se3::Model());
GeomModelPtr_t gm = GeomModelPtr_t(new ::se3::GeometryModel());
robot->setModel(m);
robot->setGeomModel(gm);
Transform3f mat; mat.setIdentity ();
std::string jointName = name + "_x";
JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
JointModelPX::TangentVector_t max_velocity = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
JointModelPX::ConfigVector_t lower_position = JointModelPY::ConfigVector_t::Constant(-4);
JointModelPX::ConfigVector_t upper_position = JointModelPY::ConfigVector_t::Constant(4);
JointIndex idJoint = 0;
std::ostringstream oss;
oss
<< "<robot name='test'>"
<< "<link name='link0'/>";
for(int i = 0 ; i < 10 ; ++i){
idJoint = robot->model().addJoint(idJoint,JointModelPX(), mat,jointName + TOSTR(i),max_effort,max_velocity,lower_position,upper_position);
oss << "<joint name='joint" << i << "' type='prismatic'>"
<< "<parent link='link" << i << "'/>"
<< "<child link='link" << i+1 << "'/>"
<< "<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
<< "</joint>"
<< "<link name='link" << i+1 << "'/>";
}
oss << "</robot>";
std::string urdf (oss.str());
robot->createData();
robot->createGeomData();
DevicePtr_t robot = Device::create ("test");
urdf::loadModelFromString (robot, 0, "", "anchor", urdf, "");
return robot;
/*DevicePtr_t robot = Device::create ("test");
const std::string& name = robot->name ();
fcl::Transform3f mat; mat.setIdentity ();
JointPtr_t joint, parentJoint;
std::string jointName = name + "_x";
// Translation along x
fcl::Matrix3f permutation;
for (int i = 0; i < 10; ++i) {
joint = objectFactory.createJointTranslation (mat);
joint->name (jointName + TOSTR(i));
joint->isBounded (0, 1);
joint->lowerBound (0, -4);
joint->upperBound (0, +4);
if (i == 0) robot->rootJoint (joint);
else parentJoint->addChildJoint (joint);
parentJoint = joint;
}
return robot;*/
}
typedef std::pair<value_type, value_type> Pair_t;
......
tests/relative-motion.cc
View file @ 1e03ac79
......@@ -24,6 +24,7 @@
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/configuration.hh>
#include <hpp/pinocchio/urdf/util.hh>
#include <hpp/constraints/generic-transformation.hh>
......@@ -33,12 +34,6 @@
#include <hpp/constraints/locked-joint.hh>
#include <hpp/constraints/implicit.hh>
#include <hpp/constraints/explicit/relative-pose.hh>
#include <pinocchio/multibody/joint/joint-variant.hpp>
#include <pinocchio/multibody/geometry.hpp>
using hpp::pinocchio::Device;
using hpp::pinocchio::DevicePtr_t;
using hpp::pinocchio::JointPtr_t;
using hpp::constraints::RelativeTransformation;
using hpp::constraints::Implicit;
......@@ -47,12 +42,6 @@ using hpp::constraints::explicit_::RelativePose;
using namespace hpp::core;
using namespace hpp::pinocchio;
using ::se3::JointModelFreeFlyer;
using ::se3::JointModelPX;
using ::se3::JointModelPY;
using ::se3::JointModelPZ;
using ::se3::JointIndex;
bool verbose = true;
#define TOSTR( x ) static_cast< std::ostringstream & >( ( std::ostringstream() << x ) ).str()
......@@ -74,37 +63,52 @@ bool verbose = true;
*/
DevicePtr_t createRobot ()
{
std::string urdf ("<robot name='test'>"
"<link name='base_link'/>"
"<link name='link_test_x'/>"
"<joint name='test_x' type='prismatic'>"
"<parent link='base_link'/>"
"<child link='link_test_x'/>"
"<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
"</joint>"
"<link name='link_a0'/>"
"<link name='link_a1'/>"
"<joint name='joint_a0' type='prismatic'>"
"<parent link='link_test_x'/>"
"<child link='link_a0'/>"
"<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
"</joint>"
"<joint name='joint_a1' type='prismatic'>"
"<parent link='link_a0'/>"
"<child link='link_a1'/>"
"<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
"</joint>"
"<link name='link_b0'/>"
"<link name='link_b1'/>"
"<link name='link_b2'/>"
"<joint name='joint_b0' type='prismatic'>"
"<parent link='link_test_x'/>"
"<child link='link_b0'/>"
"<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
"</joint>"
"<joint name='joint_b1' type='prismatic'>"
"<parent link='link_b0'/>"
"<child link='link_b1'/>"
"<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
"</joint>"
"<joint name='joint_b2' type='floating'>"
"<parent link='link_b1'/>"
"<child link='link_b2'/>"
"</joint>"
"</robot>"
);
DevicePtr_t robot = Device::create ("test");
const std::string& name = robot->name ();
ModelPtr_t m = ModelPtr_t(new ::se3::Model());
GeomModelPtr_t gm = GeomModelPtr_t(new ::se3::GeometryModel());
robot->setModel(m);
robot->setGeomModel(gm);
Transform3f mat; mat.setIdentity ();
std::string jointName = name + "_x";
JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
JointModelPX::TangentVector_t max_velocity = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
JointModelPX::ConfigVector_t lower_position = JointModelPY::ConfigVector_t::Constant(-4);
JointModelPX::ConfigVector_t upper_position = JointModelPY::ConfigVector_t::Constant(4);
JointIndex idJoint = robot->model().addJoint(0,JointModelPX(), mat,jointName,max_effort,max_velocity,lower_position,upper_position);
std::string bname = "joint_a";
for (int i = 0; i < 2; ++i) {
idJoint = robot->model().addJoint(idJoint,JointModelPX(), mat,bname+TOSTR(i),max_effort,max_velocity,lower_position,upper_position);
}
bname = "joint_b";
idJoint = 1;
for (int i = 0; i < 2; ++i) {
idJoint = robot->model().addJoint(idJoint,JointModelPX(), mat,bname+TOSTR(i),max_effort,max_velocity,lower_position,upper_position);
}
int i = 2;
idJoint = robot->model().addJoint(idJoint,JointModelFreeFlyer(), mat,bname+TOSTR(i));
robot->createData();
robot->createGeomData();
DevicePtr_t robot = Device::create ("test");
urdf::loadModelFromString (