Bind and document some more methods

include/pyhpp/constraints/fwd.hh

@@ -36,6 +36,7 @@ namespace pyhpp {
 namespace constraints {
 void exposeDifferentiableFunction();
 void exposeGenericTransformations();
+void exposeManipulability();
 void exposeImplicit();
 void exposeExplicitConstraintSet();
 void exposeExplicit();

src/pyhpp/constraints/bindings.cc

@@ -42,6 +42,7 @@ BOOST_PYTHON_MODULE(bindings) {
       "segment");
 
   pyhpp::constraints::exposeDifferentiableFunction();
+  pyhpp::constraints::exposeManipulability();
   pyhpp::constraints::exposeGenericTransformations();
   pyhpp::constraints::exposeExplicit();
   pyhpp::constraints::exposeImplicit();

src/pyhpp/constraints/by-substitution.cc

@@ -65,11 +65,22 @@ void exposeBySubstitution() {
            static_cast<ExplicitConstraintSet& (BySubstitution::*)()>(
                &BySubstitution::explicitConstraintSet),
            return_internal_reference<>(), DocClassMethod(explicitConstraintSet))
-      .add_property("errorThreshold",
-                    static_cast<value_type (BySubstitution::*)() const>(
-                        &BySubstitution::errorThreshold),
-                    static_cast<void (BySubstitution::*)(const value_type&)>(
-                        &BySubstitution::errorThreshold));
+      .def("rightHandSideFromConfig",
+           static_cast<vector_t (BySubstitution::*)(ConfigurationIn_t)>(
+               &BySubstitution::rightHandSideFromConfig))
+      .def("rightHandSideFromConfig",
+           static_cast<bool (BySubstitution::*)(const ImplicitPtr_t&,
+                                                ConfigurationIn_t)>(
+               &BySubstitution::rightHandSideFromConfig))
+      .def("rightHandSide", static_cast<bool (HierarchicalIterative::*)(
+                                const ImplicitPtr_t&, vectorIn_t)>(
+                                &HierarchicalIterative::rightHandSide))
+      .def("rightHandSide",
+           static_cast<void (HierarchicalIterative::*)(vectorIn_t)>(
+               &HierarchicalIterative::rightHandSide))
+      .def("rightHandSide",
+           static_cast<vector_t (HierarchicalIterative::*)() const>(
+               &HierarchicalIterative::rightHandSide));
 }
 }  // namespace constraints
 }  // namespace pyhpp

src/pyhpp/constraints/differentiable-function.cc

@@ -31,6 +31,7 @@
 #include <boost/python/suite/indexing/vector_indexing_suite.hpp>
 #include <eigenpy/eigenpy.hpp>
 #include <hpp/constraints/differentiable-function.hh>
+#include <hpp/constraints/manipulability.hh>
 #include <pyhpp/constraints/fwd.hh>
 #include <pyhpp/util.hh>
 
@@ -135,5 +136,11 @@ void exposeDifferentiableFunction() {
       .def("impl_compute", pure_virtual(&DFWrapper::impl_compute))
       .def("impl_jacobian", pure_virtual(&DFWrapper::impl_jacobian));
 }
+void exposeManipulability() {
+  class_<Manipulability, ManipulabilityPtr_t, bases<DifferentiableFunction>,
+         boost::noncopyable>("Manipulability", no_init)
+      .def("__init__", make_constructor(&Manipulability::create));
+}
+
 }  // namespace constraints
 }  // namespace pyhpp

src/pyhpp/constraints/generic-transformation.cc

@@ -57,7 +57,16 @@ void exposeAbsoluteGenericTransformation(const char* name) {
   class_<GT_t, bases<DifferentiableFunction>, typename GT_t::Ptr_t,
          boost::noncopyable>(name, no_init)
       .def("__init__",
-           make_constructor(&AbsoluteGenericTransformation_create<GT_t>));
+           make_constructor(&AbsoluteGenericTransformation_create<GT_t>),
+           "name: name of the constraint,\n"
+           "robot: device the constraint applies to,\n"
+           "j2: index of joint that holds frame 2,\n"
+           "frame2 (SE3): pose of frame 2 in joint 2,\n"
+           "frame1 (SE3): pose of frame 1 in world frame,\n"
+           "mask: tuple of Boolean.\n\n"
+           "Create a absolute (Position, Orientation, Transformation) "
+           "constraint of a frame "
+           "(frame 2) in a fixed frame (frame 1).");
 }
 
 template <typename GT_t>
@@ -77,7 +86,17 @@ void exposeRelativeGenericTransformation(const char* name) {
   class_<GT_t, bases<DifferentiableFunction>, typename GT_t::Ptr_t,
          boost::noncopyable>(name, no_init)
       .def("__init__",
-           make_constructor(&RelativeGenericTransformation_create<GT_t>));
+           make_constructor(&RelativeGenericTransformation_create<GT_t>),
+           "name: name of the constraint,\n"
+           "robot: device the constraint applies to,\n"
+           "j1: index of joint that holds frame 1,\n"
+           "j2: index of joint that holds frame 2,\n"
+           "frame1 (SE3): pose of frame 1 in joint 1,\n"
+           "frame2 (SE3): pose of frame 2 in joint 2,\n"
+           "mask: tuple of Boolean.\n\n"
+           "Create a relative (Position, Orientation, Transformation) "
+           "constraint between "
+           "two frames.");
 }
 
 void exposeGenericTransformations() {

src/pyhpp/constraints/iterative-solver.cc

@@ -62,11 +62,43 @@ void exposeHierarchicalIterativeSolver() {
               &HierarchicalIterative::errorThreshold),
           static_cast<void (HierarchicalIterative::*)(const value_type&)>(
               &HierarchicalIterative::errorThreshold))
