diff --git a/bindings/python/robotoc/constraints/constraints.cpp b/bindings/python/robotoc/constraints/constraints.cpp index 7862d73e4..3f580503e 100644 --- a/bindings/python/robotoc/constraints/constraints.cpp +++ b/bindings/python/robotoc/constraints/constraints.cpp @@ -16,10 +16,18 @@ PYBIND11_MODULE(constraints, m) { py::class_>(m, "Constraints") .def(py::init(), py::arg("barrier_param")=1.0e-03, py::arg("fraction_to_boundary_rule")=0.995) - .def("push_back", static_cast(&Constraints::push_back), - py::arg("constraint_component")) - .def("push_back", static_cast(&Constraints::push_back), - py::arg("constraint_component")) + .def("exist", &Constraints::exist, + py::arg("name")) + .def("add", static_cast(&Constraints::add), + py::arg("name"), py::arg("constraint")) + .def("add", static_cast(&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")) @@ -27,7 +35,14 @@ PYBIND11_MODULE(constraints, m) { 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 diff --git a/bindings/python/robotoc/cost/cost_function.cpp b/bindings/python/robotoc/cost/cost_function.cpp index 778704412..cc236d488 100644 --- a/bindings/python/robotoc/cost/cost_function.cpp +++ b/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 diff --git a/bindings/python/robotoc/sto/sto_cost_function.cpp b/bindings/python/robotoc/sto/sto_cost_function.cpp index 6491bb90f..de2163a66 100644 --- a/bindings/python/robotoc/sto/sto_cost_function.cpp +++ b/bindings/python/robotoc/sto/sto_cost_function.cpp @@ -14,7 +14,14 @@ namespace py = pybind11; PYBIND11_MODULE(sto_cost_function, m) { py::class_>(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); } diff --git a/doc/examples/ocp_solver_cpp.dox b/doc/examples/ocp_solver_cpp.dox index f43342a09..e6076818f 100644 --- a/doc/examples/ocp_solver_cpp.dox +++ b/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(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(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(robot); auto joint_torques_upper = std::make_shared(robot); auto friction_cone = std::make_shared(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 diff --git a/doc/examples/ocp_solver_py.dox b/doc/examples/ocp_solver_py.dox index b9f7f37d9..b8b87e863 100644 --- a/doc/examples/ocp_solver_py.dox +++ b/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 diff --git a/doc/examples/ocp_solver_sto_cpp.dox b/doc/examples/ocp_solver_sto_cpp.dox index ef5623b77..f5c7536a9 100644 --- a/doc/examples/ocp_solver_sto_cpp.dox +++ b/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(robot); auto joint_torques_upper = std::make_shared(robot); auto friction_cone = std::make_shared(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 diff --git a/doc/examples/ocp_solver_sto_py.dox b/doc/examples/ocp_solver_sto_py.dox index 65fd9137e..c1deab718 100644 --- a/doc/examples/ocp_solver_sto_py.dox +++ b/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 diff --git a/doc/examples/surface_contacts_py.dox b/doc/examples/surface_contacts_py.dox index 1e1761e8a..0d0aa6f48 100644 --- a/doc/examples/surface_contacts_py.dox +++ b/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 diff --git a/doc/examples/unconstr_ocp_solver_cpp.dox b/doc/examples/unconstr_ocp_solver_cpp.dox index 58b7b3dba..334c35020 100644 --- a/doc/examples/unconstr_ocp_solver_cpp.dox +++ b/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(); auto task_cost = std::make_shared(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(robot); auto joint_torques_lower = std::make_shared(robot); auto joint_torques_upper = std::make_shared(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); ``` diff --git a/doc/examples/unconstr_ocp_solver_py.dox b/doc/examples/unconstr_ocp_solver_py.dox index de43f98b7..5d7b307f1 100644 --- a/doc/examples/unconstr_ocp_solver_py.dox +++ b/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! diff --git a/examples/a1/python/crawl.py b/examples/a1/python/crawl.py index afa8ed49d..55c6cef49 100644 --- a/examples/a1/python/crawl.py +++ b/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,10 +87,10 @@ 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 @@ -98,7 +98,7 @@ 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) diff --git a/examples/a1/python/flying_trot.py b/examples/a1/python/flying_trot.py index d845ad6a0..63bf2a40d 100644 --- a/examples/a1/python/flying_trot.py +++ b/examples/a1/python/flying_trot.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,10 +87,10 @@ 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.5 * step_length / (stance_time+flying_time) @@ -98,7 +98,7 @@ 0, True) com_cost = robotoc.CoMCost(robot, com_ref) com_cost.set_weight(np.full(3, 1.0e05)) -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) diff --git a/examples/a1/python/trot.py b/examples/a1/python/trot.py index 8afb3ca31..d354cf802 100644 --- a/examples/a1/python/trot.py +++ b/examples/a1/python/trot.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,10 +87,10 @@ 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.5 * step_length / swing_time @@ -98,7 +98,7 @@ 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) diff --git a/examples/anymal/bounce.cpp b/examples/anymal/bounce.cpp index ed7c61cc1..2055cf38f 100644 --- a/examples/anymal/bounce.cpp +++ b/examples/anymal/bounce.cpp @@ -82,7 +82,7 @@ int main(int argc, char *argv[]) { 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"); @@ -114,10 +114,10 @@ int main(int argc, char *argv[]) { 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); const Eigen::Vector3d com_ref0 = robot.CoM(); const Eigen::Vector3d vcom_ref = 0.5 * step_length / swing_time; @@ -125,7 +125,7 @@ int main(int argc, char *argv[]) { double_support_time, false); auto com_cost = std::make_shared(robot, com_ref); com_cost->set_weight(Eigen::Vector3d::Constant(1.0e06)); - cost->push_back(com_cost); + cost->add("com_cost", com_cost); // Create the constraints const double barrier_param = 1.0e-03; @@ -138,13 +138,13 @@ int main(int argc, char *argv[]) { auto joint_torques_lower = std::make_shared(robot); auto joint_torques_upper = std::make_shared(robot); auto friction_cone = std::make_shared(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(robot); diff --git a/examples/anymal/crawl.cpp b/examples/anymal/crawl.cpp index ac5882023..028eb2953 100644 --- a/examples/anymal/crawl.cpp +++ b/examples/anymal/crawl.cpp @@ -82,7 +82,7 @@ int main(int argc, char *argv[]) { 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"); @@ -114,10 +114,10 @@ int main(int argc, char *argv[]) { 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); const Eigen::Vector3d com_ref0 = robot.CoM(); const Eigen::Vector3d vcom_ref = 0.25 * step_length / swing_time; @@ -125,7 +125,7 @@ int main(int argc, char *argv[]) { double_support_time, true); auto com_cost = std::make_shared(robot, com_ref); com_cost->set_weight(Eigen::Vector3d::Constant(1.0e06)); - cost->push_back(com_cost); + cost->add("com_cost", com_cost); // Create the constraints const double barrier_param = 1.0e-03; @@ -138,13 +138,13 @@ int main(int argc, char *argv[]) { auto joint_torques_lower = std::make_shared(robot); auto joint_torques_upper = std::make_shared(robot); auto friction_cone = std::make_shared(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(robot); diff --git a/examples/anymal/jump.cpp b/examples/anymal/jump.cpp index cdf91d4fa..2fef115bb 100644 --- a/examples/anymal/jump.cpp +++ b/examples/anymal/jump.cpp @@ -83,7 +83,7 @@ int main(int argc, char *argv[]) { 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"); @@ -99,7 +99,7 @@ int main(int argc, char *argv[]) { flying_down_time+2*ground_time, false); auto com_cost_flying_up = std::make_shared(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(); @@ -108,7 +108,7 @@ int main(int argc, char *argv[]) { ground_time+flying_time, false); auto com_cost_landed = std::make_shared(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); // Create the constraints const double barrier_param = 1.0e-03; @@ -121,13 +121,13 @@ int main(int argc, char *argv[]) { auto joint_torques_lower = std::make_shared(robot); auto joint_torques_upper = std::make_shared(robot); auto friction_cone = std::make_shared(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(robot); diff --git a/examples/anymal/jump_sto.cpp b/examples/anymal/jump_sto.cpp index 6b2e043ea..5edd3895b 100644 --- a/examples/anymal/jump_sto.cpp +++ b/examples/anymal/jump_sto.cpp @@ -83,7 +83,7 @@ int main(int argc, char *argv[]) { 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); // Create the constraints const double barrier_param = 1.0e-03; @@ -96,13 +96,13 @@ int main(int argc, char *argv[]) { auto joint_torques_lower = std::make_shared(robot); auto joint_torques_upper = std::make_shared(robot); auto friction_cone = std::make_shared(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(robot); diff --git a/examples/anymal/ocp_benchmark.cpp b/examples/anymal/ocp_benchmark.cpp index 89b28a024..ef0cdc5ba 100644 --- a/examples/anymal/ocp_benchmark.cpp +++ b/examples/anymal/ocp_benchmark.cpp @@ -56,7 +56,7 @@ int main () { config_cost->set_v_weight(Eigen::VectorXd::Constant(robot.dimv(), 1)); config_cost->set_v_weight_terminal(Eigen::VectorXd::Constant(robot.dimv(), 1)); config_cost->set_a_weight(Eigen::VectorXd::Constant(robot.dimv(), 0.01)); - cost->push_back(config_cost); + cost->add("config_cost", config_cost); auto local_contact_force_cost = std::make_shared(robot); std::vector f_weight, f_ref; for (int i=0; iset_f_weight(f_weight); local_contact_force_cost->set_f_ref(f_ref); - cost->push_back(local_contact_force_cost); + cost->add("local_contact_force_cost", local_contact_force_cost); // Create inequality constraints. auto constraints = std::make_shared(); @@ -80,13 +80,13 @@ int main () { auto joint_torques_lower = std::make_shared(robot); auto joint_torques_upper = std::make_shared(robot); auto friction_cone = std::make_shared(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(robot); diff --git a/examples/anymal/pace.cpp b/examples/anymal/pace.cpp index 1a3348d6b..1d00b6583 100644 --- a/examples/anymal/pace.cpp +++ b/examples/anymal/pace.cpp @@ -82,7 +82,7 @@ int main(int argc, char *argv[]) { 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"); @@ -114,10 +114,10 @@ int main(int argc, char *argv[]) { 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); const Eigen::Vector3d com_ref0 = robot.CoM(); const Eigen::Vector3d vcom_ref = 0.5 * step_length / swing_time; @@ -125,7 +125,7 @@ int main(int argc, char *argv[]) { double_support_time, true); auto com_cost = std::make_shared(robot, com_ref); com_cost->set_weight(Eigen::Vector3d::Constant(1.0e06)); - cost->push_back(com_cost); + cost->add("com_cost", com_cost); // Create the constraints const double barrier_param = 1.0e-03; @@ -138,13 +138,13 @@ int main(int argc, char *argv[]) { auto joint_torques_lower = std::make_shared(robot); auto joint_torques_upper = std::make_shared(robot); auto friction_cone = std::make_shared(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(robot); diff --git a/examples/anymal/python/bounce.py b/examples/anymal/python/bounce.py index 5e1d59265..9613fe564 100644 --- a/examples/anymal/python/bounce.py +++ b/examples/anymal/python/bounce.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('LF_FOOT') @@ -87,10 +87,10 @@ 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.5 * step_length / swing_time @@ -98,7 +98,7 @@ double_support_time, False) 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) diff --git a/examples/anymal/python/crawl.py b/examples/anymal/python/crawl.py index 14d632a12..59bdfd726 100644 --- a/examples/anymal/python/crawl.py +++ b/examples/anymal/python/crawl.py @@ -54,7 +54,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('LF_FOOT') @@ -86,10 +86,10 @@ 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 @@ -97,7 +97,7 @@ 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) @@ -108,13 +108,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) diff --git a/examples/anymal/python/flying_trot.py b/examples/anymal/python/flying_trot.py index 849056c1f..d40254a38 100644 --- a/examples/anymal/python/flying_trot.py +++ b/examples/anymal/python/flying_trot.py @@ -54,7 +54,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('LF_FOOT') @@ -86,10 +86,10 @@ 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.5 * step_length / (stance_time+flying_time) @@ -97,7 +97,7 @@ 0, 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) @@ -108,13 +108,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) diff --git a/examples/anymal/python/jump.py b/examples/anymal/python/jump.py index 15fdcc0b4..ff72b186e 100644 --- a/examples/anymal/python/jump.py +++ b/examples/anymal/python/jump.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('LF_FOOT') @@ -70,7 +70,7 @@ 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 @@ -80,7 +80,7 @@ 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) # Create the constraints constraints = robotoc.Constraints(barrier_param=1.0e-03, fraction_to_boundary_rule=0.995) @@ -91,13 +91,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) diff --git a/examples/anymal/python/jump_sto.py b/examples/anymal/python/jump_sto.py index 6cbbec9e5..f7d6827e4 100644 --- a/examples/anymal/python/jump_sto.py +++ b/examples/anymal/python/jump_sto.py @@ -54,7 +54,7 @@ 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) # Create the constraints constraints = robotoc.Constraints(barrier_param=1.0e-03, fraction_to_boundary_rule=0.995) @@ -65,13 +65,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 diff --git a/examples/anymal/python/pace.py b/examples/anymal/python/pace.py index b6ff9fbc8..5648390b6 100644 --- a/examples/anymal/python/pace.py +++ b/examples/anymal/python/pace.py @@ -54,7 +54,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('LF_FOOT') @@ -86,10 +86,10 @@ 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.5 * step_length / swing_time @@ -97,7 +97,7 @@ 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) @@ -108,13 +108,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) diff --git a/examples/anymal/python/trot.py b/examples/anymal/python/trot.py index 830493c5d..dd9daa93c 100644 --- a/examples/anymal/python/trot.py +++ b/examples/anymal/python/trot.py @@ -54,7 +54,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('LF_FOOT') @@ -86,10 +86,10 @@ 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.5 * step_length / swing_time @@ -97,7 +97,7 @@ 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) @@ -108,13 +108,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) diff --git a/examples/anymal/python/trot_sto1.py b/examples/anymal/python/trot_sto1.py index 47ba98da6..1d63b27e0 100644 --- a/examples/anymal/python/trot_sto1.py +++ b/examples/anymal/python/trot_sto1.py @@ -54,7 +54,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('LF_FOOT') @@ -74,10 +74,10 @@ 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) com0 = robot.com() com_pos_to_LF_foot_pos = x3d0_LF - com0 @@ -91,7 +91,7 @@ com_ref = robotoc.DiscreteTimeCoMRef(com_pos_to_fee_pos) com_cost = robotoc.CoMCost(robot, com_ref) com_cost.set_weight(np.full(3, 1.0e04)) -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) @@ -102,13 +102,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) diff --git a/examples/anymal/python/trot_sto2.py b/examples/anymal/python/trot_sto2.py index b13406206..00eef0a1c 100644 --- a/examples/anymal/python/trot_sto2.py +++ b/examples/anymal/python/trot_sto2.py @@ -53,7 +53,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('LF_FOOT') @@ -73,10 +73,10 @@ 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) com0 = robot.com() com_pos_to_LF_foot_pos = x3d0_LF - com0 @@ -90,7 +90,7 @@ com_ref = robotoc.DiscreteTimeCoMRef(com_pos_to_fee_pos) 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) @@ -101,13 +101,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) diff --git a/examples/anymal/run.cpp b/examples/anymal/run.cpp index 030d4f7cc..9f2f4c2d4 100644 --- a/examples/anymal/run.cpp +++ b/examples/anymal/run.cpp @@ -136,7 +136,7 @@ int main(int argc, char *argv[]) { config_cost->set_v_weight_impact(v_weight); config_cost->set_a_weight(a_weight); config_cost->set_dv_weight_impact(a_weight); - cost->push_back(config_cost); + cost->add("config_cost", config_cost); Eigen::VectorXd q_standing(Eigen::VectorXd::Zero(robot.dimq())); q_standing << -3, 0, 0.4792, 0, 0, 0, 1, @@ -157,7 +157,7 @@ int main(int argc, char *argv[]) { time_varying_config_cost->set_q_weight(q_weight); time_varying_config_cost->set_q_weight_terminal(q_weight); time_varying_config_cost->set_q_weight_impact(q_weight); - cost->push_back(time_varying_config_cost); + cost->add("time_varying_config_cost", time_varying_config_cost); // Create the constraints const double barrier_param = 1.0e-03; @@ -171,13 +171,13 @@ int main(int argc, char *argv[]) { auto joint_torques_upper = std::make_shared(robot); auto friction_cone = std::make_shared(robot); auto impact_friction_cone = std::make_shared(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); constraints->push_back(impact_friction_cone); // Create the contact sequence diff --git a/examples/anymal/trot.cpp b/examples/anymal/trot.cpp index 29a9b8e90..03aa9c703 100644 --- a/examples/anymal/trot.cpp +++ b/examples/anymal/trot.cpp @@ -83,7 +83,7 @@ int main(int argc, char *argv[]) { 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"); @@ -115,10 +115,10 @@ int main(int argc, char *argv[]) { 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); const Eigen::Vector3d com_ref0 = robot.CoM(); const Eigen::Vector3d vcom_ref = 0.5 * step_length / swing_time; @@ -126,7 +126,7 @@ int main(int argc, char *argv[]) { double_support_time, true); auto com_cost = std::make_shared(robot, com_ref); com_cost->set_weight(Eigen::Vector3d::Constant(1.0e06)); - cost->push_back(com_cost); + cost->add("com_cost", com_cost); // Create the constraints const double barrier_param = 1.0e-03; @@ -139,13 +139,13 @@ int main(int argc, char *argv[]) { auto joint_torques_lower = std::make_shared(robot); auto joint_torques_upper = std::make_shared(robot); auto friction_cone = std::make_shared(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(robot); diff --git a/examples/icub/python/jump_sto.py b/examples/icub/python/jump_sto.py index ea80cce0b..ab2870fd9 100644 --- a/examples/icub/python/jump_sto.py +++ b/examples/icub/python/jump_sto.py @@ -46,7 +46,7 @@ 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) # Create the constraints constraints = robotoc.Constraints(barrier_param=1.0e-03, fraction_to_boundary_rule=0.995) @@ -58,14 +58,14 @@ joint_torques_upper = robotoc.JointTorquesUpperLimit(robot) # contact_wrench_cone = robotoc.ContactWrenchCone(robot, 0.1, 0.05) 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(contact_wrench_cone) -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("contact_wrench_cone", contact_wrench_cone) +constraints.add("friction_cone", friction_cone) # Create the contact sequence contact_sequence = robotoc.ContactSequence(robot) diff --git a/examples/iiwa14/config_space_ocp.cpp b/examples/iiwa14/config_space_ocp.cpp index cd1b893c6..0ea15c1e0 100644 --- a/examples/iiwa14/config_space_ocp.cpp +++ b/examples/iiwa14/config_space_ocp.cpp @@ -38,7 +38,7 @@ int main(int argc, char *argv[]) { config_cost->set_v_weight(Eigen::VectorXd::Constant(robot.dimv(), 0.01)); config_cost->set_v_weight_terminal(Eigen::VectorXd::Constant(robot.dimv(), 0.01)); config_cost->set_a_weight(Eigen::VectorXd::Constant(robot.dimv(), 0.01)); - cost->push_back(config_cost); + cost->add("config_cost", config_cost); // Create joint constraints. const double barrier_param = 1.0e-03; @@ -50,12 +50,12 @@ int main(int argc, char *argv[]) { auto joint_velocity_upper = std::make_shared(robot); auto joint_torques_lower = std::make_shared(robot); auto joint_torques_upper = std::make_shared(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); // Create the OCP solver for unconstrained rigid-body systems. const double T = 3; diff --git a/examples/iiwa14/ocp_benchmark.cpp b/examples/iiwa14/ocp_benchmark.cpp index 7e80a6c86..9f0965835 100644 --- a/examples/iiwa14/ocp_benchmark.cpp +++ b/examples/iiwa14/ocp_benchmark.cpp @@ -36,7 +36,7 @@ int main() { config_cost->set_v_weight_terminal(Eigen::VectorXd::Constant(robot.dimv(), 0.1)); config_cost->set_a_weight(Eigen::VectorXd::Constant(robot.dimv(), 0.01)); config_cost->set_u_weight(Eigen::VectorXd::Constant(robot.dimv(), 0.0)); - cost->push_back(config_cost); + cost->add("config_cost", config_cost); // Create joint constraints. const double barrier_param = 1.0e-03; @@ -48,12 +48,12 @@ int main() { auto joint_velocity_upper = std::make_shared(robot); auto joint_torques_lower = std::make_shared(robot); auto joint_torques_upper = std::make_shared(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); // Create the OCP solver for unconstrained rigid-body systems. auto contact_sequence = std::make_shared(robot); diff --git a/examples/iiwa14/python/config_space_ocp.py b/examples/iiwa14/python/config_space_ocp.py index abd2f71b7..ad9344a23 100644 --- a/examples/iiwa14/python/config_space_ocp.py +++ b/examples/iiwa14/python/config_space_ocp.py @@ -21,7 +21,7 @@ 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) # Create joint constraints. constraints = robotoc.Constraints(barrier_param=1.0e-03, fraction_to_boundary_rule=0.995) @@ -31,12 +31,12 @@ 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) # Create the OCP solver for unconstrained rigid-body systems. T = 3.0 diff --git a/examples/iiwa14/task_space_ocp.cpp b/examples/iiwa14/task_space_ocp.cpp index 44d081d94..e1dfb36e4 100644 --- a/examples/iiwa14/task_space_ocp.cpp +++ b/examples/iiwa14/task_space_ocp.cpp @@ -75,12 +75,12 @@ int main(int argc, char *argv[]) { 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(); auto task_cost = std::make_shared(robot, ee_frame, x6d_ref); task_cost->set_weight(Eigen::Vector3d::Constant(1000), Eigen::Vector3d::Constant(1000)); task_cost->set_weight_terminal(Eigen::Vector3d::Constant(1000), Eigen::Vector3d::Constant(1000)); - cost->push_back(task_cost); + cost->add("task_cost", task_cost); // Create joint constraints. const double barrier_param = 1.0e-03; @@ -92,12 +92,12 @@ int main(int argc, char *argv[]) { auto joint_velocity_upper = std::make_shared(robot); auto joint_torques_lower = std::make_shared(robot); auto joint_torques_upper = std::make_shared(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); // Create the OCP solver for unconstrained rigid-body systems. const double T = 6; diff --git a/examples/iiwa14/unconstr_ocp_benchmark.cpp b/examples/iiwa14/unconstr_ocp_benchmark.cpp index 4d4327937..7ad5532ca 100644 --- a/examples/iiwa14/unconstr_ocp_benchmark.cpp +++ b/examples/iiwa14/unconstr_ocp_benchmark.cpp @@ -36,7 +36,7 @@ int main() { config_cost->set_v_weight_terminal(Eigen::VectorXd::Constant(robot.dimv(), 0.1)); config_cost->set_a_weight(Eigen::VectorXd::Constant(robot.dimv(), 0.01)); config_cost->set_u_weight(Eigen::VectorXd::Constant(robot.dimv(), 0.0)); - cost->push_back(config_cost); + cost->add("config_cost", config_cost); // Create joint constraints. const double barrier_param = 1.0e-03; @@ -48,12 +48,12 @@ int main() { auto joint_velocity_upper = std::make_shared(robot); auto joint_torques_lower = std::make_shared(robot); auto joint_torques_upper = std::make_shared(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); // Create the OCP solver for unconstrained rigid-body systems. const double T = 1; diff --git a/examples/iiwa14/unconstr_parnmpc_benchmark.cpp b/examples/iiwa14/unconstr_parnmpc_benchmark.cpp index b87e75455..11ea1a0a9 100644 --- a/examples/iiwa14/unconstr_parnmpc_benchmark.cpp +++ b/examples/iiwa14/unconstr_parnmpc_benchmark.cpp @@ -36,7 +36,7 @@ int main() { config_cost->set_v_weight_terminal(Eigen::VectorXd::Constant(robot.dimv(), 0.1)); config_cost->set_a_weight(Eigen::VectorXd::Constant(robot.dimv(), 0.01)); config_cost->set_u_weight(Eigen::VectorXd::Constant(robot.dimv(), 0.0)); - cost->push_back(config_cost); + cost->add("config_cost", config_cost); // Create joint constraints. const double barrier_param = 1.0e-03; @@ -48,12 +48,12 @@ int main() { auto joint_velocity_upper = std::make_shared(robot); auto joint_torques_lower = std::make_shared(robot); auto joint_torques_upper = std::make_shared(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); // Create the ParNMPC solver for unconstrained rigid-body systems. const double T = 1; diff --git a/include/robotoc/constraints/constraint_component_base.hpp b/include/robotoc/constraints/constraint_component_base.hpp index 7dd989a3e..da59fee1e 100644 --- a/include/robotoc/constraints/constraint_component_base.hpp +++ b/include/robotoc/constraints/constraint_component_base.hpp @@ -1,6 +1,8 @@ #ifndef ROBOTOC_CONSTRAINT_COMPONENT_BASE_HPP_ #define ROBOTOC_CONSTRAINT_COMPONENT_BASE_HPP_ +#include + #include "Eigen/Core" #include "robotoc/robot/robot.hpp" @@ -29,7 +31,7 @@ enum class KinematicsLevel { /// @class ConstraintComponentBase /// @brief Base class for constraint components. /// -class ConstraintComponentBase { +class ConstraintComponentBase : public std::enable_shared_from_this { public: /// /// @brief Constructor. @@ -225,6 +227,15 @@ class ConstraintComponentBase { virtual void setFractionToBoundaryRule( const double fraction_to_boundary_rule) final; + /// + /// @brief Gets the shared ptr of this object as the specified type. If this + /// fails in dynamic casting, throws an exception. + /// @tparam Derived The derived type. + /// @return shared ptr of this object as the specified type. + /// + template + std::shared_ptr as_shared_ptr(); + protected: /// /// @brief Computes the residual in the complementarity slackness between diff --git a/include/robotoc/constraints/constraint_component_base.hxx b/include/robotoc/constraints/constraint_component_base.hxx index 074b24d58..b797ad398 100644 --- a/include/robotoc/constraints/constraint_component_base.hxx +++ b/include/robotoc/constraints/constraint_component_base.hxx @@ -5,6 +5,7 @@ #include "robotoc/constraints/pdipm.hpp" #include +#include namespace robotoc { @@ -35,6 +36,17 @@ inline void ConstraintComponentBase::updateDual(ConstraintComponentData& data, } +template +inline std::shared_ptr ConstraintComponentBase::as_shared_ptr() { + auto ptr = shared_from_this(); + auto derived_ptr = std::dynamic_pointer_cast(ptr); + if (derived_ptr == nullptr) { + throw std::runtime_error("[ConstraintComponentBase] runtime error: failed in down-casting!"); + } + return derived_ptr; +} + + inline void ConstraintComponentBase::computeComplementarySlackness( ConstraintComponentData& data) const { pdipm::computeComplementarySlackness(barrier_, data); diff --git a/include/robotoc/constraints/constraints.hpp b/include/robotoc/constraints/constraints.hpp index 72608d1d7..73ce586d9 100644 --- a/include/robotoc/constraints/constraints.hpp +++ b/include/robotoc/constraints/constraints.hpp @@ -3,6 +3,8 @@ #include #include +#include +#include #include "robotoc/robot/robot.hpp" #include "robotoc/robot/contact_status.hpp" @@ -67,20 +69,50 @@ class Constraints { Constraints& operator=(Constraints&&) noexcept = default; /// - /// @brief Appends a constraint component to the cost function. - /// @param[in, out] constraint_component Shared pointer to the constraint - /// component appended to the constraints. The internal barrier parameter and - /// the parameter of the fraction-to-boundary rule will be overitten. + /// @brief Checks if thsi has a constraint component of the specified name. + /// @param[in] name Name of the constraint component. + /// @return treu if a constraint component of the specified name exists. /// - void push_back(ConstraintComponentBasePtr constraint_component); + bool exist(const std::string& name) const; /// - /// @brief Appends a constraint component to the cost function. - /// @param[in, out] constraint_component Shared pointer to the constraint - /// component appended to the constraints. The internal barrier parameter and - /// the parameter of the fraction-to-boundary rule will be overitten. + /// @brief Adds a constraint component. If a component of the same name + /// exists, throws an exeption. + /// @param[in] name Name of the constraint component. + /// @param[in] constraint shared pointer to the constraint component. /// - void push_back(ImpactConstraintComponentBasePtr constraint_component); + void add(const std::string& name, ConstraintComponentBasePtr constraint); + + /// + /// @brief Adds a constraint component. If a component of the same name + /// exists, throws an exeption. + /// @param[in] name Name of the constraint component. + /// @param[in] constraint shared pointer to the constraint component. + /// + void add(const std::string& name, ImpactConstraintComponentBasePtr constraint); + + /// + /// @brief Erases a constraint component. If a component of the specified + /// name does not exist, throws an exeption. + /// @param[in] name Name of the constraint component. + /// + void erase(const std::string& name); + + /// + /// @brief Gets a constraint component. If a component of the specified + /// name does not exist, throws an exeption. + /// @param[in] name Name of the constraint component. + /// @return Shared ptr to the specified constraint component. + /// + ConstraintComponentBasePtr getConstraintComponent(const std::string& name) const; + + /// + /// @brief Gets a constraint component. If a component of the specified + /// name does not exist, throws an exeption. + /// @param[in] name Name of the constraint component. + /// @return Shared ptr to the specified constraint component. + /// + ImpactConstraintComponentBasePtr getImpactConstraintComponent(const std::string& name) const; /// /// @brief Clears constraints by removing all components. @@ -298,11 +330,55 @@ class Constraints { /// double getFractionToBoundaryRule() const; + /// + /// @brief Gets a list of the position-level constraints. + /// @return Name list of the position-level constraints. + /// + std::vector getPositionLevelConstraintList() const; + + /// + /// @brief Gets a list of the velocity-level constraints. + /// @return Name list of the velocity-level constraints. + /// + std::vector getVelocityLevelConstraintList() const; + + /// + /// @brief Gets a list of the acceleration-level constraints. + /// @return Name list of the acceleration-level constraints. + /// + std::vector getAccelerationLevelConstraintList() const; + + /// + /// @brief Gets a list of the impact-level constraints. + /// @return Name list of acceleration impact-level constraints. + /// + std::vector getImpactLevelConstraintList() const; + + /// + /// @brief Gets a list of the constraints. + /// @return Name list of acceleration impact-level constraints. + /// + std::vector getConstraintList() const; + + /// + /// @brief Displays the constraints onto a ostream. + /// + void disp(std::ostream& os) const; + + friend std::ostream& operator<<(std::ostream& os, const Constraints& constraints); + + friend std::ostream& operator<<(std::ostream& os, + const std::shared_ptr& constraints); + private: std::vector position_level_constraints_, velocity_level_constraints_, acceleration_level_constraints_; std::vector impact_level_constraints_; + std::unordered_map position_level_constraint_names_, + velocity_level_constraint_names_, + acceleration_level_constraint_names_, + impact_level_constraint_names_; double barrier_, fraction_to_boundary_rule_; }; diff --git a/include/robotoc/constraints/impact_constraint_component_base.hpp b/include/robotoc/constraints/impact_constraint_component_base.hpp index 7b8916948..abd145c89 100644 --- a/include/robotoc/constraints/impact_constraint_component_base.hpp +++ b/include/robotoc/constraints/impact_constraint_component_base.hpp @@ -1,6 +1,8 @@ #ifndef ROBOTOC_IMPACT_CONSTRAINT_COMPONENT_BASE_HPP_ #define ROBOTOC_IMPACT_CONSTRAINT_COMPONENT_BASE_HPP_ +#include + #include "Eigen/Core" #include "robotoc/robot/robot.hpp" @@ -19,7 +21,7 @@ namespace robotoc { /// @class ImpactConstraintComponentBase /// @brief Base class for impact constraint components. /// -class ImpactConstraintComponentBase { +class ImpactConstraintComponentBase : public std::enable_shared_from_this { public: /// /// @brief Constructor. @@ -217,6 +219,15 @@ class ImpactConstraintComponentBase { virtual void setFractionToBoundaryRule( const double fraction_to_boundary_rule) final; + /// + /// @brief Gets the shared ptr of this object as the specified type. If this + /// fails in dynamic casting, throws an exception. + /// @tparam Derived The derived type. + /// @return shared ptr of this object as the specified type. + /// + template + std::shared_ptr as_shared_ptr(); + protected: /// /// @brief Computes the residual in the complementarity slackness between diff --git a/include/robotoc/constraints/impact_constraint_component_base.hxx b/include/robotoc/constraints/impact_constraint_component_base.hxx index 7d50e7396..2771d23b9 100644 --- a/include/robotoc/constraints/impact_constraint_component_base.hxx +++ b/include/robotoc/constraints/impact_constraint_component_base.hxx @@ -35,6 +35,17 @@ inline void ImpactConstraintComponentBase::updateDual( } +template +inline std::shared_ptr ImpactConstraintComponentBase::as_shared_ptr() { + auto ptr = shared_from_this(); + auto derived_ptr = std::dynamic_pointer_cast(ptr); + if (derived_ptr == nullptr) { + throw std::runtime_error("[ImpactConstraintComponentBase] runtime error: failed in down-casting!"); + } + return derived_ptr; +} + + inline void ImpactConstraintComponentBase::computeComplementarySlackness( ConstraintComponentData& data) const { pdipm::computeComplementarySlackness(barrier_, data); diff --git a/include/robotoc/cost/cost_function.hpp b/include/robotoc/cost/cost_function.hpp index 21ef224f2..b1ed9c02c 100644 --- a/include/robotoc/cost/cost_function.hpp +++ b/include/robotoc/cost/cost_function.hpp @@ -1,9 +1,11 @@ #ifndef ROBOTOC_COST_FUNCTION_HPP_ #define ROBOTOC_COST_FUNCTION_HPP_ -#include #include #include +#include +#include +#include #include "Eigen/Core" @@ -93,11 +95,34 @@ class CostFunction { double discountTimeStep() const; /// - /// @brief Append a cost function component to the cost function. - /// @param[in] cost shared pointer to the cost function component appended - /// to the cost. + /// @brief Checks if thsi has a cost function component of the specified name. + /// @param[in] name Name of the cost function component. + /// @return treu if a cost function component of the specified name exists. + /// + bool exist(const std::string& name) const; + + /// + /// @brief Adds a cost function component. If a component of the same name + /// exists, throws an exeption. + /// @param[in] name Name of the cost function component. + /// @param[in] cost shared pointer to the cost function component. /// - void push_back(const CostFunctionComponentBasePtr& cost); + void add(const std::string& name, const CostFunctionComponentBasePtr& cost); + + /// + /// @brief Erases a cost function component. If a component of the specified + /// name does not exist, throws an exeption. + /// @param[in] name Name of the cost function component. + /// + void erase(const std::string& name); + + /// + /// @brief Gets a cost function component. If a component of the specified + /// name does not exist, throws an exeption. + /// @param[in] name Name of the cost function component. + /// @return Shared ptr to the specified cost function component. + /// + CostFunctionComponentBasePtr get(const std::string& name) const; /// /// @brief Clear cost function by removing all components. @@ -254,8 +279,26 @@ class CostFunction { SplitKKTResidual& kkt_residual, SplitKKTMatrix& kkt_matrix) const; + /// + /// @brief Gets a list of the cost components. + /// @return Name list of cost components. + /// + std::vector getCostComponentList() const; + + /// + /// @brief Displays the cost function onto a ostream. + /// + void disp(std::ostream& os) const; + + friend std::ostream& operator<<(std::ostream& os, + const CostFunction& cost_function); + + friend std::ostream& operator<<(std::ostream& os, + const std::shared_ptr& cost_function); + private: std::vector costs_; + std::unordered_map cost_names_; double discount(const double t0, const double t) const { assert(t >= t0); diff --git a/include/robotoc/cost/cost_function_component_base.hpp b/include/robotoc/cost/cost_function_component_base.hpp index 75ff4662e..75eb84453 100644 --- a/include/robotoc/cost/cost_function_component_base.hpp +++ b/include/robotoc/cost/cost_function_component_base.hpp @@ -1,6 +1,9 @@ #ifndef ROBOTOC_COST_FUNCTION_COMPONENT_BASE_HPP_ #define ROBOTOC_COST_FUNCTION_COMPONENT_BASE_HPP_ +#include +#include + #include "Eigen/Core" #include "robotoc/robot/robot.hpp" @@ -19,7 +22,7 @@ namespace robotoc { /// @class CostFunctionComponentBase /// @brief Base class of components of cost function. /// -class CostFunctionComponentBase { +class CostFunctionComponentBase : public std::enable_shared_from_this { public: /// /// @brief Default constructor. @@ -200,6 +203,22 @@ class CostFunctionComponentBase { const SplitSolution& s, SplitKKTMatrix& kkt_matrix) const = 0; + /// + /// @brief Gets the shared ptr of this object as the specified type. If this + /// fails in dynamic casting, throws an exception. + /// @tparam Derived The derived type. + /// @return shared ptr of this object as the specified type. + /// + template + std::shared_ptr as_shared_ptr() { + auto ptr = shared_from_this(); + auto derived_ptr = std::dynamic_pointer_cast(ptr); + if (derived_ptr == nullptr) { + throw std::runtime_error("[CostFunctionComponentBase] runtime error: failed in down-casting!"); + } + return derived_ptr; + } + }; } // namespace robotoc diff --git a/include/robotoc/sto/sto_cost_function.hpp b/include/robotoc/sto/sto_cost_function.hpp index 36671711b..6fe3e0091 100644 --- a/include/robotoc/sto/sto_cost_function.hpp +++ b/include/robotoc/sto/sto_cost_function.hpp @@ -3,6 +3,7 @@ #include #include +#include #include "Eigen/Core" @@ -56,11 +57,35 @@ class STOCostFunction { STOCostFunction& operator=(STOCostFunction&&) noexcept = default; /// - /// @brief Append a cost function component to the cost function. - /// @param[in] cost shared pointer to the switching tmie cost function - /// component appended to the cost. + /// @brief Checks if thsi has a STO cost function component of the specified + /// name. + /// @param[in] name Name of the STO cost function component. + /// @return treu if a STO cost function component of the specified name exists. /// - void push_back(const STOCostFunctionComponentBasePtr& cost); + bool exist(const std::string& name) const; + + /// + /// @brief Adds a STO cost function component. If a component of the same name + /// exists, throws an exeption. + /// @param[in] name Name of the STO cost function component. + /// @param[in] cost shared pointer to the STO cost function component. + /// + void add(const std::string& name, const STOCostFunctionComponentBasePtr& cost); + + /// + /// @brief Erases a STO cost function component. If a component of the + /// specified name does not exist, throws an exeption. + /// @param[in] name Name of the STO cost function component. + /// + void erase(const std::string& name); + + /// + /// @brief Gets a STO cost function component. If a component of the specified + /// name does not exist, throws an exeption. + /// @param[in] name Name of the STO cost function component. + /// @return Shared ptr to the specified STO cost function component. + /// + STOCostFunctionComponentBasePtr get(const std::string& name) const; /// /// @brief Clear cost function by removing all components. @@ -96,6 +121,8 @@ class STOCostFunction { private: std::vector costs_; + std::unordered_map cost_names_; + }; } // namespace robotoc diff --git a/include/robotoc/sto/sto_cost_function_component_base.hpp b/include/robotoc/sto/sto_cost_function_component_base.hpp index decad2392..a3f059ccf 100644 --- a/include/robotoc/sto/sto_cost_function_component_base.hpp +++ b/include/robotoc/sto/sto_cost_function_component_base.hpp @@ -1,10 +1,13 @@ #ifndef ROBOTOC_STO_COST_FUNCTION_COMPONENT_BASE_HPP_ #define ROBOTOC_STO_COST_FUNCTION_COMPONENT_BASE_HPP_ -#include "robotoc/ocp/time_discretization.hpp" +#include +#include #include "Eigen/Core" +#include "robotoc/ocp/time_discretization.hpp" + namespace robotoc { @@ -13,7 +16,7 @@ namespace robotoc { /// @brief Base class of components of the cost function of the switching time /// optimization (STO) problem. /// -class STOCostFunctionComponentBase { +class STOCostFunctionComponentBase : public std::enable_shared_from_this { public: /// /// @brief Default constructor. @@ -77,6 +80,22 @@ class STOCostFunctionComponentBase { virtual void evalCostHessian(const TimeDiscretization& time_discretization, Eigen::MatrixXd& Qtt) const = 0; + /// + /// @brief Gets the shared ptr of this object as the specified type. If this + /// fails in dynamic casting, throws an exception. + /// @tparam Derived The derived type. + /// @return shared ptr of this object as the specified type. + /// + template + std::shared_ptr as_shared_ptr() { + auto ptr = shared_from_this(); + auto derived_ptr = std::dynamic_pointer_cast(ptr); + if (derived_ptr == nullptr) { + throw std::runtime_error("[STOCostFunctionComponentBase] runtime error: failed in down-casting!"); + } + return derived_ptr; + } + }; } // namespace robotoc diff --git a/src/constraints/constraints.cpp b/src/constraints/constraints.cpp index 0c896b574..b29261327 100644 --- a/src/constraints/constraints.cpp +++ b/src/constraints/constraints.cpp @@ -13,6 +13,10 @@ Constraints::Constraints(const double barrier_param, velocity_level_constraints_(), acceleration_level_constraints_(), impact_level_constraints_(), + position_level_constraint_names_(), + velocity_level_constraint_names_(), + acceleration_level_constraint_names_(), + impact_level_constraint_names_(), barrier_(barrier_param), fraction_to_boundary_rule_(fraction_to_boundary_rule) { if (barrier_param <= 0) { @@ -30,40 +34,128 @@ Constraints::Constraints(const double barrier_param, } -void Constraints::push_back(ConstraintComponentBasePtr constraint_component) { - constraint_component->setBarrierParam(barrier_); - constraint_component->setFractionToBoundaryRule(fraction_to_boundary_rule_); - if (constraint_component->kinematicsLevel() +bool Constraints::exist(const std::string& name) const { + return ((position_level_constraint_names_.find(name) != position_level_constraint_names_.end()) + || (velocity_level_constraint_names_.find(name) != velocity_level_constraint_names_.end()) + || (acceleration_level_constraint_names_.find(name) != acceleration_level_constraint_names_.end()) + || (impact_level_constraint_names_.find(name) != impact_level_constraint_names_.end())); +} + + +void Constraints::add(const std::string& name, + ConstraintComponentBasePtr constraint) { + if (exist(name)) { + throw std::runtime_error("[Constraints] invalid argument: constraint component '" + name + "' already exists!"); + } + constraint->setBarrierParam(barrier_); + constraint->setFractionToBoundaryRule(fraction_to_boundary_rule_); + if (constraint->kinematicsLevel() == KinematicsLevel::PositionLevel) { - position_level_constraints_.push_back(constraint_component); + position_level_constraint_names_.emplace(name, position_level_constraints_.size()); + position_level_constraints_.push_back(constraint); } - else if (constraint_component->kinematicsLevel() + else if (constraint->kinematicsLevel() == KinematicsLevel::VelocityLevel) { - velocity_level_constraints_.push_back(constraint_component); + velocity_level_constraint_names_.emplace(name, velocity_level_constraints_.size()); + velocity_level_constraints_.push_back(constraint); + } + else if (constraint->kinematicsLevel() + == KinematicsLevel::AccelerationLevel) { + acceleration_level_constraint_names_.emplace(name, acceleration_level_constraints_.size()); + acceleration_level_constraints_.push_back(constraint); + } +} + + +void Constraints::add(const std::string& name, + ImpactConstraintComponentBasePtr constraint) { + if (exist(name)) { + throw std::runtime_error("[Constraints] invalid argument: constraint component '" + name + "' already exists!"); } - else if (constraint_component->kinematicsLevel() + constraint->setBarrierParam(barrier_); + constraint->setFractionToBoundaryRule(fraction_to_boundary_rule_); + if (constraint->kinematicsLevel() == KinematicsLevel::AccelerationLevel) { - acceleration_level_constraints_.push_back(constraint_component); + impact_level_constraint_names_.emplace(name, impact_level_constraints_.size()); + impact_level_constraints_.push_back(constraint); + } +} + + +void Constraints::erase(const std::string& name) { + if (!exist(name)) { + throw std::runtime_error("[Constraints] invalid argument: constraint component '" + name + "' does not exist!"); + } + if (position_level_constraint_names_.find(name) != position_level_constraint_names_.end()) { + const int index = position_level_constraint_names_.at(name); + position_level_constraint_names_.erase(name); + position_level_constraints_.erase(position_level_constraints_.begin()+index); + } + else if (velocity_level_constraint_names_.find(name) != velocity_level_constraint_names_.end()) { + const int index = velocity_level_constraint_names_.at(name); + velocity_level_constraint_names_.erase(name); + velocity_level_constraints_.erase(velocity_level_constraints_.begin()+index); + } + else if (acceleration_level_constraint_names_.find(name) != acceleration_level_constraint_names_.end()) { + const int index = acceleration_level_constraint_names_.at(name); + acceleration_level_constraint_names_.erase(name); + acceleration_level_constraints_.erase(acceleration_level_constraints_.begin()+index); + } + else if (impact_level_constraint_names_.find(name) != impact_level_constraint_names_.end()) { + const int index = impact_level_constraint_names_.at(name); + impact_level_constraint_names_.erase(name); + impact_level_constraints_.erase(impact_level_constraints_.begin()+index); + } +} + + +std::shared_ptr +Constraints::getConstraintComponent(const std::string& name) const { + if (!exist(name)) { + throw std::runtime_error("[Constraints] invalid argument: constraint component '" + name + "' does not exist!"); + } + if (position_level_constraint_names_.find(name) != position_level_constraint_names_.end()) { + const int index = position_level_constraint_names_.at(name); + return position_level_constraints_.at(index); + } + else if (velocity_level_constraint_names_.find(name) != velocity_level_constraint_names_.end()) { + const int index = velocity_level_constraint_names_.at(name); + return velocity_level_constraints_.at(index); + } + else if (acceleration_level_constraint_names_.find(name) != acceleration_level_constraint_names_.end()) { + const int index = acceleration_level_constraint_names_.at(name); + return acceleration_level_constraints_.at(index); + } + else { + throw std::runtime_error("[Constraints] invalid argument: constraint component '" + name + "' does not exist!"); } } -void Constraints::push_back(ImpactConstraintComponentBasePtr constraint_component) { - constraint_component->setBarrierParam(barrier_); - constraint_component->setFractionToBoundaryRule(fraction_to_boundary_rule_); - if (constraint_component->kinematicsLevel() - == KinematicsLevel::AccelerationLevel) { - // Only the acceleration level constraints are valid at impact stage. - impact_level_constraints_.push_back(constraint_component); +std::shared_ptr +Constraints::getImpactConstraintComponent(const std::string& name) const { + if (!exist(name)) { + throw std::runtime_error("[Constraints] invalid argument: constraint component '" + name + "' does not exist!"); + } + if (impact_level_constraint_names_.find(name) != impact_level_constraint_names_.end()) { + const int index = impact_level_constraint_names_.at(name); + return impact_level_constraints_.at(index); + } + else { + throw std::runtime_error("[Constraints] invalid argument: constraint component '" + name + "' does not exist!"); } } void Constraints::clear() { - constraintsimpl::clear(position_level_constraints_); - constraintsimpl::clear(velocity_level_constraints_); - constraintsimpl::clear(acceleration_level_constraints_); - constraintsimpl::clear(impact_level_constraints_); + position_level_constraints_.clear(); + velocity_level_constraints_.clear(); + acceleration_level_constraints_.clear(); + impact_level_constraints_.clear(); + position_level_constraint_names_.clear(); + velocity_level_constraint_names_.clear(); + acceleration_level_constraint_names_.clear(); + impact_level_constraint_names_.clear(); } @@ -445,4 +537,92 @@ double Constraints::getFractionToBoundaryRule() const { return fraction_to_boundary_rule_; } + +std::vector Constraints::getPositionLevelConstraintList() const { + std::vector constraint_list; + for (std::pair e : position_level_constraint_names_) { + constraint_list.push_back(e.first); + } + return constraint_list; +} + + +std::vector Constraints::getVelocityLevelConstraintList() const { + std::vector constraint_list; + for (std::pair e : velocity_level_constraint_names_) { + constraint_list.push_back(e.first); + } + return constraint_list; +} + + +std::vector Constraints::getAccelerationLevelConstraintList() const { + std::vector constraint_list; + for (std::pair e : acceleration_level_constraint_names_) { + constraint_list.push_back(e.first); + } + return constraint_list; +} + + +std::vector Constraints::getImpactLevelConstraintList() const { + std::vector constraint_list; + for (std::pair e : impact_level_constraint_names_) { + constraint_list.push_back(e.first); + } + return constraint_list; +} + + +std::vector Constraints::getConstraintList() const { + std::vector constraint_list; + for (const auto& e : getPositionLevelConstraintList()) { + constraint_list.push_back(e); + } + for (const auto& e : getVelocityLevelConstraintList()) { + constraint_list.push_back(e); + } + for (const auto& e : getAccelerationLevelConstraintList()) { + constraint_list.push_back(e); + } + for (const auto& e : getImpactLevelConstraintList()) { + constraint_list.push_back(e); + } + return constraint_list; +} + + +void Constraints::disp(std::ostream& os) const { + os << "Constraints:" << "\n"; + os << " position-level constraints:" << "\n"; + for (const auto& e : getPositionLevelConstraintList()) { + os << " - " << e << "\n"; + } + os << " velocity-level constraints:" << "\n"; + for (const auto& e : getVelocityLevelConstraintList()) { + os << " - " << e << "\n"; + } + os << " acceleration-level constraints:" << "\n"; + for (const auto& e : getAccelerationLevelConstraintList()) { + os << " - " << e << "\n"; + } + os << " impact-level constraints:" << "\n"; + for (const auto& e : getImpactLevelConstraintList()) { + os << " - " << e << "\n"; + } +} + + +std::ostream& operator<<(std::ostream& os, const Constraints& constraints) { + constraints.disp(os); + return os; +} + + +std::ostream& operator<<(std::ostream& os, + const std::shared_ptr& constraints) { + constraints->disp(os); + return os; +} + } // namespace robotoc \ No newline at end of file diff --git a/src/cost/cost_function.cpp b/src/cost/cost_function.cpp index 244bb3141..12cdf2a6e 100644 --- a/src/cost/cost_function.cpp +++ b/src/cost/cost_function.cpp @@ -10,6 +10,7 @@ namespace robotoc { CostFunction::CostFunction(const double discount_factor, const double discount_time_step) : costs_(), + cost_names_(), discount_factor_(discount_factor), discount_time_step_(discount_time_step), discounted_cost_(true) { @@ -27,6 +28,7 @@ CostFunction::CostFunction(const double discount_factor, CostFunction::CostFunction() : costs_(), + cost_names_(), discount_factor_(1.0), discount_time_step_(0.0), discounted_cost_(false) { @@ -68,13 +70,44 @@ double CostFunction::discountTimeStep() const { } -void CostFunction::push_back(const CostFunctionComponentBasePtr& cost) { +bool CostFunction::exist(const std::string& name) const { + return (cost_names_.find(name) != cost_names_.end()); +} + + +void CostFunction::add(const std::string& name, + const CostFunctionComponentBasePtr& cost) { + if (exist(name)) { + throw std::runtime_error("[CostFunction] invalid argument: cost component '" + name + "' already exists!"); + } + cost_names_.emplace(name, costs_.size()); costs_.push_back(cost); } +void CostFunction::erase(const std::string& name) { + if (!exist(name)) { + throw std::runtime_error("[CostFunction] invalid argument: cost component '" + name + "' does not exist!"); + } + const int index = cost_names_.at(name); + cost_names_.erase(name); + costs_.erase(costs_.begin()+index); +} + + +std::shared_ptr +CostFunction::get(const std::string& name) const { + if (!exist(name)) { + throw std::runtime_error("[CostFunction] invalid argument: cost component '" + name + "' does not exist!"); + } + const int index = cost_names_.at(name); + return costs_.at(index); +} + + void CostFunction::clear() { costs_.clear(); + cost_names_.clear(); } @@ -293,4 +326,34 @@ double CostFunction::quadratizeImpactCost(Robot& robot, return l; } + +std::vector CostFunction::getCostComponentList() const { + std::vector cost_component_list; + for (std::pair e : cost_names_) { + cost_component_list.push_back(e.first); + } + return cost_component_list; +} + + +void CostFunction::disp(std::ostream& os) const { + os << "CostFunction:" << "\n"; + for (const auto& e : getCostComponentList()) { + os << " - " << e << "\n"; + } +} + + +std::ostream& operator<<(std::ostream& os, const CostFunction& cost_function) { + cost_function.disp(os); + return os; +} + + +std::ostream& operator<<(std::ostream& os, + const std::shared_ptr& cost_function) { + cost_function->disp(os); + return os; +} + } // namespace robotoc \ No newline at end of file diff --git a/src/mpc/mpc_biped_walk.cpp b/src/mpc/mpc_biped_walk.cpp index ef79e6dfc..2dd89da7c 100644 --- a/src/mpc/mpc_biped_walk.cpp +++ b/src/mpc/mpc_biped_walk.cpp @@ -66,11 +66,11 @@ MPCBipedWalk::MPCBipedWalk(const Robot& robot, const double T, const int N) R_foot_cost_->set_weight(Eigen::Vector3d::Constant(1.0e04)); com_cost_ = std::make_shared(robot, com_ref_); com_cost_->set_weight(Eigen::Vector3d::Constant(1.0e03)); - cost_->push_back(config_cost_); - cost_->push_back(base_rot_cost_); - cost_->push_back(L_foot_cost_); - cost_->push_back(R_foot_cost_); - cost_->push_back(com_cost_); + cost_->add("config_cost", config_cost_); + cost_->add("base_rot_cost", base_rot_cost_); + cost_->add("L_foot_cost", L_foot_cost_); + cost_->add("R_foot_cost", R_foot_cost_); + cost_->add("com_cost", com_cost_); // create constraints auto joint_position_lower = std::make_shared(robot); auto joint_position_upper = std::make_shared(robot); @@ -82,14 +82,14 @@ MPCBipedWalk::MPCBipedWalk(const Robot& robot, const double T, const int N) const double Y = 0.05; contact_wrench_cone_ = std::make_shared(robot, X, Y); impact_wrench_cone_ = std::make_shared(robot, X, Y); - 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(contact_wrench_cone_); - constraints_->push_back(impact_wrench_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("contact_wrench_cone_", contact_wrench_cone_); + constraints_->add("impact_wrench_cone_", impact_wrench_cone_); // create contact status cs_standing_.activateContacts(std::vector({0, 1})); cs_right_swing_.activateContacts(std::vector({0})); diff --git a/src/mpc/mpc_crawl.cpp b/src/mpc/mpc_crawl.cpp index 50945d310..b9c23af84 100644 --- a/src/mpc/mpc_crawl.cpp +++ b/src/mpc/mpc_crawl.cpp @@ -74,13 +74,13 @@ MPCCrawl::MPCCrawl(const Robot& robot, const double T, const int N) RH_foot_cost_->set_weight(Eigen::Vector3d::Constant(1.0e04)); com_cost_ = std::make_shared(robot, com_ref_); com_cost_->set_weight(Eigen::Vector3d::Constant(1.0e03)); - cost_->push_back(config_cost_); - cost_->push_back(base_rot_cost_); - cost_->push_back(LF_foot_cost_); - cost_->push_back(LH_foot_cost_); - cost_->push_back(RF_foot_cost_); - cost_->push_back(RH_foot_cost_); - cost_->push_back(com_cost_); + cost_->add("config_cost", config_cost_); + cost_->add("base_rot_cost", base_rot_cost_); + cost_->add("LF_foot_cost", LF_foot_cost_); + cost_->add("LH_foot_cost", LH_foot_cost_); + cost_->add("RF_foot_cost", RF_foot_cost_); + cost_->add("RH_foot_cost", RH_foot_cost_); + cost_->add("com_cost", com_cost_); // create constraints auto joint_position_lower = std::make_shared(robot); auto joint_position_upper = std::make_shared(robot); @@ -89,13 +89,13 @@ MPCCrawl::MPCCrawl(const Robot& robot, const double T, const int N) auto joint_torques_lower = std::make_shared(robot); auto joint_torques_upper = std::make_shared(robot); friction_cone_ = std::make_shared(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_); // init contact status cs_standing_.activateContacts(std::vector({0, 1, 2, 3})); cs_lf_.activateContacts(std::vector({1, 2, 3})); diff --git a/src/mpc/mpc_flying_trot.cpp b/src/mpc/mpc_flying_trot.cpp index d14921b78..65b012852 100644 --- a/src/mpc/mpc_flying_trot.cpp +++ b/src/mpc/mpc_flying_trot.cpp @@ -72,13 +72,13 @@ MPCFlyingTrot::MPCFlyingTrot(const Robot& robot, const double T, const int N) RH_foot_cost_->set_weight(Eigen::Vector3d::Constant(1.0e04)); com_cost_ = std::make_shared(robot, com_ref_); com_cost_->set_weight(Eigen::Vector3d::Constant(1.0e05)); - cost_->push_back(config_cost_); - cost_->push_back(base_rot_cost_); - cost_->push_back(LF_foot_cost_); - cost_->push_back(LH_foot_cost_); - cost_->push_back(RF_foot_cost_); - cost_->push_back(RH_foot_cost_); - cost_->push_back(com_cost_); + cost_->add("config_cost", config_cost_); + cost_->add("base_rot_cost", base_rot_cost_); + cost_->add("LF_foot_cost", LF_foot_cost_); + cost_->add("LH_foot_cost", LH_foot_cost_); + cost_->add("RF_foot_cost", RF_foot_cost_); + cost_->add("RH_foot_cost", RH_foot_cost_); + cost_->add("com_cost", com_cost_); // create constraints auto joint_position_lower = std::make_shared(robot); auto joint_position_upper = std::make_shared(robot); @@ -88,13 +88,13 @@ MPCFlyingTrot::MPCFlyingTrot(const Robot& robot, const double T, const int N) auto joint_torques_upper = std::make_shared(robot); friction_cone_ = std::make_shared(robot); // auto impact_friction_cone = std::make_shared(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_); // constraints_->push_back(impact_friction_cone); // create contact status cs_standing_.activateContacts(std::vector({0, 1, 2, 3})); diff --git a/src/mpc/mpc_jump.cpp b/src/mpc/mpc_jump.cpp index b0aa85147..1bdc468a9 100644 --- a/src/mpc/mpc_jump.cpp +++ b/src/mpc/mpc_jump.cpp @@ -49,7 +49,7 @@ MPCJump::MPCJump(const Robot& robot, const double T, const int N) config_cost_->set_a_weight(Eigen::VectorXd::Constant(robot.dimv(), 1.0e-03)); config_cost_->set_v_weight_impact(Eigen::VectorXd::Constant(robot.dimv(), 10.0)); config_cost_->set_dv_weight_impact(Eigen::VectorXd::Constant(robot.dimv(), 1.0e-03)); - cost_->push_back(config_cost_); + cost_->add("config_cost", config_cost_); // create constraints auto joint_position_lower = std::make_shared(robot); auto joint_position_upper = std::make_shared(robot); @@ -57,22 +57,22 @@ MPCJump::MPCJump(const Robot& robot, const double T, const int N) auto joint_velocity_upper = std::make_shared(robot); auto joint_torques_lower = std::make_shared(robot); auto joint_torques_upper = std::make_shared(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); if (robot.pointContactFrames().size() == 4) { friction_cone_ = std::make_shared(robot); - constraints_->push_back(friction_cone_); + constraints_->add("friction_cone", friction_cone_); contact_wrench_cone_ = nullptr; } else if (robot.surfaceContactFrames().size() == 2) { const double X = 0.1; const double Y = 0.05; contact_wrench_cone_ = std::make_shared(robot, X, Y); - constraints_->push_back(contact_wrench_cone_); + constraints_->add("contact_wrench_cone_", contact_wrench_cone_); friction_cone_ = nullptr; } else { diff --git a/src/mpc/mpc_pace.cpp b/src/mpc/mpc_pace.cpp index 7a2d4be91..b6e5aebd9 100644 --- a/src/mpc/mpc_pace.cpp +++ b/src/mpc/mpc_pace.cpp @@ -72,13 +72,13 @@ MPCPace::MPCPace(const Robot& robot, const double T, const int N) RH_foot_cost_->set_weight(Eigen::Vector3d::Constant(1.0e04)); com_cost_ = std::make_shared(robot, com_ref_); com_cost_->set_weight(Eigen::Vector3d::Constant(1.0e03)); - cost_->push_back(config_cost_); - cost_->push_back(base_rot_cost_); - cost_->push_back(LF_foot_cost_); - cost_->push_back(LH_foot_cost_); - cost_->push_back(RF_foot_cost_); - cost_->push_back(RH_foot_cost_); - cost_->push_back(com_cost_); + cost_->add("config_cost", config_cost_); + cost_->add("base_rot_cost", base_rot_cost_); + cost_->add("LF_foot_cost", LF_foot_cost_); + cost_->add("LH_foot_cost", LH_foot_cost_); + cost_->add("RF_foot_cost", RF_foot_cost_); + cost_->add("RH_foot_cost", RH_foot_cost_); + cost_->add("com_cost", com_cost_); // create constraints auto joint_position_lower = std::make_shared(robot); auto joint_position_upper = std::make_shared(robot); @@ -88,13 +88,13 @@ MPCPace::MPCPace(const Robot& robot, const double T, const int N) auto joint_torques_upper = std::make_shared(robot); friction_cone_ = std::make_shared(robot); // auto impact_friction_cone = std::make_shared(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_); // constraints_->push_back(impact_friction_cone); // create contact status cs_standing_.activateContacts(std::vector({0, 1, 2, 3})); diff --git a/src/mpc/mpc_trot.cpp b/src/mpc/mpc_trot.cpp index 8091f526e..af08079e8 100644 --- a/src/mpc/mpc_trot.cpp +++ b/src/mpc/mpc_trot.cpp @@ -72,13 +72,13 @@ MPCTrot::MPCTrot(const Robot& robot, const double T, const int N) RH_foot_cost_->set_weight(Eigen::Vector3d::Constant(1.0e04)); com_cost_ = std::make_shared(robot, com_ref_); com_cost_->set_weight(Eigen::Vector3d::Constant(1.0e03)); - cost_->push_back(config_cost_); - cost_->push_back(base_rot_cost_); - cost_->push_back(LF_foot_cost_); - cost_->push_back(LH_foot_cost_); - cost_->push_back(RF_foot_cost_); - cost_->push_back(RH_foot_cost_); - cost_->push_back(com_cost_); + cost_->add("config_cost", config_cost_); + cost_->add("base_rot_cost", base_rot_cost_); + cost_->add("LF_foot_cost", LF_foot_cost_); + cost_->add("LH_foot_cost", LH_foot_cost_); + cost_->add("RF_foot_cost", RF_foot_cost_); + cost_->add("RH_foot_cost", RH_foot_cost_); + cost_->add("com_cost", com_cost_); // create constraints auto joint_position_lower = std::make_shared(robot); auto joint_position_upper = std::make_shared(robot); @@ -88,13 +88,13 @@ MPCTrot::MPCTrot(const Robot& robot, const double T, const int N) auto joint_torques_upper = std::make_shared(robot); friction_cone_ = std::make_shared(robot); // auto impact_friction_cone = std::make_shared(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_); // constraints_->push_back(impact_friction_cone); // create contact status cs_standing_.activateContacts(std::vector({0, 1, 2, 3})); diff --git a/src/sto/sto_cost_function.cpp b/src/sto/sto_cost_function.cpp index 824926754..a7aee42f3 100644 --- a/src/sto/sto_cost_function.cpp +++ b/src/sto/sto_cost_function.cpp @@ -8,13 +8,44 @@ STOCostFunction::STOCostFunction() } -void STOCostFunction::push_back(const STOCostFunctionComponentBasePtr& cost) { +bool STOCostFunction::exist(const std::string& name) const { + return (cost_names_.find(name) != cost_names_.end()); +} + + +void STOCostFunction::add(const std::string& name, + const STOCostFunctionComponentBasePtr& cost) { + if (exist(name)) { + throw std::runtime_error("[STOCostFunction] invalid argument: cost component '" + name + "' already exists!"); + } + cost_names_.emplace(name, costs_.size()); costs_.push_back(cost); } +void STOCostFunction::erase(const std::string& name) { + if (!exist(name)) { + throw std::runtime_error("[STOCostFunction] invalid argument: cost component '" + name + "' does not exist!"); + } + const int index = cost_names_.at(name); + cost_names_.erase(name); + costs_.erase(costs_.begin()+index); +} + + +std::shared_ptr +STOCostFunction::get(const std::string& name) const { + if (!exist(name)) { + throw std::runtime_error("[STOCostFunction] invalid argument: cost component '" + name + "' does not exist!"); + } + const int index = cost_names_.at(name); + return costs_.at(index); +} + + void STOCostFunction::clear() { costs_.clear(); + cost_names_.clear(); } diff --git a/test/constraints/constraints_test.cpp b/test/constraints/constraints_test.cpp index 162a31319..cb46fbc1a 100644 --- a/test/constraints/constraints_test.cpp +++ b/test/constraints/constraints_test.cpp @@ -63,15 +63,15 @@ std::shared_ptr ConstraintsTest::createConstraints(const Robot& rob auto joint_accel_upper = std::make_shared(robot, Eigen::VectorXd::Constant(robot.dimv(), 10)); auto friction_cone = std::make_shared(robot); auto constraints = std::make_shared(barrier_param, fraction_to_boundary_rule); - 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(joint_accel_lower); - constraints->push_back(joint_accel_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("joint_accel_lower", joint_accel_lower); + constraints->add("joint_accel_upper", joint_accel_upper); + constraints->add("friction_cone", friction_cone); return constraints; } @@ -115,6 +115,9 @@ void ConstraintsTest::timeStage0(Robot& robot, EXPECT_FALSE(kkt_residual.lf().isZero()); EXPECT_FALSE(kkt_matrix.Qff().isZero()); } + EXPECT_NO_THROW( + std::cout << constraints << std::endl; + ); } @@ -157,6 +160,9 @@ void ConstraintsTest::timeStage1(Robot& robot, EXPECT_FALSE(kkt_residual.lf().isZero()); EXPECT_FALSE(kkt_matrix.Qff().isZero()); } + EXPECT_NO_THROW( + std::cout << constraints << std::endl; + ); } @@ -199,6 +205,10 @@ void ConstraintsTest::timeStage2(Robot& robot, EXPECT_FALSE(kkt_residual.lf().isZero()); EXPECT_FALSE(kkt_matrix.Qff().isZero()); } + EXPECT_NO_THROW( + std::cout << constraints << std::endl; + std::cout << *constraints.get() << std::endl; + ); } @@ -230,9 +240,9 @@ TEST_F(ConstraintsTest, floatingBase) { TEST_F(ConstraintsTest, testParams) { auto robot = testhelper::CreateQuadrupedalRobot(0.001); - auto friction_cone = std::make_shared(robot); + auto friction_cone = std::make_shared(robot); auto constraints = std::make_shared(0.1, 0.5); - constraints->push_back(friction_cone); + constraints->add("friction_cone", friction_cone); EXPECT_DOUBLE_EQ(friction_cone->getBarrierParam(), 0.1); EXPECT_DOUBLE_EQ(friction_cone->getFractionToBoundaryRule(), 0.5); constraints->setBarrierParam(0.2); @@ -241,6 +251,21 @@ TEST_F(ConstraintsTest, testParams) { EXPECT_DOUBLE_EQ(friction_cone->getFractionToBoundaryRule(), 0.8); } + +TEST_F(ConstraintsTest, testDownCast) { + auto robot = testhelper::CreateQuadrupedalRobot(0.001); + auto constraints = createConstraints(robot); + auto constraint_component = constraints->getConstraintComponent("friction_cone"); + EXPECT_NO_THROW( + auto friction_cone = constraint_component->as_shared_ptr(); + ); + EXPECT_THROW( + auto joint_position_upper = constraint_component->as_shared_ptr(), + std::runtime_error + ); + +} + } // namespace robotoc diff --git a/test/cost/cost_function_test.cpp b/test/cost/cost_function_test.cpp index ce5b24c64..01638fe82 100644 --- a/test/cost/cost_function_test.cpp +++ b/test/cost/cost_function_test.cpp @@ -7,6 +7,7 @@ #include "robotoc/cost/cost_function.hpp" #include "robotoc/cost/cost_function_data.hpp" #include "robotoc/cost/configuration_space_cost.hpp" +#include "robotoc/cost/com_cost.hpp" #include "robotoc/core/split_solution.hpp" #include "robotoc/core/split_kkt_residual.hpp" #include "robotoc/core/split_kkt_matrix.hpp" @@ -56,16 +57,23 @@ void CostFunctionTest::testStageCost(Robot& robot) { kkt_res.lu.setRandom(); auto kkt_mat_ref = kkt_mat; auto kkt_res_ref = kkt_res; - const Eigen::VectorXd q_weight = Eigen::VectorXd::Random(dimv); - const Eigen::VectorXd v_weight = Eigen::VectorXd::Random(dimv); - const Eigen::VectorXd a_weight = Eigen::VectorXd::Random(dimv); - const Eigen::VectorXd u_weight = Eigen::VectorXd::Random(dimu); + const Eigen::VectorXd q_weight = Eigen::VectorXd::Random(dimv).array().abs().matrix(); + const Eigen::VectorXd v_weight = Eigen::VectorXd::Random(dimv).array().abs().matrix(); + const Eigen::VectorXd a_weight = Eigen::VectorXd::Random(dimv).array().abs().matrix(); + const Eigen::VectorXd u_weight = Eigen::VectorXd::Random(dimu).array().abs().matrix(); const Eigen::VectorXd q_ref = robot.generateFeasibleConfiguration(); const Eigen::VectorXd v_ref = Eigen::VectorXd::Random(dimv); const Eigen::VectorXd u_ref = Eigen::VectorXd::Random(dimu); auto config_cost = std::make_shared(robot); - non_discounted_cost->push_back(config_cost); - discounted_cost->push_back(config_cost); + config_cost->set_q_weight(q_weight); + config_cost->set_v_weight(v_weight); + config_cost->set_a_weight(a_weight); + config_cost->set_u_weight(u_weight); + config_cost->set_q_ref(q_ref); + config_cost->set_v_ref(v_ref); + config_cost->set_u_ref(u_ref); + non_discounted_cost->add("config_cost", config_cost); + discounted_cost->add("config_cost", config_cost); auto contact_status = robot.createContactStatus(); contact_status.setRandom(); @@ -78,9 +86,23 @@ void CostFunctionTest::testStageCost(Robot& robot) { const double non_discounted_value = non_discounted_cost->evalStageCost(robot, contact_status, data, grid_info, s); const double discounted_value = discounted_cost->evalStageCost(robot, contact_status, data, grid_info, s); - EXPECT_DOUBLE_EQ(non_discounted_value*std::pow(discount_factor, (grid_info.t-grid_info.t0)/discount_time_step), discounted_value); + // EXPECT_DOUBLE_EQ(non_discounted_value*std::pow(discount_factor, (grid_info.t-grid_info.t0)/discount_time_step), discounted_value); non_discounted_cost->setDiscountFactor(discount_factor, discount_time_step); EXPECT_DOUBLE_EQ(discounted_value, non_discounted_cost->evalStageCost(robot, contact_status, data, grid_info, s)); + + auto cost_component = non_discounted_cost->get("config_cost"); + EXPECT_NO_THROW( + auto config_cost_component = cost_component->as_shared_ptr(); + config_cost_component->set_q_weight(q_weight); + ); + EXPECT_THROW( + cost_component->as_shared_ptr(), + std::runtime_error + ); + EXPECT_NO_THROW( + std::cout << non_discounted_cost << std::endl; + std::cout << *non_discounted_cost.get() << std::endl; + ); } diff --git a/test/solver/ocp_solver_test.cpp b/test/solver/ocp_solver_test.cpp index c1721e164..210480787 100644 --- a/test/solver/ocp_solver_test.cpp +++ b/test/solver/ocp_solver_test.cpp @@ -64,7 +64,7 @@ TEST_F(OCPSolverTest, test) { config_cost->set_v_weight(Eigen::VectorXd::Constant(robot.dimv(), 1)); config_cost->set_v_weight_terminal(Eigen::VectorXd::Constant(robot.dimv(), 1)); config_cost->set_a_weight(Eigen::VectorXd::Constant(robot.dimv(), 0.01)); - cost->push_back(config_cost); + cost->add("config_cost", config_cost); auto local_contact_force_cost = std::make_shared(robot); std::vector f_weight, f_ref; for (int i=0; iset_f_weight(f_weight); local_contact_force_cost->set_f_ref(f_ref); - cost->push_back(local_contact_force_cost); + cost->add("local_contact_force_cost", local_contact_force_cost); // Create inequality constraints. auto constraints = std::make_shared(); @@ -88,13 +88,13 @@ TEST_F(OCPSolverTest, test) { auto joint_torques_lower = std::make_shared(robot); auto joint_torques_upper = std::make_shared(robot); auto friction_cone = std::make_shared(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(robot); diff --git a/test/solver/unconstr_ocp_solver_test.cpp b/test/solver/unconstr_ocp_solver_test.cpp index 609090e55..f97dee58a 100644 --- a/test/solver/unconstr_ocp_solver_test.cpp +++ b/test/solver/unconstr_ocp_solver_test.cpp @@ -48,7 +48,7 @@ TEST_F(UnconstrOCPSolverTest, test) { config_cost->set_v_weight(Eigen::VectorXd::Constant(robot.dimv(), 0.01)); config_cost->set_v_weight_terminal(Eigen::VectorXd::Constant(robot.dimv(), 0.01)); config_cost->set_a_weight(Eigen::VectorXd::Constant(robot.dimv(), 0.01)); - cost->push_back(config_cost); + cost->add("config_cost", config_cost); // Create joint constraints. const double barrier_param = 1.0e-03; @@ -60,12 +60,12 @@ TEST_F(UnconstrOCPSolverTest, test) { auto joint_velocity_upper = std::make_shared(robot); auto joint_torques_lower = std::make_shared(robot); auto joint_torques_upper = std::make_shared(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); // Create the OCP solver for unconstrained rigid-body systems. const double T = 1; diff --git a/test/solver/unconstr_parnmpc_solver_test.cpp b/test/solver/unconstr_parnmpc_solver_test.cpp index dcf3762cb..1605a7f03 100644 --- a/test/solver/unconstr_parnmpc_solver_test.cpp +++ b/test/solver/unconstr_parnmpc_solver_test.cpp @@ -48,7 +48,7 @@ TEST_F(UnconstrParNMPCSolverTest, test) { config_cost->set_v_weight(Eigen::VectorXd::Constant(robot.dimv(), 0.01)); config_cost->set_v_weight_terminal(Eigen::VectorXd::Constant(robot.dimv(), 0.01)); config_cost->set_a_weight(Eigen::VectorXd::Constant(robot.dimv(), 0.01)); - cost->push_back(config_cost); + cost->add("config_cost", config_cost); // Create joint constraints. const double barrier_param = 1.0e-03; @@ -60,12 +60,12 @@ TEST_F(UnconstrParNMPCSolverTest, test) { auto joint_velocity_upper = std::make_shared(robot); auto joint_torques_lower = std::make_shared(robot); auto joint_torques_upper = std::make_shared(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); // Create the ParNMPC solver for unconstrained rigid-body systems. const double T = 1; diff --git a/test/test_helper/constraints_factory.cpp b/test/test_helper/constraints_factory.cpp index 9ff6054d8..253ec179d 100644 --- a/test/test_helper/constraints_factory.cpp +++ b/test/test_helper/constraints_factory.cpp @@ -21,17 +21,17 @@ std::shared_ptr CreateConstraints(const Robot& robot) { auto torques_lower_limit = std::make_shared(robot); auto torques_upper_limit = std::make_shared(robot); auto constraints = std::make_shared(); - constraints->push_back(joint_upper_limit); - constraints->push_back(joint_lower_limit); - constraints->push_back(velocity_lower_limit); - constraints->push_back(velocity_upper_limit); - constraints->push_back(torques_lower_limit); - constraints->push_back(torques_upper_limit); + constraints->add("joint_upper_limit", joint_upper_limit); + constraints->add("joint_lower_limit", joint_lower_limit); + constraints->add("velocity_lower_limit", velocity_lower_limit); + constraints->add("velocity_upper_limit", velocity_upper_limit); + constraints->add("torques_lower_limit", torques_lower_limit); + constraints->add("torques_upper_limit", torques_upper_limit); if (robot.maxNumContacts() > 0) { auto friction_cone = std::make_shared(robot); auto impact_friction_cone = std::make_shared(robot); - constraints->push_back(friction_cone); - constraints->push_back(impact_friction_cone); + constraints->add("friction_cone", friction_cone); + constraints->add("impact_friction_cone", impact_friction_cone); } return constraints; } diff --git a/test/test_helper/cost_factory.cpp b/test/test_helper/cost_factory.cpp index 169a78c8c..b165812aa 100644 --- a/test/test_helper/cost_factory.cpp +++ b/test/test_helper/cost_factory.cpp @@ -55,7 +55,7 @@ std::shared_ptr CreateCost(const Robot& robot) { config_cost->set_q_weight_impact(q_weight_impact); config_cost->set_v_weight_impact(v_weight_impact); config_cost->set_dv_weight_impact(dv_weight_impact); - cost->push_back(config_cost); + cost->add("config_cost", config_cost); if (robot.maxNumContacts() > 0) { auto local_contact_force_cost = std::make_shared(robot); @@ -66,7 +66,7 @@ std::shared_ptr CreateCost(const Robot& robot) { } local_contact_force_cost->set_f_weight(f_weight); local_contact_force_cost->set_fi_weight(fi_weight); - cost->push_back(local_contact_force_cost); + cost->add("local_contact_force_cost", local_contact_force_cost); } const Eigen::VectorXd q0_ref = robot.generateFeasibleConfiguration(); @@ -77,7 +77,7 @@ std::shared_ptr CreateCost(const Robot& robot) { time_varying_config_cost->set_q_weight(q_weight); time_varying_config_cost->set_q_weight_terminal(q_weight_terminal); time_varying_config_cost->set_q_weight_impact(q_weight_impact); - cost->push_back(time_varying_config_cost); + cost->add("time_varying_config_cost", time_varying_config_cost); return cost; }