From 1da962185e068f48659c516e4f7655fb37e41b31 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sun, 11 Jun 2023 11:51:59 -0400 Subject: [PATCH 1/9] setLocalJCS( --- include/RigidBody/Segment.h | 7 +++++++ src/RigidBody/Segment.cpp | 9 +++++++++ 2 files changed, 16 insertions(+) diff --git a/include/RigidBody/Segment.h b/include/RigidBody/Segment.h index ba9427fc..ff67d8c8 100644 --- a/include/RigidBody/Segment.h +++ b/include/RigidBody/Segment.h @@ -210,6 +210,13 @@ class BIORBD_API Segment : public utils::Node /// utils::RotoTrans localJCS() const; + /// + /// \brief set the joint coordinate system (JCS) in the parent reference frame + /// \param rototrans The rototranslation object + /// + void SetLocalJCS( + utils::RotoTrans& rototrans); + /// /// \brief updateCharacteristics Change the inertia characteristics of the segment /// \param model The underlying model to update diff --git a/src/RigidBody/Segment.cpp b/src/RigidBody/Segment.cpp index cb458ebf..3d6778ae 100644 --- a/src/RigidBody/Segment.cpp +++ b/src/RigidBody/Segment.cpp @@ -7,6 +7,7 @@ #include "Utils/Matrix3d.h" #include "Utils/Vector3d.h" #include "Utils/RotoTrans.h" +#include "Utils/Rotation.h" #include "RigidBody/Joints.h" #include "RigidBody/Mesh.h" #include "RigidBody/SegmentCharacteristics.h" @@ -300,6 +301,14 @@ utils::RotoTrans rigidbody::Segment::localJCS() const m_cor->r); } +void rigidbody::Segment::SetLocalJCS(utils::RotoTrans &rototrans) +{ + *m_cor = RigidBodyDynamics::Math::SpatialTransform( + rototrans.rot().transpose(), + rototrans.trans()); +} + + void rigidbody::Segment::updateCharacteristics( rigidbody::Joints& model, const rigidbody::SegmentCharacteristics& characteristics) From 283340c389e9d930275f0f2d6d5686d24d603d39 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Tue, 13 Jun 2023 22:44:55 -0400 Subject: [PATCH 2/9] test --- test/test_rigidbody.cpp | 67 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 67 insertions(+) diff --git a/test/test_rigidbody.cpp b/test/test_rigidbody.cpp index 6e52d21a..1c31c385 100644 --- a/test/test_rigidbody.cpp +++ b/test/test_rigidbody.cpp @@ -82,6 +82,73 @@ TEST(Gravity, change) } } +TEST(Set, change) +{ + { + // Create a model and set the mass of the first segment + Model model(modelPathForGeneralTesting); + const utils::Scalar v = 0.1; + rigidbody::Segment segment = model.segments()[0]; + rigidbody::SegmentCharacteristics characteristics = segment.characteristics(); + characteristics.setMass(v); + + // Check that the mass was set correctly + EXPECT_NEAR(characteristics.mass(), 0.1, requiredPrecision); + } + { + //Create a model and set the center of mass of the first segment + Model model(modelPathForGeneralTesting); + utils::Vector3d v(1, 2, 3); + rigidbody::Segment segment = model.segments()[0]; + rigidbody::SegmentCharacteristics characteristics = segment.characteristics(); + characteristics.setCoM(v); + + // Check that the center of mass was set correctly + for (unsigned int i = 0; i < 3; ++i) { + EXPECT_NEAR(static_cast(characteristics.CoM()[i]), v[i], requiredPrecision); + } + } + { + Model model(modelPathForGeneralTesting); + utils::RotoTrans rt(1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1); + rigidbody::Segment segment = model.segments()[0]; + segment.SetLocalJCS(rt); + EXPECT_NEAR(static_cast(segment.localJCS()[0,0]), 1, + requiredPrecision); + EXPECT_NEAR(static_cast(segment.localJCS()[0,1]), 0, + requiredPrecision); + EXPECT_NEAR(static_cast(segment.localJCS()[0,2]), 0, + requiredPrecision); + EXPECT_NEAR(static_cast(segment.localJCS()[0,3]), 0, + requiredPrecision); + EXPECT_NEAR(static_cast(segment.localJCS()[1,0]), 0, + requiredPrecision); + EXPECT_NEAR(static_cast(segment.localJCS()[1,1]), 1, + requiredPrecision); + EXPECT_NEAR(static_cast(segment.localJCS()[1,2]), 0, + requiredPrecision); + EXPECT_NEAR(static_cast(segment.localJCS()[1,3]), 0, + requiredPrecision); + EXPECT_NEAR(static_cast(segment.localJCS()[2,0]), 0, + requiredPrecision); + EXPECT_NEAR(static_cast(segment.localJCS()[2,1]), 0, + requiredPrecision); + EXPECT_NEAR(static_cast(segment.localJCS()[2,2]), 1, + requiredPrecision); + EXPECT_NEAR(static_cast(segment.localJCS()[2,3]), 0, + requiredPrecision); + EXPECT_NEAR(static_cast(segment.localJCS()[3,0]), 0, + requiredPrecision); + EXPECT_NEAR(static_cast(segment.localJCS()[3,1]), 0, + requiredPrecision); + EXPECT_NEAR(static_cast(segment.localJCS()[3,2]), 0, + requiredPrecision); + EXPECT_NEAR(static_cast(segment.localJCS()[3,3]), 1, + requiredPrecision); + } + +} + TEST(Contacts, unitTest) { { From b66131d3f0392e086464f1238df9f6118b1c66b9 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Tue, 13 Jun 2023 23:13:03 -0400 Subject: [PATCH 3/9] go --- test/test_rigidbody.cpp | 36 ++++++++++++++++++++---------------- 1 file changed, 20 insertions(+), 16 deletions(-) diff --git a/test/test_rigidbody.cpp b/test/test_rigidbody.cpp index 1c31c385..7167a66a 100644 --- a/test/test_rigidbody.cpp +++ b/test/test_rigidbody.cpp @@ -113,37 +113,41 @@ TEST(Set, change) utils::RotoTrans rt(1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1); rigidbody::Segment segment = model.segments()[0]; segment.SetLocalJCS(rt); - EXPECT_NEAR(static_cast(segment.localJCS()[0,0]), 1, + // row 0 + EXPECT_NEAR(static_cast(segment.localJCS()(0)), 1, requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()[0,1]), 0, + EXPECT_NEAR(static_cast(segment.localJCS()(1)), 0, requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()[0,2]), 0, + EXPECT_NEAR(static_cast(segment.localJCS()(2)), 0, requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()[0,3]), 0, + EXPECT_NEAR(static_cast(segment.localJCS()(3)), 0, requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()[1,0]), 0, + // row 1 + EXPECT_NEAR(static_cast(segment.localJCS()(4)), 0, requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()[1,1]), 1, + EXPECT_NEAR(static_cast(segment.localJCS()(5)), 1, requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()[1,2]), 0, + EXPECT_NEAR(static_cast(segment.localJCS()(6)), 0, requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()[1,3]), 0, + EXPECT_NEAR(static_cast(segment.localJCS()(7)), 0, requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()[2,0]), 0, + // row 2 + EXPECT_NEAR(static_cast(segment.localJCS()(8)), 0, requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()[2,1]), 0, + EXPECT_NEAR(static_cast(segment.localJCS()(9)), 0, requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()[2,2]), 1, + EXPECT_NEAR(static_cast(segment.localJCS()(10)), 1, requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()[2,3]), 0, + EXPECT_NEAR(static_cast(segment.localJCS()(11)), 0, requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()[3,0]), 0, + // row 2 + EXPECT_NEAR(static_cast(segment.localJCS()(12)), 0, requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()[3,1]), 0, + EXPECT_NEAR(static_cast(segment.localJCS()(13)), 0, requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()[3,2]), 0, + EXPECT_NEAR(static_cast(segment.localJCS()(14)), 0, requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()[3,3]), 1, + EXPECT_NEAR(static_cast(segment.localJCS()(15)), 1, requiredPrecision); } From ec62c5fd609e6eb6325083a796cc6bd108a91156 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Thu, 15 Jun 2023 12:01:41 -0400 Subject: [PATCH 4/9] fisrt modif --- test/test_rigidbody.cpp | 67 ++++++++++++----------------------------- 1 file changed, 19 insertions(+), 48 deletions(-) diff --git a/test/test_rigidbody.cpp b/test/test_rigidbody.cpp index 7167a66a..e6245ad5 100644 --- a/test/test_rigidbody.cpp +++ b/test/test_rigidbody.cpp @@ -82,7 +82,7 @@ TEST(Gravity, change) } } -TEST(Set, change) +TEST(Characteristics, change) { { // Create a model and set the mass of the first segment @@ -92,20 +92,22 @@ TEST(Set, change) rigidbody::SegmentCharacteristics characteristics = segment.characteristics(); characteristics.setMass(v); + SCALAR_TO_DOUBLE(mass, characteristics.mass()); // Check that the mass was set correctly - EXPECT_NEAR(characteristics.mass(), 0.1, requiredPrecision); + EXPECT_NEAR(mass, 0.1, requiredPrecision); } { - //Create a model and set the center of mass of the first segment - Model model(modelPathForGeneralTesting); - utils::Vector3d v(1, 2, 3); - rigidbody::Segment segment = model.segments()[0]; - rigidbody::SegmentCharacteristics characteristics = segment.characteristics(); - characteristics.setCoM(v); - - // Check that the center of mass was set correctly - for (unsigned int i = 0; i < 3; ++i) { - EXPECT_NEAR(static_cast(characteristics.CoM()[i]), v[i], requiredPrecision); + //Create a model and set the center of mass of the first segment + Model model(modelPathForGeneralTesting); + utils::Vector3d v(1, 2, 3); + rigidbody::Segment segment = model.segments()[0]; + rigidbody::SegmentCharacteristics characteristics = segment.characteristics(); + characteristics.setCoM(v); + + // Check that the center of mass was set correctly + for (unsigned int i = 0; i < 3; ++i) { + SCALAR_TO_DOUBLE(com, characteristics.CoM()[i]); + EXPECT_NEAR(com, v[i], requiredPrecision); } } { @@ -113,42 +115,11 @@ TEST(Set, change) utils::RotoTrans rt(1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1); rigidbody::Segment segment = model.segments()[0]; segment.SetLocalJCS(rt); - // row 0 - EXPECT_NEAR(static_cast(segment.localJCS()(0)), 1, - requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()(1)), 0, - requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()(2)), 0, - requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()(3)), 0, - requiredPrecision); - // row 1 - EXPECT_NEAR(static_cast(segment.localJCS()(4)), 0, - requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()(5)), 1, - requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()(6)), 0, - requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()(7)), 0, - requiredPrecision); - // row 2 - EXPECT_NEAR(static_cast(segment.localJCS()(8)), 0, - requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()(9)), 0, - requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()(10)), 1, - requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()(11)), 0, - requiredPrecision); - // row 2 - EXPECT_NEAR(static_cast(segment.localJCS()(12)), 0, - requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()(13)), 0, - requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()(14)), 0, - requiredPrecision); - EXPECT_NEAR(static_cast(segment.localJCS()(15)), 1, - requiredPrecision); + std::vector expected={1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1}; + for (unsigned int i = 0; i < 16; ++i) { + SCALAR_TO_DOUBLE(value, segment.localJCS()(i)); + EXPECT_NEAR(value, expected[i], requiredPrecision); + } } } From 1294c568ce9f00ab11e67e3691ed22aecc7e79f8 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Thu, 15 Jun 2023 13:27:23 -0400 Subject: [PATCH 5/9] =?UTF-8?q?=C3=A7a=20marche?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- test/test_rigidbody.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/test/test_rigidbody.cpp b/test/test_rigidbody.cpp index e6245ad5..6f4528a3 100644 --- a/test/test_rigidbody.cpp +++ b/test/test_rigidbody.cpp @@ -100,6 +100,7 @@ TEST(Characteristics, change) //Create a model and set the center of mass of the first segment Model model(modelPathForGeneralTesting); utils::Vector3d v(1, 2, 3); + std::vector expected={1, 2, 3}; rigidbody::Segment segment = model.segments()[0]; rigidbody::SegmentCharacteristics characteristics = segment.characteristics(); characteristics.setCoM(v); @@ -108,6 +109,7 @@ TEST(Characteristics, change) for (unsigned int i = 0; i < 3; ++i) { SCALAR_TO_DOUBLE(com, characteristics.CoM()[i]); EXPECT_NEAR(com, v[i], requiredPrecision); + EXPECT_NEAR(com, expected[i], requiredPrecision); } } { @@ -115,7 +117,7 @@ TEST(Characteristics, change) utils::RotoTrans rt(1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1); rigidbody::Segment segment = model.segments()[0]; segment.SetLocalJCS(rt); - std::vector expected={1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1}; + std::vector expected={1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1}; for (unsigned int i = 0; i < 16; ++i) { SCALAR_TO_DOUBLE(value, segment.localJCS()(i)); EXPECT_NEAR(value, expected[i], requiredPrecision); From 9ab81693a550499b2870f466678088a440fa368f Mon Sep 17 00:00:00 2001 From: Ipuch Date: Thu, 15 Jun 2023 13:38:12 -0400 Subject: [PATCH 6/9] withdraw a line. --- test/test_rigidbody.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/test/test_rigidbody.cpp b/test/test_rigidbody.cpp index 6f4528a3..c5020807 100644 --- a/test/test_rigidbody.cpp +++ b/test/test_rigidbody.cpp @@ -108,7 +108,6 @@ TEST(Characteristics, change) // Check that the center of mass was set correctly for (unsigned int i = 0; i < 3; ++i) { SCALAR_TO_DOUBLE(com, characteristics.CoM()[i]); - EXPECT_NEAR(com, v[i], requiredPrecision); EXPECT_NEAR(com, expected[i], requiredPrecision); } } From 54808cac947092faaad7ff7ec7ff570aedd62bb9 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Thu, 15 Jun 2023 22:16:14 -0400 Subject: [PATCH 7/9] Auto stash before cherry pick of "hello test" --- test/test_rigidbody.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/test/test_rigidbody.cpp b/test/test_rigidbody.cpp index c5020807..d1de281e 100644 --- a/test/test_rigidbody.cpp +++ b/test/test_rigidbody.cpp @@ -95,6 +95,7 @@ TEST(Characteristics, change) SCALAR_TO_DOUBLE(mass, characteristics.mass()); // Check that the mass was set correctly EXPECT_NEAR(mass, 0.1, requiredPrecision); + std::cout << "hello " << std::endl; } { //Create a model and set the center of mass of the first segment @@ -110,6 +111,7 @@ TEST(Characteristics, change) SCALAR_TO_DOUBLE(com, characteristics.CoM()[i]); EXPECT_NEAR(com, expected[i], requiredPrecision); } + std::cout << "hello " << std::endl; } { Model model(modelPathForGeneralTesting); @@ -121,6 +123,7 @@ TEST(Characteristics, change) SCALAR_TO_DOUBLE(value, segment.localJCS()(i)); EXPECT_NEAR(value, expected[i], requiredPrecision); } + std::cout << "hello " << std::endl; } } From 5d9b71ce5bf80bf86ba936daf39b8f57448e04c2 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Thu, 15 Jun 2023 22:16:40 -0400 Subject: [PATCH 8/9] =?UTF-8?q?Auto=20stash=20before=20cherry=20pick=20of?= =?UTF-8?q?=20"=C3=A7a=20passssssse"?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- test/test_rigidbody.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/test/test_rigidbody.cpp b/test/test_rigidbody.cpp index d1de281e..a3a5d092 100644 --- a/test/test_rigidbody.cpp +++ b/test/test_rigidbody.cpp @@ -95,7 +95,6 @@ TEST(Characteristics, change) SCALAR_TO_DOUBLE(mass, characteristics.mass()); // Check that the mass was set correctly EXPECT_NEAR(mass, 0.1, requiredPrecision); - std::cout << "hello " << std::endl; } { //Create a model and set the center of mass of the first segment @@ -111,7 +110,6 @@ TEST(Characteristics, change) SCALAR_TO_DOUBLE(com, characteristics.CoM()[i]); EXPECT_NEAR(com, expected[i], requiredPrecision); } - std::cout << "hello " << std::endl; } { Model model(modelPathForGeneralTesting); @@ -120,12 +118,13 @@ TEST(Characteristics, change) segment.SetLocalJCS(rt); std::vector expected={1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1}; for (unsigned int i = 0; i < 16; ++i) { - SCALAR_TO_DOUBLE(value, segment.localJCS()(i)); + std::cout << "hello 0" << std::endl; + SCALAR_TO_DOUBLE(value, segment.localJCS()(i/4,i%4)); + std::cout << "hello 1" << std::endl; EXPECT_NEAR(value, expected[i], requiredPrecision); } - std::cout << "hello " << std::endl; + std::cout << "hello 2" << std::endl; } - } TEST(Contacts, unitTest) From 91bb444e1e11a6026edbaebc6d237e7c5f19634a Mon Sep 17 00:00:00 2001 From: Ipuch Date: Thu, 15 Jun 2023 22:14:25 -0400 Subject: [PATCH 9/9] Readyyy --- include/RigidBody/Segment.h | 4 +++- src/RigidBody/Segment.cpp | 4 +++- test/test_rigidbody.cpp | 5 +---- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/include/RigidBody/Segment.h b/include/RigidBody/Segment.h index ff67d8c8..268419aa 100644 --- a/include/RigidBody/Segment.h +++ b/include/RigidBody/Segment.h @@ -212,9 +212,11 @@ class BIORBD_API Segment : public utils::Node /// /// \brief set the joint coordinate system (JCS) in the parent reference frame + /// \param The model, it's need to update the underlying rbdl model /// \param rototrans The rototranslation object /// - void SetLocalJCS( + void setLocalJCS( + rigidbody::Joints& model, utils::RotoTrans& rototrans); /// diff --git a/src/RigidBody/Segment.cpp b/src/RigidBody/Segment.cpp index 3d6778ae..a24ff050 100644 --- a/src/RigidBody/Segment.cpp +++ b/src/RigidBody/Segment.cpp @@ -301,11 +301,13 @@ utils::RotoTrans rigidbody::Segment::localJCS() const m_cor->r); } -void rigidbody::Segment::SetLocalJCS(utils::RotoTrans &rototrans) +void rigidbody::Segment::setLocalJCS(rigidbody::Joints& model, utils::RotoTrans &rototrans) { *m_cor = RigidBodyDynamics::Math::SpatialTransform( rototrans.rot().transpose(), rototrans.trans()); + // we also modify RBDL spatial transform from parent to child + model.X_T[*m_idxDof->begin()]=*m_cor; } diff --git a/test/test_rigidbody.cpp b/test/test_rigidbody.cpp index a3a5d092..0af936ad 100644 --- a/test/test_rigidbody.cpp +++ b/test/test_rigidbody.cpp @@ -115,15 +115,12 @@ TEST(Characteristics, change) Model model(modelPathForGeneralTesting); utils::RotoTrans rt(1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1); rigidbody::Segment segment = model.segments()[0]; - segment.SetLocalJCS(rt); + segment.setLocalJCS(model, rt); std::vector expected={1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1}; for (unsigned int i = 0; i < 16; ++i) { - std::cout << "hello 0" << std::endl; SCALAR_TO_DOUBLE(value, segment.localJCS()(i/4,i%4)); - std::cout << "hello 1" << std::endl; EXPECT_NEAR(value, expected[i], requiredPrecision); } - std::cout << "hello 2" << std::endl; } }