Handle stores a pointer to the device.

include/hpp/manipulation/handle.hh

@@ -42,9 +42,10 @@ namespace hpp {
       ///         the gripper.
       static HandlePtr_t create (const std::string& name,
 				 const Transform3f& localPosition,
+				 const DeviceWkPtr_t& robot,
 				 const JointPtr_t& joint)
       {
-	Handle* ptr = new Handle (name, localPosition, joint);
+	Handle* ptr = new Handle (name, localPosition, robot, joint);
 	HandlePtr_t shPtr (ptr);
 	ptr->init (shPtr);
 	return shPtr;
@@ -80,6 +81,11 @@ namespace hpp {
       {
 	joint_ = joint;
       }
+
+      DevicePtr_t robot () const
+      {
+        return robot_.lock();
+      }
       /// \}
 
       /// Get local position in joint frame
@@ -158,11 +164,9 @@ namespace hpp {
       /// \return the constraint of relative position between the handle and
       ///         the gripper.
       Handle (const std::string& name, const Transform3f& localPosition,
-	      const JointPtr_t& joint) : name_ (name),
-					 localPosition_ (localPosition),
-					 joint_ (joint), clearance_ (0),
-                                         mask_ (6, true),
-                                         weakPtr_ ()
+              const DeviceWkPtr_t& robot, const JointPtr_t& joint) :
+        name_ (name), localPosition_ (localPosition), joint_ (joint),
+        robot_ (robot), clearance_ (0), mask_ (6, true), weakPtr_ ()
       {
       }
       void init (HandleWkPtr_t weakPtr)
@@ -178,6 +182,8 @@ namespace hpp {
       Transform3f localPosition_;
       /// Joint to which the handle is linked.
       JointPtr_t joint_;
+      /// Pointer to the robot
+      DeviceWkPtr_t robot_;
       /// Clearance
       value_type clearance_;
       /// Mask

src/handle.cc

@@ -25,15 +25,15 @@
 
 #include <hpp/util/debug.hh>
 
-#include <hpp/pinocchio/device.hh>
 #include <hpp/pinocchio/joint.hh>
 #include <hpp/pinocchio/gripper.hh>
 
-#include <hpp/constraints/generic-transformation.hh>
-
+#include <hpp/constraints/differentiable-function.hh>
 #include <hpp/constraints/implicit.hh>
 #include <hpp/constraints/explicit/relative-pose.hh>
 
+#include <hpp/manipulation/device.hh>
+
 namespace hpp {
   namespace manipulation {
     using constraints::Implicit;
@@ -62,8 +62,10 @@ namespace hpp {
 
     bool isHandleOnFreeflyer (const Handle& handle)
     {
-      if (handle.joint()->jointModel().shortname() == se3::JointModelFreeFlyer::classname()
-          && !handle.joint ()->parentJoint ()) {
+      const JointPtr_t& joint = handle.joint();
+      if (   joint
+          && !joint->parentJoint()
+          && joint->jointModel().shortname() == se3::JointModelFreeFlyer::classname()) {
 	return true;
       }
       return false;
@@ -125,12 +127,12 @@ namespace hpp {
       // If handle is on a freeflying object, create an explicit constraint
       if (is6Dmask(mask_) && isHandleOnFreeflyer (*this)) {
 	return constraints::explicit_::RelativePose::create
-	  (n, gripper->joint ()->robot (), gripper->joint (), joint (),
+	  (n, robot (), gripper->joint (), joint (),
 	   gripper->objectPositionInJoint (), localPosition(),
            mask_, ComparisonTypes_t (6, constraints::EqualToZero));
       }
       return constraints::implicit::RelativePose::create
-        (n, gripper->joint ()->robot (), gripper->joint (), joint (),
+        (n, robot (), gripper->joint (), joint (),
          gripper->objectPositionInJoint (), localPosition(),
          mask_, ComparisonTypes_t (maskSize (mask_), constraints::EqualToZero));
     }
@@ -143,16 +145,16 @@ namespace hpp {
         n = gripper->name() + "_grasps_" + name() + "/complement_" +
           maskToStr (Cmask);
       }
-      core::DevicePtr_t robot = gripper->joint()->robot();
+      core::DevicePtr_t r = robot();
       if (is6Dmask(mask_)) {
         return Implicit::create (
             boost::shared_ptr <ZeroDiffFunc> (new ZeroDiffFunc (
-                robot->configSize(), robot->numberDof (), n))
+                r->configSize(), r->numberDof (), n))
             );
       } else {
         std::vector<bool> Cmask = complementMask(mask_);
         return constraints::implicit::RelativePose::create
-          (n, gripper->joint ()->robot (), gripper->joint (), joint (),
+          (n, r, gripper->joint (), joint (),
            gripper->objectPositionInJoint (), localPosition(),
            Cmask, ComparisonTypes_t (maskSize (Cmask), constraints::Equality));
       }
@@ -179,12 +181,12 @@ namespace hpp {
       // If handle is on a freeflying object, create an explicit constraint
       if (isHandleOnFreeflyer (*this)) {
 	return constraints::explicit_::RelativePose::create
-	  (n, gripper->joint ()->robot (), gripper->joint (), joint (),
+	  (n, robot (), gripper->joint (), joint (),
 	   gripper->objectPositionInJoint (), localPosition(),
            std::vector <bool> (6, true), comp);
       }
       return constraints::implicit::RelativePose::create
-        (n, gripper->joint ()->robot (), gripper->joint (), joint (),
+        (n, robot (), gripper->joint (), joint (),
          gripper->objectPositionInJoint (), localPosition(),
          std::vector <bool> (true, 6), comp);
     }
@@ -199,7 +201,7 @@ namespace hpp {
           + "_" + gripper->name ();
       ImplicitPtr_t result
         (constraints::implicit::RelativePose::create
-         (n, gripper->joint()->robot(), gripper->joint (), joint (),
+         (n, robot(), gripper->joint (), joint (),
           transform, localPosition(), mask_, ComparisonTypes_t
           (maskSize (mask_), constraints::EqualToZero)));
       return result;
@@ -207,7 +209,7 @@ namespace hpp {
 
     HandlePtr_t Handle::clone () const
     {
-      HandlePtr_t other = Handle::create (name (), localPosition (), joint ());
+      HandlePtr_t other = Handle::create (name (), localPosition (), robot(), joint ());
       other->mask(mask_);
       other->clearance(clearance_);
       return other;