improve constraint function interfaces

bindings/python/robotoc/constraints/constraints.cpp

@@ -16,10 +16,18 @@ PYBIND11_MODULE(constraints, m) {
   py::class_<Constraints, std::shared_ptr<Constraints>>(m, "Constraints")
     .def(py::init<const double, const double>(),
           py::arg("barrier_param")=1.0e-03, py::arg("fraction_to_boundary_rule")=0.995)
-    .def("push_back", static_cast<void (Constraints::*)(ConstraintComponentBasePtr)>(&Constraints::push_back),
-          py::arg("constraint_component"))
-    .def("push_back", static_cast<void (Constraints::*)(ImpactConstraintComponentBasePtr)>(&Constraints::push_back),
-          py::arg("constraint_component"))
+    .def("exist", &Constraints::exist,
+          py::arg("name"))
+    .def("add", static_cast<void (Constraints::*)(const std::string&, ConstraintComponentBasePtr)>(&Constraints::add),
+          py::arg("name"), py::arg("constraint"))
+    .def("add", static_cast<void (Constraints::*)(const std::string&, ImpactConstraintComponentBasePtr)>(&Constraints::add),
+          py::arg("name"), py::arg("constraint"))
+    .def("erase", &Constraints::exist,
+          py::arg("name"))
+    .def("get_constraint_component", &Constraints::getConstraintComponent,
+          py::arg("name"))
+    .def("get_impact_constraint_component", &Constraints::getImpactConstraintComponent,
+          py::arg("name"))
     .def("clear", &Constraints::clear)
     .def("set_barrier_param", &Constraints::setBarrierParam,
           py::arg("barrier_param"))

doc/examples/ocp_solver_cpp.dox

@@ -144,13 +144,13 @@ Next, we construct the constraints.
   auto joint_torques_lower   = std::make_shared<robotoc::JointTorquesLowerLimit>(robot);
   auto joint_torques_upper   = std::make_shared<robotoc::JointTorquesUpperLimit>(robot);
   auto friction_cone         = std::make_shared<robotoc::FrictionCone>(robot);
-  constraints->push_back(joint_position_lower);
-  constraints->push_back(joint_position_upper);
-  constraints->push_back(joint_velocity_lower);
-  constraints->push_back(joint_velocity_upper);
-  constraints->push_back(joint_torques_lower);
-  constraints->push_back(joint_torques_upper);
-  constraints->push_back(friction_cone);
+  constraints->add("joint_position_lower", joint_position_lower);
+  constraints->add("joint_position_upper", joint_position_upper);
+  constraints->add("joint_velocity_lower", joint_velocity_lower);
+  constraints->add("joint_velocity_upper", joint_velocity_upper);
+  constraints->add("joint_torques_lower", joint_torques_lower);
+  constraints->add("joint_torques_upper", joint_torques_upper);
+  constraints->add("friction_cone", friction_cone);
 ```
 
 Next, we construct the contact sequence `robotoc::ContactSequence` as 

doc/examples/ocp_solver_py.dox

@@ -116,13 +116,13 @@ joint_velocity_upper  = robotoc.JointVelocityUpperLimit(robot)
 joint_torques_lower   = robotoc.JointTorquesLowerLimit(robot)
 joint_torques_upper   = robotoc.JointTorquesUpperLimit(robot)
 friction_cone         = robotoc.FrictionCone(robot)
-constraints.push_back(joint_position_lower)
-constraints.push_back(joint_position_upper)
-constraints.push_back(joint_velocity_lower)
-constraints.push_back(joint_velocity_upper)
-constraints.push_back(joint_torques_lower)
-constraints.push_back(joint_torques_upper)
-constraints.push_back(friction_cone)
+constraints.add("joint_position_lower", joint_position_lower)
+constraints.add("joint_position_upper", joint_position_upper)
+constraints.add("joint_velocity_lower", joint_velocity_lower)
+constraints.add("joint_velocity_upper", joint_velocity_upper)
+constraints.add("joint_torques_lower", joint_torques_lower)
+constraints.add("joint_torques_upper", joint_torques_upper)
+constraints.add("friction_cone", friction_cone)
 ```
 
 Next, we construct the contact sequence `robotoc::ContactSequence` as 

doc/examples/ocp_solver_sto_cpp.dox

@@ -119,13 +119,13 @@ Next, we construct the constraints.
   auto joint_torques_lower   = std::make_shared<robotoc::JointTorquesLowerLimit>(robot);
   auto joint_torques_upper   = std::make_shared<robotoc::JointTorquesUpperLimit>(robot);
   auto friction_cone         = std::make_shared<robotoc::FrictionCone>(robot);
-  constraints->push_back(joint_position_lower);
-  constraints->push_back(joint_position_upper);
-  constraints->push_back(joint_velocity_lower);
-  constraints->push_back(joint_velocity_upper);
-  constraints->push_back(joint_torques_lower);
-  constraints->push_back(joint_torques_upper);
-  constraints->push_back(friction_cone);
+  constraints->add("joint_position_lower", joint_position_lower);
+  constraints->add("joint_position_upper", joint_position_upper);
+  constraints->add("joint_velocity_lower", joint_velocity_lower);
+  constraints->add("joint_velocity_upper", joint_velocity_upper);
+  constraints->add("joint_torques_lower", joint_torques_lower);
+  constraints->add("joint_torques_upper", joint_torques_upper);
+  constraints->add("friction_cone", friction_cone);
 ```
 
 Next, we construct the contact sequence `robotoc::ContactSequence` as

doc/examples/ocp_solver_sto_py.dox

@@ -90,13 +90,13 @@ joint_velocity_upper  = robotoc.JointVelocityUpperLimit(robot)
 joint_torques_lower   = robotoc.JointTorquesLowerLimit(robot)
 joint_torques_upper   = robotoc.JointTorquesUpperLimit(robot)
 friction_cone         = robotoc.FrictionCone(robot)
-constraints.push_back(joint_position_lower)
-constraints.push_back(joint_position_upper)
-constraints.push_back(joint_velocity_lower)
-constraints.push_back(joint_velocity_upper)
-constraints.push_back(joint_torques_lower)
-constraints.push_back(joint_torques_upper)
-constraints.push_back(friction_cone)
+constraints.add("joint_position_lower", joint_position_lower)
+constraints.add("joint_position_upper", joint_position_upper)
+constraints.add("joint_velocity_lower", joint_velocity_lower)
+constraints.add("joint_velocity_upper", joint_velocity_upper)
+constraints.add("joint_torques_lower", joint_torques_lower)
+constraints.add("joint_torques_upper", joint_torques_upper)
+constraints.add("friction_cone", friction_cone)
 ```
 
 Next, we construct the contact sequence `robotoc::ContactSequence` as 

doc/examples/surface_contacts_py.dox

@@ -83,13 +83,13 @@ joint_velocity_upper  = robotoc.JointVelocityUpperLimit(robot)
 joint_torques_lower   = robotoc.JointTorquesLowerLimit(robot)
 joint_torques_upper   = robotoc.JointTorquesUpperLimit(robot)
 friction_cone  = robotoc.FrictionCone(robot)
-constraints.push_back(joint_position_lower)
-constraints.push_back(joint_position_upper)
-constraints.push_back(joint_velocity_lower)
-constraints.push_back(joint_velocity_upper)
-constraints.push_back(joint_torques_lower)
-constraints.push_back(joint_torques_upper)
-constraints.push_back(friction_cone)
+constraints.add("joint_position_lower", joint_position_lower)
+constraints.add("joint_position_upper", joint_position_upper)
+constraints.add("joint_velocity_lower", joint_velocity_lower)
+constraints.add("joint_velocity_upper", joint_velocity_upper)
+constraints.add("joint_torques_lower", joint_torques_lower)
+constraints.add("joint_torques_upper", joint_torques_upper)
+constraints.add("friction_cone", friction_cone)
 ```
 
 Next, we construct the contact sequence `robotoc::ContactSequence` as

doc/examples/unconstr_ocp_solver_cpp.dox

@@ -104,12 +104,12 @@ We also construct the constraints.
   auto joint_velocity_upper = std::make_shared<robotoc::JointVelocityUpperLimit>(robot);
   auto joint_torques_lower  = std::make_shared<robotoc::JointTorquesLowerLimit>(robot);
   auto joint_torques_upper  = std::make_shared<robotoc::JointTorquesUpperLimit>(robot);
-  constraints->push_back(joint_position_lower);
-  constraints->push_back(joint_position_upper);
-  constraints->push_back(joint_velocity_lower);
-  constraints->push_back(joint_velocity_upper);
-  constraints->push_back(joint_torques_lower);
-  constraints->push_back(joint_torques_upper);
+  constraints->add("joint_position_lower", joint_position_lower);
+  constraints->add("joint_position_upper", joint_position_upper);
+  constraints->add("joint_velocity_lower", joint_velocity_lower);
+  constraints->add("joint_velocity_upper", joint_velocity_upper);
+  constraints->add("joint_torques_lower", joint_torques_lower);
+  constraints->add("joint_torques_upper", joint_torques_upper);
   constraints->setBarrierParam(1.0e-03);
 ```
 

doc/examples/unconstr_ocp_solver_py.dox

@@ -47,12 +47,12 @@ joint_velocity_lower = robotoc.JointVelocityLowerLimit(robot)
 joint_velocity_upper = robotoc.JointVelocityUpperLimit(robot)
 joint_torques_lower  = robotoc.JointTorquesLowerLimit(robot)
 joint_torques_upper  = robotoc.JointTorquesUpperLimit(robot)
-constraints.push_back(joint_position_lower)
-constraints.push_back(joint_position_upper)
-constraints.push_back(joint_velocity_lower)
-constraints.push_back(joint_velocity_upper)
-constraints.push_back(joint_torques_lower)
-constraints.push_back(joint_torques_upper)
+constraints.add("joint_position_lower", joint_position_lower)
+constraints.add("joint_position_upper", joint_position_upper)
+constraints.add("joint_velocity_lower", joint_velocity_lower)
+constraints.add("joint_velocity_upper", joint_velocity_upper)
+constraints.add("joint_torques_lower", joint_torques_lower)
+constraints.add("joint_torques_upper", joint_torques_upper)
 ```
 
 Finally, we can construct the optimal control solver!

examples/a1/python/crawl.py

@@ -109,13 +109,13 @@
 joint_torques_lower   = robotoc.JointTorquesLowerLimit(robot)
 joint_torques_upper   = robotoc.JointTorquesUpperLimit(robot)
 friction_cone         = robotoc.FrictionCone(robot)
-constraints.push_back(joint_position_lower)
-constraints.push_back(joint_position_upper)
-constraints.push_back(joint_velocity_lower)
-constraints.push_back(joint_velocity_upper)
-constraints.push_back(joint_torques_lower)
-constraints.push_back(joint_torques_upper)
-constraints.push_back(friction_cone)
+constraints.add("joint_position_lower", joint_position_lower)
+constraints.add("joint_position_upper", joint_position_upper)
+constraints.add("joint_velocity_lower", joint_velocity_lower)
+constraints.add("joint_velocity_upper", joint_velocity_upper)
+constraints.add("joint_torques_lower", joint_torques_lower)
+constraints.add("joint_torques_upper", joint_torques_upper)
+constraints.add("friction_cone", friction_cone)
 
 # Create the contact sequence
 contact_sequence = robotoc.ContactSequence(robot)

examples/a1/python/flying_trot.py

@@ -109,13 +109,13 @@
 joint_torques_lower   = robotoc.JointTorquesLowerLimit(robot)
 joint_torques_upper   = robotoc.JointTorquesUpperLimit(robot)
 friction_cone         = robotoc.FrictionCone(robot)
-constraints.push_back(joint_position_lower)
-constraints.push_back(joint_position_upper)
-constraints.push_back(joint_velocity_lower)
-constraints.push_back(joint_velocity_upper)
-constraints.push_back(joint_torques_lower)
-constraints.push_back(joint_torques_upper)
-constraints.push_back(friction_cone)
+constraints.add("joint_position_lower", joint_position_lower)
+constraints.add("joint_position_upper", joint_position_upper)
+constraints.add("joint_velocity_lower", joint_velocity_lower)
+constraints.add("joint_velocity_upper", joint_velocity_upper)
+constraints.add("joint_torques_lower", joint_torques_lower)
+constraints.add("joint_torques_upper", joint_torques_upper)
+constraints.add("friction_cone", friction_cone)
 
 # Create the contact sequence
 contact_sequence = robotoc.ContactSequence(robot)

examples/a1/python/trot.py

@@ -109,13 +109,13 @@
 joint_torques_lower   = robotoc.JointTorquesLowerLimit(robot)
 joint_torques_upper   = robotoc.JointTorquesUpperLimit(robot)
 friction_cone         = robotoc.FrictionCone(robot)
-constraints.push_back(joint_position_lower)
-constraints.push_back(joint_position_upper)
-constraints.push_back(joint_velocity_lower)
-constraints.push_back(joint_velocity_upper)
-constraints.push_back(joint_torques_lower)
-constraints.push_back(joint_torques_upper)
-constraints.push_back(friction_cone)
+constraints.add("joint_position_lower", joint_position_lower)
+constraints.add("joint_position_upper", joint_position_upper)
+constraints.add("joint_velocity_lower", joint_velocity_lower)
+constraints.add("joint_velocity_upper", joint_velocity_upper)
+constraints.add("joint_torques_lower", joint_torques_lower)
+constraints.add("joint_torques_upper", joint_torques_upper)
+constraints.add("friction_cone", friction_cone)
 
 # Create the contact sequence
 contact_sequence = robotoc.ContactSequence(robot)

examples/anymal/bounce.cpp

@@ -138,13 +138,13 @@ int main(int argc, char *argv[]) {
   auto joint_torques_lower  = std::make_shared<robotoc::JointTorquesLowerLimit>(robot);
   auto joint_torques_upper  = std::make_shared<robotoc::JointTorquesUpperLimit>(robot);
   auto friction_cone        = std::make_shared<robotoc::FrictionCone>(robot);
-  constraints->push_back(joint_position_lower);
-  constraints->push_back(joint_position_upper);
-  constraints->push_back(joint_velocity_lower);
-  constraints->push_back(joint_velocity_upper);
-  constraints->push_back(joint_torques_lower);
-  constraints->push_back(joint_torques_upper);
-  constraints->push_back(friction_cone);
+  constraints->add("joint_position_lower", joint_position_lower);
+  constraints->add("joint_position_upper", joint_position_upper);
+  constraints->add("joint_velocity_lower", joint_velocity_lower);
+  constraints->add("joint_velocity_upper", joint_velocity_upper);
+  constraints->add("joint_torques_lower", joint_torques_lower);
+  constraints->add("joint_torques_upper", joint_torques_upper);
+  constraints->add("friction_cone", friction_cone);
 
   // Create the contact sequence
   auto contact_sequence = std::make_shared<robotoc::ContactSequence>(robot);

examples/anymal/crawl.cpp

@@ -138,13 +138,13 @@ int main(int argc, char *argv[]) {
   auto joint_torques_lower  = std::make_shared<robotoc::JointTorquesLowerLimit>(robot);
   auto joint_torques_upper  = std::make_shared<robotoc::JointTorquesUpperLimit>(robot);
   auto friction_cone        = std::make_shared<robotoc::FrictionCone>(robot);
-  constraints->push_back(joint_position_lower);
-  constraints->push_back(joint_position_upper);
-  constraints->push_back(joint_velocity_lower);
-  constraints->push_back(joint_velocity_upper);
-  constraints->push_back(joint_torques_lower);
-  constraints->push_back(joint_torques_upper);
-  constraints->push_back(friction_cone);
+  constraints->add("joint_position_lower", joint_position_lower);
+  constraints->add("joint_position_upper", joint_position_upper);
+  constraints->add("joint_velocity_lower", joint_velocity_lower);
+  constraints->add("joint_velocity_upper", joint_velocity_upper);
+  constraints->add("joint_torques_lower", joint_torques_lower);
+  constraints->add("joint_torques_upper", joint_torques_upper);
+  constraints->add("friction_cone", friction_cone);
 
   // Create the contact sequence
   auto contact_sequence = std::make_shared<robotoc::ContactSequence>(robot);

examples/anymal/jump.cpp

@@ -121,13 +121,13 @@ int main(int argc, char *argv[]) {
   auto joint_torques_lower  = std::make_shared<robotoc::JointTorquesLowerLimit>(robot);
   auto joint_torques_upper  = std::make_shared<robotoc::JointTorquesUpperLimit>(robot);
   auto friction_cone        = std::make_shared<robotoc::FrictionCone>(robot);
-  constraints->push_back(joint_position_lower);
-  constraints->push_back(joint_position_upper);
-  constraints->push_back(joint_velocity_lower);
-  constraints->push_back(joint_velocity_upper);
-  constraints->push_back(joint_torques_lower);
-  constraints->push_back(joint_torques_upper);
-  constraints->push_back(friction_cone);
+  constraints->add("joint_position_lower", joint_position_lower);
+  constraints->add("joint_position_upper", joint_position_upper);
+  constraints->add("joint_velocity_lower", joint_velocity_lower);
+  constraints->add("joint_velocity_upper", joint_velocity_upper);
+  constraints->add("joint_torques_lower", joint_torques_lower);
+  constraints->add("joint_torques_upper", joint_torques_upper);
+  constraints->add("friction_cone", friction_cone);
 
   // Create the contact sequence
   auto contact_sequence = std::make_shared<robotoc::ContactSequence>(robot);

examples/anymal/jump_sto.cpp

@@ -96,13 +96,13 @@ int main(int argc, char *argv[]) {
   auto joint_torques_lower  = std::make_shared<robotoc::JointTorquesLowerLimit>(robot);
   auto joint_torques_upper  = std::make_shared<robotoc::JointTorquesUpperLimit>(robot);
   auto friction_cone        = std::make_shared<robotoc::FrictionCone>(robot);
-  constraints->push_back(joint_position_lower);
-  constraints->push_back(joint_position_upper);
-  constraints->push_back(joint_velocity_lower);
-  constraints->push_back(joint_velocity_upper);
-  constraints->push_back(joint_torques_lower);
-  constraints->push_back(joint_torques_upper);
-  constraints->push_back(friction_cone);
+  constraints->add("joint_position_lower", joint_position_lower);
+  constraints->add("joint_position_upper", joint_position_upper);
+  constraints->add("joint_velocity_lower", joint_velocity_lower);
+  constraints->add("joint_velocity_upper", joint_velocity_upper);
+  constraints->add("joint_torques_lower", joint_torques_lower);
+  constraints->add("joint_torques_upper", joint_torques_upper);
+  constraints->add("friction_cone", friction_cone);
 
   // Create the contact sequence
   auto contact_sequence = std::make_shared<robotoc::ContactSequence>(robot);

examples/anymal/ocp_benchmark.cpp

@@ -80,13 +80,13 @@ int main () {
   auto joint_torques_lower  = std::make_shared<robotoc::JointTorquesLowerLimit>(robot);
   auto joint_torques_upper  = std::make_shared<robotoc::JointTorquesUpperLimit>(robot);
   auto friction_cone        = std::make_shared<robotoc::FrictionCone>(robot);
-  constraints->push_back(joint_position_lower);
-  constraints->push_back(joint_position_upper);
-  constraints->push_back(joint_velocity_lower);
-  constraints->push_back(joint_velocity_upper);
-  constraints->push_back(joint_torques_lower);
-  constraints->push_back(joint_torques_upper);
-  constraints->push_back(friction_cone);
+  constraints->add("joint_position_lower", joint_position_lower);
+  constraints->add("joint_position_upper", joint_position_upper);
+  constraints->add("joint_velocity_lower", joint_velocity_lower);
+  constraints->add("joint_velocity_upper", joint_velocity_upper);
+  constraints->add("joint_torques_lower", joint_torques_lower);
+  constraints->add("joint_torques_upper", joint_torques_upper);
+  constraints->add("friction_cone", friction_cone);
 
   // Create the contact sequence
   auto contact_sequence = std::make_shared<robotoc::ContactSequence>(robot);

examples/anymal/pace.cpp

@@ -138,13 +138,13 @@ int main(int argc, char *argv[]) {
   auto joint_torques_lower  = std::make_shared<robotoc::JointTorquesLowerLimit>(robot);
   auto joint_torques_upper  = std::make_shared<robotoc::JointTorquesUpperLimit>(robot);
   auto friction_cone        = std::make_shared<robotoc::FrictionCone>(robot);
-  constraints->push_back(joint_position_lower);
-  constraints->push_back(joint_position_upper);
-  constraints->push_back(joint_velocity_lower);
-  constraints->push_back(joint_velocity_upper);
-  constraints->push_back(joint_torques_lower);
-  constraints->push_back(joint_torques_upper);
-  constraints->push_back(friction_cone);
+  constraints->add("joint_position_lower", joint_position_lower);
+  constraints->add("joint_position_upper", joint_position_upper);
+  constraints->add("joint_velocity_lower", joint_velocity_lower);
+  constraints->add("joint_velocity_upper", joint_velocity_upper);
+  constraints->add("joint_torques_lower", joint_torques_lower);
+  constraints->add("joint_torques_upper", joint_torques_upper);
+  constraints->add("friction_cone", friction_cone);
 
   // Create the contact sequence
   auto contact_sequence = std::make_shared<robotoc::ContactSequence>(robot);

examples/anymal/python/bounce.py

@@ -109,13 +109,13 @@
 joint_torques_lower   = robotoc.JointTorquesLowerLimit(robot)
 joint_torques_upper   = robotoc.JointTorquesUpperLimit(robot)
 friction_cone         = robotoc.FrictionCone(robot)
-constraints.push_back(joint_position_lower)
-constraints.push_back(joint_position_upper)
-constraints.push_back(joint_velocity_lower)
-constraints.push_back(joint_velocity_upper)
-constraints.push_back(joint_torques_lower)
-constraints.push_back(joint_torques_upper)
-constraints.push_back(friction_cone)
+constraints.add("joint_position_lower", joint_position_lower)
+constraints.add("joint_position_upper", joint_position_upper)
+constraints.add("joint_velocity_lower", joint_velocity_lower)
+constraints.add("joint_velocity_upper", joint_velocity_upper)
+constraints.add("joint_torques_lower", joint_torques_lower)
+constraints.add("joint_torques_upper", joint_torques_upper)
+constraints.add("friction_cone", friction_cone)
 
 # Create the contact sequence
 contact_sequence = robotoc.ContactSequence(robot)