diff --git a/include/hpp/manipulation/device.hh b/include/hpp/manipulation/device.hh index ac91cfdabfc4ec7d81ceef9a43f8bddde4f5ce07..10080879e0cb59e0af96a4a3d4c7b4e29f650bb9 100644 --- a/include/hpp/manipulation/device.hh +++ b/include/hpp/manipulation/device.hh @@ -68,6 +68,8 @@ namespace hpp { void setRobotRootPosition (const std::string& robotName, const Transform3f& positionWRTParentJoint); + virtual pinocchio::DevicePtr_t clone () const; + /// \name Collisions /// \{ @@ -95,6 +97,12 @@ namespace hpp { self_ = self; } + void initCopy (const DeviceWkPtr_t& self, const Device& other) + { + Parent_t::initCopy (self, other); + self_ = self; + } + private: DeviceWkPtr_t self_; diff --git a/src/device.cc b/src/device.cc index 75e9c7202652cb30b11d029f1ba9c48d887ace9c..19a83f130ec1f321d79cdaab8c78e2be1648ad05 100644 --- a/src/device.cc +++ b/src/device.cc @@ -32,6 +32,14 @@ namespace hpp { using se3::Frame; using se3::FrameIndex; + pinocchio::DevicePtr_t Device::clone () const + { + Device* ptr = new Device(*this); + DevicePtr_t shPtr (ptr); + ptr->initCopy (shPtr, *this); + return shPtr; + } + void Device::setRobotRootPosition (const std::string& rn, const Transform3f& t) { @@ -99,6 +107,5 @@ namespace hpp { Containers_t::print <GripperPtr_t> (os); return os; } - } // namespace manipulation } // namespace hpp