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

Device::model/data/geomModel/geomData now returns references, while additional...

Device::model/data/geomModel/geomData now returns references, while additional methods xxxPtr can be used to recover the pointers. Propagate the changes to the rest of the lib.
parent ce5a422e
......@@ -183,8 +183,8 @@ namespace hpp {
/// If frameIndex==-1 (after init), search in pinocchio frame list the proper index.
void searchFrameIndex() const;
ModelConstPtr_t model() const ;
ModelPtr_t model() ;
const se3::Model& model() const ;
se3::Model& model() ;
const se3::Frame & frame() const ;
se3::Frame & frame() ;
......
......@@ -43,7 +43,7 @@ namespace hpp {
ConfigurationIn_t configuration,
vectorIn_t velocity, ConfigurationOut_t result)
{
result = se3::integrate(*robot->model(), configuration, velocity);
result = se3::integrate(robot->model(), configuration, velocity);
const size_type& dim = robot->extraConfigSpace().dimension();
result.tail (dim) = configuration.tail (dim) + velocity.tail (dim);
}
......@@ -60,7 +60,7 @@ namespace hpp {
const value_type& u,
ConfigurationOut_t result)
{
result = se3::interpolate(*robot->model(), q0, q1, u);
result = se3::interpolate(robot->model(), q0, q1, u);
const size_type& dim = robot->extraConfigSpace().dimension();
result.tail (dim) = u * q1.tail (dim) + (1-u) * q0.tail (dim);
}
......@@ -78,7 +78,7 @@ namespace hpp {
void inline difference (const DevicePtr_t& robot, ConfigurationIn_t q1,
ConfigurationIn_t q2, vectorOut_t result)
{
result = se3::differentiate(*robot->model(), q2, q1);
result = se3::differentiate(robot->model(), q2, q1);
const size_type& dim = robot->extraConfigSpace().dimension();
result.tail (dim) = q1.tail (dim) - q2.tail (dim);
}
......@@ -91,7 +91,7 @@ namespace hpp {
inline value_type distance (const DevicePtr_t& robot, ConfigurationIn_t q1,
ConfigurationIn_t q2)
{
vector_t dist = se3::distance(*robot->model(), q1, q2);
vector_t dist = se3::distance(robot->model(), q1, q2);
const size_type& dim = robot->extraConfigSpace().dimension();
if (dim == 0) return dist.norm();
else return sqrt (dist.squaredNorm() + (q2.tail (dim) - q1.tail (dim)).squaredNorm ());
......@@ -105,7 +105,7 @@ namespace hpp {
/// SO3 joints and 2D-vectors for unbounded rotations.
inline void normalize (const DevicePtr_t& robot, Configuration_t& q)
{
se3::normalize(*robot->model(), q);
se3::normalize(robot->model(), q);
}
/// For backward compatibility.
......
......@@ -99,32 +99,48 @@ namespace hpp {
/// Set pinocchio model.
void model( ModelPtr_t modelPtr ) { model_ = modelPtr; }
/// Access to pinocchio model
ModelConstPtr_t model() const { return model_; }
ModelConstPtr_t modelPtr() const { return model_; }
/// Access to pinocchio model
ModelPtr_t model() { return model_; }
ModelPtr_t modelPtr() { return model_; }
/// Access to pinocchio model
const se3::Model& model() const { assert(model_); return *model_; }
/// Access to pinocchio model
se3::Model& model() { assert(model_); return *model_; }
/// Set pinocchio geom.
void geomModel( GeomModelPtr_t geomModelPtr ) { geomModel_ = geomModelPtr; }
/// Access to pinocchio geomModel
GeomModelConstPtr_t geomModel() const { return geomModel_; }
GeomModelConstPtr_t geomModelPtr() const { return geomModel_; }
/// Access to pinocchio geomModel
GeomModelPtr_t geomModelPtr() { return geomModel_; }
/// Access to pinocchio geomModel
const se3::GeometryModel & geomModel() const { assert(geomModel_); return *geomModel_; }
/// Access to pinocchio geomModel
GeomModelPtr_t geomModel() { return geomModel_; }
se3::GeometryModel & geomModel() { assert(geomModel_); return *geomModel_; }
/// Set Pinocchio data corresponding to model
void data( DataPtr_t dataPtr ) { data_ = dataPtr; resizeState(); }
/// Access to Pinocchio data/
DataConstPtr_t data() const { return data_; }
DataConstPtr_t dataPtr() const { return data_; }
/// Access to Pinocchio data/
DataPtr_t data() { return data_; }
DataPtr_t dataPtr() { return data_; }
/// Access to Pinocchio data/
const se3::Data & data() const { assert(data_); return *data_; }
/// Access to Pinocchio data/
se3::Data & data() { assert(data_); return *data_; }
/// Create Pinocchio data from model.
void createData();
/// Set Pinocchio geomData corresponding to model
void geomData( GeomDataPtr_t geomDataPtr ) { geomData_ = geomDataPtr; resizeState(); }
/// Access to Pinocchio geomData/
GeomDataConstPtr_t geomData() const { return geomData_; }
GeomDataConstPtr_t geomDataPtr() const { return geomData_; }
/// Access to Pinocchio geomData/
GeomDataPtr_t geomDataPtr() { return geomData_; }
/// Access to Pinocchio geomData/
const se3::GeometryData& geomData() const { assert(geomData_); return *geomData_; }
/// Access to Pinocchio geomData/
GeomDataPtr_t geomData() { return geomData_; }
se3::GeometryData& geomData() { assert(geomData_); return *geomData_; }
/// Create Pinocchio geomData from model.
void createGeomData();
......
......@@ -286,7 +286,7 @@ namespace hpp {
se3::JointModel& jointModel()
{
return model()->joints[index()];
return model().joints[index()];
}
/// \}
......@@ -301,10 +301,10 @@ namespace hpp {
/// Store list of childrens.
void setChildList();
ModelPtr_t model() ;
ModelConstPtr_t model() const ;
DataPtr_t data() ;
DataConstPtr_t data() const ;
se3::Model& model() ;
const se3::Model& model() const ;
se3::Data & data() ;
const se3::Data & data() const ;
/// Assert that the members of the struct are valid (no null pointer, etc).
void selfAssert() const;
......
......@@ -39,29 +39,30 @@ namespace hpp {
void Body::selfAssert() const
{
assert(devicePtr);
assert(devicePtr->model()->njoint>int(jointIndex));
assert(devicePtr->modelPtr());
assert(devicePtr->model().njoint>int(jointIndex));
if(frameIndexSet)
assert(devicePtr->model()->nFrames>int(frameIndex));
assert(devicePtr->model().nFrames>int(frameIndex));
}
ModelConstPtr_t Body::model() const { return devicePtr->model(); }
ModelPtr_t Body::model() { return devicePtr->model(); }
se3::Frame & Body::frame()
const se3::Model & Body::model() const { return devicePtr->model(); }
se3::Model & Body::model() { return devicePtr->model(); }
se3::Frame & Body::frame()
{
searchFrameIndex();
return model()->frames[frameIndex];
return model().frames[frameIndex];
}
const se3::Frame & Body::frame() const
{
searchFrameIndex();
return model()->frames[frameIndex];
return model().frames[frameIndex];
}
void Body::searchFrameIndex() const
{
if(frameIndexSet) return;
frameIndex = 0;
BOOST_FOREACH(const se3::Frame & frame,devicePtr->model()->frames)
BOOST_FOREACH(const se3::Frame & frame,devicePtr->model().frames)
{
if( (se3::BODY == frame.type) && (frame.parent == jointIndex) )
break;
......@@ -87,25 +88,25 @@ namespace hpp {
const vector3_t& Body::localCenterOfMass () const
{
selfAssert();
return model()->inertias[jointIndex].lever();
return model().inertias[jointIndex].lever();
}
matrix3_t Body::inertiaMatrix() const
{
selfAssert();
return model()->inertias[jointIndex].inertia();
return model().inertias[jointIndex].inertia();
}
value_type Body::mass() const
{
selfAssert();
return model()->inertias[jointIndex].mass();
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];
assert(devicePtr->geomDataPtr());
assert(int(devicePtr->geomData().radius.size())==model().njoint);
return devicePtr->geomData().radius[jointIndex];
}
} // namespace pinocchio
......
......@@ -37,7 +37,7 @@ namespace hpp {
void CenterOfMassComputation::compute (const Device::Computation_t& flag)
{
const se3::Model& model = *robot_->model();
const se3::Model& model = robot_->model();
bool computeCOM = (flag & Device::COM);
bool computeJac = (flag & Device::JACOBIAN);
......@@ -45,7 +45,7 @@ namespace hpp {
assert (!(computeJac && !computeCOM)); // JACOBIAN => COM
// update kinematics
se3::copy<0>(model,*robot_->data(),data);
se3::copy<0>(model,robot_->data(),data);
data.mass[0] = 0;
if(computeCOM) data.com[0].setZero();
......@@ -131,13 +131,13 @@ namespace hpp {
CenterOfMassComputation::CenterOfMassComputation (const DevicePtr_t& d) :
robot_(d), roots_ (), //mass_ (0), jacobianCom_ (3, d->numberDof ())
data(*d->model())
{ assert (d->model()); }
data(d->model())
{ assert (d->modelPtr()); }
void CenterOfMassComputation::add (const JointPtr_t& j)
{
se3::JointIndex jid = j->index();
const se3::Model& model = *robot_->model();
const se3::Model& model = robot_->model();
BOOST_FOREACH( const se3::JointIndex rootId, roots_ )
{
assert (int(rootId)<model.njoint);
......
......@@ -56,22 +56,22 @@ namespace hpp {
CollisionObject::ObjectVec_t &
CollisionObject::objectVec()
{
if(inOutType==INNER) return devicePtr->geomModel()->innerObjects;
else return devicePtr->geomModel()->outerObjects;
if(inOutType==INNER) return devicePtr->geomModel().innerObjects;
else return devicePtr->geomModel().outerObjects;
}
const CollisionObject::ObjectVec_t &
CollisionObject::objectVec() const
{
if(inOutType==INNER) return devicePtr->geomModel()->innerObjects;
else return devicePtr->geomModel()->outerObjects;
if(inOutType==INNER) return devicePtr->geomModel().innerObjects;
else return devicePtr->geomModel().outerObjects;
}
const std::string& CollisionObject::name () const { return pinocchio().name; }
const se3::GeometryObject & CollisionObject::pinocchio () const
{ return devicePtr->geomModel()->geometryObjects[geomInModelIndex]; }
{ return devicePtr->geomModel().geometryObjects[geomInModelIndex]; }
se3::GeometryObject & CollisionObject::pinocchio ()
{ return devicePtr->geomModel()->geometryObjects[geomInModelIndex]; }
{ return devicePtr->geomModel().geometryObjects[geomInModelIndex]; }
fclCollisionObjectPtr_t CollisionObject::fcl ()
{ return & pinocchio().collision_object; }
......@@ -87,16 +87,16 @@ namespace hpp {
positionInJointFrame () const { return pinocchio().placement; }
const fcl::Transform3f& CollisionObject::getFclTransform () const
{ return devicePtr->geomData()->oMg_fcl[geomInModelIndex]; }
{ return devicePtr->geomData().oMg_fcl[geomInModelIndex]; }
const Transform3f& CollisionObject::getTransform () const
{ return devicePtr->geomData()->oMg[geomInModelIndex]; }
{ return devicePtr->geomData().oMg[geomInModelIndex]; }
void CollisionObject::move (const Transform3f& position)
{
// move does not work but for object attached to the universe (joint 0)
assert( jointIndex==0 );
devicePtr->geomData()->oMg[geomInModelIndex] = position;
devicePtr->geomData()->oMg_fcl[geomInModelIndex]
devicePtr->geomData().oMg[geomInModelIndex] = position;
devicePtr->geomData().oMg_fcl[geomInModelIndex]
= toFclTransform3f(position);
pinocchio().placement = position;
}
......@@ -104,10 +104,10 @@ namespace hpp {
void CollisionObject::selfAssert() const
{
assert(devicePtr);
assert(devicePtr->model()->njoint>int(jointIndex));
assert(devicePtr->model().njoint>int(jointIndex));
if(geomInJointIndexSet)
assert(objectVec().at(jointIndex).size()>geomInJointIndex);
assert(devicePtr->geomModel()->geometryObjects.size()>geomInModelIndex);
assert(devicePtr->geomModel().geometryObjects.size()>geomInModelIndex);
}
/* --- ITERATOR --------------------------------------------------------- */
......@@ -124,15 +124,15 @@ namespace hpp {
size_type ObjectVector::size() const
{
if( inOutType==CollisionObject::INNER )
return size_type(devicePtr->geomModel()->innerObjects[jointIndex].size());
return size_type(devicePtr->geomModel().innerObjects[jointIndex].size());
else
return size_type(devicePtr->geomModel()->outerObjects[jointIndex].size());
return size_type(devicePtr->geomModel().outerObjects[jointIndex].size());
}
void ObjectVector::selfAssert(size_type i) const
{
assert(devicePtr);
assert(int(jointIndex)<devicePtr->model()->njoint);
assert(int(jointIndex)<devicePtr->model().njoint);
assert(i<size());
}
......
......@@ -39,7 +39,7 @@ namespace hpp {
}
size_type DeviceObjectVector::size() const
{ return devicePtr->geomModel()->ngeoms; }
{ return devicePtr->geomModel().ngeoms; }
void DeviceObjectVector::selfAssert(size_type i) const
{
......
......@@ -61,7 +61,7 @@ namespace hpp {
createCopy (const DevicePtr_t& device)
{
DevicePtr_t res = Device::create(device->name()); // init shared ptr
res->model(device->model()); // Copy pointer to pinocchio model
res->model(device->modelPtr()); // Copy pointer to pinocchio model
res->createData(); // Create a new data, dont copy the pointer.
return res;
}
......@@ -210,7 +210,7 @@ namespace hpp {
neutralConfiguration () const
{
Configuration_t n (configSize());
n.head(model_->nq) = model()->neutralConfiguration;
n.head(model_->nq) = model().neutralConfiguration;
n.tail(extraConfigSpace_.dimension()).setZero();
return n;
}
......@@ -290,21 +290,21 @@ namespace hpp {
{
/* Following hpp::model API, the forward kinematics (joint placement) is
* supposed to have already been computed. */
se3::updateGeometryPlacements(*model(),*data(),*geomModel(),*geomData());
return se3::computeCollisions(*geomData(),stopAtFirstCollision);
se3::updateGeometryPlacements(model(),data(),geomModel(),geomData());
return se3::computeCollisions(geomData(),stopAtFirstCollision);
}
void Device::computeDistances ()
{
/* Following hpp::model API, the forward kinematics (joint placement) is
* supposed to have already been computed. */
se3::updateGeometryPlacements(*model(),*data(),*geomModel(),*geomData());
se3::computeDistances (*geomData());
se3::updateGeometryPlacements(model(),data(),geomModel(),geomData());
se3::computeDistances (geomData());
}
const DistanceResults_t& Device::distanceResults () const
{
return geomData()->distance_results;
return geomData().distance_results;
}
......
......@@ -28,13 +28,13 @@ namespace hpp {
device_ (device),
clearance_ (0)
{
fid_ = device->model()->getFrameId (name);
fid_ = device->model().getFrameId (name);
joint_ = JointPtr_t (
new Joint(device, device->model()->getFrameParent (fid_)));
new Joint(device, device->model().getFrameParent (fid_)));
}
const Transform3f& Gripper::objectPositionInJoint () const
{
return device_->model()->getFramePlacement(fid_);
return device_->model().getFramePlacement(fid_);
}
GripperPtr_t Gripper::clone () const
......
......@@ -28,64 +28,65 @@ namespace hpp {
:devicePtr(device)
,jointIndex(indexInJointList)
{
assert (model());
assert (int(jointIndex)<model()->njoint);
assert (devicePtr);
assert (devicePtr->modelPtr());
assert (int(jointIndex)<model().njoint);
setChildList();
}
void Joint::setChildList()
{
assert(model()); assert(data());
assert(devicePtr->modelPtr()); assert(devicePtr->dataPtr());
children.clear();
for( se3::Index child=jointIndex+1;int(child)<=data()->lastChild[jointIndex];++child )
if( model()->parents[child]==jointIndex ) children.push_back (child) ;
for( se3::Index child=jointIndex+1;int(child)<=data().lastChild[jointIndex];++child )
if( model().parents[child]==jointIndex ) children.push_back (child) ;
}
void Joint::selfAssert() const
{
assert(devicePtr);
assert(model()); assert(data());
assert(devicePtr->model()->njoint>int(jointIndex));
assert(devicePtr->modelPtr()); assert(devicePtr->dataPtr());
assert(devicePtr->model().njoint>int(jointIndex));
}
ModelPtr_t Joint::model() { DevicePtr_t r = robot(); return r->model(); }
ModelConstPtr_t Joint::model() const { return devicePtr->model(); }
DataPtr_t Joint::data() { return devicePtr->data (); }
DataConstPtr_t Joint::data() const { return devicePtr->data (); }
se3::Model& Joint::model() { selfAssert(); return devicePtr->model(); }
const se3::Model& Joint::model() const { selfAssert(); return devicePtr->model(); }
se3::Data & Joint::data() { selfAssert(); return devicePtr->data (); }
const se3::Data & Joint::data() const { selfAssert(); return devicePtr->data (); }
const std::string& Joint::name() const
{
selfAssert();
return model()->names[jointIndex];
return model().names[jointIndex];
}
const Transform3f& Joint::currentTransformation () const
{
selfAssert();
return data()->oMi[jointIndex];
return data().oMi[jointIndex];
}
size_type Joint::numberDof () const
{
selfAssert();
return model()->joints[jointIndex].nv();
return model().joints[jointIndex].nv();
}
size_type Joint::configSize () const
{
selfAssert();
return model()->joints[jointIndex].nq();
return model().joints[jointIndex].nq();
}
size_type Joint::rankInConfiguration () const
{
selfAssert();
return model()->joints[jointIndex].idx_q();
return model().joints[jointIndex].idx_q();
}
size_type Joint::rankInVelocity () const
{
selfAssert();
return model()->joints[jointIndex].idx_v();
return model().joints[jointIndex].idx_v();
}
std::size_t Joint::numberChildJoints () const
......@@ -103,17 +104,17 @@ namespace hpp {
const Transform3f& Joint::positionInParentFrame () const
{
selfAssert();
return model()->jointPlacements[jointIndex];
return model().jointPlacements[jointIndex];
}
void Joint::isBounded (size_type rank, bool bounded)
{
const size_type idx = model()->joints[jointIndex].idx_q() + rank;
const size_type idx = model().joints[jointIndex].idx_q() + rank;
assert(rank < configSize());
if (!bounded) {
const value_type& inf = std::numeric_limits<value_type>::infinity();
model()->lowerPositionLimit[idx] = -inf;
model()->upperPositionLimit[idx] = inf;
model().lowerPositionLimit[idx] = -inf;
model().upperPositionLimit[idx] = inf;
} else {
assert(false && "This function can only unset bounds. "
"Use lowerBound and upperBound to set the bounds.");
......@@ -121,35 +122,35 @@ namespace hpp {
}
bool Joint::isBounded (size_type rank) const
{
const size_type idx = model()->joints[jointIndex].idx_q() + rank;
const size_type idx = model().joints[jointIndex].idx_q() + rank;
const value_type& inf = std::numeric_limits<value_type>::infinity();
assert(rank < configSize());
return !( model()->lowerPositionLimit[idx] == -inf)
|| !( model()->upperPositionLimit[idx] == inf);
return !( model().lowerPositionLimit[idx] == -inf)
|| !( model().upperPositionLimit[idx] == inf);
}
value_type Joint::lowerBound (size_type rank) const
{
const size_type idx = model()->joints[jointIndex].idx_q() + rank;
const size_type idx = model().joints[jointIndex].idx_q() + rank;
assert(rank < configSize());
return model()->lowerPositionLimit[idx];
return model().lowerPositionLimit[idx];
}
value_type Joint::upperBound (size_type rank) const
{
const size_type idx = model()->joints[jointIndex].idx_q() + rank;
const size_type idx = model().joints[jointIndex].idx_q() + rank;
assert(rank < configSize());
return model()->upperPositionLimit[idx];
return model().upperPositionLimit[idx];
}
void Joint::lowerBound (size_type rank, value_type lowerBound)
{
const size_type idx = model()->joints[jointIndex].idx_q() + rank;
const size_type idx = model().joints[jointIndex].idx_q() + rank;
assert(rank < configSize());
model()->lowerPositionLimit[idx] = lowerBound;
model().lowerPositionLimit[idx] = lowerBound;
}
void Joint::upperBound (size_type rank, value_type upperBound)
{
const size_type idx = model()->joints[jointIndex].idx_q() + rank;
const size_type idx = model().joints[jointIndex].idx_q() + rank;
assert(rank < configSize());
model()->upperPositionLimit[idx] = upperBound;
model().upperPositionLimit[idx] = upperBound;
}
//NOTYET value_type Joint::upperBoundLinearVelocity () const {}
......@@ -160,18 +161,18 @@ namespace hpp {
const JointJacobian_t& Joint::jacobian (const bool local) const
{
selfAssert(); assert(robot()->computationFlag()|Device::JACOBIAN);
if( jacobian_.cols()!=model()->nv) jacobian_ = JointJacobian_t::Zero(6,model()->nv);
if(local) se3::getJacobian<true> (*model(),*data(),jointIndex,jacobian_);
else se3::getJacobian<false>(*model(),*data(),jointIndex,jacobian_);
if( jacobian_.cols()!=model().nv) jacobian_ = JointJacobian_t::Zero(6,model().nv);
if(local) se3::getJacobian<true> (model(),data(),jointIndex,jacobian_);
else se3::getJacobian<false>(model(),data(),jointIndex,jacobian_);
return jacobian_;
}
JointJacobian_t& Joint::jacobian (const bool local)
{
selfAssert(); assert(robot()->computationFlag()|Device::JACOBIAN);
if( jacobian_.cols()!=model()->nv) jacobian_ = JointJacobian_t::Zero(6,model()->nv);
if(local) se3::getJacobian<true> (*model(),*data(),jointIndex,jacobian_);
else se3::getJacobian<false>(*model(),*data(),jointIndex,jacobian_);
if( jacobian_.cols()!=model().nv) jacobian_ = JointJacobian_t::Zero(6,model().nv);
if(local) se3::getJacobian<true> (model(),data(),jointIndex,jacobian_);
else se3::getJacobian<false>(model(),data(),jointIndex,jacobian_);
return jacobian_;
}
......@@ -243,7 +244,7 @@ namespace hpp {
{ selfAssert(i); return JointConstPtr_t(new Joint(devicePtr,i+1)); }
size_type JointVector::size() const
{ return devicePtr->model()->njoint - 1; }
{ return devicePtr->model().njoint - 1; }
size_type JointVector::iend() const
{ return size(); }
......
......@@ -142,6 +142,7 @@ struct IsCollisionObjectNamed
BOOST_AUTO_TEST_CASE(geomsAccess)
{
verbose = true;
hpp::model::DevicePtr_t model = hppModel();
hpp::pinocchio::DevicePtr_t pinocchio = hppPinocchio(true);