Merge pull request #137 from mayataka/devel

Refactored cost and constraints

bindings/python/robotoc/constraints/constraints.cpp

@@ -16,18 +16,33 @@ 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"))
     .def("set_fraction_to_boundary_rule", &Constraints::setFractionToBoundaryRule,
           py::arg("fraction_to_boundary_rule"))
     .def("get_barrier_param", &Constraints::getBarrierParam)
     .def("get_fraction_to_boundary_rule", &Constraints::getFractionToBoundaryRule)
-    DEFINE_ROBOTOC_PYBIND11_CLASS_CLONE(Constraints);
+    .def("get_position_level_constraint_list", &Constraints::getPositionLevelConstraintList)
+    .def("get_velocity_level_constraint_list", &Constraints::getVelocityLevelConstraintList)
+    .def("get_acceleration_level_constraint_list", &Constraints::getAccelerationLevelConstraintList)
+    .def("get_impact_level_constraint_list", &Constraints::getImpactLevelConstraintList)
+    .def("get_constraint_list", &Constraints::getConstraintList)
+    DEFINE_ROBOTOC_PYBIND11_CLASS_CLONE(Constraints)
+    DEFINE_ROBOTOC_PYBIND11_CLASS_PRINT(Constraints);
+
 }
 
 } // namespace python

bindings/python/robotoc/cost/cost_function.cpp

@@ -18,7 +18,14 @@ PYBIND11_MODULE(cost_function, m) {
           py::arg("discount_factor"), py::arg("discount_time_step"))
     .def("discount_factor", &CostFunction::discountFactor)
     .def("discount_time_step", &CostFunction::discountTimeStep)
-    .def("push_back", &CostFunction::push_back)
+    .def("exist", &CostFunction::exist,
+          py::arg("name"))
+    .def("add", &CostFunction::add,
+          py::arg("name"), py::arg("cost"))
+    .def("erase", &CostFunction::erase,
+          py::arg("name"))
+    .def("get", &CostFunction::get,
+          py::arg("name"))
     .def("clear", &CostFunction::clear)
     .def("create_cost_function_data", &CostFunction::createCostFunctionData,
           py::arg("robot"))
@@ -50,7 +57,9 @@ PYBIND11_MODULE(cost_function, m) {
           py::arg("robot"), py::arg("impact_status"), py::arg("data"), 
           py::arg("grid_info"), py::arg("s"), py::arg("kkt_residual"), 
           py::arg("kkt_matrix"))
-    DEFINE_ROBOTOC_PYBIND11_CLASS_CLONE(CostFunction);
+    .def("get_cost_component_list", &CostFunction::getCostComponentList)
+    DEFINE_ROBOTOC_PYBIND11_CLASS_CLONE(CostFunction)
+    DEFINE_ROBOTOC_PYBIND11_CLASS_PRINT(CostFunction);
 }
 
 } // namespace python

bindings/python/robotoc/sto/sto_cost_function.cpp

@@ -14,7 +14,14 @@ namespace py = pybind11;
 PYBIND11_MODULE(sto_cost_function, m) {
   py::class_<STOCostFunction, std::shared_ptr<STOCostFunction>>(m, "STOCostFunction")
     .def(py::init<>())
-    .def("push_back", &STOCostFunction::push_back)
+    .def("exist", &STOCostFunction::exist,
+          py::arg("name"))
+    .def("add", &STOCostFunction::add,
+          py::arg("name"), py::arg("cost"))
+    .def("erase", &STOCostFunction::erase,
+          py::arg("name"))
+    .def("get", &STOCostFunction::get,
+          py::arg("name"))
     .def("clear", &STOCostFunction::clear)
     DEFINE_ROBOTOC_PYBIND11_CLASS_CLONE(STOCostFunction);
 }

doc/examples/ocp_solver_cpp.dox

