|
| 1 | +#include <pybind11/pybind11.h> |
| 2 | +#include <pybind11/eigen.h> |
| 3 | +#include <pybind11/stl.h> |
| 4 | + |
| 5 | +#include "cddp_core/constraint.hpp" |
| 6 | + |
| 7 | +namespace py = pybind11; |
| 8 | + |
| 9 | +template <typename T> |
| 10 | +using nodeleter = std::unique_ptr<T, py::nodelete>; |
| 11 | + |
| 12 | +void bind_constraints(py::module_& m) { |
| 13 | + // Base Constraint class - py::nodelete so CDDP can take ownership |
| 14 | + py::class_<cddp::Constraint, nodeleter<cddp::Constraint>>(m, "Constraint") |
| 15 | + .def("evaluate", &cddp::Constraint::evaluate, |
| 16 | + py::arg("state"), py::arg("control"), py::arg("index") = 0) |
| 17 | + .def("get_lower_bound", &cddp::Constraint::getLowerBound) |
| 18 | + .def("get_upper_bound", &cddp::Constraint::getUpperBound) |
| 19 | + .def("get_state_jacobian", &cddp::Constraint::getStateJacobian, |
| 20 | + py::arg("state"), py::arg("control"), py::arg("index") = 0) |
| 21 | + .def("get_control_jacobian", &cddp::Constraint::getControlJacobian, |
| 22 | + py::arg("state"), py::arg("control"), py::arg("index") = 0) |
| 23 | + .def("compute_violation", &cddp::Constraint::computeViolation, |
| 24 | + py::arg("state"), py::arg("control"), py::arg("index") = 0) |
| 25 | + .def("get_dual_dim", &cddp::Constraint::getDualDim) |
| 26 | + .def_property_readonly("name", &cddp::Constraint::getName); |
| 27 | + |
| 28 | + py::class_<cddp::ControlConstraint, cddp::Constraint, nodeleter<cddp::ControlConstraint>>(m, "ControlConstraint") |
| 29 | + .def(py::init<const Eigen::VectorXd&, const Eigen::VectorXd&, double>(), |
| 30 | + py::arg("lower_bound"), py::arg("upper_bound"), |
| 31 | + py::arg("scale_factor") = 1.0); |
| 32 | + |
| 33 | + py::class_<cddp::StateConstraint, cddp::Constraint, nodeleter<cddp::StateConstraint>>(m, "StateConstraint") |
| 34 | + .def(py::init<const Eigen::VectorXd&, const Eigen::VectorXd&, double>(), |
| 35 | + py::arg("lower_bound"), py::arg("upper_bound"), |
| 36 | + py::arg("scale_factor") = 1.0); |
| 37 | + |
| 38 | + py::class_<cddp::LinearConstraint, cddp::Constraint, nodeleter<cddp::LinearConstraint>>(m, "LinearConstraint") |
| 39 | + .def(py::init<const Eigen::MatrixXd&, const Eigen::VectorXd&, double>(), |
| 40 | + py::arg("A"), py::arg("b"), py::arg("scale_factor") = 1.0); |
| 41 | + |
| 42 | + py::class_<cddp::BallConstraint, cddp::Constraint, nodeleter<cddp::BallConstraint>>(m, "BallConstraint") |
| 43 | + .def(py::init<double, const Eigen::VectorXd&, double>(), |
| 44 | + py::arg("radius"), py::arg("center"), |
| 45 | + py::arg("scale_factor") = 1.0) |
| 46 | + .def("get_center", &cddp::BallConstraint::getCenter); |
| 47 | + |
| 48 | + py::class_<cddp::PoleConstraint, cddp::Constraint, nodeleter<cddp::PoleConstraint>>(m, "PoleConstraint") |
| 49 | + .def(py::init<const Eigen::VectorXd&, char, double, double, double>(), |
| 50 | + py::arg("center"), py::arg("direction"), |
| 51 | + py::arg("radius"), py::arg("length"), |
| 52 | + py::arg("scale_factor") = 1.0); |
| 53 | + |
| 54 | + py::class_<cddp::SecondOrderConeConstraint, cddp::Constraint, nodeleter<cddp::SecondOrderConeConstraint>>(m, "SecondOrderConeConstraint") |
| 55 | + .def(py::init<const Eigen::Vector3d&, const Eigen::Vector3d&, double, double, const std::string&>(), |
| 56 | + py::arg("cone_origin"), py::arg("opening_direction"), |
| 57 | + py::arg("cone_angle_fov"), py::arg("epsilon") = 1e-6, |
| 58 | + py::arg("name") = "SecondOrderConeConstraint"); |
| 59 | + |
| 60 | + py::class_<cddp::ThrustMagnitudeConstraint, cddp::Constraint, nodeleter<cddp::ThrustMagnitudeConstraint>>(m, "ThrustMagnitudeConstraint") |
| 61 | + .def(py::init<double, double, double>(), |
| 62 | + py::arg("min_thrust"), py::arg("max_thrust"), |
| 63 | + py::arg("epsilon") = 1e-6); |
| 64 | + |
| 65 | + py::class_<cddp::MaxThrustMagnitudeConstraint, cddp::Constraint, nodeleter<cddp::MaxThrustMagnitudeConstraint>>(m, "MaxThrustMagnitudeConstraint") |
| 66 | + .def(py::init<double, double>(), |
| 67 | + py::arg("max_thrust"), py::arg("epsilon") = 1e-6); |
| 68 | +} |
0 commit comments