diff --git a/include/hpp/manipulation/device.hh b/include/hpp/manipulation/device.hh index e30899486cfbb15023951e4cd8e440a53b638c09..7fac5b140d844eccfccc310d0e87b6678a96fa0c 100644 --- a/include/hpp/manipulation/device.hh +++ b/include/hpp/manipulation/device.hh @@ -20,36 +20,37 @@ #ifndef HPP_MANIPULATION_DEVICE_HH # define HPP_MANIPULATION_DEVICE_HH -# include <hpp/model/humanoid-robot.hh> +# include <hpp/pinocchio/humanoid-robot.hh> + # include <hpp/core/container.hh> -# include "hpp/manipulation/fwd.hh" -# include "hpp/manipulation/config.hh" +# include <hpp/manipulation/fwd.hh> +# include <hpp/manipulation/config.hh> namespace hpp { namespace manipulation { /// Device with handles. /// - /// As a deriving class of hpp::model::HumanoidRobot, - /// it is compatible with hpp::model::urdf::loadHumanoidRobot + /// As a deriving class of hpp::pinocchio::HumanoidRobot, + /// it is compatible with hpp::pinocchio::urdf::loadHumanoidRobot /// - /// This class also contains model::Gripper, Handle and \ref JointAndTriangles_t + /// This class also contains pinocchio::Gripper, Handle and \ref JointAndShapes_t class HPP_MANIPULATION_DLLAPI Device : - public model::HumanoidRobot, + public pinocchio::HumanoidRobot, public core::Containers< boost::mpl::vector < HandlePtr_t, - model::GripperPtr_t, + GripperPtr_t, JointAndShapes_t, - model::JointVector_t> > + JointIndexes_t> > { public: - typedef model::HumanoidRobot Parent_t; + typedef pinocchio::HumanoidRobot Parent_t; typedef core::Containers< boost::mpl::vector < HandlePtr_t, - model::GripperPtr_t, + pinocchio::GripperPtr_t, JointAndShapes_t, - model::JointVector_t> > Containers_t; + JointIndexes_t> > Containers_t; /// Constructor /// \param name of the new instance, @@ -67,13 +68,6 @@ namespace hpp { /// \name Collisions /// \{ - /// Cache joint vector - void prepareInsertRobot () - { - didPrepare_ = true; - jointCache_ = getJointVector (); - } - /// Add collisions /// between the joint vector cache initialized by prepareInsertRobot() /// add the current Robot. @@ -89,7 +83,7 @@ namespace hpp { /// \param robot Robots that manipulate objects, /// \param objects Set of objects manipulated by the robot. Device (const std::string& name) : - Parent_t (name), jointCache_ (), didPrepare_ (false) + Parent_t (name), jointCacheSize_ (0) {} void init (const DeviceWkPtr_t& self) @@ -101,8 +95,7 @@ namespace hpp { private: DeviceWkPtr_t self_; - model::JointVector_t jointCache_; - bool didPrepare_; + std::size_t jointCacheSize_; }; // class Device } // namespace manipulation } // namespace hpp diff --git a/src/device.cc b/src/device.cc index 36a6b68d0c0e0aff0909121a5449e773ce99b06d..6bf2601d8e4667e70ca202dee6f50c519c887fd3 100644 --- a/src/device.cc +++ b/src/device.cc @@ -17,42 +17,27 @@ // hpp-manipulation. If not, see // <http://www.gnu.org/licenses/>. -#include <hpp/model/gripper.hh> #include <hpp/manipulation/device.hh> -#include <hpp/manipulation/handle.hh> -#include <hpp/model/joint.hh> +#include <hpp/pinocchio/joint.hh> +#include <hpp/pinocchio/gripper.hh> + +#include <hpp/manipulation/handle.hh> namespace hpp { namespace manipulation { - void Device::didInsertRobot (const std::string& name) - { - if (!didPrepare_) { - hppDout (error, "You must call prepareInsertRobot before."); - } - didPrepare_ = false; - /// Build list of new joints - const model::JointVector_t jv = getJointVector (); - model::JointVector_t newj; - newj.reserve (jv.size () - jointCache_.size ()); - model::JointVector_t::const_iterator retFind, it1, it2; - for (it1 = jv.begin (); it1 != jv.end (); ++it1) { - retFind = find (jointCache_.begin (), jointCache_.end (), *it1); - if (retFind == jointCache_.end ()) - newj.push_back (*it1); - } - /// Add collision between old joints and new ones. - for (it1 = newj.begin (); it1 != newj.end (); ++it1) { - if (!(*it1)->linkedBody ()) continue; - for (it2 = jointCache_.begin (); it2 != jointCache_.end (); ++it2) { - if (!(*it2)->linkedBody ()) continue; - addCollisionPairs (*it1, *it2, model::COLLISION); - addCollisionPairs (*it1, *it2, model::DISTANCE); - } - } - jointCache_.clear (); - add (name, newj); - } + void Device::didInsertRobot (const std::string& name) + { + /// Build list of new joints + std::size_t jvSize = model().joints.size(); + assert (jvSize >= jointCacheSize_); + JointIndexes_t newJ (jvSize - jointCacheSize_); + for (std::size_t i = jointCacheSize_; i < jvSize; ++i) newJ[i] = i; + + jointCacheSize_ = model().joints.size(); + add (name, newJ); + } + std::ostream& Device::print (std::ostream& os) const { Parent_t::print (os); @@ -61,7 +46,7 @@ namespace hpp { Containers_t::print <HandlePtr_t> (os); // print grippers os << "Grippers:" << std::endl; - Containers_t::print <model::GripperPtr_t> (os); + Containers_t::print <GripperPtr_t> (os); return os; }