Merge pull request #327 from Ipuch/master

[RTR] set LocalJCS of segments
pyomeca · Jun 22, 2023 · 638b0df · 638b0df
2 parents 432406d + 91bb444
commit 638b0df
Show file tree

Hide file tree

Showing 3 changed files with 62 additions and 0 deletions.
diff --git a/include/RigidBody/Segment.h b/include/RigidBody/Segment.h
@@ -210,6 +210,15 @@ class BIORBD_API Segment : public utils::Node
     ///
     utils::RotoTrans localJCS() const;
 
+    ///
+    /// \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(
+        rigidbody::Joints& model,
+        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
@@ -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,16 @@ utils::RotoTrans rigidbody::Segment::localJCS() const
             m_cor->r);
 }
 
+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;
+}
+
+
 void rigidbody::Segment::updateCharacteristics(
     rigidbody::Joints& model,
     const rigidbody::SegmentCharacteristics& characteristics)

diff --git a/test/test_rigidbody.cpp b/test/test_rigidbody.cpp
@@ -82,6 +82,48 @@ TEST(Gravity, change)
     }
 }
 
+TEST(Characteristics, 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);
+
+        SCALAR_TO_DOUBLE(mass, characteristics.mass());
+        // Check that the mass was set correctly
+        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);
+        std::vector<double> expected={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, expected[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(model, rt);
+        std::vector<double> 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/4,i%4));
+            EXPECT_NEAR(value, expected[i], requiredPrecision);
+        }
+    }
+}
+
 TEST(Contacts, unitTest)
 {
     {