Skip to content

Commit c07b602

Browse files
vweirensryan4984
vweirens
and
ryan4984
authored
Cpp control (#25)
* initial commit for control interface, currently not working. * more changes trying to get things to work * more changes, no progress * refactor into 1 module * fixing issues with interface * minor changes to control input mat * committing interface changes to accommodate cpp branch changes * start to update basic dynamics structure * refactor dynamics models * committing basic.init and cwh test changes * control changes to get cpp to work * double integrator cpp working stuff * adding interface changes to state control * interface changes for state control * removed a single d * changes to state control params * removed an s * wrong brackets * addding case to dynamics constructors * bug fixes for double int, filters, and some cwh * cwh fixes * get input mat fixes and cwh constructer change * cwh constructor bugfix * reshape prop state outputs * toml changes for release --------- Co-authored-by: ryan4984 <[email protected]>
1 parent 8a0c4c0 commit c07b602

33 files changed

+2855
-2421
lines changed

‎interface/CMakeLists.txt‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,10 @@
11
# see https://stackoverflow.com/questions/53762552/with-pybind11-how-to-split-my-code-into-multiple-modules-files for multiple directory info
22

3+
#---------------------------------------------------------------------------
4+
# setup control directory
5+
#---------------------------------------------------------------------------
6+
add_subdirectory(control)
7+
38
#---------------------------------------------------------------------------
49
# setup Dynamics directory
510
#---------------------------------------------------------------------------

‎interface/control/CMakeLists.txt‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
pybind11_add_module(_control MODULE
2+
Initialize.cpp
3+
Interface.cpp
4+
Parameters.cpp
5+
StateControl.cpp
6+
Common.h
7+
${CMAKE_CURRENT_SOURCE_DIR}/../Macros.h
8+
)
9+
target_compile_definitions(_control PRIVATE VERSION_INFO=${PROJECT_VERSION})
10+
target_link_libraries(_control
11+
PRIVATE
12+
Eigen3::Eigen
13+
lager::gncpy
14+
)
15+
compiler_options(COMPILE_TARGET _control)
16+
install(TARGETS _control
17+
DESTINATION
18+
${SKBUILD_PROJECT_NAME}/control
19+
)

‎interface/control/Common.h‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
#pragma once
2+
#include <Eigen/Dense>
3+
#include <gncpy/control/Parameters.h>
4+
#include <pybind11/pybind11.h>
5+
#include <pybind11/eigen.h>
6+
7+
void initInterface(pybind11::module&);
8+
void initParameters(pybind11::module&);
9+
void initStateControl(pybind11::module&);
10+
11+
#define GNCPY_CONTROL_ILINEARCONTROLMODEL_INTERFACE(cName) \
12+
.def("get_input_mat", pybind11::overload_cast<double, const lager::gncpy::control::ControlParams*>(&cName::getInputMat, pybind11::const_), \
13+
pybind11::arg("timestep"), pybind11::arg_v("params", static_cast<lager::gncpy::control::ControlParams *>(nullptr), "lager::gncpy::control::ControlParams*=nullptr")) \
14+
.def("get_control_inputs", pybind11::overload_cast<double, const Eigen::VectorXd&, const lager::gncpy::control::ControlParams*>(&cName::getControlInput, pybind11::const_), \
15+
pybind11::arg("state"), pybind11::arg("input"), pybind11::arg_v("params", static_cast<lager::gncpy::control::ControlParams *>(nullptr), "lager::gncpy::control::ControlParams*=nullptr"))
16+

‎interface/control/Initialize.cpp‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
#include <pybind11/pybind11.h>
2+
#include "Common.h"
3+
#include "../Macros.h"
4+
5+
6+
PYBIND11_MODULE(_control, m) {
7+
8+
initInterface(m);
9+
initParameters(m);
10+
11+
initStateControl(m);
12+
13+
#ifdef VERSION_INFO
14+
m.attr("__version__") = MACRO_STRINGIFY(VERSION_INFO);
15+
#else
16+
m.attr("__version__") = "dev";
17+
#endif
18+
}

‎interface/control/Interface.cpp‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
#include <pybind11/pybind11.h>
2+
#include <gncpy/control/IControlModel.h>
3+
#include <gncpy/control/ILinearControlModel.h>
4+
#include "Common.h"
5+
#include "../Macros.h"
6+
7+
8+
namespace py = pybind11;
9+
10+
// see https://pybind11.readthedocs.io/en/stable/advanced/classes.html#binding-classes-with-template-parameters for pybind with template class
11+
// see https://stackoverflow.com/questions/62854830/error-by-wrapping-c-abstract-class-with-pybind11 for ImportError: generic_type: type "Derived" referenced unknown base type "Base" error
12+
void initInterface(py::module& m) {
13+
using namespace lager;
14+
15+
// define these so the inherited classes import ok
16+
17+
GNCPY_PY_BASE_CLASS(gncpy::control::IControlModel)(m, "IControlModel");
18+
GNCPY_PY_CHILD_CLASS(gncpy::control::ILinearControlModel, gncpy::control::IControlModel) (m, "ILinearControlModel");
19+
}

‎interface/control/Parameters.cpp‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
#include <pybind11/pybind11.h>
2+
#include <gncpy/control/Parameters.h>
3+
#include "Common.h"
4+
#include "../Macros.h"
5+
6+
namespace py = pybind11;
7+
8+
// see https://pybind11.readthedocs.io/en/stable/advanced/classes.html#binding-classes-with-template-parameters for pybind with template class
9+
// see https://stackoverflow.com/questions/62854830/error-by-wrapping-c-abstract-class-with-pybind11 for ImportError: generic_type: type "Derived" referenced unknown base type "Base" error
10+
void initParameters(py::module& m) {
11+
12+
using namespace lager;
13+
14+
GNCPY_PY_BASE_CLASS(gncpy::control::ControlParams)(m, "ControlParams")
15+
.def(py::init())
16+
GNCPY_PICKLE(gncpy::control::ControlParams);
17+
18+
}

‎interface/control/StateControl.cpp‎

Lines changed: 54 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
#include <vector>
2+
3+
#include <pybind11/pybind11.h>
4+
#include <pybind11/stl.h>
5+
#include <gncpy/Exceptions.h>
6+
#include <gncpy/control/StateControl.h>
7+
#include <gncpy/control/Parameters.h>
8+
9+
#include "../Macros.h"
10+
#include "Common.h"
11+
12+
namespace py = pybind11;
13+
14+
// see https://pybind11.readthedocs.io/en/stable/advanced/classes.html#binding-classes-with-template-parameters for pybind with template class
15+
// see https://stackoverflow.com/questions/62854830/error-by-wrapping-c-abstract-class-with-pybind11 for ImportError: generic_type: type "Derived" referenced unknown base type "Base" error
16+
void initStateControl(py::module& m) {
17+
18+
using namespace lager;
19+
20+
GNCPY_PY_CHILD_CLASS(gncpy::control::StateControlParams, gncpy::control::ControlParams) (m, "StateControlParams")
21+
.def(py::init<const std::vector<uint8_t>&, const std::vector<uint8_t>&>())
22+
.def(py::init<const std::vector<uint8_t>&, const std::vector<uint8_t>&, const std::vector<double>&>())
23+
.def_readonly("cont_rows", &gncpy::control::StateControlParams::contRows, "Row indices of the state vector to control (read-only)")
24+
.def_readonly("cont_columns", &gncpy::control::StateControlParams::contColumns, "Column indices of the state vector to control (read-only)")
25+
.def_readonly("vals", &gncpy::control::StateControlParams::vals, "Values to be placed in the input matrix.")
26+
GNCPY_PICKLE(gncpy::control::StateControlParams);
27+
28+
GNCPY_PY_CHILD_CLASS(gncpy::control::StateControl, gncpy::control::ILinearControlModel)(m, "StateControl")
29+
.def(py::init<size_t, size_t>())
30+
GNCPY_CONTROL_ILINEARCONTROLMODEL_INTERFACE(gncpy::control::StateControl)
31+
.def("args_to_params", []([[maybe_unused]] gncpy::control::StateControl& self, py::tuple args) {
32+
if(args.size() != 3 && args.size() != 2){
33+
throw gncpy::exceptions::BadParams("Must only pass row/column indices and values to state control model");
34+
}
35+
std::vector<uint8_t> rows;
36+
std::vector<uint8_t> cols;
37+
for(auto& ii : args[0]) { // only element in args is a list of indices
38+
rows.emplace_back(py::cast<uint8_t>(ii));
39+
}
40+
for(auto& ii : args[1]) { // only element in args is a list of indices
41+
cols.emplace_back(py::cast<uint8_t>(ii));
42+
}
43+
if (args.size() == 2) {
44+
return gncpy::control::StateControlParams(rows, cols);
45+
}
46+
std::vector<double> vals;
47+
for(auto& ii : args[2]) { // only element in args is a list of indices
48+
vals.emplace_back(py::cast<double>(ii));
49+
}
50+
return gncpy::control::StateControlParams(rows, cols, vals);
51+
52+
})
53+
GNCPY_PICKLE(gncpy::control::StateControl);
54+
}

‎interface/dynamics/Common.h‎

Lines changed: 7 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,9 @@
11
#pragma once
22
#include <Eigen/Dense>
33
#include <gncpy/dynamics/Parameters.h>
4+
#include <gncpy/control/Parameters.h>
5+
#include <gncpy/control/IControlModel.h>
6+
#include <gncpy/dynamics/ILinearDynamics.h>
47
#include <pybind11/pybind11.h>
58
#include <pybind11/eigen.h>
69

@@ -15,8 +18,8 @@ extern void initClohessyWiltshire(pybind11::module&);
1518
#define GNCPY_DYNAMICS_PROPAGATE_STATE_INTERFACE(cName) \
1619
.def("propagate_state", pybind11::overload_cast<double, const Eigen::VectorXd&, const lager::gncpy::dynamics::StateTransParams*>(&cName::propagateState, pybind11::const_), \
1720
pybind11::arg("timestep"), pybind11::arg("state"), pybind11::arg_v("stateTransParams", static_cast<lager::gncpy::dynamics::StateTransParams *>(nullptr), "lager::gncpy::dynamics::StateTransParams*=nullptr")) \
18-
.def("propagate_state", pybind11::overload_cast<double, const Eigen::VectorXd&, const Eigen::VectorXd&>(&cName::propagateState, pybind11::const_)) \
19-
.def("propagate_state", pybind11::overload_cast<double, const Eigen::VectorXd&, const Eigen::VectorXd&, const lager::gncpy::dynamics::StateTransParams* const, const lager::gncpy::dynamics::ControlParams* const, const lager::gncpy::dynamics::ConstraintParams* const>(&cName::propagateState, pybind11::const_))
21+
.def("propagate_state", pybind11::overload_cast<double, const Eigen::VectorXd&, const Eigen::VectorXd&, const lager::gncpy::control::ControlParams* const>(&cName::propagateState, pybind11::const_)) \
22+
.def("propagate_state", pybind11::overload_cast<double, const Eigen::VectorXd&, const Eigen::VectorXd&, const lager::gncpy::dynamics::StateTransParams* const, const lager::gncpy::control::ControlParams* const, const lager::gncpy::dynamics::ConstraintParams* const>(&cName::propagateState, pybind11::const_))
2023

2124

2225
#define GNCPY_DYNAMICS_IDYNAMICS_INTERFACE(cName) \
@@ -28,5 +31,5 @@ extern void initClohessyWiltshire(pybind11::module&);
2831
GNCPY_DYNAMICS_IDYNAMICS_INTERFACE(cName) \
2932
.def("get_state_mat", &cName::getStateMat, \
3033
pybind11::arg("timestep"), pybind11::arg_v("stateTransParams", static_cast<lager::gncpy::dynamics::StateTransParams *>(nullptr), "lager::gncpy::dynamics::StateTransParams*=nullptr")) \
31-
.def("get_input_mat", &cName::getInputMat, \
32-
pybind11::arg("timestep"), pybind11::arg_v("controlParams", static_cast<lager::gncpy::dynamics::ControlParams *>(nullptr), "lager::gncpy::dynamics::ControlParams*=nullptr"))
34+
.def("set_control_model", &cName::setControlModel)
35+

‎interface/dynamics/Parameters.cpp‎

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -15,10 +15,6 @@ void initParameters(py::module& m) {
1515
GNCPY_PY_BASE_CLASS(gncpy::dynamics::StateTransParams)(m, "StateTransParams")
1616
.def(py::init())
1717
GNCPY_PICKLE(gncpy::dynamics::StateTransParams);
18-
19-
GNCPY_PY_BASE_CLASS(gncpy::dynamics::ControlParams)(m, "ControlParams")
20-
.def(py::init())
21-
GNCPY_PICKLE(gncpy::dynamics::ControlParams);
2218

2319
GNCPY_PY_BASE_CLASS(gncpy::dynamics::ConstraintParams)(m, "ConstraintParams")
2420
.def(py::init())

‎interface/filters/Parameters.cpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
#include <pybind11/pybind11.h>
22
#include <pybind11/stl.h> // needed because some backend gncpy functions retrun stl types
33
#include <gncpy/dynamics/Parameters.h>
4+
#include <gncpy/control/Parameters.h>
45
#include <gncpy/filters/Parameters.h>
56

67
#include "Common.h"

0 commit comments

Comments
 (0)