From b9e8bcbaaa89f8be27f945de057d881843c1bb7b Mon Sep 17 00:00:00 2001 From: Guilhem Saurel Date: Fri, 12 Jul 2024 15:05:53 +0200 Subject: [PATCH 1/3] test: setZero() MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit fix for `-DINITIALIZE_WITH_NAN`: ``` …/pinocchio/unittest/constrained-dynamics.cpp(1512): error: in "Test/test_sparse_forward_dynamics_in_contact_specifying_joint2id_case3D": check contact_force.isApprox(-contact_force_bis) has failed contact_force: 6.13593 2.26573 -1.29027 nan nan nan contact_force_bis: -6.13593 -2.26573 1.29027 0 0 0 cdata.c1Mc2: ``` --- unittest/constrained-dynamics.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/unittest/constrained-dynamics.cpp b/unittest/constrained-dynamics.cpp index 8c07066063..619b949f5d 100644 --- a/unittest/constrained-dynamics.cpp +++ b/unittest/constrained-dynamics.cpp @@ -1501,8 +1501,10 @@ BOOST_AUTO_TEST_CASE(test_sparse_forward_dynamics_in_contact_specifying_joint2id case LOCAL: { contact_force_bis = cdata_bis.contact_force; - if (cmodel.type == CONTACT_3D) + if (cmodel.type == CONTACT_3D) { contact_force.linear() = cdata.c1Mc2.actInv(cdata.contact_force).linear(); + contact_force.angular().setZero(); + } else { contact_force = cdata.c1Mc2.actInv(cdata.contact_force); From 4aa482ae9c3e2ba2beb5ca65790e9a90edfdabbd Mon Sep 17 00:00:00 2001 From: Guilhem Saurel Date: Fri, 12 Jul 2024 16:24:50 +0200 Subject: [PATCH 2/3] RigidConstraintDataTpl: update ctor MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit fix test-cpp-constraint-variants with `-DINITIALIZE_WITH_NAN`: ``` Running 2 test cases... …/pinocchio/unittest/constraint-variants.cpp(50): error: in "Test/contact_visitors": check ConstraintData(rcd) == ConstraintData(rcd) has failed …/pinocchio/unittest/constraint-variants.cpp(51): error: in "Test/contact_visitors": check ConstraintData(rcd) == rcd has failed …/pinocchio/unittest/constraint-variants.cpp(62): error: in "Test/contact_visitors": check rcd == constraint_data has failed ``` --- include/pinocchio/algorithm/contact-info.hpp | 27 +++++++++++--------- 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/include/pinocchio/algorithm/contact-info.hpp b/include/pinocchio/algorithm/contact-info.hpp index 7fe4d7ba64..b40366de32 100644 --- a/include/pinocchio/algorithm/contact-info.hpp +++ b/include/pinocchio/algorithm/contact-info.hpp @@ -904,6 +904,9 @@ namespace pinocchio explicit RigidConstraintDataTpl(const ContactModel & contact_model) : contact_force(Force::Zero()) + , oMc1(SE3::Identity()) + , oMc2(SE3::Identity()) + , c1Mc2(SE3::Identity()) , contact_placement_error(Motion::Zero()) , contact1_velocity(Motion::Zero()) , contact2_velocity(Motion::Zero()) @@ -917,18 +920,18 @@ namespace pinocchio , extended_motion_propagators_joint1(contact_model.depth_joint1, Matrix6::Zero()) , lambdas_joint1(contact_model.depth_joint1, Matrix6::Zero()) , extended_motion_propagators_joint2(contact_model.depth_joint2, Matrix6::Zero()) - , dv1_dq(6, contact_model.nv) - , da1_dq(6, contact_model.nv) - , da1_dv(6, contact_model.nv) - , da1_da(6, contact_model.nv) - , dv2_dq(6, contact_model.nv) - , da2_dq(6, contact_model.nv) - , da2_dv(6, contact_model.nv) - , da2_da(6, contact_model.nv) - , dvc_dq(contact_model.size(), contact_model.nv) - , dac_dq(contact_model.size(), contact_model.nv) - , dac_dv(contact_model.size(), contact_model.nv) - , dac_da(contact_model.size(), contact_model.nv) + , dv1_dq(Matrix6x::Zero(6, contact_model.nv)) + , da1_dq(Matrix6x::Zero(6, contact_model.nv)) + , da1_dv(Matrix6x::Zero(6, contact_model.nv)) + , da1_da(Matrix6x::Zero(6, contact_model.nv)) + , dv2_dq(Matrix6x::Zero(6, contact_model.nv)) + , da2_dq(Matrix6x::Zero(6, contact_model.nv)) + , da2_dv(Matrix6x::Zero(6, contact_model.nv)) + , da2_da(Matrix6x::Zero(6, contact_model.nv)) + , dvc_dq(MatrixX::Zero(contact_model.size(), contact_model.nv)) + , dac_dq(MatrixX::Zero(contact_model.size(), contact_model.nv)) + , dac_dv(MatrixX::Zero(contact_model.size(), contact_model.nv)) + , dac_da(MatrixX::Zero(contact_model.size(), contact_model.nv)) { } From 126903b3bbbac4d89ef2172940134b38cb74df6c Mon Sep 17 00:00:00 2001 From: Guilhem Saurel Date: Tue, 16 Jul 2024 14:57:00 +0200 Subject: [PATCH 3/3] add changelog entry --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5decbe56fc..5388f9c61d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ### Fixed - Append pinocchio optional libraries into pkg-config file ([#2322](https://github.com/stack-of-tasks/pinocchio/pull/2322)) +- Fixed RigidConstraintData constructor and some variable initialization for NaN ([#2332](https://github.com/stack-of-tasks/pinocchio/pull/2332)) ### Added - Add getMotionAxis method to helical, prismatic, revolute and ubounded revolute joint ([#2315](https://github.com/stack-of-tasks/pinocchio/pull/2315))