From 306de26e8da3f139fa37a24bf91fa7f31bbd8d8d Mon Sep 17 00:00:00 2001 From: Fabian Schramm <55981657+fabinsch@users.noreply.github.com> Date: Tue, 4 Jun 2024 10:48:18 +0200 Subject: [PATCH 1/7] py/test: use `dIntegrateTransport` to transport vector back and forth --- unittest/python/bindings_liegroups.py | 29 +++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/unittest/python/bindings_liegroups.py b/unittest/python/bindings_liegroups.py index df3a40bf11..c583fea639 100644 --- a/unittest/python/bindings_liegroups.py +++ b/unittest/python/bindings_liegroups.py @@ -104,6 +104,35 @@ def test_dIntegrateTransport(self): Jout1_ref = Jint.dot(J0) self.assertApprox(Jout1, Jout1_ref) + def test_dIntegrateTransport_inverse(self): + for lg in [ + pin.liegroups.R3(), + pin.liegroups.SO3(), + pin.liegroups.SO2(), + pin.liegroups.SE3(), + pin.liegroups.SE2(), + pin.liegroups.R3() * pin.liegroups.SO3(), + ]: + q0 = lg.random() + v = np.random.rand(lg.nv) + q1 = lg.integrate(q0, v) + + # transport random tangent vector from q1 to q0 + tvec_at_q1 = np.random.rand(lg.nv) + tvec_at_q0 = lg.dIntegrateTransport(q0, v, tvec_at_q1, pin.ARG0) + + # test opposite direction + q0_ = q1.copy() + v_ = -v.copy() # reverse path + q1_ = lg.integrate(q0_, v_) + + self.assertApprox(q0, q1_) # recover init point on manifold + + tvec_at_q1_ = tvec_at_q0 + tvec_at_q0_ = lg.dIntegrateTransport(q0_, v_, tvec_at_q1_, pin.ARG0) + + self.assertApprox(tvec_at_q1, tvec_at_q0_) + if __name__ == "__main__": unittest.main() From d8bfd40d36a001ef7f6ee85adf390b40815a6f17 Mon Sep 17 00:00:00 2001 From: Fabian Schramm <55981657+fabinsch@users.noreply.github.com> Date: Tue, 4 Jun 2024 10:55:13 +0200 Subject: [PATCH 2/7] update CHANGELOG --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 708c6ff000..0623c5fe9f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -15,6 +15,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ### Added - Python unittest for `contactInverseDynamics` function ([#2263](https://github.com/stack-of-tasks/pinocchio/pull/2263)) +- Python unittest for `dIntegrateTransport` to check vector transport and its inverse ([#2273](https://github.com/stack-of-tasks/pinocchio/pull/2273)) ### Removed From f53cfb053436e86818843643f24f770433d276e4 Mon Sep 17 00:00:00 2001 From: Fabian Schramm <55981657+fabinsch@users.noreply.github.com> Date: Tue, 4 Jun 2024 11:27:53 +0200 Subject: [PATCH 3/7] py/test: use `dIntegrateTransport` to transport matrix back and forth --- unittest/python/bindings_liegroups.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/unittest/python/bindings_liegroups.py b/unittest/python/bindings_liegroups.py index c583fea639..c251e13a23 100644 --- a/unittest/python/bindings_liegroups.py +++ b/unittest/python/bindings_liegroups.py @@ -133,6 +133,13 @@ def test_dIntegrateTransport_inverse(self): self.assertApprox(tvec_at_q1, tvec_at_q0_) + # same test for matrix + J_at_q1 = np.random.rand(lg.nv, lg.nv) + J_at_q0 = lg.dIntegrateTransport(q0, v, J_at_q1, pin.ARG0) + self.assertApprox( + J_at_q1, lg.dIntegrateTransport(q0_, v_, J_at_q0, pin.ARG0) + ) + if __name__ == "__main__": unittest.main() From e28ed375ccb04d4d5bb3f67d342bc70b241e02dc Mon Sep 17 00:00:00 2001 From: Fabian Schramm <55981657+fabinsch@users.noreply.github.com> Date: Wed, 5 Jun 2024 14:55:48 +0200 Subject: [PATCH 4/7] cpp/test: use `dIntegrateTransport` to transport vector/matrix back and forth --- unittest/liegroups.cpp | 72 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 72 insertions(+) diff --git a/unittest/liegroups.cpp b/unittest/liegroups.cpp index aaa3dc582e..ae518a9d96 100644 --- a/unittest/liegroups.cpp +++ b/unittest/liegroups.cpp @@ -490,6 +490,53 @@ struct LieGroup_JintegrateJdifference } }; +struct LieGroup_dIntegrateTransport +{ + template + void operator()(const T) const + { + typedef typename T::ConfigVector_t ConfigVector_t; + typedef typename T::TangentVector_t TangentVector_t; + typedef typename T::JacobianMatrix_t JacobianMatrix_t; + + PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH + PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED + T lg; + BOOST_TEST_MESSAGE(lg.name()); + ConfigVector_t qa, qb(lg.nq()); + qa = lg.random(); + TangentVector_t v(lg.nv()), tvec_at_q1(lg.nv()), tvec_at_q0(lg.nv()), tvec_at_q0_(lg.nv()); + v.setRandom(); + lg.integrate(qa, v, qb); + + // transport random tangent vector from q1 to q0 + tvec_at_q1.setRandom(); + lg.dIntegrateTransport(qa, v, tvec_at_q1, tvec_at_q0, ARG0); + + // test opposite direction + ConfigVector_t qa_ = qb; + TangentVector_t v_ = -v; // reverse path + ConfigVector_t qb_ = lg.integrate(qa_, v_); + + BOOST_CHECK_SMALL((qa - qb_).norm(), 1e-6); // recover init point on manifold + + TangentVector_t tvec_at_q1_ = tvec_at_q0; + lg.dIntegrateTransport(qa_, v_, tvec_at_q1_, tvec_at_q0_, ARG0); + + BOOST_CHECK_SMALL((tvec_at_q1 - tvec_at_q0_).norm(), 1e-6); + + // same test for matrix + JacobianMatrix_t J_at_q1(lg.nv(), lg.nv()); + J_at_q1.setRandom(); + JacobianMatrix_t J_at_q0(lg.nv(), lg.nv()); + lg.dIntegrateTransport(qa, v, J_at_q1, J_at_q0, ARG0); + JacobianMatrix_t J_at_q1_(lg.nv(), lg.nv()); + lg.dIntegrateTransport(qa_, v_, J_at_q0, J_at_q1_, ARG0); + + BOOST_CHECK_SMALL((J_at_q1 - J_at_q1_).norm(), 1e-6); + } +}; + struct LieGroup_JintegrateCoeffWise { template @@ -566,6 +613,31 @@ BOOST_AUTO_TEST_CASE(Jdifference) boost::mpl::for_each(LieGroup_Jdifference()); } +BOOST_AUTO_TEST_CASE(dIntegrateTransport) +{ + typedef double Scalar; + enum + { + Options = 0 + }; + + typedef boost::mpl::vector< + VectorSpaceOperationTpl<1, Scalar, Options>, VectorSpaceOperationTpl<2, Scalar, Options>, + SpecialOrthogonalOperationTpl<2, Scalar, Options>, + SpecialOrthogonalOperationTpl<3, Scalar, Options>, + SpecialEuclideanOperationTpl<2, Scalar, Options>, + SpecialEuclideanOperationTpl<3, Scalar, Options>, + CartesianProductOperation< + VectorSpaceOperationTpl<2, Scalar, Options>, + SpecialOrthogonalOperationTpl<2, Scalar, Options>>, + CartesianProductOperation< + VectorSpaceOperationTpl<3, Scalar, Options>, + SpecialOrthogonalOperationTpl<3, Scalar, Options>>> + Types; + for (int i = 0; i < 20; ++i) + boost::mpl::for_each(LieGroup_dIntegrateTransport()); +} + BOOST_AUTO_TEST_CASE(Jintegrate) { typedef double Scalar; From f2db9dfe2e9ac36ae36710ce7374da99da9e722f Mon Sep 17 00:00:00 2001 From: Fabian Schramm <55981657+fabinsch@users.noreply.github.com> Date: Wed, 5 Jun 2024 14:56:43 +0200 Subject: [PATCH 5/7] update CHANGELOG --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 0623c5fe9f..137aa7a6aa 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -15,6 +15,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ### Added - Python unittest for `contactInverseDynamics` function ([#2263](https://github.com/stack-of-tasks/pinocchio/pull/2263)) +- Cpp unittest for `dIntegrateTransport` to check vector transport and its inverse ([#2273](https://github.com/stack-of-tasks/pinocchio/pull/2273)) - Python unittest for `dIntegrateTransport` to check vector transport and its inverse ([#2273](https://github.com/stack-of-tasks/pinocchio/pull/2273)) ### Removed From 8470fe6986ec0577c3b2a470724289d3dd237105 Mon Sep 17 00:00:00 2001 From: Fabian Schramm <55981657+fabinsch@users.noreply.github.com> Date: Wed, 5 Jun 2024 19:09:20 +0200 Subject: [PATCH 6/7] rename vars in unittest dIntegrateTransport --- unittest/liegroups.cpp | 38 ++++++++++++--------------- unittest/python/bindings_liegroups.py | 16 +++++------ 2 files changed, 24 insertions(+), 30 deletions(-) diff --git a/unittest/liegroups.cpp b/unittest/liegroups.cpp index ae518a9d96..3249470680 100644 --- a/unittest/liegroups.cpp +++ b/unittest/liegroups.cpp @@ -505,35 +505,31 @@ struct LieGroup_dIntegrateTransport BOOST_TEST_MESSAGE(lg.name()); ConfigVector_t qa, qb(lg.nq()); qa = lg.random(); - TangentVector_t v(lg.nv()), tvec_at_q1(lg.nv()), tvec_at_q0(lg.nv()), tvec_at_q0_(lg.nv()); + TangentVector_t v(lg.nv()), tvec_at_qb(lg.nv()), tvec_at_qa(lg.nv()), tvec_at_qa_r(lg.nv()); v.setRandom(); lg.integrate(qa, v, qb); // transport random tangent vector from q1 to q0 - tvec_at_q1.setRandom(); - lg.dIntegrateTransport(qa, v, tvec_at_q1, tvec_at_q0, ARG0); + tvec_at_qb.setRandom(); + lg.dIntegrateTransport(qa, v, tvec_at_qb, tvec_at_qa, ARG0); - // test opposite direction - ConfigVector_t qa_ = qb; - TangentVector_t v_ = -v; // reverse path - ConfigVector_t qb_ = lg.integrate(qa_, v_); + // test reverse direction + TangentVector_t v_r = -v; // reverse path + ConfigVector_t qa_r = lg.integrate(qb, v_r); + lg.dIntegrateTransport(qa_r, v_r, tvec_at_qa, tvec_at_qa_r, ARG0); - BOOST_CHECK_SMALL((qa - qb_).norm(), 1e-6); // recover init point on manifold - - TangentVector_t tvec_at_q1_ = tvec_at_q0; - lg.dIntegrateTransport(qa_, v_, tvec_at_q1_, tvec_at_q0_, ARG0); - - BOOST_CHECK_SMALL((tvec_at_q1 - tvec_at_q0_).norm(), 1e-6); + BOOST_CHECK_SMALL((qa - qa_r).norm(), 1e-6); // recover init point on manifold + BOOST_CHECK_SMALL((tvec_at_qb - tvec_at_qa_r).norm(), 1e-6); // same test for matrix - JacobianMatrix_t J_at_q1(lg.nv(), lg.nv()); - J_at_q1.setRandom(); - JacobianMatrix_t J_at_q0(lg.nv(), lg.nv()); - lg.dIntegrateTransport(qa, v, J_at_q1, J_at_q0, ARG0); - JacobianMatrix_t J_at_q1_(lg.nv(), lg.nv()); - lg.dIntegrateTransport(qa_, v_, J_at_q0, J_at_q1_, ARG0); - - BOOST_CHECK_SMALL((J_at_q1 - J_at_q1_).norm(), 1e-6); + JacobianMatrix_t J_at_qa(lg.nv(), lg.nv()); + J_at_qa.setRandom(); + JacobianMatrix_t J_at_qb(lg.nv(), lg.nv()); + lg.dIntegrateTransport(qa, v, J_at_qa, J_at_qb, ARG0); + JacobianMatrix_t J_at_qa_r(lg.nv(), lg.nv()); + lg.dIntegrateTransport(qa_r, v_r, J_at_qb, J_at_qa_r, ARG0); + + BOOST_CHECK_SMALL((J_at_qa - J_at_qa_r).norm(), 1e-6); } }; diff --git a/unittest/python/bindings_liegroups.py b/unittest/python/bindings_liegroups.py index c251e13a23..b2b2a777ad 100644 --- a/unittest/python/bindings_liegroups.py +++ b/unittest/python/bindings_liegroups.py @@ -121,23 +121,21 @@ def test_dIntegrateTransport_inverse(self): tvec_at_q1 = np.random.rand(lg.nv) tvec_at_q0 = lg.dIntegrateTransport(q0, v, tvec_at_q1, pin.ARG0) - # test opposite direction - q0_ = q1.copy() - v_ = -v.copy() # reverse path - q1_ = lg.integrate(q0_, v_) + # test reverse direction + v_r = -v.copy() # reverse path + q0_r = lg.integrate(q1, v_r) - self.assertApprox(q0, q1_) # recover init point on manifold + self.assertApprox(q0, q0_r) # recover init point on manifold - tvec_at_q1_ = tvec_at_q0 - tvec_at_q0_ = lg.dIntegrateTransport(q0_, v_, tvec_at_q1_, pin.ARG0) + tvec_at_q1_r = lg.dIntegrateTransport(q1, v_r, tvec_at_q0, pin.ARG0) - self.assertApprox(tvec_at_q1, tvec_at_q0_) + self.assertApprox(tvec_at_q1, tvec_at_q1_r) # same test for matrix J_at_q1 = np.random.rand(lg.nv, lg.nv) J_at_q0 = lg.dIntegrateTransport(q0, v, J_at_q1, pin.ARG0) self.assertApprox( - J_at_q1, lg.dIntegrateTransport(q0_, v_, J_at_q0, pin.ARG0) + J_at_q1, lg.dIntegrateTransport(q1, v_r, J_at_q0, pin.ARG0) ) From 5eb8be3c6679b45330416058b6fa1e84f5f3bd61 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Thu, 6 Jun 2024 16:28:06 +0200 Subject: [PATCH 7/7] changelog: clean --- CHANGELOG.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 137aa7a6aa..ac5c2bf953 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -15,8 +15,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ### Added - Python unittest for `contactInverseDynamics` function ([#2263](https://github.com/stack-of-tasks/pinocchio/pull/2263)) -- Cpp unittest for `dIntegrateTransport` to check vector transport and its inverse ([#2273](https://github.com/stack-of-tasks/pinocchio/pull/2273)) -- Python unittest for `dIntegrateTransport` to check vector transport and its inverse ([#2273](https://github.com/stack-of-tasks/pinocchio/pull/2273)) +- c++ and Python unittest for `dIntegrateTransport` to check vector transport and its inverse ([#2273](https://github.com/stack-of-tasks/pinocchio/pull/2273)) ### Removed