Various fixes

src/pyhpp/manipulation/constraint_graph_factory.py

@@ -708,20 +708,13 @@ def buildGrasp(self, g, h):
         if not graspAlreadyCreated:
             cname = n + "/complement"
             bname = n + "/hold"
-            gripper = self.graph.robot.grippers()[g]
-            handle = self.graph.robot.handles()[h]
-            constraint = handle.createGrasp(gripper, n)
-            complement = handle.createGraspComplement(gripper, cname)
-            both = handle.createGraspAndComplement(gripper, bname)
-            self.registerConstraint(constraint, n)
-            self.registerConstraint(complement, cname)
-            self.registerConstraint(both, bname)
+            constraints = self.graph.createGraspConstraint(n, g, h)
+            self.registerConstraint(constraints[0], n)
+            self.registerConstraint(constraints[1], cname)
+            self.registerConstraint(constraints[2], bname)
         if not pregraspAlreadyCreated:
-            gripper = self.graph.robot.grippers()[g]
-            handle = self.graph.robot.handles()[h]
-            c = handle.clearance + gripper.clearance
-            pregrasp = handle.createPreGrasp(gripper, c, pn)
-            self.registerConstraint(pregrasp, pn)
+            constraint = self.graph.createPreGraspConstraint(pn, g, h)
+            self.registerConstraint(constraint, pn)
         return dict(
             list(
                 zip(
@@ -1070,10 +1063,9 @@ def _createWaypointState(name, constraints):
                 self.edge_objects[nf] = forward_trans
                 self.edge_objects[nb] = backward_trans
 
-                nf_ls = nf
-                nb_ls = nb
-
                 if crossedFoliation:
+                    nf_ls = nf
+                    nb_ls = nb
                     if i == 0:
                         edgeName = nf_ls = nf + "_ls"
                         containingState = (

src/pyhpp/manipulation/graph.cc

@@ -42,6 +42,9 @@
 #include <hpp/manipulation/graph/state.hh>
 #include <hpp/manipulation/steering-method/graph.hh>
 #include <pinocchio/spatial/se3.hpp>
+
+#include "hpp/manipulation/constraint-set.hh"
+
 namespace {
 
 const char* DOC_CREATESTATE =
@@ -662,7 +665,8 @@ void PyWGraph::registerConstraints(const ImplicitPtr_t& constraint,
                                    const ImplicitPtr_t& complement,
                                    const ImplicitPtr_t& both) {
   try {
-    obj->registerConstraints(constraint, complement, both);
+    constraintsAndComplements.push_back(
+        ConstraintAndComplement_t(constraint, complement, both));
   } catch (const std::exception& exc) {
     throw std::logic_error(exc.what());
   }
@@ -672,19 +676,19 @@ void PyWGraph::registerConstraints(const ImplicitPtr_t& constraint,
 // Configuration error checking
 // =============================================================================
 
-bool PyWGraph::getConfigErrorForState(PyWStatePtr_t component,
-                                      ConfigurationIn_t input,
-                                      hpp::core::vector_t& error) {
+boost::python::tuple PyWGraph::getConfigErrorForState(PyWStatePtr_t component,
+                                                      ConfigurationIn_t input) {
   try {
-    return obj->getConfigErrorForState(input, component->obj, error);
+    hpp::core::vector_t error;
+    bool res = obj->getConfigErrorForState(input, component->obj, error);
+    return boost::python::make_tuple(error, res);
   } catch (const std::exception& exc) {
     throw std::logic_error(exc.what());
   }
 }
 
-bool PyWGraph::getConfigErrorForTransition(PyWEdgePtr_t edge,
-                                           ConfigurationIn_t input,
-                                           hpp::core::vector_t& error) {
+boost::python::tuple PyWGraph::getConfigErrorForTransition(
+    PyWEdgePtr_t edge, ConfigurationIn_t input) {
   try {
     // Check if steering method is properly initialized
     if (!edge->obj->parentGraph()->problem()->manipulationSteeringMethod() ||
@@ -694,22 +698,30 @@ bool PyWGraph::getConfigErrorForTransition(PyWEdgePtr_t edge,
              ->innerSteeringMethod()) {
       throw std::logic_error("Could not initialize the steering method.");
     }
-    return obj->getConfigErrorForEdge(input, edge->obj, error);
+    hpp::core::vector_t error;
+    bool res = obj->getConfigErrorForEdge(input, edge->obj, error);
+    return boost::python::make_tuple(error, res);
   } catch (const std::exception& exc) {
     throw std::logic_error(exc.what());
   }
 }
 
-bool PyWGraph::getConfigErrorForTransitionLeaf(
-    ConfigurationIn_t leafConfig, ConfigurationIn_t config,
-    const PyWEdgePtr_t& edge, hpp::core::vector_t& error) const {
-  return obj->getConfigErrorForEdgeLeaf(leafConfig, config, edge->obj, error);
+boost::python::tuple PyWGraph::getConfigErrorForTransitionLeaf(
+    const PyWEdgePtr_t& edge, ConfigurationIn_t leafConfig,
+    ConfigurationIn_t config) const {
+  hpp::core::vector_t error;
+  bool res =
+      obj->getConfigErrorForEdgeLeaf(leafConfig, config, edge->obj, error);
+  return boost::python::make_tuple(error, res);
 }
 
-bool PyWGraph::getConfigErrorForTransitionTarget(
-    ConfigurationIn_t leafConfig, ConfigurationIn_t config,
-    const PyWEdgePtr_t& edge, hpp::core::vector_t& error) const {
-  return obj->getConfigErrorForEdgeTarget(leafConfig, config, edge->obj, error);
+boost::python::tuple PyWGraph::getConfigErrorForTransitionTarget(
+    const PyWEdgePtr_t& edge, ConfigurationIn_t leafConfig,
+    ConfigurationIn_t config) const {
+  hpp::core::vector_t error;
+  bool res =
+      obj->getConfigErrorForEdgeTarget(leafConfig, config, edge->obj, error);
+  return boost::python::make_tuple(error, res);
 }
 
 // =============================================================================
@@ -975,6 +987,11 @@ void PyWGraph::display(const char* filename) {
 
 void PyWGraph::initialize() {
   try {
+    obj->clearConstraintsAndComplement();
+    for (std::size_t i = 0; i < constraintsAndComplements.size(); ++i) {
+      const ConstraintAndComplement_t& c = constraintsAndComplements[i];
+      obj->registerConstraints(c.constraint, c.complement, c.both);
+    }
     obj->initialize();
   } catch (const std::exception& exc) {
     throw std::logic_error(exc.what());
@@ -1041,6 +1058,10 @@ boost::python::tuple PyWGraph::createPlacementConstraint(
               std::get<0>(constraints)->functionPtr()));
       contactFunction->setNormalMargin(margin);
 
+      constraintsAndComplements.push_back(ConstraintAndComplement_t(
+          std::get<0>(constraints), std::get<1>(constraints),
+          std::get<2>(constraints)));
+
       return boost::python::make_tuple(std::get<0>(constraints),
                                        std::get<1>(constraints),
                                        std::get<2>(constraints));
@@ -1139,6 +1160,43 @@ ImplicitPtr_t PyWGraph::createPrePlacementConstraint2(
   return createPrePlacementConstraint(name, py_surface1, py_surface2, width);
 }
 
+boost::python::list PyWGraph::createGraspConstraint(const std::string& name,
+                                                    const std::string& gripper,
+                                                    const std::string& handle) {
+  boost::python::list result;
+
+  GripperPtr_t g = robot->obj->grippers.get(gripper, GripperPtr_t());
+  if (!g) throw std::runtime_error("No gripper with name " + gripper + ".");
+  HandlePtr_t h = robot->obj->handles.get(handle, HandlePtr_t());
+  if (!h) throw std::runtime_error("No handle with name " + handle + ".");
+  const std::string cname = name + "/complement";
+  const std::string bname = name + "/hold";
+  ImplicitPtr_t constraint(h->createGrasp(g, name));
+  ImplicitPtr_t complement(h->createGraspComplement(g, cname));
+  ImplicitPtr_t both(h->createGraspAndComplement(g, bname));
+
+  result.append(constraint);
+  result.append(complement);
+  result.append(both);
+
+  constraintsAndComplements.push_back(
+      ConstraintAndComplement_t(constraint, complement, both));
+
+  return result;
+}
+
+ImplicitPtr_t PyWGraph::createPreGraspConstraint(const std::string& name,
+                                                 const std::string& gripper,
+                                                 const std::string& handle) {
+  GripperPtr_t g = robot->obj->grippers.get(gripper, GripperPtr_t());
+  if (!g) throw std::runtime_error("No gripper with name " + gripper + ".");
+  HandlePtr_t h = robot->obj->handles.get(handle, HandlePtr_t());
+  if (!h) throw std::runtime_error("No handle with name " + handle + ".");
+
+  value_type c = h->clearance() + g->clearance();
+  ImplicitPtr_t constraint = h->createPreGrasp(g, c, name);
+  return constraint;
+}
 // =============================================================================
 // Boost.Python bindings
 // =============================================================================
@@ -1222,7 +1280,8 @@ void exposeGraph() {
       .def("createPrePlacementConstraint",
            &PyWGraph::createPrePlacementConstraint2,
            DOC_CREATEPREPLACEMENTCONSTRAINT)
-
+      .def("createGraspConstraint", &PyWGraph::createGraspConstraint)
+      .def("createPreGraspConstraint", &PyWGraph::createPreGraspConstraint)
       // Configuration error checking
       .PYHPP_DEFINE_METHOD1(PyWGraph, getConfigErrorForState,
                             DOC_GETCONFIGERRORFORSTATE)

src/pyhpp/manipulation/graph.hh

@@ -48,6 +48,9 @@ typedef hpp::manipulation::graph::EdgePtr_t EdgePtr_t;
 typedef hpp::manipulation::graph::State State;
 typedef hpp::manipulation::graph::StatePtr_t StatePtr_t;
 typedef hpp::manipulation::graph::GraphPtr_t GraphPtr_t;
+typedef hpp::manipulation::ConstraintsAndComplements_t
+    ConstraintsAndComplements_t;
+typedef hpp::manipulation::ConstraintAndComplement_t ConstraintAndComplement_t;
 
 /// Result structure for constraint operations
 struct ConstraintResult {
@@ -88,6 +91,8 @@ struct PyWGraph {
   PyWDevicePtr_t robot;
   PyWProblemPtr_t problem;
 
+  ConstraintsAndComplements_t constraintsAndComplements;
+
   // Constructors
   PyWGraph(const GraphPtr_t& object);
   PyWGraph(const std::string& name, const PyWDevicePtr_t& d,
@@ -174,23 +179,27 @@ struct PyWGraph {
       const std::string& name, const boost::python::list& py_surface1,
       const boost::python::list& py_surface2, const value_type& width);
 
+  boost::python::list createGraspConstraint(const std::string& name,
+                                            const std::string& gripper,
+                                            const std::string& handle);
+  ImplicitPtr_t createPreGraspConstraint(const std::string& name,
+                                         const std::string& gripper,
+                                         const std::string& handle);
   boost::python::list getNumericalConstraintsForState(PyWStatePtr_t component);
   boost::python::list getNumericalConstraintsForEdge(PyWEdgePtr_t component);
   boost::python::list getNumericalConstraintsForGraph();
 
   // Configuration error checking
-  bool getConfigErrorForState(PyWStatePtr_t component, ConfigurationIn_t input,
-                              hpp::core::vector_t& error);
-  bool getConfigErrorForTransition(PyWEdgePtr_t edge, ConfigurationIn_t input,
-                                   hpp::core::vector_t& error);
-  bool getConfigErrorForTransitionLeaf(ConfigurationIn_t leafConfig,
-                                       ConfigurationIn_t config,
-                                       const PyWEdgePtr_t& edge,
-                                       hpp::core::vector_t& error) const;
-  bool getConfigErrorForTransitionTarget(ConfigurationIn_t leafConfig,
-                                         ConfigurationIn_t config,
-                                         const PyWEdgePtr_t& edge,
-                                         hpp::core::vector_t& error) const;
+  boost::python::tuple getConfigErrorForState(PyWStatePtr_t component,
+                                              ConfigurationIn_t input);
+  boost::python::tuple getConfigErrorForTransition(PyWEdgePtr_t edge,
+                                                   ConfigurationIn_t input);
+  boost::python::tuple getConfigErrorForTransitionLeaf(
+      const PyWEdgePtr_t& edge, ConfigurationIn_t leafConfig,
+      ConfigurationIn_t config) const;
+  boost::python::tuple getConfigErrorForTransitionTarget(
+      const PyWEdgePtr_t& edge, ConfigurationIn_t leafConfig,
+      ConfigurationIn_t config) const;
 
   // Constraint application
   ConstraintResult applyStateConstraints(PyWStatePtr_t state,