Skip to content

Commit baab93e

Browse files
ManifoldFRManifoldFR
authored
Merge pull request #261 from Simple-Robotics/topic/use-more-pinocchio-instantiated-functions
Use more Pinocchio instantiated functions
2 parents ef77166 + f903ed4 commit baab93e

14 files changed

+217
-202
lines changed

‎CHANGELOG.md

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -7,15 +7,16 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
77

88
## [Unreleased]
99

10+
### Changed
11+
12+
- Only link against needed pinocchio libraries ([#260](https://github.com/Simple-Robotics/aligator/pull/260))
13+
- Use Pinocchio instantiated functions ([#261](https://github.com/Simple-Robotics/aligator/pull/261))
14+
1015
### Fixed
1116

1217
- `LinesearchVariant::init()` should not be called unless the step accpetance strategy is a linesearch
1318
- Fixed compilation issues with C++20 (resolving [#246](https://github.com/Simple-Robotics/aligator/issues/246) and [#254](https://github.com/Simple-Robotics/aligator/discussions/254))
1419

15-
### Changed
16-
17-
- Only link against needed pinocchio libraries ([#260](https://github.com/Simple-Robotics/aligator/pull/260))
18-
1920
## [0.10.0] - 2024-12-09
2021

2122
### Added

‎include/aligator/modelling/dynamics/kinodynamics-fwd.hxx

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -39,9 +39,9 @@ void KinodynamicsFwdDynamicsTpl<Scalar>::forward(const ConstVectorRef &x,
3939
BaseData &data) const {
4040
Data &d = static_cast<Data &>(data);
4141
pinocchio::DataTpl<Scalar> &pdata = d.pin_data_;
42-
const auto q = x.head(pin_model_.nq);
43-
const auto v = x.tail(pin_model_.nv);
44-
const auto a = u.tail(pin_model_.nv - 6);
42+
const ConstVectorRef q = x.head(pin_model_.nq);
43+
const ConstVectorRef v = x.tail(pin_model_.nv);
44+
const ConstVectorRef a = u.tail(pin_model_.nv - 6);
4545

4646
pinocchio::ccrba(pin_model_, pdata, q, v); // Compute Ag
4747
pinocchio::dccrba(pin_model_, pdata, q, v); // Compute Ag_dot
@@ -99,9 +99,9 @@ void KinodynamicsFwdDynamicsTpl<Scalar>::dForward(const ConstVectorRef &x,
9999
BaseData &data) const {
100100
Data &d = static_cast<Data &>(data);
101101
pinocchio::DataTpl<Scalar> &pdata = d.pin_data_;
102-
const auto q = x.head(pin_model_.nq);
103-
const auto v = x.tail(pin_model_.nv);
104-
const auto a = u.tail(pin_model_.nv - 6);
102+
const ConstVectorRef q = x.head(pin_model_.nq);
103+
const ConstVectorRef v = x.tail(pin_model_.nv);
104+
const ConstVectorRef a = u.tail(pin_model_.nv - 6);
105105

106106
pinocchio::centerOfMass(pin_model_, pdata, q, v);
107107
pinocchio::jacobianCenterOfMass(pin_model_, pdata, q);

‎include/aligator/modelling/dynamics/multibody-constraint-fwd.hxx

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -34,13 +34,14 @@ void MultibodyConstraintFwdDynamicsTpl<Scalar>::forward(const ConstVectorRef &x,
3434
const pinocchio::ModelTpl<Scalar> &model = space_.getModel();
3535
const int nq = model.nq;
3636
const int nv = model.nv;
37-
const auto q = x.head(nq);
38-
const auto v = x.segment(nq, nv);
37+
const ConstVectorRef q = x.head(nq);
38+
const ConstVectorRef v = x.segment(nq, nv);
39+
const ConstVectorRef tau = d.tau_;
3940
d.xdot_.head(nv) = v;
4041
#pragma GCC diagnostic push
4142
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
4243
d.xdot_.segment(nv, nv) = pinocchio::constraintDynamics(
43-
model, d.pin_data_, q, v, d.tau_, constraint_models_, d.constraint_datas_,
44+
model, d.pin_data_, q, v, tau, constraint_models_, d.constraint_datas_,
4445
d.settings);
4546
#pragma GCC diagnostic pop
4647
}

‎include/aligator/modelling/dynamics/multibody-free-fwd.hxx

Lines changed: 13 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55

66
#include <pinocchio/algorithm/aba.hpp>
77
#include <pinocchio/algorithm/aba-derivatives.hpp>
8+
#include <pinocchio/multibody/fwd.hpp>
89

910
namespace aligator {
1011
namespace dynamics {
@@ -40,10 +41,12 @@ void MultibodyFreeFwdDynamicsTpl<Scalar>::forward(const ConstVectorRef &x,
4041
const pinocchio::ModelTpl<Scalar> &model = space_.getModel();
4142
const int nq = model.nq;
4243
const int nv = model.nv;
43-
const auto q = x.head(nq);
44-
const auto v = x.segment(nq, nv);
44+
const ConstVectorRef q = x.head(nq);
45+
const ConstVectorRef v = x.segment(nq, nv);
46+
const ConstVectorRef tau = d.tau_;
4547
d.xdot_.head(nv) = v;
46-
d.xdot_.segment(nv, nv) = pinocchio::aba(model, d.pin_data_, q, v, d.tau_);
48+
d.xdot_.segment(nv, nv) = pinocchio::aba(model, d.pin_data_, q, v, tau,
49+
pinocchio::Convention::LOCAL);
4750
}
4851

4952
template <typename Scalar>
@@ -55,9 +58,13 @@ void MultibodyFreeFwdDynamicsTpl<Scalar>::dForward(const ConstVectorRef &x,
5558
const int nq = model.nq;
5659
const int nv = model.nv;
5760
auto da_dx = d.Jx_.bottomRows(nv);
58-
pinocchio::computeABADerivatives(model, d.pin_data_, x.head(nq), x.tail(nv),
59-
d.tau_, da_dx.leftCols(nv),
60-
da_dx.rightCols(nv), d.pin_data_.Minv);
61+
const ConstVectorRef q = x.head(nq);
62+
const ConstVectorRef v = x.tail(nv);
63+
const ConstVectorRef tau = d.tau_;
64+
pinocchio::computeABADerivatives(model, d.pin_data_, q, v, tau,
65+
pinocchio::make_ref(da_dx.leftCols(nv)),
66+
pinocchio::make_ref(da_dx.rightCols(nv)),
67+
pinocchio::make_ref(d.pin_data_.Minv));
6168
d.Ju_.bottomRows(nv) = d.pin_data_.Minv * d.dtau_du_;
6269
}
6370

‎include/aligator/modelling/multibody/centroidal-momentum-derivative.hxx

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -34,8 +34,8 @@ void CentroidalMomentumDerivativeResidualTpl<Scalar>::evaluate(
3434
Data &d = static_cast<Data &>(data);
3535
pinocchio::DataTpl<Scalar> &pdata = d.pin_data_;
3636

37-
const auto q = x.head(pin_model_.nq);
38-
const auto v = x.tail(pin_model_.nv);
37+
const ConstVectorRef q = x.head(pin_model_.nq);
38+
const ConstVectorRef v = x.tail(pin_model_.nv);
3939

4040
pinocchio::forwardKinematics(pin_model_, pdata, q);
4141
pinocchio::centerOfMass(pin_model_, pdata, q, v);

‎include/aligator/modelling/multibody/contact-force.hxx

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -15,13 +15,13 @@ void ContactForceResidualTpl<Scalar>::evaluate(const ConstVectorRef &x,
1515
BaseData &data) const {
1616
Data &d = static_cast<Data &>(data);
1717

18-
const auto q = x.head(pin_model_.nq);
19-
const auto v = x.tail(pin_model_.nv);
18+
const ConstVectorRef q = x.head(pin_model_.nq);
19+
const ConstVectorRef v = x.tail(pin_model_.nv);
2020

2121
d.tau_.noalias() = actuation_matrix_ * u;
22-
pinocchio::constraintDynamics(pin_model_, d.pin_data_, q, v, d.tau_,
23-
constraint_models_, d.constraint_datas_,
24-
d.settings);
22+
pinocchio::constraintDynamics(
23+
pin_model_, d.pin_data_, q, v, pinocchio::make_const_ref(d.tau_),
24+
constraint_models_, d.constraint_datas_, d.settings);
2525
d.value_ =
2626
d.pin_data_.lambda_c.segment(contact_id_ * force_size_, force_size_) -
2727
fref_;

‎include/aligator/modelling/multibody/fly-high.hxx

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -19,8 +19,8 @@ template <typename Scalar>
1919
void FlyHighResidualTpl<Scalar>::evaluate(const ConstVectorRef &x,
2020
BaseData &data) const {
2121
Data &d = static_cast<Data &>(data);
22-
auto q = x.head(pin_model_.nq);
23-
auto v = x.segment(pin_model_.nq, pin_model_.nv);
22+
const ConstVectorRef q = x.head(pin_model_.nq);
23+
const ConstVectorRef v = x.segment(pin_model_.nq, pin_model_.nv);
2424
pinocchio::forwardKinematics(pin_model_, d.pdata_, q, v);
2525
pinocchio::updateFramePlacement(pin_model_, d.pdata_, pin_frame_id_);
2626

@@ -39,11 +39,12 @@ void FlyHighResidualTpl<Scalar>::computeJacobians(const ConstVectorRef &x,
3939
BaseData &data) const {
4040
Data &d = static_cast<Data &>(data);
4141
const int nv = pin_model_.nv;
42-
auto q = x.head(pin_model_.nq);
43-
auto v = x.segment(pin_model_.nq, nv);
44-
auto a = VectorXs::Zero(nv);
42+
const ConstVectorRef q = x.head(pin_model_.nq);
43+
const ConstVectorRef v = x.segment(pin_model_.nq, nv);
44+
VectorXs a = VectorXs::Zero(nv);
4545

46-
pinocchio::computeForwardKinematicsDerivatives(pin_model_, d.pdata_, q, v, a);
46+
pinocchio::computeForwardKinematicsDerivatives(pin_model_, d.pdata_, q, v,
47+
pinocchio::make_const_ref(a));
4748
pinocchio::getFrameVelocityDerivatives(pin_model_, d.pdata_, pin_frame_id_,
4849
pinocchio::LOCAL, d.l_dnu_dq,
4950
d.l_dnu_dv);

‎include/aligator/modelling/multibody/frame-placement.hxx

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,8 @@ void FramePlacementResidualTpl<Scalar>::evaluate(const ConstVectorRef &x,
1111
BaseData &data) const {
1212
Data &d = static_cast<Data &>(data);
1313
pinocchio::DataTpl<Scalar> &pdata = d.pin_data_;
14-
pinocchio::forwardKinematics(pin_model_, pdata, x.head(pin_model_.nq));
14+
const ConstVectorRef q = x.head(pin_model_.nq);
15+
pinocchio::forwardKinematics(pin_model_, pdata, q);
1516
pinocchio::updateFramePlacement(pin_model_, pdata, pin_frame_id_);
1617

1718
d.rMf_ = p_ref_inverse_ * pdata.oMf[pin_frame_id_];

‎include/aligator/modelling/multibody/frame-translation.hxx

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,8 @@ void FrameTranslationResidualTpl<Scalar>::evaluate(const ConstVectorRef &x,
1818
BaseData &data) const {
1919
Data &d = static_cast<Data &>(data);
2020
pinocchio::DataTpl<Scalar> &pdata = d.pin_data_;
21-
pinocchio::forwardKinematics(pin_model_, pdata, x.head(pin_model_.nq));
21+
const ConstVectorRef q = x.head(pin_model_.nq);
22+
pinocchio::forwardKinematics(pin_model_, pdata, q);
2223
pinocchio::updateFramePlacement(pin_model_, pdata, pin_frame_id_);
2324

2425
d.value_ = pdata.oMf[pin_frame_id_].translation() - p_ref_;

‎include/aligator/modelling/multibody/frame-velocity.hxx

Lines changed: 8 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -23,8 +23,8 @@ template <typename Scalar>
2323
void FrameVelocityResidualTpl<Scalar>::evaluate(const ConstVectorRef &x,
2424
BaseData &data) const {
2525
Data &d = static_cast<Data &>(data);
26-
auto q = x.head(pin_model_.nq);
27-
auto v = x.segment(pin_model_.nq, pin_model_.nv);
26+
const ConstVectorRef q = x.head(pin_model_.nq);
27+
const ConstVectorRef v = x.segment(pin_model_.nq, pin_model_.nv);
2828
pinocchio::forwardKinematics(pin_model_, d.pin_data_, q, v);
2929
pinocchio::updateFramePlacement(pin_model_, d.pin_data_, pin_frame_id_);
3030
d.value_ = (pinocchio::getFrameVelocity(pin_model_, d.pin_data_,
@@ -37,14 +37,15 @@ template <typename Scalar>
3737
void FrameVelocityResidualTpl<Scalar>::computeJacobians(const ConstVectorRef &x,
3838
BaseData &data) const {
3939
Data &d = static_cast<Data &>(data);
40-
auto q = x.head(pin_model_.nq);
41-
auto v = x.segment(pin_model_.nq, pin_model_.nv);
40+
const ConstVectorRef q = x.head(pin_model_.nq);
41+
const ConstVectorRef v = x.segment(pin_model_.nq, pin_model_.nv);
4242
VectorXs a = VectorXs::Zero(pin_model_.nv);
4343
pinocchio::computeForwardKinematicsDerivatives(pin_model_, d.pin_data_, q, v,
44-
a);
44+
pinocchio::make_const_ref(a));
45+
Eigen::Ref<Matrix6Xs> Jq = d.Jx_.leftCols(pin_model_.nv);
46+
Eigen::Ref<Matrix6Xs> Jv = d.Jx_.rightCols(pin_model_.nv);
4547
pinocchio::getFrameVelocityDerivatives(pin_model_, d.pin_data_, pin_frame_id_,
46-
type_, d.Jx_.leftCols(pin_model_.nv),
47-
d.Jx_.rightCols(pin_model_.nv));
48+
type_, Jq, Jv);
4849
}
4950

5051
template <typename Scalar>

‎include/aligator/modelling/multibody/gravity-compensation-residual.hxx

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,7 @@ void GravityCompensationResidualTpl<Scalar>::evaluate(const ConstVectorRef &x,
2121
const ConstVectorRef &u,
2222
BaseData &data_) const {
2323
Data &data = static_cast<Data &>(data_);
24-
ConstVectorRef q = x.head(pin_model_.nq);
24+
const ConstVectorRef q = x.head(pin_model_.nq);
2525
data.value_ =
2626
-pinocchio::computeGeneralizedGravity(pin_model_, data.pin_data_, q);
2727
if (use_actuation_matrix) {
@@ -33,11 +33,13 @@ void GravityCompensationResidualTpl<Scalar>::evaluate(const ConstVectorRef &x,
3333

3434
template <typename Scalar>
3535
void GravityCompensationResidualTpl<Scalar>::computeJacobians(
36-
const ConstVectorRef &x, const ConstVectorRef &u, BaseData &data_) const {
36+
const ConstVectorRef &x, const ConstVectorRef & /*u*/,
37+
BaseData &data_) const {
3738
Data &data = static_cast<Data &>(data_);
3839
ConstVectorRef q = x.head(pin_model_.nq);
39-
pinocchio::computeGeneralizedGravityDerivatives(pin_model_, data.pin_data_, q,
40-
data.gravity_partial_dq_);
40+
pinocchio::computeGeneralizedGravityDerivatives(
41+
pin_model_, data.pin_data_, q,
42+
pinocchio::make_ref(data.gravity_partial_dq_));
4143
if (use_actuation_matrix) {
4244
data_.Ju_ = actuation_matrix_;
4345
} else {

‎include/aligator/modelling/multibody/multibody-friction-cone.hxx

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -14,13 +14,13 @@ void MultibodyFrictionConeResidualTpl<Scalar>::evaluate(const ConstVectorRef &x,
1414
BaseData &data) const {
1515
Data &d = static_cast<Data &>(data);
1616

17-
const auto q = x.head(pin_model_.nq);
18-
const auto v = x.tail(pin_model_.nv);
17+
const ConstVectorRef q = x.head(pin_model_.nq);
18+
const ConstVectorRef v = x.tail(pin_model_.nv);
1919

2020
d.tau_.noalias() = actuation_matrix_ * u;
21-
pinocchio::constraintDynamics(pin_model_, d.pin_data_, q, v, d.tau_,
22-
constraint_models_, d.constraint_datas_,
23-
d.settings);
21+
pinocchio::constraintDynamics(
22+
pin_model_, d.pin_data_, q, v, pinocchio::make_const_ref(d.tau_),
23+
constraint_models_, d.constraint_datas_, d.settings);
2424

2525
// Unilateral contact
2626
d.value_[0] = -d.pin_data_.lambda_c[contact_id_ * 3 + 2];

‎include/aligator/modelling/multibody/multibody-wrench-cone.hxx

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -14,13 +14,13 @@ void MultibodyWrenchConeResidualTpl<Scalar>::evaluate(const ConstVectorRef &x,
1414
BaseData &data) const {
1515
Data &d = static_cast<Data &>(data);
1616

17-
const auto q = x.head(pin_model_.nq);
18-
const auto v = x.tail(pin_model_.nv);
17+
const ConstVectorRef q = x.head(pin_model_.nq);
18+
const ConstVectorRef v = x.tail(pin_model_.nv);
1919

2020
d.tau_.noalias() = actuation_matrix_ * u;
21-
pinocchio::constraintDynamics(pin_model_, d.pin_data_, q, v, d.tau_,
22-
constraint_models_, d.constraint_datas_,
23-
d.settings);
21+
pinocchio::constraintDynamics(
22+
pin_model_, d.pin_data_, q, v, pinocchio::make_const_ref(d.tau_),
23+
constraint_models_, d.constraint_datas_, d.settings);
2424

2525
// Unilateral contact
2626
d.value_.noalias() =

0 commit comments

Comments
 (0)