+      .def("rightHandSideFromConfig",
+           static_cast<vector_t (HierarchicalIterative::*)(ConfigurationIn_t)>(
+               &HierarchicalIterative::rightHandSideFromConfig))
+      .def("rightHandSideFromConfig",
+           static_cast<bool (HierarchicalIterative::*)(const ImplicitPtr_t&,
+                                                       ConfigurationIn_t)>(
+               &HierarchicalIterative::rightHandSideFromConfig))
+      .def("rightHandSide", static_cast<bool (HierarchicalIterative::*)(
+                                const ImplicitPtr_t&, vectorIn_t)>(
+                                &HierarchicalIterative::rightHandSide))
+      .def("rightHandSide",
+           static_cast<void (HierarchicalIterative::*)(vectorIn_t)>(
+               &HierarchicalIterative::rightHandSide))
+      .def("rightHandSide",
+           static_cast<vector_t (HierarchicalIterative::*)() const>(
+               &HierarchicalIterative::rightHandSide))
       .add_property("maxIterations",
                     static_cast<size_type (HierarchicalIterative::*)() const>(
                         &HierarchicalIterative::maxIterations),
                     static_cast<void (HierarchicalIterative::*)(size_type)>(
-                        &HierarchicalIterative::maxIterations));
+                        &HierarchicalIterative::maxIterations))
+      .add_property(
+          "errorThreshold",
+          static_cast<value_type (HierarchicalIterative::*)() const>(
+              &HierarchicalIterative::errorThreshold),
+          static_cast<void (HierarchicalIterative::*)(const value_type&)>(
+              &HierarchicalIterative::errorThreshold))
+      .add_property("lastIsOptional",
+                    static_cast<bool (HierarchicalIterative::*)() const>(
+                        &HierarchicalIterative::lastIsOptional),
+                    static_cast<void (HierarchicalIterative::*)(bool)>(
+                        &HierarchicalIterative::lastIsOptional))
+      .add_property("solveLevelByLevel",
+                    static_cast<bool (HierarchicalIterative::*)() const>(
+                        &HierarchicalIterative::solveLevelByLevel),
+                    static_cast<void (HierarchicalIterative::*)(bool)>(
+                        &HierarchicalIterative::solveLevelByLevel));
 }
 }  // namespace constraints
 }  // namespace pyhpp

src/pyhpp/core/constraint.cc

@@ -106,6 +106,11 @@ void exposeConstraint() {
       .def("configProjector", &ConstraintSet::configProjector,
            DocClassMethod(configProjector));
 
+  enum_<ConfigProjector::LineSearchType>("LineSearchType")
+      .value("Backtracking", ConfigProjector::Backtracking)
+      .value("ErrorNormBased", ConfigProjector::ErrorNormBased)
+      .value("FixedSequence", ConfigProjector::FixedSequence)
+      .value("Constant", ConfigProjector::Constant);
   // DocClass(ConfigProjector)
   class_<ConfigProjector, ConfigProjectorPtr_t, boost::noncopyable,
          bases<Constraint> >("ConfigProjector", no_init)
@@ -114,6 +119,13 @@ void exposeConstraint() {
            static_cast<BySubstitution& (ConfigProjector::*)()>(
                &ConfigProjector::solver),
            return_internal_reference<>(), DocClassMethod(solver))
+      .add_property(
+          "lineSearchType",
+          static_cast<ConfigProjector::LineSearchType (ConfigProjector::*)()
+                          const>(&ConfigProjector::lineSearchType),
+          static_cast<void (ConfigProjector::*)(
+              ConfigProjector::LineSearchType)>(
+              &ConfigProjector::lineSearchType))
       .def("add", &ConfigProjector::add, DocClassMethod(add))
       .def("lastIsOptional", static_cast<bool (ConfigProjector::*)() const>(
                                  &ConfigProjector::lastIsOptional))

src/pyhpp/tools/xacro.py

@@ -24,33 +24,17 @@ def process_xacro(*args):
         # open and process file
         doc = xacro.process_file(retrieve_resource(input_file_name, dirs), **vars(opts))
 
-    # error handling
-    except xml.parsers.expat.ExpatError as e:
-        xacro.error(f"XML parsing error: {unicode(e)}", alt_text=None)
-        if xacro.verbosity > 0:
-            xacro.print_location()
-            print(file=sys.stderr)  # add empty separator line before error
-            print("Check that:", file=sys.stderr)
-            print(" - Your XML is well-formed", file=sys.stderr)
-            print(
-                " - You have the xacro xmlns declaration:",
-                'xmlns:xacro="http://www.ros.org/wiki/xacro"',
-                file=sys.stderr,
-            )
-        sys.exit(2)  # indicate failure, but don't print stack trace on XML errors
-
     except Exception as e:
         msg = unicode(e)
         if not msg:
             msg = repr(e)
         xacro.error(msg)
         if xacro.verbosity > 0:
             xacro.print_location()
+            raise
         if xacro.verbosity > 1:
             print(file=sys.stderr)  # add empty separator line before error
             raise  # create stack trace
-        else:
-            sys.exit(2)  # gracefully exit with error condition
 
     # write output
     return doc.toprettyxml(indent="  ", **encoding)