Added native support for quaternions #2
Conversation
No constraints
There was a problem hiding this comment.
First off, thanks for the PR! Lots of good stuff in here. I do think we can clean it up a bit more, though, especially with all of the if (use_quaternion) statements.
You also have included an "Error Input Dim." I'm not really sure this makes any sense, and we haven't even done an example with a quaternion as an input. I'd suggest removing this as an argument from the relevant functions (at the very least from the API).
| @@ -9,6 +9,8 @@ add_library(altro_altro2 | |||
| altro_solver.cpp | |||
|
|
|||
| utils/formatting.hpp | |||
| utils/quaternion_utils.hpp | |||
There was a problem hiding this comment.
nit: leave a space between files, same as above
| @@ -235,6 +292,7 @@ ErrorCodes ALTROSolver::SetState(const a_float *x, int n, int k_start, int k_sto | |||
| for (int k = k_start; k < k_stop; ++k) { | |||
| AssertStateDim(k, n); | |||
| solver_->data_[k].x_ = Eigen::Map<const Vector>(x, n); | |||
| solver_->data_[k].x_ref = Eigen::Map<const Vector>(x, n); | |||
There was a problem hiding this comment.
Wondering why these extra fields are needed?
| @@ -22,6 +22,9 @@ add_library(altro_impl STATIC | |||
|
|
|||
| cones.cpp | |||
| cones.hpp | |||
|
|
|||
| ../utils/quaternion_utils.cpp | |||
There was a problem hiding this comment.
Usually you only include files from the current or sub-directories. If you think the quaternion files should go in utils instead solver, maybe we should make utils a library
| @@ -10,7 +10,7 @@ | |||
|
|
|||
| namespace altro { | |||
|
|
|||
| using Vector = Eigen::Vector<a_float, Eigen::Dynamic>; | |||
| using Vector = Eigen::Matrix<a_float, Eigen::Dynamic, 1>; | |||
| proj_hess_.emplace_back(Matrix::Zero(p, p)); | ||
| jac_tmp_.emplace_back(Matrix::Zero(p, n + m)); | ||
| rho_.emplace_back(1.0); | ||
| if (use_quaternion) { |
There was a problem hiding this comment.
please refactor to reduce duplicate code (place the if statement inside of the for loop instead of outside)
| gtest_main | ||
| linesearch::linesearch | ||
| ) | ||
| # add_executable(linesearch_tests |
There was a problem hiding this comment.
why are you commenting this out?
| # add_executable(tvlqr_test |
There was a problem hiding this comment.
why are you commenting this out?
| @@ -17,7 +17,7 @@ TEST(TVLQR_Tests, DoubleIntegrator) { | |||
| // Problem definition | |||
| ///////////////////////////////////////////// | |||
| using Matrix = Eigen::Matrix<lqr_float, Eigen::Dynamic, Eigen::Dynamic>; | |||
| using Vector = Eigen::Vector<lqr_float, Eigen::Dynamic>; | |||
| using Vector = Eigen::Matrix<lqr_float, Eigen::Dynamic, 1>; | |||
| using Matrix = Eigen::Map<Eigen::Matrix<lqr_float, Eigen::Dynamic, Eigen::Dynamic>>; | ||
| using Vector = Eigen::Map<Eigen::Vector<lqr_float, Eigen::Dynamic>>; | ||
| using Vector = Eigen::Map<Eigen::Matrix<lqr_float, Eigen::Dynamic, 1>>; |
| @@ -129,6 +147,12 @@ class KnotPointData { | |||
| ExplicitDynamicsFunction dynamics_function_; | |||
| ExplicitDynamicsJacobian dynamics_jacobian_; | |||
|
|
|||
| // Error dynamics | |||
| ExplicitDynamicsFunction error_dynamics_function_; | |||
There was a problem hiding this comment.
are these even used? what are they supposed to be?
|
Pulled code into the quat branch for more development |
Implemented the techniques of quaternions in "Planning with Attitude". Added two test cases: trajectory optimization for quadruped robots using quaternions, and a simple attitude tracking problem using quaternions.