Commit ff0cf979 authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Nicolas Mansard
Browse files

Added Body::radius.

parent 90791b26
......@@ -113,7 +113,8 @@ namespace hpp {
///
/// Radius is defined as an upper-bound to the distance of all points of
/// the body to the origin of the joint that holds the body.
//NOTYET value_type radius () const
value_type radius () const;
/// \}
//DEPREC /// \name Outer objects
......@@ -205,7 +206,7 @@ namespace hpp {
//DEPREC fcl::Vec3f localCom_;
//DEPREC matrix3_t inertiaMatrix_;
//DEPREC value_type mass_;
//NOTYET value_type radius_;
//DEPREC value_type radius_;
}; // class Body
} // namespace pinocchio
} // namespace hpp
......
......@@ -100,6 +100,14 @@ namespace hpp {
return model()->inertias[jointIndex].mass();
}
value_type Body::radius () const
{
selfAssert();
assert(devicePtr->geomData());
assert(int(devicePtr->geomData()->radius.size())==model()->njoint);
return devicePtr->geomData()->radius[jointIndex];
}
} // namespace pinocchio
} // namespace hpp
......@@ -26,7 +26,7 @@
#include <pinocchio/algorithm/center-of-mass.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/collisions.hpp>
#include <pinocchio/algorithm/geometry.hpp>
#include <boost/foreach.hpp>
namespace hpp {
......@@ -88,6 +88,7 @@ namespace hpp {
createGeomData()
{
geomData_ = GeomDataPtr_t( new se3::GeometryData(*geomModel_) );
se3::computeBodyRadius(*model_,*geomModel_,*geomData_);
}
/* ---------------------------------------------------------------------- */
......
......@@ -38,7 +38,7 @@ static bool verbose = false;
// const std::string & name () const;
// JointPtr_t joint () const;
// const ObjectVector_t& innerObjects () const { return innerObjects_; }
//NOCHECKED value_type radius () const
// value_type radius () const
// const ObjectVector_t& outerObjects () const { return outerObjects_; }
// vector3_t localCenterOfMass () const;
// matrix3_t inertiaMatrix() const;
......@@ -50,7 +50,6 @@ static bool verbose = false;
// ModelPtr_t model() ;
// const se3::Frame & frame() const ;
// se3::Frame & frame() ;
//NOCHECKED void updateRadius (const CollisionObjectPtr_t& object);
/* --- UNIT TESTS ----------------------------------------------------------- */
/* --- UNIT TESTS ----------------------------------------------------------- */
......@@ -59,7 +58,7 @@ static bool verbose = false;
BOOST_AUTO_TEST_CASE(body)
{
hpp::model::DevicePtr_t model = hppModel();
hpp::pinocchio::DevicePtr_t pinocchio = hppPinocchio();
hpp::pinocchio::DevicePtr_t pinocchio = hppPinocchio(true);
Eigen::VectorXd q = Eigen::VectorXd::Zero( model->configSize() ); // would be better with neutral
q[3] += 1.0;
......@@ -101,6 +100,7 @@ BOOST_AUTO_TEST_CASE(body)
BOOST_CHECK( bp->name() == bm->name() );
BOOST_CHECK( jp->name() == bp->joint()->name() );
BOOST_CHECK( bp->mass() == bm->mass() );
BOOST_CHECK( std::abs(bp->radius()-bm->radius())<1e-6 );
/* COM and Inertia are expressed in local frames, which are not the same
* in model and pinocchio because of the trick in model of having only
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment