diff --git a/include/hpp/pinocchio/device-object-vector.hh b/include/hpp/pinocchio/device-object-vector.hh index b85f6bb..9f19a9e 100644 --- a/include/hpp/pinocchio/device-object-vector.hh +++ b/include/hpp/pinocchio/device-object-vector.hh @@ -32,7 +32,7 @@ namespace hpp { struct DeviceObjectVector : public FakeContainer { - DeviceObjectVector(DevicePtr_t device) + DeviceObjectVector(DeviceWkPtr_t device) : FakeContainer(device) {} DeviceObjectVector() {} @@ -52,7 +52,7 @@ namespace hpp { JointIndex jointIndex; InOutType inOutType; - ObjectVector(DevicePtr_t device,const JointIndex i, InOutType inout) + ObjectVector(DeviceWkPtr_t device,const JointIndex i, InOutType inout) : FakeContainer(device) , jointIndex(i), inOutType(inout) {} ObjectVector() {} @@ -78,7 +78,7 @@ namespace hpp { struct JointVector : public FakeContainer { - JointVector(DevicePtr_t device) : FakeContainer(device) {} + JointVector(DeviceWkPtr_t device) : FakeContainer(device) {} JointVector() {} virtual ~JointVector() {} diff --git a/include/hpp/pinocchio/fake-container.hh b/include/hpp/pinocchio/fake-container.hh index 5cbaca9..d52eed0 100644 --- a/include/hpp/pinocchio/fake-container.hh +++ b/include/hpp/pinocchio/fake-container.hh @@ -39,9 +39,9 @@ namespace hpp { virtual size_type ibegin() const { return 0; } virtual size_type iend () const { return size(); } - DevicePtr_t devicePtr; - FakeContainer( DevicePtr_t device ) : devicePtr(device) {} - FakeContainer() : devicePtr() {} + DeviceWkPtr_t deviceWkPtr_; + FakeContainer( DeviceWkPtr_t device ) : deviceWkPtr_(device) {} + FakeContainer() : deviceWkPtr_() {} struct iterator { @@ -76,18 +76,20 @@ namespace hpp { bool operator!= (const const_iterator & i2){ return idx!=i2.idx; } }; - iterator begin() { assert(devicePtr); return iterator (*this,ibegin() ); } - iterator end() { assert(devicePtr); return iterator (*this,iend() ); } - iterator rbegin() { assert(devicePtr); return iterator (*this,iend()-1 ); } - iterator rend() { assert(devicePtr); return iterator (*this,ibegin()-1); } + iterator begin() { return iterator (*this,ibegin() ); } + iterator end() { return iterator (*this,iend() ); } + iterator rbegin() { return iterator (*this,iend()-1 ); } + iterator rend() { return iterator (*this,ibegin()-1); } - const_iterator begin() const { assert(devicePtr); return const_iterator(*this,ibegin() ); } - const_iterator end() const { assert(devicePtr); return const_iterator(*this,iend() ); } - const_iterator rbegin() const { assert(devicePtr); return const_iterator(*this,iend()-1 ); } - const_iterator rend() const { assert(devicePtr); return const_iterator(*this,ibegin()-1); } + const_iterator begin() const { return const_iterator(*this,ibegin() ); } + const_iterator end() const { return const_iterator(*this,iend() ); } + const_iterator rbegin() const { return const_iterator(*this,iend()-1 ); } + const_iterator rend() const { return const_iterator(*this,ibegin()-1); } T operator[](const int idx) { return at(idx); } Tconst operator[](const int idx) const { return at(idx); } + + DevicePtr_t device () const { assert(!deviceWkPtr_.expired()); return deviceWkPtr_.lock(); } }; } // namespace pinocchio diff --git a/src/device-object-vector.cc b/src/device-object-vector.cc index 416444b..52ebc6c 100644 --- a/src/device-object-vector.cc +++ b/src/device-object-vector.cc @@ -31,33 +31,33 @@ namespace hpp { CollisionObjectPtr_t DeviceObjectVector::at(const size_type i) { selfAssert(); - return CollisionObjectPtr_t (new CollisionObject(devicePtr,i)); + return CollisionObjectPtr_t (new CollisionObject(device(),i)); } CollisionObjectConstPtr_t DeviceObjectVector::at(const size_type i) const { selfAssert(); - return CollisionObjectConstPtr_t (new CollisionObject(devicePtr,i)); + return CollisionObjectConstPtr_t (new CollisionObject(device(),i)); } size_type DeviceObjectVector::size() const - { return devicePtr->geomModel().ngeoms; } + { return device()->geomModel().ngeoms; } void DeviceObjectVector::selfAssert(size_type i) const { - assert(devicePtr); + assert(device()); assert(imodel().njoint); + assert(device()); + assert(int(jointIndex)model().njoint); assert(igeomModel().innerObjects[jointIndex]; - else return devicePtr->geomModel().outerObjects[jointIndex]; + if(inOutType==INNER) return device()->geomModel().innerObjects[jointIndex]; + else return device()->geomModel().outerObjects[jointIndex]; } /* --- JointVector --------------------------------------------------------- */ /* Access to pinocchio index + 1 because pinocchio first joint is the universe. */ JointPtr_t JointVector::at(const size_type i) - { selfAssert(i); return JointPtr_t(new Joint(devicePtr,i+1)); } + { selfAssert(i); return JointPtr_t(new Joint(device(),i+1)); } /* Access to pinocchio index + 1 because pinocchio first joint is the universe. */ JointConstPtr_t JointVector::at(const size_type i) const - { selfAssert(i); return JointConstPtr_t(new Joint(devicePtr,i+1)); } + { selfAssert(i); return JointConstPtr_t(new Joint(device(),i+1)); } size_type JointVector::size() const - { return devicePtr->model().njoint - 1; } + { return device()->model().njoint - 1; } size_type JointVector::iend() const { return size(); } void JointVector::selfAssert(size_type i) const { - assert(devicePtr); + assert(device()); assert(i>=ibegin()); assert(i