add gaussian_noise to Statics::solve and default to 0.0

borglab · Oct 19, 2023 · 12abb15 · 12abb15
1 parent 0afa9df
commit 12abb15
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 3 deletions.
diff --git a/gtdynamics/statics/Statics.h b/gtdynamics/statics/Statics.h
@@ -124,9 +124,11 @@ class Statics : public Kinematics {
    * @param slice Slice instance.
    * @param robot Robot specification from URDF/SDF.
    * @param configuration A known kinematics configuration.
+   * @param gaussian_noise noise (stddev) added to initial values (default 0.0).
    */
   gtsam::Values solve(const Slice& slice, const Robot& robot,
-                      const gtsam::Values& configuration) const;
+                      const gtsam::Values& configuration,
+                      double gaussian_noise = 0.0) const;
 
   /**
    * Solve for wrenches and kinematics configuration.

diff --git a/gtdynamics/statics/StaticsSlice.cpp b/gtdynamics/statics/StaticsSlice.cpp
@@ -112,10 +112,11 @@ gtsam::Values Statics::initialValues(const Slice& slice, const Robot& robot,
 }
 
 gtsam::Values Statics::solve(const Slice& slice, const Robot& robot,
-                             const gtsam::Values& configuration) const {
+                             const gtsam::Values& configuration,
+                             double gaussian_noise) const {
   auto graph = this->graph(slice, robot);
   gtsam::Values initial_values;
-  initial_values.insert(initialValues(slice, robot));
+  initial_values.insert(initialValues(slice, robot, gaussian_noise));
 
   // In this function we assume the kinematics is given, and we add priors to
   // the graph to enforce this. Would be much nicer with constant expressions.