Commit 255d4396 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Merge remote-tracking branch 'jmirabel/master'

parents 3e3e9108 e3ab37b2
......@@ -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
......@@ -187,7 +188,6 @@ namespace hpp {
const se3::Frame & frame() const ;
se3::Frame & frame() ;
//NOTYET void updateRadius (const CollisionObjectPtr_t& object);
//DEPREC ObjectVector_t collisionInnerObjects_;
//DEPREC ObjectVector_t collisionOuterObjects_;
//DEPREC ObjectVector_t distanceInnerObjects_;
......@@ -198,14 +198,13 @@ namespace hpp {
mutable bool frameIndexSet;
ObjectVector innerObjects_,outerObjects_;
//DEPREC JointPtr_t joint_;
//DEPREC std::string name_;
//DEPREC /// Inertial information
//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
......
......@@ -46,7 +46,6 @@ namespace hpp {
/// http://www.boost.org/libs/smart_ptr/smart_ptr.htm
class HPP_PINOCCHIO_DLLAPI Device
{
//NOTYET friend class Body;
friend class Joint;
public:
/// Flags to select computation
......@@ -215,7 +214,7 @@ namespace hpp {
virtual bool currentConfiguration (ConfigurationIn_t configuration);
/// Get the neutral configuration
Configuration_t neutralConfiguration () const;
const Configuration_t & neutralConfiguration () const;
/// Get current velocity
const vector_t& currentVelocity () const
......
......@@ -57,7 +57,7 @@ namespace hpp {
HPP_PREDEF_CLASS (Joint);
HPP_PREDEF_CLASS (JointConfiguration);
HPP_PREDEF_CLASS (Gripper);
enum Request_t {COLLISION, DISTANCE} HPP_PINOCCHIO_DEPRECATED;
enum Request_t {COLLISION, DISTANCE};
typedef double value_type;
typedef Eigen::Matrix <value_type, Eigen::Dynamic, 1> vector_t;
......
......@@ -111,8 +111,8 @@ namespace hpp {
//DEPREC /// Compute jacobian matrix of joint and all its descendents.
//DEPREC void computeJacobian ();
/// Get neutral configuration of joint
//NOTYET vector_t neutralConfiguration () const;
//DEPREC /// Get neutral configuration of joint
//DEPREC vector_t neutralConfiguration () const;
///\}
// -----------------------------------------------------------------------
......
......@@ -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_);
}
/* ---------------------------------------------------------------------- */
......@@ -199,6 +200,10 @@ namespace hpp {
return false;
}
const Configuration_t & Device::
neutralConfiguration () const
{ return model()->neutralConfiguration; }
const value_type& Device::
mass () const
{
......
......@@ -65,8 +65,6 @@ namespace hpp {
return data()->oMi[jointIndex];
}
//NOTYET vector_t Joint::neutralConfiguration () const {}
size_type Joint::numberDof () const
{
selfAssert();
......@@ -154,7 +152,6 @@ namespace hpp {
model()->upperPositionLimit[idx] = upperBound;
}
//NOTYET value_type Joint::upperBoundLinearVelocity () const {}
//NOTYET value_type Joint::upperBoundAngularVelocity () const {}
//NOTYET const value_type& m Joint::aximalDistanceToParent () const {}
......
......@@ -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
......
......@@ -57,7 +57,7 @@ static bool verbose = false;
//NOTCHECKED virtual void setDimensionExtraConfigSpace (const size_type& dimension)
// const Configuration_t& currentConfiguration () const
// virtual bool currentConfiguration (ConfigurationIn_t configuration);
//NOTCHECKED Configuration_t neutralConfiguration () const;
// Configuration_t neutralConfiguration () const;
// const vector_t& currentVelocity () const
// void currentVelocity (vectorIn_t velocity)
// const vector_t& currentAcceleration () const
......@@ -146,6 +146,11 @@ BOOST_AUTO_TEST_CASE (easyGetter)
q = qcopy;
pinocchio->currentConfiguration(q);
BOOST_CHECK( pinocchio->currentConfiguration() == qcopy );
std::cout << pinocchio->neutralConfiguration().transpose() << std::endl;
std::cout << model->neutralConfiguration().transpose() << std::endl;
BOOST_CHECK( pinocchio->neutralConfiguration().isApprox( model->neutralConfiguration() ) );
}
/* --- Check vel acc */
......
......@@ -49,10 +49,10 @@ static bool verbose = false;
//NOTCHECKED const value_type& maximalDistanceToParent () const;
//NOTCHECKED void computeMaximalDistanceToParent ();
// const JointJacobian_t& jacobian () const;
//NOTCHECKED JointJacobian_t& jacobian ();
// JointJacobian_t& jacobian ();
// DeviceConstPtr_t robot () const { assert(robot_.lock()); return robot_.lock ();}
// DevicePtr_t robot () { assert(robot_.lock()); return robot_.lock ();}
//NOTCHECKED BodyPtr_t linkedBody () const;
// BodyPtr_t linkedBody () const;
//NOTCHECKED virtual std::ostream& display (std::ostream& os) const;
// protected:
//NOTCHECKED value_type maximalDistanceToParent_;
......
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