Skip to content

Commit 1bc3298

Browse files
ManifoldFRManifoldFR
authored
Merge pull request #378 from EtienneAr/two_frame_equality
Add two frame equality residual
2 parents 03d5356 + 566a3b4 commit 1bc3298

File tree

6 files changed

+285
-0
lines changed

6 files changed

+285
-0
lines changed

‎CHANGELOG.md‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
1919
- change several classes (including solvers and contact-related modelling) to take `std::string_view` where possible (https://github.com/Simple-Robotics/aligator/pull/364)
2020
- fixed missing `num_threads` passed to `problem.evaluate()` in `SolverProxDDP::tryLinearStep()` (https://github.com/Simple-Robotics/aligator/pull/373)
2121
- Fix pixi build and Tracy integration issues: add ninja dependency and update configs (https://github.com/Simple-Robotics/aligator/pull/375)
22+
- Added TwoFrameEquality residual to enforce relative pose equality between two Pinocchio frames (https://github.com/Simple-Robotics/aligator/pull/378)
2223

2324
### Changed
2425

‎bindings/python/src/modelling/multibody/expose-pinocchio-functions.cpp‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55
#include "aligator/python/modelling/multibody-utils.hpp"
66

77
#include "aligator/modelling/multibody/frame-placement.hpp"
8+
#include "aligator/modelling/multibody/frame-equality.hpp"
89
#include "aligator/modelling/multibody/frame-velocity.hpp"
910
#include "aligator/modelling/multibody/frame-translation.hpp"
1011
#include "aligator/modelling/multibody/frame-collision.hpp"
@@ -43,6 +44,9 @@ void exposeFrameFunctions() {
4344
using SE3 = pinocchio::SE3Tpl<Scalar>;
4445
using Motion = pinocchio::MotionTpl<Scalar>;
4546

47+
using FrameEquality = FrameEqualityResidualTpl<Scalar>;
48+
using FrameEqualityData = FrameEqualityDataTpl<Scalar>;
49+
4650
using FramePlacement = FramePlacementResidualTpl<Scalar>;
4751
using FramePlacementData = FramePlacementDataTpl<Scalar>;
4852

@@ -62,6 +66,36 @@ void exposeFrameFunctions() {
6266

6367
PolymorphicMultiBaseVisitor<UnaryFunction, StageFunction> unary_visitor;
6468

69+
bp::class_<FrameEquality, bp::bases<UnaryFunction>>(
70+
"FrameEqualityResidual", "Frame equality residual function.",
71+
bp::init<int, int, const PinModel &, pinocchio::FrameIndex,
72+
pinocchio::FrameIndex>(
73+
("self"_a, "ndx", "nu", "model", "frame_id1", "frame_id2")))
74+
.def(unary_visitor)
75+
.def("getReference", &FrameEquality::getReference, "self"_a,
76+
bp::return_internal_reference<>(),
77+
"Get the target transform between frame1 and frame2.")
78+
.def("setReference", &FrameEquality::setReference, ("self"_a, "p_new"),
79+
"Set the target transform between frame1 and frame2.")
80+
.add_property("frame1_id", &FrameEquality::getFrame1Id,
81+
&FrameEquality::setFrame1Id,
82+
"Get the Pinocchio ID of frame1.")
83+
.add_property("frame2_id", &FrameEquality::getFrame2Id,
84+
&FrameEquality::setFrame2Id,
85+
"Get the Pinocchio ID of frame2.");
86+
87+
bp::register_ptr_to_python<shared_ptr<FrameEqualityData>>();
88+
89+
bp::class_<FrameEqualityData, bp::bases<context::StageFunctionData>>(
90+
"FrameEqualityData", "Data struct for FrameEqualityResidual.",
91+
bp::no_init)
92+
.def_readonly("RMf2", &FrameEqualityData::RMf2_, "Frame placement error.")
93+
.def_readonly("RJlog6f2", &FrameEqualityData::RJlog6f2_)
94+
.def_readonly("wJf1", &FrameEqualityData::wJf1_)
95+
.def_readonly("wJf2", &FrameEqualityData::wJf2_)
96+
.def_readonly("pin_data", &FrameEqualityData::pin_data_,
97+
"Pinocchio data struct.");
98+
6599
bp::class_<FramePlacement, bp::bases<UnaryFunction>>(
66100
"FramePlacementResidual", "Frame placement residual function.",
67101
bp::init<int, int, const PinModel &, const SE3 &, pinocchio::FrameIndex>(

‎include/aligator/modelling/multibody/frame-equality.hpp‎

Lines changed: 103 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,103 @@
1+
#pragma once
2+
3+
#include "aligator/modelling/multibody/fwd.hpp"
4+
#include "aligator/core/unary-function.hpp"
5+
6+
#include <pinocchio/multibody/model.hpp>
7+
#include <pinocchio/multibody/frame.hpp>
8+
9+
#include "aligator/third-party/polymorphic_cxx14.h"
10+
11+
namespace aligator {
12+
13+
template <typename Scalar> struct FrameEqualityDataTpl;
14+
15+
/**
16+
* @brief Residual enforcing equality between two Pinocchio frames.
17+
*
18+
* Computes the 6D log error between frame `frame_id1` and frame `frame_id2`.
19+
* The residual is:
20+
*
21+
* r = log6( f1^{-1} * f1Mf2_ref * f2 )
22+
*
23+
* where:
24+
* - f1 and f2 are the world placements of the two frames,
25+
* - f1Mf2_ref is the desired relative transform (reference).
26+
*
27+
* All computations (residual and Jacobians) are expressed in an R frame
28+
* attached to f1 (f1 -> f1Mf2_ref -> R -> error -> f2)
29+
*/
30+
template <typename _Scalar>
31+
struct FrameEqualityResidualTpl : UnaryFunctionTpl<_Scalar> {
32+
public:
33+
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
34+
using Scalar = _Scalar;
35+
ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
36+
ALIGATOR_UNARY_FUNCTION_INTERFACE(Scalar);
37+
using BaseData = StageFunctionDataTpl<Scalar>;
38+
using Model = pinocchio::ModelTpl<Scalar>;
39+
using PolyManifold = xyz::polymorphic<ManifoldAbstractTpl<Scalar>>;
40+
using SE3 = pinocchio::SE3Tpl<Scalar>;
41+
using Data = FrameEqualityDataTpl<Scalar>;
42+
43+
Model pin_model_;
44+
45+
FrameEqualityResidualTpl(const int ndx, const int nu, const Model &model,
46+
const pinocchio::FrameIndex frame_id1,
47+
const pinocchio::FrameIndex frame_id2,
48+
const SE3 &f1Mf2_ref = SE3::Identity())
49+
: Base(ndx, nu, 6)
50+
, pin_model_(model)
51+
, pin_frame_id1_(frame_id1)
52+
, pin_frame_id2_(frame_id2)
53+
, f1MR_ref_(f1Mf2_ref) {}
54+
55+
// Getters and setters
56+
pinocchio::FrameIndex getFrame1Id() const { return pin_frame_id1_; }
57+
void setFrame1Id(const std::size_t id) { pin_frame_id1_ = id; }
58+
pinocchio::FrameIndex getFrame2Id() const { return pin_frame_id2_; }
59+
void setFrame2Id(const std::size_t id) { pin_frame_id2_ = id; }
60+
61+
const SE3 &getReference() const { return f1MR_ref_; }
62+
void setReference(const SE3 &f1Mf2_ref) { f1MR_ref_ = f1Mf2_ref; }
63+
64+
void evaluate(const ConstVectorRef &x, BaseData &data) const;
65+
66+
void computeJacobians(const ConstVectorRef &x, BaseData &data) const;
67+
68+
shared_ptr<BaseData> createData() const {
69+
return std::make_shared<Data>(*this);
70+
}
71+
72+
protected:
73+
pinocchio::FrameIndex pin_frame_id1_;
74+
pinocchio::FrameIndex pin_frame_id2_;
75+
SE3 f1MR_ref_;
76+
};
77+
78+
template <typename Scalar>
79+
struct FrameEqualityDataTpl : StageFunctionDataTpl<Scalar> {
80+
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
81+
using Base = StageFunctionDataTpl<Scalar>;
82+
using PinData = pinocchio::DataTpl<Scalar>;
83+
using SE3 = pinocchio::SE3Tpl<Scalar>;
84+
85+
/// Pinocchio data object.
86+
PinData pin_data_;
87+
/// Equality error between the frames.
88+
SE3 RMf2_;
89+
/// Jacobian of the error (log6)
90+
typename math_types<Scalar>::Matrix6s RJlog6f2_;
91+
/// Jacobian of frame 1 expressed in WORLD
92+
typename math_types<Scalar>::Matrix6Xs wJf1_;
93+
/// Jacobian of frame 2 expressed in WORLD
94+
typename math_types<Scalar>::Matrix6Xs wJf2_;
95+
96+
FrameEqualityDataTpl(const FrameEqualityResidualTpl<Scalar> &model);
97+
};
98+
99+
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
100+
extern template struct FrameEqualityResidualTpl<context::Scalar>;
101+
extern template struct FrameEqualityDataTpl<context::Scalar>;
102+
#endif
103+
} // namespace aligator

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

Lines changed: 54 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
#pragma once
2+
3+
#include "aligator/modelling/multibody/frame-equality.hpp"
4+
#include <pinocchio/algorithm/frames.hpp>
5+
#include <pinocchio/algorithm/kinematics.hpp>
6+
7+
namespace aligator {
8+
9+
template <typename Scalar>
10+
void FrameEqualityResidualTpl<Scalar>::evaluate(const ConstVectorRef &x,
11+
BaseData &data) const {
12+
Data &d = static_cast<Data &>(data);
13+
pinocchio::DataTpl<Scalar> &pdata = d.pin_data_;
14+
const ConstVectorRef q = x.head(pin_model_.nq);
15+
pinocchio::forwardKinematics(pin_model_, pdata, q);
16+
pinocchio::updateFramePlacement(pin_model_, pdata, pin_frame_id1_);
17+
pinocchio::updateFramePlacement(pin_model_, pdata, pin_frame_id2_);
18+
19+
d.RMf2_ = pdata.oMf[pin_frame_id1_].act(f1MR_ref_).actInv(
20+
pdata.oMf[pin_frame_id2_]);
21+
d.value_ = pinocchio::log6(d.RMf2_).toVector();
22+
}
23+
24+
template <typename Scalar>
25+
void FrameEqualityResidualTpl<Scalar>::computeJacobians(const ConstVectorRef &,
26+
BaseData &data) const {
27+
Data &d = static_cast<Data &>(data);
28+
pinocchio::DataTpl<Scalar> &pdata = d.pin_data_;
29+
pinocchio::Jlog6(d.RMf2_, d.RJlog6f2_);
30+
pinocchio::computeJointJacobians(pin_model_, pdata);
31+
pinocchio::getFrameJacobian(pin_model_, pdata, pin_frame_id1_,
32+
pinocchio::WORLD, d.wJf1_);
33+
pinocchio::getFrameJacobian(pin_model_, pdata, pin_frame_id2_,
34+
pinocchio::WORLD, d.wJf2_);
35+
36+
d.Jx_.leftCols(pin_model_.nv) =
37+
d.RJlog6f2_ * pdata.oMf[pin_frame_id2_].toActionMatrixInverse() *
38+
(d.wJf2_ - d.wJf1_);
39+
}
40+
41+
template <typename Scalar>
42+
FrameEqualityDataTpl<Scalar>::FrameEqualityDataTpl(
43+
const FrameEqualityResidualTpl<Scalar> &model)
44+
: Base(model.ndx1, model.nu, 6)
45+
, pin_data_(model.pin_model_)
46+
, RJlog6f2_(6, 6)
47+
, wJf1_(6, model.pin_model_.nv)
48+
, wJf2_(6, model.pin_model_.nv) {
49+
wJf1_.setZero();
50+
wJf2_.setZero();
51+
RJlog6f2_.setZero();
52+
}
53+
54+
} // namespace aligator

‎src/modelling/multibody/frame-equality.cpp‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
#include "aligator/modelling/multibody/frame-equality.hxx"
2+
3+
namespace aligator {
4+
5+
template struct FrameEqualityResidualTpl<context::Scalar>;
6+
template struct FrameEqualityDataTpl<context::Scalar>;
7+
8+
} // namespace aligator

‎tests/python/test_frames.py‎

Lines changed: 85 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,91 @@ def sample_gauss(space):
3131
return x0
3232

3333

34+
def test_frame_equality_finite_differences():
35+
fr_name1 = "larm_effector_body"
36+
fr_id1 = model.getFrameId(fr_name1)
37+
38+
fr_name2 = "rarm_effector_body"
39+
fr_id2 = model.getFrameId(fr_name2)
40+
41+
space = manifolds.MultibodyConfiguration(model)
42+
43+
two_frame_residual = aligator.FrameEqualityResidual(
44+
space.ndx, nu, model, fr_id1, fr_id2
45+
)
46+
two_frame_residual_data = two_frame_residual.createData()
47+
assert fr_id1 == two_frame_residual.frame1_id
48+
assert fr_id2 == two_frame_residual.frame2_id
49+
50+
x = np.zeros(space.nx)
51+
for _ in range(10):
52+
# Set a random reference pose
53+
reference_transform = pin.SE3.Random()
54+
two_frame_residual.setReference(reference_transform)
55+
assert reference_transform == two_frame_residual.getReference()
56+
57+
# Take a random state
58+
x[3:] = space.rand()[3:]
59+
u0 = np.zeros(nu)
60+
61+
# Re-create finite differences helper
62+
finite_differences = aligator.FiniteDifferenceHelper(
63+
space, two_frame_residual, FD_EPS
64+
)
65+
finite_differences_data = finite_differences.createData()
66+
67+
two_frame_residual.evaluate(x, two_frame_residual_data)
68+
two_frame_residual.computeJacobians(x, two_frame_residual_data)
69+
finite_differences.evaluate(x, u0, finite_differences_data)
70+
finite_differences.computeJacobians(x, u0, finite_differences_data)
71+
assert np.allclose(
72+
two_frame_residual_data.Jx, finite_differences_data.Jx, atol=ATOL
73+
)
74+
75+
76+
def test_frame_equality_against_frame_placement():
77+
fr_name1 = "universe"
78+
fr_id1 = model.getFrameId(fr_name1)
79+
80+
fr_name2 = "rarm_effector_body"
81+
fr_id2 = model.getFrameId(fr_name2)
82+
83+
space = manifolds.MultibodyConfiguration(model)
84+
85+
two_frame_residual = aligator.FrameEqualityResidual(
86+
space.ndx, nu, model, fr_id1, fr_id2
87+
)
88+
two_frame_residual_data = two_frame_residual.createData()
89+
assert fr_id1 == two_frame_residual.frame1_id
90+
assert fr_id2 == two_frame_residual.frame2_id
91+
92+
frame_placement_residual = aligator.FramePlacementResidual(
93+
space.ndx, nu, model, pin.SE3.Identity(), fr_id2
94+
)
95+
frame_placement_residual_data = frame_placement_residual.createData()
96+
97+
x = np.zeros(space.nx)
98+
for _ in range(10):
99+
# Set a random reference pose
100+
reference_transform = pin.SE3.Random()
101+
frame_placement_residual.setReference(reference_transform)
102+
two_frame_residual.setReference(reference_transform)
103+
assert reference_transform == two_frame_residual.getReference()
104+
105+
# Take a random state
106+
x[3:] = space.rand()[3:]
107+
u0 = np.zeros(nu)
108+
109+
two_frame_residual.evaluate(x, two_frame_residual_data)
110+
two_frame_residual.computeJacobians(x, two_frame_residual_data)
111+
frame_placement_residual.evaluate(x, u0, frame_placement_residual_data)
112+
frame_placement_residual.computeJacobians(x, u0, frame_placement_residual_data)
113+
assert np.allclose(
114+
two_frame_residual_data.value, frame_placement_residual_data.value
115+
)
116+
assert np.allclose(two_frame_residual_data.Jx, frame_placement_residual_data.Jx)
117+
118+
34119
def test_frame_placement():
35120
fr_name1 = "larm_shoulder2_body"
36121
fr_id1 = model.getFrameId(fr_name1)

0 commit comments

Comments
 (0)