Added a setter for the markers

include/RigidBody/Markers.h

@@ -11,6 +11,7 @@ namespace utils
 {
 class String;
 class Matrix;
+class Vector3d;
 }
 
 namespace rigidbody
@@ -74,6 +75,16 @@ class BIORBD_API Markers
         int id = -1
     );
 
+    /// 
+    /// \brief Change the values of a specific marker without changing other parameters
+    /// \param index The index of the marker to change. If biorbd is compiled with SKIP_ASSERT, then no check is performed to ensure index is within bounds
+    /// \param pos The new position of the marker
+    /// 
+    void setMarker(
+        size_t index,
+        const utils::Vector3d &pos
+    );
+
     ///
     /// \brief Return the marker of index idx
     /// \param idx The marker we want to return
@@ -159,7 +170,7 @@ class BIORBD_API Markers
         bool updateKin = true);
 
     ///
-    /// \brief Return all the markers in their respective parent reference frame
+    /// \brief Return a copy of all the markers in their respective parent reference frame
     /// \param removeAxis If there are axis to remove from the position variables
     /// \return All the markers
     ///

include/RigidBody/NodeSegment.h

@@ -119,6 +119,12 @@ class BIORBD_API NodeSegment : public utils::Vector3d
 
     // Get and Set
 
+    /// 
+    /// \brief Set internal values of the node without changing other parameters
+    /// \param values The new values
+    /// 
+    void setValues(const Vector3d& values);
+
     ///
     /// \brief Return if node is technical
     /// \return If node is technical

src/RigidBody/Markers.cpp

@@ -5,6 +5,7 @@
 #include <rbdl/Kinematics.h>
 #include "Utils/String.h"
 #include "Utils/Matrix.h"
+#include "Utils/Error.h"
 #include "RigidBody/GeneralizedCoordinates.h"
 #include "RigidBody/GeneralizedVelocity.h"
 #include "RigidBody/GeneralizedAcceleration.h"
@@ -57,11 +58,22 @@ void rigidbody::Markers::addMarker(
     const utils::String& axesToRemove,
     int id)
 {
-    rigidbody::NodeSegment tp(pos, name, parentName, technical, anatomical,
-                                      axesToRemove, id);
+    rigidbody::NodeSegment tp(
+        pos, name, parentName, technical, anatomical, axesToRemove, id);
     m_marks->push_back(tp);
 }
 
+
+void rigidbody::Markers::setMarker(
+    size_t index,
+    const utils::Vector3d& pos) 
+{
+#ifndef SKIP_ASSERT
+    utils::Error::check(index < m_marks->size(), utils::String("The index ") + index + " is larger than the number of markers (" + m_marks->size() + ")");
+#endif // SKIP_ASSERT
+    (*m_marks)[index].setValues(pos);
+}
+
 const rigidbody::NodeSegment &rigidbody::Markers::marker(
     size_t idx) const
 {
@@ -161,7 +173,7 @@ std::vector<rigidbody::NodeSegment> rigidbody::Markers::markers(
 {
     std::vector<rigidbody::NodeSegment> pos;
     for (size_t i=0; i<nbMarkers(); ++i) {
-        pos.push_back(marker(i, removeAxis));    // Forward kinematics
+        pos.push_back(marker(i, removeAxis)); 
     }
 
     return pos;

src/RigidBody/NodeSegment.cpp

@@ -107,8 +107,8 @@ rigidbody::NodeSegment rigidbody::NodeSegment::DeepCopy() const
     return copy;
 }
 
-void rigidbody::NodeSegment::DeepCopy(const
-        rigidbody::NodeSegment&other)
+void rigidbody::NodeSegment::DeepCopy(
+    const rigidbody::NodeSegment& other)
 {
     utils::Node::DeepCopy(other);
     *m_axesRemoved = *other.m_axesRemoved;
@@ -118,6 +118,12 @@ void rigidbody::NodeSegment::DeepCopy(const
     *m_id = *other.m_id;
 }
 
+void rigidbody::NodeSegment::setValues(
+    const utils::Vector3d& other) 
+{
+    *this = other;
+}
+
 
 bool rigidbody::NodeSegment::isAnatomical() const
 {

test/test_rigidbody.cpp

@@ -994,6 +994,44 @@ TEST(Markers, copy)
     }
 }
 
+TEST(Markers, set)
+{
+    Model model(modelPathForGeneralTesting);
+    rigidbody::Markers markers(model);
+
+    auto marker0 = markers.markers()[0];
+    EXPECT_STREQ(marker0.name().c_str(), "pelv1");
+    EXPECT_STREQ(marker0.parent().c_str(), "Pelvis");
+    EXPECT_EQ(marker0.isTechnical(), true);
+    EXPECT_EQ(marker0.isAnatomical(), false);
+    EXPECT_STREQ(marker0.axesToRemoveAsString().c_str(), "");
+    EXPECT_EQ(marker0.parentId(), 3);
+
+    SCALAR_TO_DOUBLE(xValuePre, marker0.x());
+    SCALAR_TO_DOUBLE(yValuePre, marker0.y());
+    SCALAR_TO_DOUBLE(zValuePre, marker0.z());
+    EXPECT_NEAR(xValuePre, -0.1038, requiredPrecision);
+    EXPECT_NEAR(yValuePre, 0.0821, requiredPrecision);
+    EXPECT_NEAR(zValuePre, 0.0, requiredPrecision);
+
+    markers.setMarker(0, utils::Vector3d(1, 2, 3));
+    marker0 = markers.markers()[0];
+    EXPECT_STREQ(marker0.name().c_str(), "pelv1");
+    EXPECT_STREQ(marker0.parent().c_str(), "Pelvis");
+    EXPECT_EQ(marker0.isTechnical(), true);
+    EXPECT_EQ(marker0.isAnatomical(), false);
+    EXPECT_STREQ(marker0.axesToRemoveAsString().c_str(), "");
+    EXPECT_EQ(marker0.parentId(), 3);
+
+    SCALAR_TO_DOUBLE(xValuePost, marker0.x());
+    SCALAR_TO_DOUBLE(yValuePost, marker0.y());
+    SCALAR_TO_DOUBLE(zValuePost, marker0.z());
+    EXPECT_NEAR(xValuePost, 1.0, requiredPrecision);
+    EXPECT_NEAR(yValuePost, 2.0, requiredPrecision);
+    EXPECT_NEAR(zValuePost, 3.0, requiredPrecision);
+
+}
+
 TEST(SegmentCharacteristics, length)
 {
     rigidbody::SegmentCharacteristics segmentCharac;