small update

gtdynamics/kinematics/Kinematics.h

@@ -181,7 +181,8 @@ class Kinematics : public Optimizer {
   /**
    * @fn Create a pose prior for a given link for each given pose.
    * @param context Slice or Interval instance.
-   * @param pose_goals goals for poses
+   * @param pose_goals an object of PoseGoals: map of time k to desired pose at
+   * that time, which will be used as mean of the prior
    * @returns graph with pose goal factors.
    */
   template <class CONTEXT>

gtdynamics/kinematics/KinematicsSlice.cpp

@@ -175,10 +175,10 @@ NonlinearFactorGraph Kinematics::jointAngleLimits<Slice>(
   NonlinearFactorGraph graph;
   for (auto&& joint : robot.joints()) {
     graph.add(JointLimitFactor(
-        JointAngleKey(joint->id(), slice.k), gtsam::noiseModel::Isotropic::Sigma(1, 0.00001),
+        JointAngleKey(joint->id(), slice.k), gtsam::noiseModel::Isotropic::Sigma(1, 0.001),
         joint->parameters().scalar_limits.value_lower_limit,
         joint->parameters().scalar_limits.value_upper_limit,
-        0.04));
+        0.04));//joint->parameters().scalar_limits.value_limit_threshold));
   }
   return graph;
 }

gtdynamics/pandarobot/CMakeLists.txt

@@ -1,4 +1,4 @@
-# add cablerobot subfolders to gtdynamics' SOURCE_SUBDIRS list
+# add pandarobot subfolders to gtdynamics' SOURCE_SUBDIRS list
 list(APPEND SOURCE_SUBDIRS pandarobot/ikfast pandarobot/gpmp)
 set(SOURCE_SUBDIRS ${SOURCE_SUBDIRS} PARENT_SCOPE)
 

gtdynamics/pandarobot/tests/testGaussianProcessPrior.cpp

@@ -0,0 +1,87 @@
+
+/**
+*  @file testGaussianProcessPrior.cpp
+*  @author Wanli Qian, Toni Jubes, Frank Dellaert
+**/
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/PriorFactor.h>
+
+
+#include <gtdynamics/pandarobot/gpmp/GaussianProcessPrior.h>
+
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+using namespace gtdynamics;
+
+
+/* ************************************************************************** */
+
+TEST(GaussianProcessPrior, Optimization) {
+
+  /**
+   * A simple graph:
+   *
+   * p1   p2
+   * |    |
+   * x1   x2
+   *  \  /
+   *   gp
+   *  /  \
+   * v1  v2
+   *
+   * p1 and p2 are pose prior factor to fix the poses, gp is the GP factor
+   * that get correct velocity of v1 and v2
+   */
+
+  noiseModel::Isotropic::shared_ptr model_prior =
+      noiseModel::Isotropic::Sigma(3, 0.001);
+
+  //noiseModel::Gaussian::shared_ptr Qc_model = noiseModel::Gaussian::Covariance(Qc);
+
+  double theta1 = 0;
+  double theta1d = 5;
+  double theta2 = 4;
+  double theta2d = 6;
+
+  double EXPECTEDfi = 7;
+  gtsam::Vector2 expected_error;
+  expected_error << 1 , -1;
+
+  double key1 = 0;
+  double key2 = 1;
+  double key3 = 2;
+  double key4 = 3;
+  double delta_t = 1;
+  double Qc = 2;
+
+  auto factor = GaussianProcessPrior(key1,key2,key3,key4,delta_t, Qc);
+  gtsam::Values v;
+  v.insert(key1,theta1);
+  v.insert(key2,theta1d);
+  v.insert(key3,theta2);
+  v.insert(key4,theta2d);
+  gtsam::Vector2 actual_error = factor.evaluateError(theta1,theta1d,theta2,theta2d);
+  double ACTUALfi = factor.error(v);
+
+EXPECT(assert_equal(expected_error,actual_error,1e-06))
+EXPECT_DOUBLES_EQUAL(EXPECTEDfi,ACTUALfi,1e-06)
+
+}
+
+/* ************************************************************************** */
+/* main function */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}