From 4e6c453fe7422a430fd3adb94572bf2e246eb60e Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 27 Jan 2016 13:57:52 +0100
Subject: [PATCH] Add ProblemSolver::createPrePlacementConstraint
---
include/hpp/manipulation/problem-solver.hh | 15 ++++++++-
src/problem-solver.cc | 39 ++++++++++++++++++++++
2 files changed, 53 insertions(+), 1 deletion(-)
diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh
index 91c0fcd1..98d3669e 100644
--- a/include/hpp/manipulation/problem-solver.hh
+++ b/include/hpp/manipulation/problem-solver.hh
@@ -96,13 +96,26 @@ namespace hpp {
/// \param name name of the placement constraint,
/// \param triangleName name of the first list of triangles,
/// \param envContactName name of the second list of triangles.
- /// \param margin see hpp::constraints::QPStaticStability constructor
+ /// \param margin see hpp::constraints::ConvexShapeContact::setNormalMargin
///
void createPlacementConstraint (const std::string& name,
const std::string& surface1,
const std::string& surface2,
const value_type& margin = 1e-4);
+ /// Create pre-placement constraint
+ /// \param name name of the placement constraint,
+ /// \param triangleName name of the first list of triangles,
+ /// \param envContactName name of the second list of triangles.
+ /// \param width approaching distance.
+ /// \param margin see hpp::constraints::ConvexShapeContact::setNormalMargin
+ ///
+ void createPrePlacementConstraint (const std::string& name,
+ const std::string& surface1,
+ const std::string& surface2,
+ const value_type& width,
+ const value_type& margin = 1e-4);
+
/// Reset constraint set and put back the disable collisions
/// between gripper and handle
virtual void resetConstraints ();
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 6700497b..53c439cb 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -32,6 +32,7 @@
#include <hpp/core/path-optimization/partial-shortcut.hh>
#include <hpp/core/roadmap.hh>
#include <hpp/core/steering-method-straight.hh>
+#include <hpp/core/comparison-type.hh>
#include "hpp/manipulation/problem.hh"
#include "hpp/manipulation/device.hh"
@@ -189,6 +190,44 @@ namespace hpp {
(constraints.second, core::Equality::create ()));
}
+ void ProblemSolver::createPrePlacementConstraint
+ (const std::string& name, const std::string& surface1,
+ const std::string& surface2, const value_type& width,
+ const value_type& margin)
+ {
+ if (!robot_) throw std::runtime_error ("No robot loaded");
+ using constraints::ConvexShape;
+ using constraints::ConvexShapeContact;
+ using constraints::ConvexShapeContactPtr_t;
+
+ ConvexShapeContactPtr_t cvxShape = ConvexShapeContact::create (name, robot_);
+
+ if (!robot_->has <JointAndShapes_t> (surface1))
+ throw std::runtime_error ("First list of triangles not found.");
+ JointAndShapes_t l = robot_->get <JointAndShapes_t> (surface1);
+
+ for (JointAndShapes_t::const_iterator it = l.begin ();
+ it != l.end(); ++it) {
+ cvxShape->addObject (ConvexShape (it->second, it->first));
+ }
+
+ // Search first robot triangles
+ if (robot_->has <JointAndShapes_t> (surface2))
+ l = robot_->get <JointAndShapes_t> (surface2);
+ // and then environment triangles.
+ else if (has <JointAndShapes_t> (surface2))
+ l = get <JointAndShapes_t> (surface2);
+ else throw std::runtime_error ("Second list of triangles not found.");
+
+ for (JointAndShapes_t::const_iterator it = l.begin ();
+ it != l.end(); ++it) {
+ cvxShape->addFloor (ConvexShape (it->second, it->first));
+ }
+
+ cvxShape->setNormalMargin (margin + width);
+
+ addNumericalConstraint (name, NumericalConstraint::create (cvxShape));
+ }
void ProblemSolver::resetConstraints ()
{
--
GitLab