Skip to content
GitLab
Commit 3054137b authored by Valenza Florian's avatar Valenza Florian
Browse files

split Index into JointIndex, BodyIndex, FrameIndex GeomIndex

parent 443243b7
Hide whitespace changes
Inline Side-by-side
src/algorithm/center-of-mass.hxx
View file @ 3054137b
......@@ -36,7 +36,7 @@ namespace se3
if (updateKinematics)
forwardKinematics(model, data, q);
for(Model::Index i=1;i<(Model::Index)(model.nbody);++i)
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.nbody);++i)
{
const double mass = model.inertias[i].mass();
const SE3::Vector3 & lever = model.inertias[i].lever();
......@@ -46,9 +46,9 @@ namespace se3
}
// Backward Step
for(Model::Index i=(Model::Index)(model.nbody-1); i>0; --i)
for(Model::JointIndex i=(Model::JointIndex)(model.nbody-1); i>0; --i)
{
const Model::Index & parent = model.parents[i];
const Model::JointIndex & parent = model.parents[i];
const SE3 & liMi = data.liMi[i];
......@@ -86,7 +86,7 @@ namespace se3
if (updateKinematics)
forwardKinematics(model, data, q, v, a);
for(Model::Index i=1;i<(Model::Index)(model.nbody);++i)
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.nbody);++i)
{
const double mass = model.inertias[i].mass();
const SE3::Vector3 & lever = model.inertias[i].lever();
......@@ -102,9 +102,9 @@ namespace se3
}
// Backward Step
for(Model::Index i=(Model::Index)(model.nbody-1); i>0; --i)
for(Model::JointIndex i=(Model::JointIndex)(model.nbody-1); i>0; --i)
{
const Model::Index & parent = model.parents[i];
const Model::JointIndex & parent = model.parents[i];
const SE3 & liMi = data.liMi[i];
......@@ -156,8 +156,8 @@ namespace se3
using namespace Eigen;
using namespace se3;
const Model::Index & i = (Model::Index) jmodel.id();
const Model::Index & parent = model.parents[i];
const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
const Model::JointIndex & parent = model.parents[i];
jmodel.calc(jdata.derived(),q);
......@@ -191,8 +191,8 @@ namespace se3
using namespace Eigen;
using namespace se3;
const Model::Index & i = (Model::Index) jmodel.id();
const Model::Index & parent = model.parents[i];
const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
const Model::JointIndex & parent = model.parents[i];
data.com[parent] += data.com[i];
data.mass[parent] += data.mass[i];
......@@ -223,13 +223,13 @@ namespace se3
{
data.com[0].setZero ();
data.mass[0] = 0;
for( Model::Index i=1;i<(Model::Index)model.nbody;++i )
for( Model::JointIndex i=1;i<(Model::JointIndex)model.nbody;++i )
{
JacobianCenterOfMassForwardStep
::run(model.joints[i],data.joints[i],
JacobianCenterOfMassForwardStep::ArgsType(model,data,q));
}
for( Model::Index i= (Model::Index) (model.nbody-1);i>0;--i )
for( Model::JointIndex i= (Model::JointIndex) (model.nbody-1);i>0;--i )
{
JacobianCenterOfMassBackwardStep
::run(model.joints[i],data.joints[i],
......
src/algorithm/collisions.hpp
View file @ 3054137b
......@@ -102,9 +102,9 @@ namespace se3
GeometryData & data_geom
)
{
for (GeometryData::Index i=0; i < (GeometryData::Index) data_geom.model_geom.ngeom; ++i)
for (GeometryData::GeomIndex i=0; i < (GeometryData::GeomIndex) data_geom.model_geom.ngeom; ++i)
{
const Model::Index & parent = model_geom.geom_parents[i];
const Model::JointIndex & parent = model_geom.geom_parents[i];
data_geom.oMg[i] = (data.oMi[parent] * model_geom.geometryPlacement[i]);
data_geom.oMg_fcl[i] = toFclTransform3f(data_geom.oMg[i]);
}
......
src/algorithm/crba.hpp
View file @ 3054137b
......@@ -70,7 +70,7 @@ namespace se3
using namespace Eigen;
using namespace se3;
const Model::Index & i = (Model::Index) jmodel.id();
const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
jmodel.calc(jdata.derived(),q);
data.liMi[i] = model.jointPlacements[i]*jdata.M();
......@@ -99,7 +99,7 @@ namespace se3
* Yli += liXi Yi
* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE]
*/
const Model::Index & i = (Model::Index) jmodel.id();
const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
/* F[1:6,i] = Y*S */
//data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S();
......@@ -109,7 +109,7 @@ namespace se3
data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i])
= jdata.S().transpose()*data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
const Model::Index & parent = model.parents[i];
const Model::JointIndex & parent = model.parents[i];
if(parent>0)
{
/* Yli += liXi Yi */
......@@ -134,13 +134,13 @@ namespace se3
crba(const Model & model, Data& data,
const Eigen::VectorXd & q)
{
for( Model::Index i=1;i<(Model::Index)(model.nbody);++i )
for( Model::JointIndex i=1;i<(Model::JointIndex)(model.nbody);++i )
{
CrbaForwardStep::run(model.joints[i],data.joints[i],
CrbaForwardStep::ArgsType(model,data,q));
}
for( Model::Index i=(Model::Index)(model.nbody-1);i>0;--i )
for( Model::JointIndex i=(Model::JointIndex)(model.nbody-1);i>0;--i )
{
CrbaBackwardStep::run(model.joints[i],data.joints[i],
CrbaBackwardStep::ArgsType(model,data));
......
src/algorithm/energy.hpp
View file @ 3054137b
......@@ -76,7 +76,7 @@ namespace se3
if (update_kinematics)
forwardKinematics(model,data,q,v);
for(Model::Index i=1;i<(Model::Index)(model.nbody);++i)
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.nbody);++i)
data.kinetic_energy += model.inertias[i].vtiv(data.v[i]);
data.kinetic_energy *= .5;
......@@ -96,7 +96,7 @@ namespace se3
if (update_kinematics)
forwardKinematics(model,data,q);
for(Model::Index i=1;i<(Model::Index)(model.nbody);++i)
for(Model::JointIndex i=1;i<(Model::JointIndex)(model.nbody);++i)
{
com_global = data.oMi[i].translation() + data.oMi[i].rotation() * model.inertias[i].lever();
data.potential_energy += model.inertias[i].mass() * com_global.dot(g);
......
src/algorithm/jacobian.hpp
View file @ 3054137b
......@@ -53,7 +53,7 @@ namespace se3
template<bool localFrame>
void getJacobian(const Model & model,
const Data & data,
const Model::Index jointId,
const Model::JointIndex jointId,
Data::Matrix6x & J);
///
......@@ -70,7 +70,7 @@ namespace se3
jacobian(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Model::Index jointId);
const Model::JointIndex jointId);
} // namespace se3
......
src/algorithm/jacobian.hxx
View file @ 3054137b
......@@ -44,8 +44,8 @@ namespace se3
using namespace Eigen;
using namespace se3;
const Model::Index & i = (Model::Index) jmodel.id();
const Model::Index & parent = model.parents[i];
const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
const Model::JointIndex & parent = model.parents[i];
jmodel.calc(jdata.derived(),q);
......@@ -63,7 +63,7 @@ namespace se3
computeJacobians(const Model & model, Data & data,
const Eigen::VectorXd & q)
{
for( Model::Index i=1; i< (Model::Index) model.nbody;++i )
for( Model::JointIndex i=1; i< (Model::JointIndex) model.nbody;++i )
{
JacobiansForwardStep::run(model.joints[i],data.joints[i],
JacobiansForwardStep::ArgsType(model,data,q));
......@@ -78,7 +78,7 @@ namespace se3
template<bool localFrame>
void getJacobian(const Model & model,
const Data & data,
const Model::Index jointId,
const Model::JointIndex jointId,
Data::Matrix6x & J)
{
assert( J.rows() == data.J.rows() );
......@@ -113,8 +113,8 @@ namespace se3
using namespace Eigen;
using namespace se3;
const Model::Index & i = (Model::Index) jmodel.id();
const Model::Index & parent = model.parents[i];
const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
const Model::JointIndex & parent = model.parents[i];
jmodel.calc(jdata.derived(),q);
......@@ -129,10 +129,10 @@ namespace se3
inline const Data::Matrix6x &
jacobian(const Model & model, Data & data,
const Eigen::VectorXd & q,
const Model::Index jointId)
const Model::JointIndex jointId)
{
data.iMf[jointId].setIdentity();
for( Model::Index i=jointId;i>0;i=model.parents[i] )
for( Model::JointIndex i=jointId;i>0;i=model.parents[i] )
{
JacobianForwardStep::run(model.joints[i],data.joints[i],
JacobianForwardStep::ArgsType(model,data,q));
......
src/algorithm/joint-limits.hpp
View file @ 3054137b
......@@ -67,7 +67,7 @@ namespace se3
const Eigen::VectorXd & q)
{
for( Model::Index i=1; i<(Model::Index) model.nbody; ++i )
for( Model::JointIndex i=1; i<(Model::JointIndex) model.nbody; ++i )
{
JointLimitsStep::run(model.joints[i],
data.joints[i],
......
src/algorithm/kinematics.hpp
View file @ 3054137b
......@@ -74,7 +74,7 @@ namespace se3
inline void emptyForwardPass(const Model & model,
Data & data)
{
for (Model::Index i=1; i < (Model::Index) model.nbody; ++i)
for (Model::JointIndex i=1; i < (Model::JointIndex) model.nbody; ++i)
{
emptyForwardStep::run(model.joints[i],
data.joints[i],
......@@ -87,7 +87,7 @@ namespace se3
{
typedef boost::fusion::vector<const se3::Model &,
se3::Data &,
const Model::Index,
const Model::JointIndex,
const Eigen::VectorXd &
> ArgsType;
......@@ -98,14 +98,14 @@ namespace se3
se3::JointDataBase<typename JointModel::JointData> & jdata,
const se3::Model & model,
se3::Data & data,
const Model::Index i,
const Model::JointIndex i,
const Eigen::VectorXd & q)
{
using namespace se3;
jmodel.calc (jdata.derived (), q);
const Model::Index & parent = model.parents[i];
const Model::JointIndex & parent = model.parents[i];
data.liMi[i] = model.jointPlacements[i] * jdata.M ();
if (parent>0)
......@@ -121,7 +121,7 @@ namespace se3
Data & data,
const Eigen::VectorXd & q)
{
for (Model::Index i=1; i < (Model::Index) model.nbody; ++i)
for (Model::JointIndex i=1; i < (Model::JointIndex) model.nbody; ++i)
{
ForwardKinematicZeroStep::run(model.joints[i],
data.joints[i],
......@@ -134,7 +134,7 @@ namespace se3
{
typedef boost::fusion::vector< const se3::Model&,
se3::Data&,
const Model::Index,
const Model::JointIndex,
const Eigen::VectorXd &,
const Eigen::VectorXd &
> ArgsType;
......@@ -146,7 +146,7 @@ namespace se3
se3::JointDataBase<typename JointModel::JointData> & jdata,
const se3::Model& model,
se3::Data& data,
const Model::Index i,
const Model::JointIndex i,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v)
{
......@@ -155,7 +155,7 @@ namespace se3
jmodel.calc(jdata.derived(),q,v);
const Model::Index & parent = model.parents[i];
const Model::JointIndex & parent = model.parents[i];
data.v[i] = jdata.v();
data.liMi[i] = model.jointPlacements[i]*jdata.M();
......@@ -177,7 +177,7 @@ namespace se3
{
data.v[0].setZero();
for( Model::Index i=1; i<(Model::Index) model.nbody; ++i )
for( Model::JointIndex i=1; i<(Model::JointIndex) model.nbody; ++i )
{
ForwardKinematicFirstStep::run(model.joints[i],data.joints[i],
ForwardKinematicFirstStep::ArgsType(model,data,i,q,v));
......@@ -188,7 +188,7 @@ namespace se3
{
typedef boost::fusion::vector< const se3::Model&,
se3::Data&,
const Model::Index,
const Model::JointIndex,
const Eigen::VectorXd &,
const Eigen::VectorXd &,
const Eigen::VectorXd &
......@@ -201,14 +201,14 @@ namespace se3
se3::JointDataBase<typename JointModel::JointData> & jdata,
const se3::Model & model,
se3::Data & data,
const Model::Index i,
const Model::JointIndex i,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a)
{
jmodel.calc(jdata.derived(),q,v);
const Model::Index & parent = model.parents[i];
const Model::JointIndex & parent = model.parents[i];
data.v[i] = jdata.v();
data.liMi[i] = model.jointPlacements[i] * jdata.M();
......@@ -234,7 +234,7 @@ namespace se3
data.v[0].setZero();
data.a[0].setZero();
for( Model::Index i=1; i < (Model::Index) model.nbody; ++i )
for( Model::JointIndex i=1; i < (Model::JointIndex) model.nbody; ++i )
{
ForwardKinematicSecondStep::run(model.joints[i],data.joints[i],
ForwardKinematicSecondStep::ArgsType(model,data,i,q,v,a));
......
src/algorithm/non-linear-effects.hpp
View file @ 3054137b
......@@ -58,7 +58,7 @@ namespace se3
jmodel.calc(jdata.derived(),q,v);
const Model::Index & parent = model.parents[i];
const Model::JointIndex & parent = model.parents[i];
data.liMi[i] = model.jointPlacements[i]*jdata.M();
data.v[i] = jdata.v();
......@@ -87,7 +87,7 @@ namespace se3
Data & data,
const size_t i)
{
const Model::Index & parent = model.parents[i];
const Model::JointIndex & parent = model.parents[i];
jmodel.jointVelocitySelector(data.nle) = jdata.S().transpose()*data.f[i];
if(parent>0) data.f[(size_t) parent] += data.liMi[i].act(data.f[i]);
}
......
src/algorithm/operational-frames.hpp
View file @ 3054137b
......@@ -69,7 +69,7 @@ namespace se3
template<bool localFrame>
inline void getFrameJacobian(const Model & model,
const Data& data,
Model::Index frame_id,
Model::FrameIndex frame_id,
Data::Matrix6x & J
);
......@@ -86,9 +86,9 @@ inline void framesForwardKinematic(const Model & model,
{
using namespace se3;
for (Model::Index i=0; i < (Model::Index) model.nOperationalFrames; ++i)
for (Model::FrameIndex i=0; i < (Model::FrameIndex) model.nOperationalFrames; ++i)
{
const Model::Index & parent = model.operational_frames[i].parent_id;
const Model::JointIndex & parent = model.operational_frames[i].parent_id;
data.oMof[i] = (data.oMi[parent] * model.operational_frames[i].framePlacement);
}
}
......@@ -109,12 +109,12 @@ inline void framesForwardKinematic(const Model & model,
template<bool localFrame>
inline void getFrameJacobian(const Model & model, const Data& data,
Model::Index frame_id, Data::Matrix6x & J)
Model::FrameIndex frame_id, Data::Matrix6x & J)
{
assert( J.rows() == data.J.rows() );
assert( J.cols() == data.J.cols() );
const Model::Index & parent = model.operational_frames[frame_id].parent_id;
const Model::JointIndex & parent = model.operational_frames[frame_id].parent_id;
const SE3 & oMframe = data.oMof[frame_id];
int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1;
......
src/algorithm/rnea.hpp
View file @ 3054137b
......@@ -38,7 +38,7 @@ namespace se3
{
typedef boost::fusion::vector< const se3::Model&,
se3::Data&,
const int&,
const Model::Index,
const Eigen::VectorXd &,
const Eigen::VectorXd &,
const Eigen::VectorXd &
......@@ -51,7 +51,7 @@ namespace se3
se3::JointDataBase<typename JointModel::JointData> & jdata,
const se3::Model& model,
se3::Data& data,
const int &i,
const Model::Index i,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a)
......@@ -61,16 +61,16 @@ namespace se3
jmodel.calc(jdata.derived(),q,v);
const Model::Index & parent = model.parents[(Model::Index)i];
data.liMi[(Model::Index)i] = model.jointPlacements[(Model::Index)i]*jdata.M();
const Model::JointIndex & parent = model.parents[i];
data.liMi[i] = model.jointPlacements[i]*jdata.M();
data.v[(Model::Index)i] = jdata.v();
if(parent>0) data.v[(Model::Index)i] += data.liMi[(Model::Index)i].actInv(data.v[parent]);
data.v[i] = jdata.v();
if(parent>0) data.v[i] += data.liMi[i].actInv(data.v[parent]);
data.a_gf[(Model::Index)i] = jdata.S()*jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[(Model::Index)i] ^ jdata.v()) ;
data.a_gf[(Model::Index)i] += data.liMi[(Model::Index)i].actInv(data.a_gf[parent]);
data.a_gf[i] = jdata.S()*jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()) ;
data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]);
data.f[(Model::Index)i] = model.inertias[(Model::Index)i]*data.a_gf[(Model::Index)i] + model.inertias[(Model::Index)i].vxiv(data.v[(Model::Index)i]); // -f_ext
data.f[i] = model.inertias[i]*data.a_gf[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
}
};
......@@ -79,7 +79,7 @@ namespace se3
{
typedef boost::fusion::vector<const Model&,
Data&,
const int &> ArgsType;
const Model::Index> ArgsType;
JOINT_VISITOR_INIT(RneaBackwardStep);
......@@ -88,11 +88,11 @@ namespace se3
JointDataBase<typename JointModel::JointData> & jdata,
const Model& model,
Data& data,
int i)
Model::Index i)
{
const Model::Index & parent = model.parents[(Model::Index)i];
jmodel.jointVelocitySelector(data.tau) = jdata.S().transpose()*data.f[(Model::Index)i];
if(parent>0) data.f[(Model::Index)parent] += data.liMi[(Model::Index)i].act(data.f[(Model::Index)i]);
const Model::JointIndex & parent = model.parents[i];
jmodel.jointVelocitySelector(data.tau) = jdata.S().transpose()*data.f[i];
if(parent>0) data.f[parent] += data.liMi[i].act(data.f[i]);
}
};
......@@ -105,15 +105,15 @@ namespace se3
data.v[0].setZero();
data.a_gf[0] = -model.gravity;
for( int i=1;i<model.nbody;++i )
for( Model::JointIndex i=1;i<(Model::JointIndex)model.nbody;++i )
{
RneaForwardStep::run(model.joints[(Model::Index)i],data.joints[(Model::Index)i],
RneaForwardStep::run(model.joints[i],data.joints[i],
RneaForwardStep::ArgsType(model,data,i,q,v,a));
}
for( int i=model.nbody-1;i>0;--i )
for( Model::JointIndex i=(Model::JointIndex)model.nbody-1;i>0;--i )
{
RneaBackwardStep::run(model.joints[(Model::Index)i],data.joints[(Model::Index)i],
RneaBackwardStep::run(model.joints[i],data.joints[i],
RneaBackwardStep::ArgsType(model,data,i));
}
......
src/multibody/geometry.hpp
View file @ 3054137b
......@@ -41,10 +41,11 @@
namespace se3
{
struct CollisionPair: public std::pair<Model::Index, Model::Index>
struct CollisionPair: public std::pair<Model::GeomIndex, Model::GeomIndex>
{
typedef Model::Index Index;
typedef std::pair<Model::Index, Model::Index> Base;
typedef Model::GeomIndex GeomIndex;
typedef std::pair<Model::GeomIndex, Model::GeomIndex> Base;
///
/// \brief Default constructor of a collision pair from two collision object indexes.
......@@ -53,7 +54,7 @@ namespace se3
/// \param[in] co1 Index of the first collision object
/// \param[in] co2 Index of the second collision object
///
CollisionPair(const Index co1, const Index co2) : Base(co1,co2)
CollisionPair(const GeomIndex co1, const GeomIndex co2) : Base(co1,co2)
{
assert(co1 != co2 && "The index of collision objects must not be equal.");
if (co1 > co2)
......@@ -73,8 +74,9 @@ namespace se3
struct DistanceResult
{
typedef Model::Index Index;
typedef Model::GeomIndex GeomIndex;
DistanceResult(fcl::DistanceResult dist_fcl, const Index co1, const Index co2)
DistanceResult(fcl::DistanceResult dist_fcl, const GeomIndex co1, const GeomIndex co2)
: fcl_distance_result(dist_fcl), object1(co1), object2(co2)
{}
......@@ -103,24 +105,26 @@ namespace se3
fcl::DistanceResult fcl_distance_result;
/// Index of the first colision object
Index object1;
GeomIndex object1;
/// Index of the second colision object
Index object2;
GeomIndex object2;
}; // struct DistanceResult
struct GeometryModel
{
typedef Model::Index Index;
typedef Model::JointIndex JointIndex;
typedef Model::GeomIndex GeomIndex;
Index ngeom;
std::vector<fcl::CollisionObject> collision_objects;
std::vector<std::string> geom_names;
std::vector<Index> geom_parents; // Joint parent of body <i>, denoted <li> (li==parents[i])
std::vector<JointIndex> geom_parents; // Joint parent of body <i>, denoted <li> (li==parents[i])
std::vector<SE3> geometryPlacement; // Position of geometry object in parent joint's frame
std::map < Index, std::list<Index> > innerObjects; // Associate a list of CollisionObjects to a given joint Id
std::map < Index, std::list<Index> > outerObjects; // Associate a list of CollisionObjects to a given joint Id
std::map < JointIndex, std::list<GeomIndex> > innerObjects; // Associate a list of CollisionObjects to a given joint Id
std::map < JointIndex, std::list<GeomIndex> > outerObjects; // Associate a list of CollisionObjects to a given joint Id
GeometryModel()
: ngeom(0)
......@@ -135,13 +139,13 @@ namespace se3
~GeometryModel() {};
Index addGeomObject(const Index parent, const fcl::CollisionObject & co, const SE3 & placement, const std::string & geoName = "");
Index getGeomId(const std::string & name) const;
GeomIndex addGeomObject(const JointIndex parent, const fcl::CollisionObject & co, const SE3 & placement, const std::string & geoName = "");
GeomIndex getGeomId(const std::string & name) const;
bool existGeomName(const std::string & name) const;
const std::string & getGeomName(const Index index) const;
const std::string & getGeomName(const GeomIndex index) const;
void addInnerObject(const Index joint, const Index inner_object);
void addOutterObject(const Index joint, const Index outer_object);
void addInnerObject(const JointIndex joint, const GeomIndex inner_object);
void addOutterObject(const JointIndex joint, const GeomIndex outer_object);
friend std::ostream& operator<<(std::ostream & os, const GeometryModel & model_geom);
}; // struct GeometryModel
......@@ -149,6 +153,7 @@ namespace se3
struct GeometryData
{
typedef Model::Index Index;
typedef Model::GeomIndex GeomIndex;
typedef CollisionPair CollisionPair_t;
typedef std::vector<CollisionPair_t> CollisionPairsVector_t;
......@@ -222,7 +227,7 @@ namespace se3
/// \param[in] co1 Index of the first colliding geometry.
/// \param[in] co2 Index of the second colliding geometry.
///
void addCollisionPair (const Index co1, const Index co2);
void addCollisionPair (const GeomIndex co1, const GeomIndex co2);
///
/// \brief Add a collision pair into the vector of collision_pairs.
......@@ -243,7 +248,7 @@ namespace se3
/// \param[in] co1 Index of the first colliding geometry.