@@ -104,7 +104,7 @@ Next, we construct the cost function (TODO: write details about the cost functio
   config_cost->set_v_weight_terminal(v_weight);
   config_cost->set_v_weight_impact(v_weight_impact);
   config_cost->set_u_weight(u_weight);
-  cost->push_back(config_cost);
+  cost->add("config_cost", config_cost);
 
   robot.updateFrameKinematics(q_standing);
   const Eigen::Vector3d x3d0_LF = robot.framePosition("LF_FOOT");
@@ -120,7 +120,7 @@ Next, we construct the cost function (TODO: write details about the cost functio
                                                                      flying_down_time+2*ground_time, false);
   auto com_cost_flying_up = std::make_shared<robotoc::CoMCost>(robot, com_ref_flying_up);
   com_cost_flying_up->set_weight(Eigen::Vector3d::Constant(1.0e06));
-  cost->push_back(com_cost_flying_up);
+  cost->add("com_cost_flying_up", com_cost_flying_up);
 
   const Eigen::Vector3d com_ref0_landed = robot.CoM() + jump_length;
   const Eigen::Vector3d vcom_ref_landed = Eigen::Vector3d::Zero();
@@ -129,7 +129,7 @@ Next, we construct the cost function (TODO: write details about the cost functio
                                                                   ground_time+flying_time, false);
   auto com_cost_landed = std::make_shared<robotoc::CoMCost>(robot, com_ref_landed);
   com_cost_landed->set_weight(Eigen::Vector3d::Constant(1.0e06));
-  cost->push_back(com_cost_landed);
+  cost->add("com_cost_landed", com_cost_landed);
 ```
 
 Next, we construct the constraints.
@@ -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

@@ -78,7 +78,7 @@ config_cost.set_v_weight(v_weight)
 config_cost.set_v_weight_terminal(v_weight)
 config_cost.set_v_weight_impact(v_weight_impact)
 config_cost.set_u_weight(u_weight)
-cost.push_back(config_cost)
+cost.add("config_cost", config_cost)
 
 robot.forward_kinematics(q_standing)
 x3d0_LF = robot.frame_position('LF_FOOT')
@@ -93,7 +93,7 @@ com_ref_flying_up = robotoc.PeriodicCoMRef(com_ref0_flying_up, vcom_ref_flying_u
                                            flying_down_time+2*ground_time, False)
 com_cost_flying_up = robotoc.CoMCost(robot, com_ref_flying_up)
 com_cost_flying_up.set_weight(np.full(3, 1.0e06))
-cost.push_back(com_cost_flying_up)
+cost.add("com_cost_flying_up", com_cost_flying_up)
 
 com_ref0_landed = robot.com()
 com_ref0_landed += jump_length
@@ -103,7 +103,7 @@ com_ref_landed = robotoc.PeriodicCoMRef(com_ref0_landed, vcom_ref_landed,
                                         ground_time+flying_time, False)
 com_cost_landed = robotoc.CoMCost(robot, com_ref_landed)
 com_cost_landed.set_weight(np.full(3, 1.0e06))
-cost.push_back(com_cost_landed)
+cost.add("com_cost_landed", com_cost_landed)
 ```
 
 Next, we construct the constraints.
@@ -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

@@ -104,7 +104,7 @@ Next, we construct the cost function. The below is the simple quadratic cost for
   config_cost->set_v_weight_impact(v_weight_impact);
   config_cost->set_dv_weight_impact(dv_weight_impact);
   config_cost->set_a_weight(a_weight);
-  cost->push_back(config_cost);
+  cost->add("config_cost", config_cost);
 ```
 
 Next, we construct the constraints.
@@ -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

@@ -77,7 +77,7 @@ config_cost.set_v_weight_terminal(v_weight)
 config_cost.set_v_weight_impact(v_weight_impact)
 config_cost.set_dv_weight_impact(dv_weight_impact)
 config_cost.set_a_weight(a_weight)
-cost.push_back(config_cost)
+cost.add("config_cost", config_cost)
 ```
 
 Next, we construct the constraints.
@@ -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

@@ -70,7 +70,7 @@ config_cost.set_v_weight(v_weight)
 config_cost.set_v_weight_terminal(v_weight)
 config_cost.set_v_weight_impact(v_weight_impact)
 config_cost.set_a_weight(a_weight)
-cost.push_back(config_cost)
+cost.add("config_cost", config_cost)
 ```
 
 Next, we construct the constraints.
@@ -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

@@ -85,12 +85,12 @@ Next, we construct the cost function.
   config_cost->set_v_weight(Eigen::VectorXd::Constant(robot.dimv(), 0.0001));
   config_cost->set_v_weight_terminal(Eigen::VectorXd::Constant(robot.dimv(), 0.0001));
   config_cost->set_a_weight(Eigen::VectorXd::Constant(robot.dimv(), 0.0001));
-  cost->push_back(config_cost);
+  cost->add("config_cost", config_cost);
   auto x6d_ref = std::make_shared<TaskSpace6DRef>();
   auto task_cost = std::make_shared<robotoc::TaskSpace6DCost>(robot, ee_frame, x6d_ref);
   task_cost->set_x6d_weight(Eigen::Vector3d::Constant(1000), Eigen::Vector3d::Constant(1000));
   task_cost->set_x6df_weight(Eigen::Vector3d::Constant(1000), Eigen::Vector3d::Constant(1000));
-  cost->push_back(task_cost);
+  cost->add("task_cost", task_cost);
 ```
 
 We also construct the constraints.
@@ -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

@@ -35,7 +35,7 @@ config_cost.set_q_weight_terminal(np.full(robot.dimv(), 10))
 config_cost.set_v_weight(np.full(robot.dimv(), 0.01))
 config_cost.set_v_weight_terminal(np.full(robot.dimv(), 0.01))
 config_cost.set_a_weight(np.full(robot.dimv(), 0.01))
-cost.push_back(config_cost)
+cost.add("config_cost", config_cost)
 ```
 
 Next, we construct the constraints.
@@ -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

@@ -55,7 +55,7 @@
 config_cost.set_v_weight_terminal(v_weight)
 config_cost.set_v_weight_impact(v_weight_impact)
 config_cost.set_u_weight(u_weight)
-cost.push_back(config_cost)
+cost.add("config_cost", config_cost)
 
 robot.forward_kinematics(q_standing)
 x3d0_LF = robot.frame_position('FL_foot')
@@ -87,18 +87,18 @@
 LH_cost.set_weight(foot_track_weight)
 RF_cost.set_weight(foot_track_weight)
 RH_cost.set_weight(foot_track_weight)
-cost.push_back(LF_cost)
-cost.push_back(LH_cost)
-cost.push_back(RF_cost)
-cost.push_back(RH_cost)
+cost.add("LF_cost", LF_cost)
+cost.add("LH_cost", LH_cost)
+cost.add("RF_cost", RF_cost)
+cost.add("RH_cost", RH_cost)
 
 com_ref0 = robot.com()
 vcom_ref = 0.25 * step_length / swing_time
 com_ref = robotoc.PeriodicCoMRef(com_ref0, vcom_ref, t0, 2*swing_time, 
                                  double_support_time, True)
 com_cost = robotoc.CoMCost(robot, com_ref)
 com_cost.set_weight(np.full(3, 1.0e06))
-cost.push_back(com_cost)
+cost.add("com_cost", com_cost)
 
 # Create the constraints
 constraints           = robotoc.Constraints(barrier_param=1.0e-03, fraction_to_boundary_rule=0.995)
@@ -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)