diff --git a/include/pyhpp/constraints/fwd.hh b/include/pyhpp/constraints/fwd.hh index b2162c4..624ed09 100644 --- a/include/pyhpp/constraints/fwd.hh +++ b/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(); diff --git a/src/pyhpp/constraints/bindings.cc b/src/pyhpp/constraints/bindings.cc index dcc26db..ef924fe 100644 --- a/src/pyhpp/constraints/bindings.cc +++ b/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(); diff --git a/src/pyhpp/constraints/by-substitution.cc b/src/pyhpp/constraints/by-substitution.cc index 6b2e3c9..277b332 100644 --- a/src/pyhpp/constraints/by-substitution.cc +++ b/src/pyhpp/constraints/by-substitution.cc @@ -65,11 +65,22 @@ void exposeBySubstitution() { static_cast( &BySubstitution::explicitConstraintSet), return_internal_reference<>(), DocClassMethod(explicitConstraintSet)) - .add_property("errorThreshold", - static_cast( - &BySubstitution::errorThreshold), - static_cast( - &BySubstitution::errorThreshold)); + .def("rightHandSideFromConfig", + static_cast( + &BySubstitution::rightHandSideFromConfig)) + .def("rightHandSideFromConfig", + static_cast( + &BySubstitution::rightHandSideFromConfig)) + .def("rightHandSide", static_cast( + &HierarchicalIterative::rightHandSide)) + .def("rightHandSide", + static_cast( + &HierarchicalIterative::rightHandSide)) + .def("rightHandSide", + static_cast( + &HierarchicalIterative::rightHandSide)); } } // namespace constraints } // namespace pyhpp diff --git a/src/pyhpp/constraints/differentiable-function.cc b/src/pyhpp/constraints/differentiable-function.cc index f0ec356..ab07f89 100644 --- a/src/pyhpp/constraints/differentiable-function.cc +++ b/src/pyhpp/constraints/differentiable-function.cc @@ -31,6 +31,7 @@ #include #include #include +#include #include #include @@ -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_, + boost::noncopyable>("Manipulability", no_init) + .def("__init__", make_constructor(&Manipulability::create)); +} + } // namespace constraints } // namespace pyhpp diff --git a/src/pyhpp/constraints/generic-transformation.cc b/src/pyhpp/constraints/generic-transformation.cc index 425c0df..ddc9839 100644 --- a/src/pyhpp/constraints/generic-transformation.cc +++ b/src/pyhpp/constraints/generic-transformation.cc @@ -57,7 +57,16 @@ void exposeAbsoluteGenericTransformation(const char* name) { class_, typename GT_t::Ptr_t, boost::noncopyable>(name, no_init) .def("__init__", - make_constructor(&AbsoluteGenericTransformation_create)); + make_constructor(&AbsoluteGenericTransformation_create), + "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 @@ -77,7 +86,17 @@ void exposeRelativeGenericTransformation(const char* name) { class_, typename GT_t::Ptr_t, boost::noncopyable>(name, no_init) .def("__init__", - make_constructor(&RelativeGenericTransformation_create)); + make_constructor(&RelativeGenericTransformation_create), + "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() { diff --git a/src/pyhpp/constraints/iterative-solver.cc b/src/pyhpp/constraints/iterative-solver.cc index f9e01d4..d90e4af 100644 --- a/src/pyhpp/constraints/iterative-solver.cc +++ b/src/pyhpp/constraints/iterative-solver.cc @@ -62,11 +62,43 @@ void exposeHierarchicalIterativeSolver() { &HierarchicalIterative::errorThreshold), static_cast( &HierarchicalIterative::errorThreshold)) + .def("rightHandSideFromConfig", + static_cast( + &HierarchicalIterative::rightHandSideFromConfig)) + .def("rightHandSideFromConfig", + static_cast( + &HierarchicalIterative::rightHandSideFromConfig)) + .def("rightHandSide", static_cast( + &HierarchicalIterative::rightHandSide)) + .def("rightHandSide", + static_cast( + &HierarchicalIterative::rightHandSide)) + .def("rightHandSide", + static_cast( + &HierarchicalIterative::rightHandSide)) .add_property("maxIterations", static_cast( &HierarchicalIterative::maxIterations), static_cast( - &HierarchicalIterative::maxIterations)); + &HierarchicalIterative::maxIterations)) + .add_property( + "errorThreshold", + static_cast( + &HierarchicalIterative::errorThreshold), + static_cast( + &HierarchicalIterative::errorThreshold)) + .add_property("lastIsOptional", + static_cast( + &HierarchicalIterative::lastIsOptional), + static_cast( + &HierarchicalIterative::lastIsOptional)) + .add_property("solveLevelByLevel", + static_cast( + &HierarchicalIterative::solveLevelByLevel), + static_cast( + &HierarchicalIterative::solveLevelByLevel)); } } // namespace constraints } // namespace pyhpp diff --git a/src/pyhpp/core/constraint.cc b/src/pyhpp/core/constraint.cc index 4e76df8..187fee0 100644 --- a/src/pyhpp/core/constraint.cc +++ b/src/pyhpp/core/constraint.cc @@ -106,6 +106,11 @@ void exposeConstraint() { .def("configProjector", &ConstraintSet::configProjector, DocClassMethod(configProjector)); + enum_("LineSearchType") + .value("Backtracking", ConfigProjector::Backtracking) + .value("ErrorNormBased", ConfigProjector::ErrorNormBased) + .value("FixedSequence", ConfigProjector::FixedSequence) + .value("Constant", ConfigProjector::Constant); // DocClass(ConfigProjector) class_ >("ConfigProjector", no_init) @@ -114,6 +119,13 @@ void exposeConstraint() { static_cast( &ConfigProjector::solver), return_internal_reference<>(), DocClassMethod(solver)) + .add_property( + "lineSearchType", + static_cast(&ConfigProjector::lineSearchType), + static_cast( + &ConfigProjector::lineSearchType)) .def("add", &ConfigProjector::add, DocClassMethod(add)) .def("lastIsOptional", static_cast( &ConfigProjector::lastIsOptional)) diff --git a/src/pyhpp/tools/xacro.py b/src/pyhpp/tools/xacro.py index fcc4c74..a4315cd 100644 --- a/src/pyhpp/tools/xacro.py +++ b/src/pyhpp/tools/xacro.py @@ -24,21 +24,6 @@ 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: @@ -46,11 +31,10 @@ def process_xacro(*args): 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)