From 4bca67dd8ec1b57d874ad9682c0d8989097eea72 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Mon, 28 Sep 2015 18:13:32 +0200
Subject: [PATCH] Add a method in ProblemSolver to create a placement
constraint
- also create complement constraint.
---
include/hpp/manipulation/problem-solver.hh | 9 ++---
src/problem-solver.cc | 41 ++++++++++++++++++++++
2 files changed, 46 insertions(+), 4 deletions(-)
diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh
index 042da6d4..98383ef9 100644
--- a/include/hpp/manipulation/problem-solver.hh
+++ b/include/hpp/manipulation/problem-solver.hh
@@ -94,12 +94,13 @@ namespace hpp {
GraspPtr_t grasp(const DifferentiableFunctionPtr_t& constraint) const;
/// Create placement constraint
- /// \param name name of the first 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.
///
void createPlacementConstraint (const std::string& name,
- const std::string& jointName,
- const std::string& triangleName,
- const std::string& envContactName);
+ const std::string& surface1,
+ const std::string& surface2);
/// Reset constraint set and put back the disable collisions
/// between gripper and handle
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index b15aedd0..85b1151f 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -22,6 +22,8 @@
#include <hpp/model/gripper.hh>
+#include <hpp/constraints/static-stability.hh>
+
#include <hpp/core/roadmap.hh>
#include "hpp/manipulation/device.hh"
@@ -82,6 +84,45 @@ namespace hpp {
return it->second;
}
+ void ProblemSolver::createPlacementConstraint
+ (const std::string& name, const std::string& surface1,
+ const std::string& surface2)
+ {
+ if (!robot_) throw std::runtime_error ("No robot loaded");
+ using constraints::StaticStabilityGravityPtr_t;
+ using constraints::StaticStabilityGravityComplement;
+ using constraints::StaticStabilityGravityComplementPtr_t;
+ std::string complementName (name + "/complement");
+ std::pair < StaticStabilityGravityPtr_t,
+ StaticStabilityGravityComplementPtr_t > constraints
+ (StaticStabilityGravityComplement::createPair
+ (name, complementName, robot_));
+ JointAndTriangles_t l = robot_->get <JointAndTriangles_t> (surface1);
+ if (l.empty ()) throw std::runtime_error
+ ("First list of triangles not found.");
+ for (JointAndTriangles_t::const_iterator it = l.begin ();
+ it != l.end(); ++it) {
+ constraints.first->addObjectTriangle (it->second, it->first);
+ }
+ // Search first robot triangles
+ l = robot_->get <JointAndTriangles_t> (surface2);
+ if (l.empty ()) {
+ // and then environment triangles.
+ l = get <JointAndTriangles_t> (surface2);
+ if (l.empty ()) throw std::runtime_error
+ ("Second list of triangles not found.");
+ }
+ for (JointAndTriangles_t::const_iterator it = l.begin ();
+ it != l.end(); ++it) {
+ constraints.first->addFloorTriangle (it->second, it->first);
+ }
+
+ addNumericalConstraint (name, constraints.first);
+ addNumericalConstraint (complementName, constraints.second);
+ comparisonType (complementName, core::Equality::create ());
+ }
+
+
void ProblemSolver::resetConstraints ()
{
if (robot_)
--
GitLab