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

[C++] Make distinction between joint and body in Model API ( addJoint,...

[C++] Make distinction between joint and body in Model API ( addJoint, appendBodyToJoint, addJointAndBody instead of addBody)
parent af5b7c54
......@@ -59,6 +59,9 @@ namespace se3
/// \brief Dimension of the velocity vector space.
int nv;
/// \brief Number of joints .
int njoint;
/// \brief Number of bodies (= number of joints + 1).
int nbody;
......@@ -73,12 +76,18 @@ 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;
......@@ -123,6 +132,7 @@ namespace se3
Model()
: nq(0)
, nv(0)
, njoint(1)
, nbody(1)
, nFixBody(0)
, nOperationalFrames(0)
......@@ -149,15 +159,13 @@ namespace se3
/// \param[in] Y Spatial inertia of the body.
/// \param[in] jointName Name of the joint.
/// \param[in] bodyName Name of the body.
/// \param[in] visual Inform if the current body has a visual or not.
///
/// \return The index of the new added joint.
///
template<typename D>
JointIndex addBody(JointIndex parent, const JointModelBase<D> & j, const SE3 & placement,
const Inertia & Y,
const std::string & jointName = "", const std::string & bodyName = "",
bool visual = false);
JointIndex addJointAndBody(JointIndex parent, const JointModelBase<D> & j, const SE3 & placement,
const Inertia & Y, const std::string & jointName = "",
const std::string & bodyName = "");
///
/// \brief Add a body to the kinematic tree.
......@@ -172,18 +180,64 @@ namespace se3
/// \param[in] upPos Upper joint configuration.
/// \param[in] jointName Name of the joint.
/// \param[in] bodyName Name of the body.
/// \param[in] visual Inform if the current body has a visual or not.
///
/// \return The index of the new added joint.
///
template<typename D>
JointIndex addBody(JointIndex parent,const JointModelBase<D> & j,const SE3 & placement,
const Inertia & Y,
JointIndex addJointAndBody(JointIndex parent,const JointModelBase<D> & j,const SE3 & placement,
const Inertia & Y,
const Eigen::VectorXd & effort, const Eigen::VectorXd & velocity,
const Eigen::VectorXd & lowPos, const Eigen::VectorXd & upPos,
const std::string & jointName = "", const std::string & bodyName = "");
///
/// \brief Add a Joint with no body (Zero inertia) to the kinematic tree.
///
/// \param[in] parent Index of the parent joint.
/// \param[in] j The joint model.
/// \param[in] placement The relative placement of the joint j regarding to the parent joint.
/// \param[in] jointName Name of the joint.
///
/// \return The index of the new added joint.
///
template<typename D>
JointIndex addJoint(JointIndex parent,const JointModelBase<D> & j,const SE3 & placement,
const std::string & jointName = "");
///
/// \brief Add a Joint with no body (Zero inertia) to the kinematic tree.
///
/// \param[in] parent Index of the parent joint.
/// \param[in] j The joint model.
/// \param[in] placement The relative placement of the joint j regarding to the parent joint.
/// \param[in] effort Maximal joint torque.
/// \param[in] velocity Maximal joint velocity.
/// \param[in] lowPos Lower joint configuration.
/// \param[in] upPos Upper joint configuration.
/// \param[in] jointName Name of the joint.
///
/// \return The index of the new added joint.
///
template<typename D>
JointIndex addJoint(JointIndex parent,const JointModelBase<D> & j,const SE3 & placement,
const Eigen::VectorXd & effort, const Eigen::VectorXd & velocity,
const Eigen::VectorXd & lowPos, const Eigen::VectorXd & upPos,
const std::string & jointName = "", const std::string & bodyName = "",
bool visual = false);
const std::string & jointName = "");
///
/// \brief Append a body to a given Joint of the kinematic tree.
///
/// \param[in] parent Index of the parent joint.
/// \param[in] placement The relative placement of the body regarding to the parent joint.
/// \param[in] Y Spatial inertia of the body.
/// \param[in] bodyName Name of the body.
///
///
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.
///
......@@ -199,14 +253,6 @@ namespace se3
const std::string & jointName = "",
bool visual=false);
///
/// \brief Merge a body to a parent body in the kinematic tree.
///
/// \param[in] parent Index of the parent body to merge with.
/// \param[in] placement Relative placement between the body and its parent body.
/// \param[in] Y Spatial inertia of the body.
///
void mergeFixedBody(const JointIndex parent, const SE3 & placement, const Inertia & Y);
///
/// \brief Return the index of a body given by its name.
......
......@@ -45,29 +45,52 @@ namespace se3
return os;
}
template<typename D>
Model::JointIndex Model::addJointAndBody(JointIndex parent, const JointModelBase<D> & j, const SE3 & placement,
const Inertia & Y, const std::string & jointName,
const std::string & bodyName)
{
JointIndex idx = addJoint(parent,j,placement,jointName);
appendBodyToJoint(idx, SE3::Identity(), Y, bodyName);
return idx;
}
template<typename D>
Model::JointIndex Model::addBody (JointIndex parent, const JointModelBase<D> & j, const SE3 & placement,
const Inertia & Y, const std::string & jointName,
const std::string & bodyName, bool visual)
Model::JointIndex Model::addJointAndBody(JointIndex parent, const JointModelBase<D> & j, const SE3 & placement,
const Inertia & Y,
const Eigen::VectorXd & effort, const Eigen::VectorXd & velocity,
const Eigen::VectorXd & lowPos, const Eigen::VectorXd & upPos,
const std::string & jointName,
const std::string & bodyName)
{
assert( (nbody==(int)joints.size())&&(nbody==(int)inertias.size())
&&(nbody==(int)parents.size())&&(nbody==(int)jointPlacements.size()) );
JointIndex idx = addJoint(parent,j,placement,
effort, velocity, lowPos, upPos,
jointName);
appendBodyToJoint(idx, SE3::Identity(), Y, bodyName);
return idx;
}
template<typename D>
Model::JointIndex Model::addJoint(JointIndex parent, const JointModelBase<D> & j, const SE3 & placement,
const std::string & jointName)
{
assert( (njoint==(int)joints.size())&&(njoint==(int)inertias.size())
&&(njoint==(int)parents.size())&&(njoint==(int)jointPlacements.size()) );
assert( (j.nq()>=0)&&(j.nv()>=0) );
Model::JointIndex idx = (Model::JointIndex) (nbody ++);
Model::JointIndex idx = (Model::JointIndex) (njoint ++);
joints .push_back(j.derived());
boost::get<D>(joints.back()).setIndexes(idx,nq,nv);
inertias .push_back(Y);
inertias .push_back(Inertia::Zero());
parents .push_back(parent);
jointPlacements.push_back(placement);
names .push_back( (jointName!="")?jointName:random(8) );
hasVisual .push_back(visual);
bodyNames .push_back( (bodyName!="")?bodyName:random(8));
nq += j.nq();
nv += j.nv();
......@@ -79,33 +102,24 @@ namespace se3
}
template<typename D>
Model::JointIndex Model::addBody (JointIndex parent, const JointModelBase<D> & j, const SE3 & placement,
const Inertia & Y,
const Eigen::VectorXd & effort, const Eigen::VectorXd & velocity,
const Eigen::VectorXd & lowPos, const Eigen::VectorXd & upPos,
const std::string & jointName,
const std::string & bodyName, bool visual)
Model::JointIndex Model::addJoint(JointIndex parent,const JointModelBase<D> & j,const SE3 & placement,
const Eigen::VectorXd & effort, const Eigen::VectorXd & velocity,
const Eigen::VectorXd & lowPos, const Eigen::VectorXd & upPos,
const std::string & jointName)
{
assert( (nbody==(int)joints.size())&&(nbody==(int)inertias.size())
&&(nbody==(int)parents.size())&&(nbody==(int)jointPlacements.size()) );
assert( (njoint==(int)joints.size())&&(njoint==(int)inertias.size())
&&(njoint==(int)parents.size())&&(njoint==(int)jointPlacements.size()) );
assert( (j.nq()>=0)&&(j.nv()>=0) );
assert( effort.size() == j.nv() && velocity.size() == j.nv()
&& lowPos.size() == j.nq() && upPos.size() == j.nq() );
Model::JointIndex idx = (Model::JointIndex) (nbody ++);
Model::JointIndex idx = (Model::JointIndex) (njoint ++);
joints .push_back(j.derived());
boost::get<D>(joints.back()).setIndexes(idx,nq,nv);
inertias .push_back(Y);
inertias .push_back(Inertia::Zero());
parents .push_back(parent);
jointPlacements.push_back(placement);
names .push_back( (jointName!="")?jointName:random(8) );
hasVisual .push_back(visual);
bodyNames .push_back( (bodyName!="")?bodyName:random(8));
nq += j.nq();
nv += j.nv();
......@@ -116,6 +130,21 @@ namespace se3
return idx;
}
inline void Model::appendBodyToJoint(const JointIndex parent, const SE3 & placement, const Inertia & Y,
const std::string & bodyName)
{
const Inertia & iYf = Y.se3Action(placement);
inertias[parent] += iYf;
bodyParents.push_back(parent);
bodyPlacements.push_back(placement);
bodyNames.push_back( (bodyName!="")?bodyName:random(8));
nbody ++;
}
inline Model::JointIndex Model::addFixedBody (JointIndex lastMovingParent,
const SE3 & placementFromLastMoving,
const std::string & bodyName,
......@@ -130,11 +159,7 @@ namespace se3
return idx;
}
inline void Model::mergeFixedBody (const JointIndex parent, const SE3 & placement, const Inertia & Y)
{
const Inertia & iYf = Y.se3Action(placement); //TODO
inertias[parent] += iYf;
}
inline Model::JointIndex Model::getBodyId (const std::string & name) const
{
......
......@@ -233,43 +233,43 @@ namespace se3
bool is_body_fixed = false;
if (joint_type == "JointTypeRevoluteX")
{
body_id = model.addBody (parent_id, JointModelRX (), global_placement, Y, joint_name, body_name, false);
body_id = model.addJointAndBody (parent_id, JointModelRX (), global_placement, Y, joint_name, body_name);
}
else if (joint_type == "JointTypeRevoluteY")
{
body_id = model.addBody (parent_id, JointModelRY (), global_placement, Y, joint_name, body_name, false);
body_id = model.addJointAndBody (parent_id, JointModelRY (), global_placement, Y, joint_name, body_name);
}
else if (joint_type == "JointTypeRevoluteZ")
{
body_id = model.addBody (parent_id, JointModelRZ (), global_placement, Y, joint_name, body_name, false);
body_id = model.addJointAndBody (parent_id, JointModelRZ (), global_placement, Y, joint_name, body_name);
}
else if (joint_type == "JointTypePrismaticX")
{
body_id = model.addBody (parent_id, JointModelPX (), global_placement, Y, joint_name, body_name, false);
body_id = model.addJointAndBody (parent_id, JointModelPX (), global_placement, Y, joint_name, body_name);
}
else if (joint_type == "JointTypePrismaticY")
{
body_id = model.addBody (parent_id, JointModelPY (), global_placement, Y, joint_name, body_name, false);
body_id = model.addJointAndBody (parent_id, JointModelPY (), global_placement, Y, joint_name, body_name);
}
else if (joint_type == "JointTypePrismaticZ")
{
body_id = model.addBody (parent_id, JointModelPZ (), global_placement, Y, joint_name, body_name, false);
body_id = model.addJointAndBody (parent_id, JointModelPZ (), global_placement, Y, joint_name, body_name);
}
else if (joint_type == "JointTypeFloatingBase" || joint_type == "JointTypeFloatbase")
{
body_id = model.addBody (parent_id, JointModelFreeFlyer (), global_placement, Y, joint_name, body_name, false);
body_id = model.addJointAndBody (parent_id, JointModelFreeFlyer (), global_placement, Y, joint_name, body_name);
}
else if (joint_type == "JointTypeSpherical")
{
body_id = model.addBody (parent_id, JointModelSpherical (), global_placement, Y, joint_name, body_name, false);
body_id = model.addJointAndBody (parent_id, JointModelSpherical (), global_placement, Y, joint_name, body_name);
}
else if (joint_type == "JointTypeEulerZYX")
{
body_id = model.addBody (parent_id, JointModelSphericalZYX (), global_placement, Y, joint_name, body_name, false);
body_id = model.addJointAndBody (parent_id, JointModelSphericalZYX (), global_placement, Y, joint_name, body_name);
}
else if (joint_type == "JointTypeFixed")
{
model.mergeFixedBody(parent_id, global_placement, Y);
model.appendBodyToJoint(parent_id, global_placement, Y, "");
fixed_body_table_id_map[body_name] = parent_id;
fixed_placement_map[body_name] = global_placement;
......
......@@ -29,34 +29,34 @@ namespace se3
void humanoid2d(Model& model)
{
model.addBody(model.getBodyId("universe"),JointModelRX(),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("universe"),JointModelRX(),SE3::Identity(),Inertia::Random(),
"ff1_joint", "ff1_body");
model.addBody(model.getBodyId("ff1_body"),JointModelRY(),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("ff1_body"),JointModelRY(),SE3::Identity(),Inertia::Random(),
"root_joint", "root_body");
model.addBody(model.getBodyId("root_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("root_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
"lleg1_joint", "lleg1_body");
model.addBody(model.getBodyId("lleg1_body"),JointModelRY(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("lleg1_body"),JointModelRY(),SE3::Random(),Inertia::Random(),
"lleg2_joint", "lleg2_body");
model.addBody(model.getBodyId("root_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("root_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
"rleg1_joint", "rleg1_body");
model.addBody(model.getBodyId("rleg1_body"),JointModelRY(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("rleg1_body"),JointModelRY(),SE3::Random(),Inertia::Random(),
"rleg2_joint", "rleg2_body");
model.addBody(model.getBodyId("root_body"),JointModelRY(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("root_body"),JointModelRY(),SE3::Random(),Inertia::Random(),
"torso1_joint", "torso1_body");
model.addBody(model.getBodyId("torso1_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("torso1_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
"chest_joint", "chest_body");
model.addBody(model.getBodyId("chest_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("chest_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
"rarm1_joint", "rarm1_body");
model.addBody(model.getBodyId("rarm1_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("rarm1_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
"rarm2_joint", "rarm2_body");
model.addBody(model.getBodyId("chest_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("chest_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
"larm1_joint", "larm1_body");
model.addBody(model.getBodyId("larm1_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("larm1_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
"larm2_joint", "larm2_body");
}
......@@ -64,22 +64,22 @@ namespace se3
{
if(! usingFF )
{
model.addBody(model.getBodyId("universe"),JointModelRX(),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("universe"),JointModelRX(),SE3::Identity(),Inertia::Random(),
"ff1_joint", "ff1_body");
model.addBody(model.getBodyId("ff1_body"),JointModelRY(),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("ff1_body"),JointModelRY(),SE3::Identity(),Inertia::Random(),
"ff2_joint", "ff2_body");
model.addBody(model.getBodyId("ff2_body"),JointModelRZ(),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("ff2_body"),JointModelRZ(),SE3::Identity(),Inertia::Random(),
"ff3_joint", "ff3_body");
model.addBody(model.getBodyId("ff3_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("ff3_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
"ff4_joint", "ff4_body");
model.addBody(model.getBodyId("ff4_body"),JointModelRY(),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("ff4_body"),JointModelRY(),SE3::Identity(),Inertia::Random(),
"ff5_joint", "ff5_body");
model.addBody(model.getBodyId("ff5_body"),JointModelRX(),SE3::Identity(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("ff5_body"),JointModelRX(),SE3::Identity(),Inertia::Random(),
"root_joint", "root_body");
}
else
{
model.addBody(model.getBodyId("universe"),JointModelFreeFlyer(),
model.addJointAndBody(model.getBodyId("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.addBody(model.getBodyId("root_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("root_body"),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.addBody(model.getBodyId("lleg1_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("lleg1_body"),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.addBody(model.getBodyId("lleg2_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("lleg2_body"),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.addBody(model.getBodyId("lleg3_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("lleg3_body"),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.addBody(model.getBodyId("lleg4_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("lleg4_body"),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.addBody(model.getBodyId("lleg5_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("lleg5_body"),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.addBody(model.getBodyId("root_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("root_body"),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.addBody(model.getBodyId("rleg1_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("rleg1_body"),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.addBody(model.getBodyId("rleg2_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("rleg2_body"),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.addBody(model.getBodyId("rleg3_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("rleg3_body"),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.addBody(model.getBodyId("rleg4_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("rleg4_body"),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.addBody(model.getBodyId("rleg5_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("rleg5_body"),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.addBody(model.getBodyId("root_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("root_body"),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.addBody(model.getBodyId("torso1_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("torso1_body"),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.addBody(model.getBodyId("chest_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("chest_body"),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.addBody(model.getBodyId("rarm1_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("rarm1_body"),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.addBody(model.getBodyId("rarm2_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("rarm2_body"),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.addBody(model.getBodyId("rarm3_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("rarm3_body"),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.addBody(model.getBodyId("rarm4_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("rarm4_body"),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.addBody(model.getBodyId("rarm5_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("rarm5_body"),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.addBody(model.getBodyId("chest_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("chest_body"),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.addBody(model.getBodyId("larm1_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("larm1_body"),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.addBody(model.getBodyId("larm2_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("larm2_body"),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.addBody(model.getBodyId("larm3_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("larm3_body"),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.addBody(model.getBodyId("larm4_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("larm4_body"),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.addBody(model.getBodyId("larm5_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
model.addJointAndBody(model.getBodyId("larm5_body"),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");
......
......@@ -103,7 +103,6 @@ namespace se3
const Inertia & Y = (link->inertial) ? convertFromUrdf(*link->inertial) :
Inertia::Zero();
const bool has_visual = (link->visual) ? true : false;
switch(joint->type)
{
......@@ -128,9 +127,9 @@ namespace se3
TangentVector_t max_velocity;