Skip to content

Use Eigen::Ref types more #25

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 2 commits into from
Dec 18, 2024
Merged

Use Eigen::Ref types more #25

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
bd31387
Update pre-commit config
ManifoldFR Dec 18, 2024
6b6b989
lowlevel-control : use our Eigen::Ref typedefs
ManifoldFR Dec 18, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion .pre-commit-config.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@ ci:
submodules: true
repos:
- repo: https://github.com/pre-commit/mirrors-clang-format
rev: v19.1.4
rev: v19.1.5
hooks:
- id: clang-format
types_or: []
Expand Down
2 changes: 2 additions & 0 deletions include/simple-mpc/fwd.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,9 @@ namespace simple_mpc

using Eigen::MatrixXd;
using Eigen::VectorXd;
using MatrixRef = Eigen::Ref<MatrixXd>;
using VectorRef = Eigen::Ref<VectorXd>;
using ConstMatrixRef = Eigen::Ref<const MatrixXd>;
using ConstVectorRef = Eigen::Ref<const VectorXd>;

using Vector6d = Eigen::Matrix<double, 6, 1>;
Expand Down
43 changes: 21 additions & 22 deletions include/simple-mpc/lowlevel-control.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,6 @@
namespace simple_mpc
{
using namespace proxsuite;
using pin::SE3;

struct IDSettings
{
Expand Down Expand Up @@ -88,23 +87,23 @@ namespace simple_mpc
void computeMatrices(
pin::Data & data,
const std::vector<bool> & contact_state,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a,
const Eigen::VectorXd & tau,
const Eigen::VectorXd & forces,
const Eigen::MatrixXd & M);
const ConstVectorRef & v,
const ConstVectorRef & a,
const ConstVectorRef & tau,
const ConstVectorRef & forces,
const ConstMatrixRef & M);

public:
explicit IDSolver(const IDSettings & settings, const pin::Model & model);

void solveQP(
pin::Data & data,
const std::vector<bool> & contact_state,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a,
const Eigen::VectorXd & tau,
const Eigen::VectorXd & forces,
const Eigen::MatrixXd & M);
const ConstVectorRef & v,
const ConstVectorRef & a,
const ConstVectorRef & tau,
const ConstVectorRef & forces,
const ConstMatrixRef & M);

SIMPLE_MPC_DEPRECATED
proxqp::dense::Model<double> getQP()
Expand Down Expand Up @@ -181,27 +180,27 @@ namespace simple_mpc
void computeMatrices(
pin::Data & data,
const std::vector<bool> & contact_state,
const Eigen::VectorXd & x_measured,
const Eigen::VectorXd & forces,
const Eigen::VectorXd & dH,
const Eigen::MatrixXd & M);
const ConstVectorRef & x_measured,
const ConstVectorRef & forces,
const ConstVectorRef & dH,
const ConstMatrixRef & M);

public:
explicit IKIDSolver(const IKIDSettings & settings, const pin::Model & model);

void computeDifferences(
pin::Data & data,
const Eigen::VectorXd & x_measured,
const std::vector<pin::SE3> foot_refs,
const std::vector<pin::SE3> foot_refs_next);
const ConstVectorRef & x_measured,
const std::vector<pin::SE3> & foot_refs,
const std::vector<pin::SE3> & foot_refs_next);

void solve_qp(
pin::Data & data,
const std::vector<bool> & contact_state,
const Eigen::VectorXd & x_measured,
const Eigen::VectorXd & forces,
const Eigen::VectorXd & dH,
const Eigen::MatrixXd & M);
const ConstVectorRef & x_measured,
const ConstVectorRef & forces,
const ConstVectorRef & dH,
const ConstMatrixRef & M);

SIMPLE_MPC_DEPRECATED
proxqp::dense::Model<double> getQP()
Expand Down
42 changes: 21 additions & 21 deletions src/lowlevel-control.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,11 +99,11 @@ namespace simple_mpc
void IDSolver::computeMatrices(
pinocchio::Data & data,
const std::vector<bool> & contact_state,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a,
const Eigen::VectorXd & tau,
const Eigen::VectorXd & forces,
const Eigen::MatrixXd & M)
const ConstVectorRef & v,
const ConstVectorRef & a,
const ConstVectorRef & tau,
const ConstVectorRef & forces,
const ConstMatrixRef & M)
{
// Reset matrices
Jc_.setZero();
Expand Down Expand Up @@ -160,11 +160,11 @@ namespace simple_mpc
void IDSolver::solveQP(
pinocchio::Data & data,
const std::vector<bool> & contact_state,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a,
const Eigen::VectorXd & tau,
const Eigen::VectorXd & forces,
const Eigen::MatrixXd & M)
const ConstVectorRef & v,
const ConstVectorRef & a,
const ConstVectorRef & tau,
const ConstVectorRef & forces,
const ConstMatrixRef & M)
{

computeMatrices(data, contact_state, v, a, tau, forces, M);
Expand Down Expand Up @@ -292,9 +292,9 @@ namespace simple_mpc

void IKIDSolver::computeDifferences(
pinocchio::Data & data,
const Eigen::VectorXd & x_measured,
const std::vector<pinocchio::SE3> foot_refs,
const std::vector<pinocchio::SE3> foot_refs_next)
const ConstVectorRef & x_measured,
const std::vector<pinocchio::SE3> & foot_refs,
const std::vector<pinocchio::SE3> & foot_refs_next)
{
difference(model_, x_measured.head(model_.nq), settings_.x0.head(model_.nq), q_diff_);
dq_diff_ = settings_.x0.tail(model_.nv) - x_measured.tail(model_.nv);
Expand Down Expand Up @@ -322,10 +322,10 @@ namespace simple_mpc
void IKIDSolver::computeMatrices(
pinocchio::Data & data,
const std::vector<bool> & contact_state,
const Eigen::VectorXd & v_current,
const Eigen::VectorXd & forces,
const Eigen::VectorXd & dH,
const Eigen::MatrixXd & M)
const ConstVectorRef & v_current,
const ConstVectorRef & forces,
const ConstVectorRef & dH,
const ConstMatrixRef & M)
{

H_.topLeftCorner(model_.nv, model_.nv) = settings_.w_qref * Eigen::MatrixXd::Identity(model_.nv, model_.nv);
Expand Down Expand Up @@ -409,10 +409,10 @@ namespace simple_mpc
void IKIDSolver::solve_qp(
pinocchio::Data & data,
const std::vector<bool> & contact_state,
const Eigen::VectorXd & v_current,
const Eigen::VectorXd & forces,
const Eigen::VectorXd & dH,
const Eigen::MatrixXd & M)
const ConstVectorRef & v_current,
const ConstVectorRef & forces,
const ConstVectorRef & dH,
const ConstMatrixRef & M)
{
computeMatrices(data, contact_state, v_current, forces, dH, M);

Expand Down