diff --git a/include/hpp/manipulation/axial-handle.hh b/include/hpp/manipulation/axial-handle.hh
index 66c5ab1c62597db975eee4e9d0e6579d18363a24..5cd036895f03411c998e6f6dd7f5ef2928c8da47 100644
--- a/include/hpp/manipulation/axial-handle.hh
+++ b/include/hpp/manipulation/axial-handle.hh
@@ -51,7 +51,7 @@ namespace hpp {
/// \return the constraint of relative transformation between the handle and
/// the gripper. The rotation around x is not constrained.
virtual NumericalConstraintPtr_t createGrasp
- (const GripperPtr_t& gripper) const;
+ (const GripperPtr_t& gripper, std::string name) const;
/// Create constraint that acts on the non-constrained axis of the
/// constraint generated by Handle::createGrasp.
@@ -59,7 +59,7 @@ namespace hpp {
/// \return a relative orientation constraint between the handle and
/// the gripper. Only the rotation around the x-axis is constrained.
virtual NumericalConstraintPtr_t createGraspComplement
- (const GripperPtr_t& gripper) const;
+ (const GripperPtr_t& gripper, std::string name) const;
/// Create constraint corresponding to a pregrasping task.
/// \param gripper object containing the gripper information
@@ -69,7 +69,7 @@ namespace hpp {
/// \todo this function is never called. It should follow changes of
/// Handle::createPreGrasp prototype.
virtual NumericalConstraintPtr_t createPreGrasp
- (const GripperPtr_t& gripper, const value_type& shift) const;
+ (const GripperPtr_t& gripper, const value_type& shift, std::string name) const;
/// Create constraint that acts on the non-constrained axis of the
/// constraint generated by Handle::createPreGrasp.
@@ -81,7 +81,7 @@ namespace hpp {
/// \note The translation along x-axis and the rotation around z-axis are constrained.
virtual NumericalConstraintPtr_t createPreGraspComplement
(const GripperPtr_t& gripper, const value_type& shift,
- const value_type& width) const;
+ const value_type& width, std::string name) const;
virtual std::ostream& print (std::ostream& os) const;
protected:
diff --git a/include/hpp/manipulation/handle.hh b/include/hpp/manipulation/handle.hh
index 7575518c7f6b5cd0fb0a749db86f442c506b27f7..fdf75c6db5bc063864864160fa0b1817101f31ac 100644
--- a/include/hpp/manipulation/handle.hh
+++ b/include/hpp/manipulation/handle.hh
@@ -98,14 +98,14 @@ namespace hpp {
/// the gripper.
/// \note The 6 DOFs of the relative transformation are constrained.
virtual NumericalConstraintPtr_t createGrasp
- (const GripperPtr_t& gripper) const;
+ (const GripperPtr_t& gripper, std::string name) const;
/// Create constraint that acts on the non-constrained axis of the
/// constraint generated by Handle::createGrasp.
/// \param gripper object containing the gripper information
/// \return a constraints that is not doing anything.
virtual NumericalConstraintPtr_t createGraspComplement
- (const GripperPtr_t& gripper) const;
+ (const GripperPtr_t& gripper, std::string name) const;
/// Create constraint corresponding to a pregrasping task.
/// \param gripper object containing the gripper information
@@ -115,12 +115,12 @@ namespace hpp {
/// are constrained. The transformation is shifted along x-axis of
/// value shift.
virtual NumericalConstraintPtr_t createPreGrasp
- (const GripperPtr_t& gripper, const value_type& shift) const;
+ (const GripperPtr_t& gripper, const value_type& shift, std::string name) const;
static NumericalConstraintPtr_t createGrasp
- (const GripperPtr_t& gripper,const HandlePtr_t& handle)
+ (const GripperPtr_t& gripper,const HandlePtr_t& handle, std::string name)
{
- return handle->createGrasp(gripper);
+ return handle->createGrasp(gripper, name);
}
/// Get the clearance
diff --git a/src/axial-handle.cc b/src/axial-handle.cc
index 7a263cb2da49c87e0d903ff57ceaa572fbecce8a..0cf828f3a8bbe33395b2e1ce5decce33bf46c3d0 100644
--- a/src/axial-handle.cc
+++ b/src/axial-handle.cc
@@ -33,14 +33,15 @@ namespace hpp {
static const matrix3_t I3 = matrix3_t::Identity();
NumericalConstraintPtr_t AxialHandle::createGrasp
- (const GripperPtr_t& gripper) const
+ (const GripperPtr_t& gripper, std::string n) const
{
using boost::assign::list_of;
std::vector <bool> mask = list_of (true)(true)(true)(true)(true)(false);
+ if (n.empty())
+ n = "Transformation_(1,1,1,1,1,0)_" + name() + "_" + gripper->name();
return NumericalConstraintPtr_t
(NumericalConstraint::create (RelativeTransformation::create
- ("Transformation_(1,1,1,1,1,0)_" + name ()
- + "_" + gripper->name (),
+ (n,
gripper->joint()->robot(),
gripper->joint (), joint (),
gripper->objectPositionInJoint (),
@@ -48,15 +49,16 @@ namespace hpp {
}
NumericalConstraintPtr_t AxialHandle::createGraspComplement
- (const GripperPtr_t& gripper) const
+ (const GripperPtr_t& gripper, std::string n) const
{
using boost::assign::list_of;
std::vector <bool> mask = list_of (false)(false)(false)(false)(false)
(true);
+ if (n.empty())
+ n = "Transformation_(0,0,0,0,0,1)_" + name() + "_" + gripper->name();
return NumericalConstraintPtr_t
(NumericalConstraint::create (RelativeTransformation::create
- ("Transformation_(0,0,0,0,0,1)_" + name ()
- + "_" + gripper->name (),
+ (n,
gripper->joint()->robot(),
gripper->joint (), joint (),
gripper->objectPositionInJoint (),
@@ -65,16 +67,17 @@ namespace hpp {
}
NumericalConstraintPtr_t AxialHandle::createPreGrasp
- (const GripperPtr_t& gripper, const value_type& shift) const
+ (const GripperPtr_t& gripper, const value_type& shift, std::string n) const
{
using boost::assign::list_of;
std::vector <bool> mask = list_of (true)(true)(true)(true)(true)(false);
Transform3f transform = gripper->objectPositionInJoint ()
* Transform3f (I3, vector3_t (shift,0,0));
+ if (n.empty())
+ n = "Transformation_(1,1,1,1,1,0)_" + name () + "_" + gripper->name ();
return NumericalConstraintPtr_t
(NumericalConstraint::create (RelativeTransformation::create
- ("Transformation_(1,1,1,1,1,0)_" + name ()
- + "_" + gripper->name (),
+ (n,
gripper->joint()->robot(),
gripper->joint (), joint (),
transform,
@@ -83,7 +86,7 @@ namespace hpp {
NumericalConstraintPtr_t AxialHandle::createPreGraspComplement
(const GripperPtr_t& gripper, const value_type& shift,
- const value_type& width) const
+ const value_type& width, std::string n) const
{
using boost::assign::list_of;
using core::DoubleInequality;
@@ -91,10 +94,11 @@ namespace hpp {
(false);
Transform3f transform = gripper->objectPositionInJoint ()
* Transform3f (I3, vector3_t (shift,0,0));
+ if (n.empty())
+ n = "Transformation_(1,0,0,0,0,0)_" + name() + "_" + gripper->name();
return NumericalConstraintPtr_t
(NumericalConstraint::create (RelativeTransformation::create
- ("Transformation_(1,0,0,0,0,0)_" + name ()
- + "_" + gripper->name (),
+ (n,
gripper->joint()->robot(),
gripper->joint (), joint (),
transform, localPosition(), mask),
diff --git a/src/graph/helper.cc b/src/graph/helper.cc
index 93e546d52c807321d3167edb01d3ea7ab78e3afc..62c52a72026630d08d6704e00cffa250d5dbba49 100644
--- a/src/graph/helper.cc
+++ b/src/graph/helper.cc
@@ -444,20 +444,20 @@ namespace hpp {
const GripperPtr_t& gripper, const HandlePtr_t& handle,
FoliatedManifold& grasp, FoliatedManifold& pregrasp)
{
- NumericalConstraintPtr_t gc = handle->createGrasp (gripper);
+ NumericalConstraintPtr_t gc = handle->createGrasp (gripper, "");
grasp.nc.nc.push_back (gc);
grasp.nc.pdof.push_back (SizeIntervals_t ());
grasp.nc_path.nc.push_back (gc);
// TODO: see function declaration
grasp.nc_path.pdof.push_back (SizeIntervals_t ());
- NumericalConstraintPtr_t gcc = handle->createGraspComplement (gripper);
+ NumericalConstraintPtr_t gcc = handle->createGraspComplement (gripper, "");
if (gcc->function ().outputSize () > 0) {
grasp.nc_fol.nc.push_back (gcc);
grasp.nc_fol.pdof.push_back (SizeIntervals_t());
}
const value_type c = handle->clearance () + gripper->clearance ();
- NumericalConstraintPtr_t pgc = handle->createPreGrasp (gripper, c);
+ NumericalConstraintPtr_t pgc = handle->createPreGrasp (gripper, c, "");
pregrasp.nc.nc.push_back (pgc);
pregrasp.nc.pdof.push_back (SizeIntervals_t());
pregrasp.nc_path.nc.push_back (pgc);
diff --git a/src/handle.cc b/src/handle.cc
index 1ef224ab3d5db2271f0fe64be724800330bb3863..2df138874f0d2f5ededbfcd118016d109c5e64f7 100644
--- a/src/handle.cc
+++ b/src/handle.cc
@@ -106,20 +106,22 @@ namespace hpp {
}
NumericalConstraintPtr_t Handle::createGrasp
- (const GripperPtr_t& gripper) const
+ (const GripperPtr_t& gripper, std::string n) const
{
using core::ExplicitRelativeTransformation;
// If handle is on a freeflying object, create an explicit constraint
if (is6Dmask(mask_) && isHandleOnR3SO3 (*this)) {
+ if (n.empty())
+ n = "Explicit_relative_transform_" + name() + "_" + gripper->name();
return ExplicitRelativeTransformation::create
- ("Explicit_relative_transform_" + name () + "_" + gripper->name (),
- gripper->joint ()->robot (), gripper->joint (), joint (),
+ (n, gripper->joint ()->robot (), gripper->joint (), joint (),
gripper->objectPositionInJoint (), localPosition())->createNumericalConstraint();
}
+ if (n.empty())
+ n = "Grasp_" + maskToStr(mask_) + "_" + name() + "_" + gripper->name();
return NumericalConstraintPtr_t
(NumericalConstraint::create (RelativeTransformation::create
- ("Grasp_" + maskToStr(mask_) + "_" + name ()
- + "_" + gripper->name (),
+ (n,
gripper->joint()->robot(),
gripper->joint (), joint (),
gripper->objectPositionInJoint (),
@@ -127,22 +129,24 @@ namespace hpp {
}
NumericalConstraintPtr_t Handle::createGraspComplement
- (const GripperPtr_t& gripper) const
+ (const GripperPtr_t& gripper, std::string n) const
{
core::DevicePtr_t robot = gripper->joint()->robot();
if (is6Dmask(mask_)) {
+ if (n.empty())
+ n = "GraspComp_(0,0,0,0,0,0)_" + name () + "_" + gripper->name ();
return NumericalConstraint::create (
boost::shared_ptr <ZeroDiffFunc> (new ZeroDiffFunc (
- robot->configSize(), robot->numberDof (),
- "GraspComp_(0,0,0,0,0,0)_" + name () + "_" + gripper->name ()
- ))
+ robot->configSize(), robot->numberDof (), n))
);
} else {
// TODO handle cases where rotations or translation are allowed.
std::vector<bool> Cmask = complementMask(mask_);
+ if (n.empty())
+ n = "Transformation_" + maskToStr(Cmask) + "_" + name ()
+ + "_" + gripper->name ();
RelativeTransformationPtr_t function = RelativeTransformation::create
- ("Transformation_" + maskToStr(Cmask) + "_" + name ()
- + "_" + gripper->name (),
+ (n,
gripper->joint()->robot(),
gripper->joint (), joint (),
gripper->objectPositionInJoint (),
@@ -156,15 +160,17 @@ namespace hpp {
}
NumericalConstraintPtr_t Handle::createPreGrasp
- (const GripperPtr_t& gripper, const value_type& shift) const
+ (const GripperPtr_t& gripper, const value_type& shift, std::string n) const
{
using boost::assign::list_of;
Transform3f transform = gripper->objectPositionInJoint ()
* Transform3f (I3, vector3_t (shift,0,0));
+ if (n.empty())
+ n = "Pregrasp_ " + maskToStr(mask_) + "_" + name ()
+ + "_" + gripper->name ();
return NumericalConstraintPtr_t
(NumericalConstraint::create (RelativeTransformation::create
- ("Pregrasp_ " + maskToStr(mask_) + "_" + name ()
- + "_" + gripper->name (),
+ (n,
gripper->joint()->robot(),
gripper->joint (), joint (),
transform, localPosition(), mask_)));
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index d61ee2043219c2d7468c81e8bd002ceedbacb2d5..b93edd8536da71516b7b7f182d541118f3f8c174 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -296,10 +296,11 @@ namespace hpp {
if (!g) throw std::runtime_error ("No gripper with name " + gripper + ".");
HandlePtr_t h = robot_->get <HandlePtr_t> (handle);
if (!h) throw std::runtime_error ("No handle with name " + handle + ".");
- NumericalConstraintPtr_t constraint (h->createGrasp (g));
- NumericalConstraintPtr_t complement (h->createGraspComplement (g));
- addNumericalConstraint (name, constraint);
- addNumericalConstraint (name + "/complement", complement);
+ const std::string cname = name + "/complement";
+ NumericalConstraintPtr_t constraint (h->createGrasp (g, name));
+ NumericalConstraintPtr_t complement (h->createGraspComplement (g, cname));
+ addNumericalConstraint ( name, constraint);
+ addNumericalConstraint (cname, complement);
}
void ProblemSolver::createPreGraspConstraint
@@ -312,7 +313,7 @@ namespace hpp {
if (!h) throw std::runtime_error ("No handle with name " + handle + ".");
value_type c = h->clearance () + g->clearance ();
- NumericalConstraintPtr_t constraint = h->createPreGrasp (g, c);
+ NumericalConstraintPtr_t constraint = h->createPreGrasp (g, c, name);
addNumericalConstraint (name, constraint);
}