From 88cf8030fab867b551734bbfff900a0d1d1aabdb Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 5 Dec 2018 14:10:24 +0100
Subject: [PATCH] Handle stores a pointer to the device.
---
include/hpp/manipulation/handle.hh | 18 ++++++++++++------
src/handle.cc | 30 ++++++++++++++++--------------
2 files changed, 28 insertions(+), 20 deletions(-)
diff --git a/include/hpp/manipulation/handle.hh b/include/hpp/manipulation/handle.hh
index d53869b1..6574da17 100644
--- a/include/hpp/manipulation/handle.hh
+++ b/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
diff --git a/src/handle.cc b/src/handle.cc
index 1022ad1b..0027bac7 100644
--- a/src/handle.cc
+++ b/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;
--
GitLab