From 5b13133e8cd76df3ccb58b43a46883d53ae7fcd1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 14 Feb 2022 16:37:18 -0500 Subject: [PATCH 01/12] test IK for Panda, initial check-in --- tests/testKinematicsSlice.cpp | 81 +++++++++++++++++++++++++++++++++++ 1 file changed, 81 insertions(+) diff --git a/tests/testKinematicsSlice.cpp b/tests/testKinematicsSlice.cpp index 4b329809..9da2b2d2 100644 --- a/tests/testKinematicsSlice.cpp +++ b/tests/testKinematicsSlice.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include "contactGoalsExample.h" @@ -81,6 +82,86 @@ TEST(Slice, InverseKinematics) { } } +// Values tonisFunction(const Slice& slice, const Robot& robot, +// const ContactGoals& contact_goals) const { +// auto graph = this->graph(slice, robot); + +// // Add prior. +// graph.add(jointAngleObjectives(slice, robot)); + +// graph.addPrior(PoseKey(0, slice.k), +// gtsam::Pose3(), nullptr); + +// auto initial_values = initialValues(slice, robot); + +// return optimize(graph, initial_values); +// } + +gtsam::Values ikWithPose(const Kinematics* self, const Slice& slice, + const Robot& robot, + const std::map constrained_poses, + const std::map desired_poses, + const gtsam::Vector7& initial) { + auto graph = self->graph(slice, robot); + + // Add prior on joint angles to constrain the solution + graph.add(self->jointAngleObjectives(slice, robot)); + + // Add priors on link poses with constrained poses from argument + // TODO: do real pose constraints - ask Yetong + for (auto&& kv : constrained_poses) { + const size_t link_index = kv.first; + const gtsam::Pose3& constrained_pose = kv.second; + graph.addPrior(PoseKey(link_index, slice.k), constrained_pose, + gtsam::noiseModel::Constrained::All(6)); + } + + // Add priors on link poses with desired poses from argument + for (auto&& kv : desired_poses) { + const size_t link_index = kv.first; + const gtsam::Pose3& desired_pose = kv.second; + graph.addPrior(PoseKey(link_index, slice.k), desired_pose, + nullptr); + } + + // TODO: make use of initial? + auto initial_values = self->initialValues(slice, robot); + + return self->optimize(graph, {}, initial_values); +} + +TEST(Slice, PandaIK) { + using gtsam::Pose3; + using gtsam::Rot3; + using gtsam::Values; + using gtsam::Vector7; + + const Robot panda = + CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); + const Robot robot = panda.fixLink("link0"); + + // Create a slice. + const size_t k = 777; + const Slice slice(k); + + // Instantiate kinematics algorithms + KinematicsParameters parameters; + parameters.method = OptimizationParameters::Method::AUGMENTED_LAGRANGIAN; + Kinematics kinematics(parameters); + + Vector7 initial = Vector7::Zero(); + Rot3 sR7({{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}); + Pose3 sM7(sR7, Point3(0.0882972, 0.00213401, 0.933844)); + auto base_link = robot.link("link0"); + const Pose3 sM0 = base_link->bMcom(); + auto values = + ikWithPose(&kinematics, slice, robot, {{0, sM0}}, {{7, sM7}}, initial); + Vector7 optimal_q; + for (size_t j = 0; j < 7; j++) + optimal_q[j] = values.at(JointAngleKey(j, slice.k)); + EXPECT(assert_equal(initial, optimal_q)); +} + int main() { TestResult tr; return TestRegistry::runAllTests(tr); From 5197e2325b6ae05cdb051298c805bdd166f2f6e0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 14 Feb 2022 17:27:01 -0500 Subject: [PATCH 02/12] Make fix/unfix methods const --- gtdynamics/universal_robot/Robot.cpp | 4 ++-- gtdynamics/universal_robot/Robot.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtdynamics/universal_robot/Robot.cpp b/gtdynamics/universal_robot/Robot.cpp index 48ea48e6..6857f26c 100644 --- a/gtdynamics/universal_robot/Robot.cpp +++ b/gtdynamics/universal_robot/Robot.cpp @@ -77,7 +77,7 @@ LinkSharedPtr Robot::link(const std::string &name) const { return name_to_link_.at(name); } -Robot Robot::fixLink(const std::string &name) { +Robot Robot::fixLink(const std::string &name) const { if (name_to_link_.find(name) == name_to_link_.end()) { throw std::runtime_error("no link named " + name); } @@ -87,7 +87,7 @@ Robot Robot::fixLink(const std::string &name) { return fixed_robot; } -Robot Robot::unfixLink(const std::string &name) { +Robot Robot::unfixLink(const std::string &name) const { if (name_to_link_.find(name) == name_to_link_.end()) { throw std::runtime_error("no link named " + name); } diff --git a/gtdynamics/universal_robot/Robot.h b/gtdynamics/universal_robot/Robot.h index 6296ce6d..1ea88ef6 100644 --- a/gtdynamics/universal_robot/Robot.h +++ b/gtdynamics/universal_robot/Robot.h @@ -89,7 +89,7 @@ class Robot { * @param name The name of the link to fix. * @return Robot */ - Robot fixLink(const std::string &name); + Robot fixLink(const std::string &name) const; /** * @brief Return a copy of this robot after unfixing the link corresponding to @@ -98,7 +98,7 @@ class Robot { * @param name The name of the link to unfix. * @return Robot */ - Robot unfixLink(const std::string &name); + Robot unfixLink(const std::string &name) const; /// Return the joint corresponding to the input string. JointSharedPtr joint(const std::string &name) const; From ea19ce1a563539316ff1ab6b7eaae82b55bfef8a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 14 Feb 2022 18:10:39 -0500 Subject: [PATCH 03/12] Removed logmap and use gtsam --- gtdynamics/universal_robot/Joint.cpp | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/gtdynamics/universal_robot/Joint.cpp b/gtdynamics/universal_robot/Joint.cpp index cbb3fd1b..57bd5e63 100644 --- a/gtdynamics/universal_robot/Joint.cpp +++ b/gtdynamics/universal_robot/Joint.cpp @@ -266,15 +266,6 @@ std::ostream &operator<<(std::ostream &os, const JointSharedPtr &j) { return j->to_stream(os); } -/* ************************************************************************* */ -// TODO(yetong): Remove the logmap and use the one in gtsam/slam/expressions.h. -template -gtsam::Expression::TangentVector> logmap( - const gtsam::Expression &x1, const gtsam::Expression &x2) { - return gtsam::Expression::TangentVector>( - gtsam::traits::Logmap, between(x1, x2)); -} - /* ************************************************************************* */ gtsam::Vector6_ Joint::poseConstraint(uint64_t t) const { using gtsam::Pose3_; @@ -291,7 +282,7 @@ gtsam::Vector6_ Joint::poseConstraint(uint64_t t) const { Pose3_ wTc_hat = wTp * pTc; // Return the error in tangent space - return gtdynamics::logmap(wTc, wTc_hat); + return gtsam::logmap(wTc, wTc_hat); } /* ************************************************************************* */ From 54b26c103231c3cc6247d43c600832956d06f6b5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 14 Feb 2022 18:10:58 -0500 Subject: [PATCH 04/12] Add constraints for fixed links --- gtdynamics/kinematics/KinematicsSlice.cpp | 21 +++++ tests/testKinematicsSlice.cpp | 95 ++++++++++++++--------- 2 files changed, 78 insertions(+), 38 deletions(-) diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index 852ba885..9353fcfa 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -18,6 +18,7 @@ #include #include #include +#include namespace gtdynamics { @@ -63,6 +64,26 @@ EqualityConstraints Kinematics::constraints(const Slice& slice, tolerance); } + // Constrain fixed links + for (auto&& link : robot.links()) { + if (link->isFixed()) { + using gtsam::Pose3_; + + // Get an expression for the unknown link pose. + Pose3_ bTcom(PoseKey(link->id(), slice.k)); + + // Kust make sure it does not move from its original rest pose + Pose3_ bMcom(link->bMcom()); + + // Create expression to calculate the error in tangent space + auto constraint_expr = gtsam::logmap(bTcom, bMcom); + + // Add constriant + constraints.emplace_shared>(constraint_expr, + tolerance); + } + } + return constraints; } diff --git a/tests/testKinematicsSlice.cpp b/tests/testKinematicsSlice.cpp index 9da2b2d2..e053ae33 100644 --- a/tests/testKinematicsSlice.cpp +++ b/tests/testKinematicsSlice.cpp @@ -24,21 +24,21 @@ using gtsam::Point3; using std::map; using std::string; +// Create a slice. +static const size_t k = 777; +static const Slice kSlice(k); + TEST(Slice, InverseKinematics) { // Load robot and establish contact/goal pairs using namespace contact_goals_example; - // Create a slice. - const size_t k = 777; - const Slice slice(k); - // Instantiate kinematics algorithms KinematicsParameters parameters; parameters.method = OptimizationParameters::Method::AUGMENTED_LAGRANGIAN; Kinematics kinematics(parameters); // Create initial values - auto values = kinematics.initialValues(slice, robot, 0.0); + auto values = kinematics.initialValues(kSlice, robot, 0.0); EXPECT_LONGS_EQUAL(13 + 12, values.size()); // Set twists to zero for FK. TODO(frank): separate kinematics from velocity? @@ -57,16 +57,16 @@ TEST(Slice, InverseKinematics) { EXPECT(goal.satisfied(fk, k, 0.05)); } - auto graph = kinematics.graph(slice, robot); + auto graph = kinematics.graph(kSlice, robot); EXPECT_LONGS_EQUAL(12, graph.size()); - auto objectives = kinematics.pointGoalObjectives(slice, contact_goals); + auto objectives = kinematics.pointGoalObjectives(kSlice, contact_goals); EXPECT_LONGS_EQUAL(4, objectives.size()); - auto objectives2 = kinematics.jointAngleObjectives(slice, robot); + auto objectives2 = kinematics.jointAngleObjectives(kSlice, robot); EXPECT_LONGS_EQUAL(12, objectives2.size()); - auto result = kinematics.inverse(slice, robot, contact_goals); + auto result = kinematics.inverse(kSlice, robot, contact_goals); // Check that well-determined graph.add(objectives); @@ -82,52 +82,59 @@ TEST(Slice, InverseKinematics) { } } -// Values tonisFunction(const Slice& slice, const Robot& robot, +// Values tonisFunction(const Slice& kSlice, const Robot& robot, // const ContactGoals& contact_goals) const { -// auto graph = this->graph(slice, robot); +// auto graph = this->graph(kSlice, robot); // // Add prior. -// graph.add(jointAngleObjectives(slice, robot)); +// graph.add(jointAngleObjectives(kSlice, robot)); -// graph.addPrior(PoseKey(0, slice.k), +// graph.addPrior(PoseKey(0, k), // gtsam::Pose3(), nullptr); -// auto initial_values = initialValues(slice, robot); +// auto initial_values = initialValues(kSlice, robot); // return optimize(graph, initial_values); // } -gtsam::Values ikWithPose(const Kinematics* self, const Slice& slice, +gtsam::Values ikWithPose(const Kinematics* self, const Slice& kSlice, const Robot& robot, - const std::map constrained_poses, const std::map desired_poses, const gtsam::Vector7& initial) { - auto graph = self->graph(slice, robot); + auto graph = self->graph(kSlice, robot); // Add prior on joint angles to constrain the solution - graph.add(self->jointAngleObjectives(slice, robot)); - - // Add priors on link poses with constrained poses from argument - // TODO: do real pose constraints - ask Yetong - for (auto&& kv : constrained_poses) { - const size_t link_index = kv.first; - const gtsam::Pose3& constrained_pose = kv.second; - graph.addPrior(PoseKey(link_index, slice.k), constrained_pose, - gtsam::noiseModel::Constrained::All(6)); - } + graph.add(self->jointAngleObjectives(kSlice, robot)); // Add priors on link poses with desired poses from argument for (auto&& kv : desired_poses) { const size_t link_index = kv.first; const gtsam::Pose3& desired_pose = kv.second; - graph.addPrior(PoseKey(link_index, slice.k), desired_pose, - nullptr); + graph.addPrior(PoseKey(link_index, k), desired_pose, nullptr); } + // Robot kinematics constraints + auto constraints = self->constraints(kSlice, robot); + // TODO: make use of initial? - auto initial_values = self->initialValues(slice, robot); + auto initial_values = self->initialValues(kSlice, robot); - return self->optimize(graph, {}, initial_values); + return self->optimize(graph, constraints, initial_values); +} + +TEST(Slice, panda_constraints) { + const Robot panda = + CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); + const Robot robot = panda.fixLink("link0"); + + // Instantiate kinematics algorithms + KinematicsParameters parameters; + parameters.method = OptimizationParameters::Method::AUGMENTED_LAGRANGIAN; + Kinematics kinematics(parameters); + + // We should only have 7 constraints plus one for the fixed link + auto constraints = kinematics.constraints(kSlice, robot); + EXPECT_LONGS_EQUAL(8, constraints.size()); } TEST(Slice, PandaIK) { @@ -140,25 +147,37 @@ TEST(Slice, PandaIK) { CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); const Robot robot = panda.fixLink("link0"); - // Create a slice. - const size_t k = 777; - const Slice slice(k); - // Instantiate kinematics algorithms KinematicsParameters parameters; parameters.method = OptimizationParameters::Method::AUGMENTED_LAGRANGIAN; Kinematics kinematics(parameters); + // We should only have 7 unknown joint angles and That is still 7 factors. + auto graph = kinematics.graph(kSlice, robot); + EXPECT_LONGS_EQUAL(7, graph.size()); + + // We should only have 8 unknown links and 7 unknown joint angles, i.e., 15 + // values: + auto initial_values = kinematics.initialValues(kSlice, robot); + EXPECT_LONGS_EQUAL(15, initial_values.size()); + Vector7 initial = Vector7::Zero(); Rot3 sR7({{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}); Pose3 sM7(sR7, Point3(0.0882972, 0.00213401, 0.933844)); auto base_link = robot.link("link0"); + auto values = ikWithPose(&kinematics, kSlice, robot, {{7, sM7}}, initial); + + // Check that base link did not budge (much) const Pose3 sM0 = base_link->bMcom(); - auto values = - ikWithPose(&kinematics, slice, robot, {{0, sM0}}, {{7, sM7}}, initial); + EXPECT(assert_equal(sM0, values.at(PoseKey(0, k)))); + + // Check that desired pose was achieved + EXPECT(assert_equal(sM7, values.at(PoseKey(7, k)))); + + // Check joint angles Vector7 optimal_q; for (size_t j = 0; j < 7; j++) - optimal_q[j] = values.at(JointAngleKey(j, slice.k)); + optimal_q[j] = values.at(JointAngleKey(j, k)); EXPECT(assert_equal(initial, optimal_q)); } From 7e4440218e0fbd3b10a4e0349bd35494774c5a1e Mon Sep 17 00:00:00 2001 From: Joobz Date: Wed, 16 Feb 2022 17:34:47 +0100 Subject: [PATCH 05/12] initial values function prototype --- tests/testKinematicsSlice.cpp | 81 +++++++++++++++++++++++++++++------ 1 file changed, 68 insertions(+), 13 deletions(-) diff --git a/tests/testKinematicsSlice.cpp b/tests/testKinematicsSlice.cpp index e053ae33..ac57c444 100644 --- a/tests/testKinematicsSlice.cpp +++ b/tests/testKinematicsSlice.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include "contactGoalsExample.h" @@ -97,7 +98,25 @@ TEST(Slice, InverseKinematics) { // return optimize(graph, initial_values); // } -gtsam::Values ikWithPose(const Kinematics* self, const Slice& kSlice, +gtsam::Values initialValues(const Slice& slice, const Robot& robot, + const gtsam::Vector7& initial) { + gtsam::Values values; + + // Initialize all joint angles. + for (auto&& joint : robot.joints()) { + InsertJointAngle(&values, joint->id(), slice.k, initial(joint->id())); + } + + // Initialize poses with fk of initialized values + auto fk = robot.forwardKinematics(values, k); + for (auto&& link : robot.links()) { + InsertPose(&values, link->id(), slice.k, fk.at(PoseKey(link->id(), k))); + } + + return values; +} + +gtsam::Values inverseWithPose(const Kinematics* self, const Slice& kSlice, const Robot& robot, const std::map desired_poses, const gtsam::Vector7& initial) { @@ -110,18 +129,47 @@ gtsam::Values ikWithPose(const Kinematics* self, const Slice& kSlice, for (auto&& kv : desired_poses) { const size_t link_index = kv.first; const gtsam::Pose3& desired_pose = kv.second; - graph.addPrior(PoseKey(link_index, k), desired_pose, nullptr); + // TODO: put it as the kinematic parameters + graph.addPrior(PoseKey(link_index, k), desired_pose, gtsam::noiseModel::Isotropic::Sigma(6,0.00005)); } // Robot kinematics constraints auto constraints = self->constraints(kSlice, robot); - // TODO: make use of initial? - auto initial_values = self->initialValues(kSlice, robot); + auto initial_values = initialValues(kSlice, robot, initial); return self->optimize(graph, constraints, initial_values); } +TEST(Slice, initial_values) { + + const Robot panda = + CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); + const Robot robot = panda.fixLink("link0"); + + gtsam::Vector7 initial; + initial << 0.1, 0.2, 0.3, -0.4, 0.5, 0.6, 0.7; + + gtsam::Values initial_values = initialValues(kSlice, robot, initial); + + // We should only have 7 values for joints and 8 for link poses + EXPECT_LONGS_EQUAL(15, initial_values.size()) + + // check that joint angles are the same + gtsam::Vector7 actual_initial; + for (size_t j = 0; j < 7; j++) + actual_initial[j] = initial_values.at(JointAngleKey(j, k)); + double tol = 1e-5; + EXPECT(assert_equal(initial, actual_initial, tol)) + + // check that the last fk is the same + gtsam::Rot3 sR7 {{ 0.98161623, 0.13223102, -0.13763916}, + { 0.08587125, -0.9499891 , -0.30024463}, + {-0.17045735, 0.28290575, -0.94387956}}; + gtsam::Pose3 sT7(sR7, Point3(0.323914, 0.167266, 0.905973)); + EXPECT(assert_equal(sT7, initial_values.at(PoseKey(7,k)), tol)) +} + TEST(Slice, panda_constraints) { const Robot panda = CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); @@ -161,24 +209,31 @@ TEST(Slice, PandaIK) { auto initial_values = kinematics.initialValues(kSlice, robot); EXPECT_LONGS_EQUAL(15, initial_values.size()); - Vector7 initial = Vector7::Zero(); - Rot3 sR7({{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}); - Pose3 sM7(sR7, Point3(0.0882972, 0.00213401, 0.933844)); - auto base_link = robot.link("link0"); - auto values = ikWithPose(&kinematics, kSlice, robot, {{7, sM7}}, initial); + Rot3 sR7 {{ 0.98161623, 0.13223102, -0.13763916}, + { 0.08587125, -0.9499891 , -0.30024463}, + {-0.17045735, 0.28290575, -0.94387956}}; + Pose3 sT7(sR7, Point3(0.323914, 0.167266, 0.905973)); + Vector7 initial; + initial << 0.1, 0.2, 0.3, -0.4, 0.5, 0.6, 0.7; + Vector7 noise; + noise << 0.04, -0.1, 0.07, 0.14, -0.05, 0.02, 0.1; + auto values = inverseWithPose(&kinematics, kSlice, robot, {{7, sT7}}, initial+noise); // Check that base link did not budge (much) + auto base_link = robot.link("link0"); const Pose3 sM0 = base_link->bMcom(); - EXPECT(assert_equal(sM0, values.at(PoseKey(0, k)))); + double tol = 1e-5; + EXPECT(assert_equal(sM0, values.at(PoseKey(0, k)), tol)); // Check that desired pose was achieved - EXPECT(assert_equal(sM7, values.at(PoseKey(7, k)))); + EXPECT(assert_equal(sT7, values.at(PoseKey(7, k)), tol)); - // Check joint angles + // Check that joint angles are not too different Vector7 optimal_q; for (size_t j = 0; j < 7; j++) optimal_q[j] = values.at(JointAngleKey(j, k)); - EXPECT(assert_equal(initial, optimal_q)); + tol = 0.001; + EXPECT(assert_equal(initial, optimal_q, tol)); } int main() { From e5343447c6dba658ffd9c846cd2a31af45155477 Mon Sep 17 00:00:00 2001 From: Joobz Date: Wed, 23 Feb 2022 01:24:05 +0100 Subject: [PATCH 06/12] Added new functions in kinematics --- gtdynamics/kinematics/Kinematics.h | 46 ++++- gtdynamics/kinematics/KinematicsInterval.cpp | 19 +- gtdynamics/kinematics/KinematicsSlice.cpp | 80 +++++++- .../kinematics/KinematicsTrajectory.cpp | 18 +- tests/testKinematicsSlice.cpp | 190 +++++++++++------- 5 files changed, 254 insertions(+), 99 deletions(-) diff --git a/gtdynamics/kinematics/Kinematics.h b/gtdynamics/kinematics/Kinematics.h index 4d8d681f..4cf00b8c 100644 --- a/gtdynamics/kinematics/Kinematics.h +++ b/gtdynamics/kinematics/Kinematics.h @@ -131,29 +131,49 @@ class Kinematics : public Optimizer { EqualityConstraints pointGoalConstraints( const CONTEXT& context, const ContactGoals& contact_goals) const; + /** + * @fn Create pose goal objectives. + * @param context Slice or Interval instance. + * @param pose_goals goals for poses + * @returns graph with pose goal factors. + */ + template + gtsam::NonlinearFactorGraph poseGoalObjectives( + const CONTEXT& context, const Robot& robot, + const gtsam::Values& pose_goals) const; + /** * @fn Factors that minimize joint angles. * @param context Slice or Interval instance. * @param robot Robot specification from URDF/SDF. + * @param joint_priors Values where the mean of the priors is specified * @returns graph with prior factors on joint angles. */ template - gtsam::NonlinearFactorGraph jointAngleObjectives(const CONTEXT& context, - const Robot& robot) const; + gtsam::NonlinearFactorGraph jointAngleObjectives( + const CONTEXT& context, const Robot& robot, + const gtsam::Values& joint_priors = gtsam::Values()) const; /** * @fn Initialize kinematics. * - * Use wTcom for poses and zero-mean noise for joint angles. + * If no values in initial_joints are given, use wTcom for poses and zero-mean + * noise for joint angles. + * If values are given, initialize joints with their given values, and + * zero-mean noise to the ones that without a given value. Poses are + * initialized with their fk values. + * * * @param context Slice or Interval instance. * @param robot Robot specification from URDF/SDF. * @param gaussian_noise time step to check (default 0.1). + * @param initial_joints initial values for joints * @returns values with identity poses and zero joint angles. */ template - gtsam::Values initialValues(const CONTEXT& context, const Robot& robot, - double gaussian_noise = 0.1) const; + gtsam::Values initialValues( + const CONTEXT& context, const Robot& robot, double gaussian_noise = 0.1, + const gtsam::Values& initial_joints = gtsam::Values()) const; /** * @fn Inverse kinematics given a set of contact goals. @@ -169,6 +189,22 @@ class Kinematics : public Optimizer { const ContactGoals& contact_goals, bool contact_goals_as_constraints = true) const; + /** + * @fn Inverse kinematics given a set of desired poses + * @fn This function does inverse kinematics separately on each slice + * @param context Slice or Interval instance + * @param robot Robot specification from URDF/SDF + * @param goal_poses goals for EE poses + * @param joint_priors optional argument to put priors centered on given + * values. If empty, the priors will be centered on the origin + * @return values with poses and joint angles + */ + template + gtsam::Values inverseWithPose( + const CONTEXT& context, const Robot& robot, + const gtsam::Values& goal_poses, + const gtsam::Values& joint_priors = gtsam::Values()) const; + /** * Interpolate using inverse kinematics: the goals are linearly interpolated. * @param context Interval instance diff --git a/gtdynamics/kinematics/KinematicsInterval.cpp b/gtdynamics/kinematics/KinematicsInterval.cpp index 5e571736..e132c67c 100644 --- a/gtdynamics/kinematics/KinematicsInterval.cpp +++ b/gtdynamics/kinematics/KinematicsInterval.cpp @@ -44,6 +44,17 @@ EqualityConstraints Kinematics::constraints( return constraints; } +template <> +NonlinearFactorGraph Kinematics::poseGoalObjectives( + const Interval& interval, const Robot& robot, + const gtsam::Values& goal_poses) const { + NonlinearFactorGraph graph; + for (size_t k = interval.k_start; k <= interval.k_end; k++) { + graph.add(poseGoalObjectives(Slice(k), robot, goal_poses)); + } + return graph; +} + template <> NonlinearFactorGraph Kinematics::pointGoalObjectives( const Interval& interval, const ContactGoals& contact_goals) const { @@ -66,10 +77,10 @@ EqualityConstraints Kinematics::pointGoalConstraints( template <> NonlinearFactorGraph Kinematics::jointAngleObjectives( - const Interval& interval, const Robot& robot) const { + const Interval& interval, const Robot& robot, const Values& mean) const { NonlinearFactorGraph graph; for (size_t k = interval.k_start; k <= interval.k_end; k++) { - graph.add(jointAngleObjectives(Slice(k), robot)); + graph.add(jointAngleObjectives(Slice(k), robot, mean)); } return graph; } @@ -77,10 +88,10 @@ NonlinearFactorGraph Kinematics::jointAngleObjectives( template <> Values Kinematics::initialValues(const Interval& interval, const Robot& robot, - double gaussian_noise) const { + double gaussian_noise, const gtsam::Values& joint_priors) const { Values values; for (size_t k = interval.k_start; k <= interval.k_end; k++) { - values.insert(initialValues(Slice(k), robot, gaussian_noise)); + values.insert(initialValues(Slice(k), robot, gaussian_noise, joint_priors)); } return values; } diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index 9353fcfa..7bc3cce4 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -119,23 +119,46 @@ EqualityConstraints Kinematics::pointGoalConstraints( return constraints; } +template <> +NonlinearFactorGraph Kinematics::poseGoalObjectives( + const Slice& slice, const Robot& robot, + const gtsam::Values& goal_poses) const { + gtsam::NonlinearFactorGraph graph; + + // Add priors on link poses with desired poses from argument + for (auto&& link : robot.links()) { + auto pose_key = PoseKey(link->id(), slice.k); + if (goal_poses.exists(pose_key)) { + const gtsam::Pose3& desired_pose = goal_poses.at(pose_key); + // TODO: use poseprior from unstable gtsam slam or create new factors, to + // add pose from link7 + graph.addPrior(pose_key, desired_pose, p_.p_cost_model); + } + } + + return graph; +} + template <> NonlinearFactorGraph Kinematics::jointAngleObjectives( - const Slice& slice, const Robot& robot) const { + const Slice& slice, const Robot& robot, const Values& mean) const { NonlinearFactorGraph graph; // Minimize the joint angles. for (auto&& joint : robot.joints()) { const gtsam::Key key = JointAngleKey(joint->id(), slice.k); - graph.addPrior(key, 0.0, p_.prior_q_cost_model); + double joint_mean = 0.0; + if (mean.exists(key)) joint_mean = mean.at(key); + graph.addPrior(key, joint_mean, p_.prior_q_cost_model); } return graph; } template <> -Values Kinematics::initialValues(const Slice& slice, const Robot& robot, - double gaussian_noise) const { +Values Kinematics::initialValues( + const Slice& slice, const Robot& robot, double gaussian_noise, + const gtsam::Values& initial_joints) const { Values values; auto sampler_noise_model = @@ -143,14 +166,33 @@ Values Kinematics::initialValues(const Slice& slice, const Robot& robot, gtsam::Sampler sampler(sampler_noise_model); // Initialize all joint angles. + bool any_value = false; for (auto&& joint : robot.joints()) { - InsertJointAngle(&values, joint->id(), slice.k, sampler.sample()[0]); + auto key = JointAngleKey(joint->id(), slice.k); + double value; + if (initial_joints.exists(key)) { + value = initial_joints.at(key); + any_value = true; + } else + value = sampler.sample()[0]; + InsertJointAngle(&values, joint->id(), slice.k, value); } - // Initialize all poses. - for (auto&& link : robot.links()) { - const gtsam::Vector6 xi = sampler.sample(); - InsertPose(&values, link->id(), slice.k, link->bMcom().expmap(xi)); + // Maybe fk takes a long time, so only compute it if there was a given initial + // joint value in this slice + if (any_value) { + // Initialize poses with fk of initialized values + auto fk = robot.forwardKinematics(values, slice.k); + for (auto&& link : robot.links()) { + InsertPose(&values, link->id(), slice.k, + fk.at(PoseKey(link->id(), slice.k))); + } + } else { + // Initialize all poses. + for (auto&& link : robot.links()) { + const gtsam::Vector6 xi = sampler.sample(); + InsertPose(&values, link->id(), slice.k, link->bMcom().expmap(xi)); + } } return values; @@ -182,4 +224,24 @@ Values Kinematics::inverse(const Slice& slice, const Robot& robot, return optimize(graph, constraints, initial_values); } + +template <> +gtsam::Values Kinematics::inverseWithPose( + const Slice& slice, const Robot& robot, const gtsam::Values& goal_poses, + const gtsam::Values& joint_priors) const { + auto graph = this->graph(slice, robot); + + // Add prior on joint angles to constrain the solution + graph.add(this->jointAngleObjectives(slice, robot, joint_priors)); + + // Add priors on link poses with desired poses from argument + graph.add(this->poseGoalObjectives(slice, robot, goal_poses)); + + // Robot kinematics constraints + auto constraints = this->constraints(slice, robot); + + auto initial_values = this->initialValues(slice, robot, 0.1, joint_priors); + + return this->optimize(graph, constraints, initial_values); +} } // namespace gtdynamics diff --git a/gtdynamics/kinematics/KinematicsTrajectory.cpp b/gtdynamics/kinematics/KinematicsTrajectory.cpp index 83a9ae37..d632ab92 100644 --- a/gtdynamics/kinematics/KinematicsTrajectory.cpp +++ b/gtdynamics/kinematics/KinematicsTrajectory.cpp @@ -35,8 +35,8 @@ NonlinearFactorGraph Kinematics::graph(const Trajectory& trajectory, } template <> -EqualityConstraints Kinematics::constraints(const Trajectory& trajectory, - const Robot& robot) const { +EqualityConstraints Kinematics::constraints( + const Trajectory& trajectory, const Robot& robot) const { EqualityConstraints constraints; for (auto&& phase : trajectory.phases()) { constraints.add(this->constraints(phase, robot)); @@ -66,21 +66,23 @@ EqualityConstraints Kinematics::pointGoalConstraints( template <> NonlinearFactorGraph Kinematics::jointAngleObjectives( - const Trajectory& trajectory, const Robot& robot) const { + const Trajectory& trajectory, const Robot& robot, + const Values& mean) const { NonlinearFactorGraph graph; for (auto&& phase : trajectory.phases()) { - graph.add(jointAngleObjectives(phase, robot)); + graph.add(jointAngleObjectives(phase, robot, mean)); } return graph; } template <> -Values Kinematics::initialValues(const Trajectory& trajectory, - const Robot& robot, - double gaussian_noise) const { +Values Kinematics::initialValues( + const Trajectory& trajectory, const Robot& robot, double gaussian_noise, + const gtsam::Values& initial_joints) const { Values values; for (auto&& phase : trajectory.phases()) { - values.insert(initialValues(phase, robot, gaussian_noise)); + values.insert( + initialValues(phase, robot, gaussian_noise, initial_joints)); } return values; } diff --git a/tests/testKinematicsSlice.cpp b/tests/testKinematicsSlice.cpp index ac57c444..a275b7af 100644 --- a/tests/testKinematicsSlice.cpp +++ b/tests/testKinematicsSlice.cpp @@ -83,74 +83,32 @@ TEST(Slice, InverseKinematics) { } } -// Values tonisFunction(const Slice& kSlice, const Robot& robot, -// const ContactGoals& contact_goals) const { -// auto graph = this->graph(kSlice, robot); - -// // Add prior. -// graph.add(jointAngleObjectives(kSlice, robot)); - -// graph.addPrior(PoseKey(0, k), -// gtsam::Pose3(), nullptr); - -// auto initial_values = initialValues(kSlice, robot); - -// return optimize(graph, initial_values); -// } - -gtsam::Values initialValues(const Slice& slice, const Robot& robot, - const gtsam::Vector7& initial) { - gtsam::Values values; - - // Initialize all joint angles. - for (auto&& joint : robot.joints()) { - InsertJointAngle(&values, joint->id(), slice.k, initial(joint->id())); +gtsam::Values jointVectorToValues(const Robot& robot, + const gtsam::Vector7& joints) { + gtsam::Values joint_values; + for (auto&& j : robot.joints()) { + size_t joint_id = j->id(); + joint_values.insert(JointAngleKey(joint_id, k), joints(joint_id)); } - // Initialize poses with fk of initialized values - auto fk = robot.forwardKinematics(values, k); - for (auto&& link : robot.links()) { - InsertPose(&values, link->id(), slice.k, fk.at(PoseKey(link->id(), k))); - } - - return values; -} - -gtsam::Values inverseWithPose(const Kinematics* self, const Slice& kSlice, - const Robot& robot, - const std::map desired_poses, - const gtsam::Vector7& initial) { - auto graph = self->graph(kSlice, robot); - - // Add prior on joint angles to constrain the solution - graph.add(self->jointAngleObjectives(kSlice, robot)); - - // Add priors on link poses with desired poses from argument - for (auto&& kv : desired_poses) { - const size_t link_index = kv.first; - const gtsam::Pose3& desired_pose = kv.second; - // TODO: put it as the kinematic parameters - graph.addPrior(PoseKey(link_index, k), desired_pose, gtsam::noiseModel::Isotropic::Sigma(6,0.00005)); - } - - // Robot kinematics constraints - auto constraints = self->constraints(kSlice, robot); - - auto initial_values = initialValues(kSlice, robot, initial); - - return self->optimize(graph, constraints, initial_values); + return joint_values; } TEST(Slice, initial_values) { - + // Load robot from urdf file const Robot panda = - CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); + CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); const Robot robot = panda.fixLink("link0"); + Kinematics kinematics; + + // Create Values where initial joint angles are stored gtsam::Vector7 initial; initial << 0.1, 0.2, 0.3, -0.4, 0.5, 0.6, 0.7; + gtsam::Values initial_joints = jointVectorToValues(robot, initial); - gtsam::Values initial_values = initialValues(kSlice, robot, initial); + gtsam::Values initial_values = + kinematics.initialValues(kSlice, robot, 0.0, initial_joints); // We should only have 7 values for joints and 8 for link poses EXPECT_LONGS_EQUAL(15, initial_values.size()) @@ -163,11 +121,93 @@ TEST(Slice, initial_values) { EXPECT(assert_equal(initial, actual_initial, tol)) // check that the last fk is the same - gtsam::Rot3 sR7 {{ 0.98161623, 0.13223102, -0.13763916}, - { 0.08587125, -0.9499891 , -0.30024463}, - {-0.17045735, 0.28290575, -0.94387956}}; + gtsam::Rot3 sR7{{0.98161623, 0.13223102, -0.13763916}, + {0.08587125, -0.9499891, -0.30024463}, + {-0.17045735, 0.28290575, -0.94387956}}; gtsam::Pose3 sT7(sR7, Point3(0.323914, 0.167266, 0.905973)); - EXPECT(assert_equal(sT7, initial_values.at(PoseKey(7,k)), tol)) + EXPECT(assert_equal(sT7, initial_values.at(PoseKey(7, k)), tol)) +} + +TEST(Slice, JointAngleObjectives) { + const Robot panda = + CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); + const Robot robot = panda.fixLink("link0"); + + // Instantiate kinematics algorithms + KinematicsParameters parameters; + parameters.method = OptimizationParameters::Method::AUGMENTED_LAGRANGIAN; + Kinematics kinematics(parameters); + + // Priors with means at 0 + auto joint_priors = kinematics.jointAngleObjectives(kSlice, robot); + EXPECT_LONGS_EQUAL(7, joint_priors.size()) + + // Check means at 0 + gtsam::Vector7 means_vector; + means_vector << 0, 0, 0, 0, 0, 0, 0; + gtsam::Values expected_means = jointVectorToValues(robot, means_vector); + gtsam::Values initial = + kinematics.initialValues(kSlice, robot, 0.0, expected_means); + double tol = 1e-5; + EXPECT_DOUBLES_EQUAL(0.0, joint_priors.error(initial), tol) + + // Define some means different than 0 + gtsam::Values means; + means.insert(JointAngleKey(0, k), 1.0); + means.insert(JointAngleKey(2, k), 1.0); + means.insert(JointAngleKey(4, k), 1.0); + means.insert(JointAngleKey(6, k), 1.0); + joint_priors = kinematics.jointAngleObjectives(kSlice, robot, means); + EXPECT_LONGS_EQUAL(7, joint_priors.size()) + + // check means + means_vector << 1, 0, 1, 0, 1, 0, 1; + expected_means = jointVectorToValues(robot, means_vector); + initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means); + EXPECT_DOUBLES_EQUAL(0.0, joint_priors.error(initial), tol) + + // Define means of all joints different than 0 + means.insert(JointAngleKey(1, k), 0.5); + means.insert(JointAngleKey(3, k), -1.0); + means.insert(JointAngleKey(5, k), 0.5); + joint_priors = kinematics.jointAngleObjectives(kSlice, robot, means); + EXPECT_LONGS_EQUAL(7, joint_priors.size()) + + // check means + means_vector << 1, 0.5, 1, -1, 1, 0.5, 1; + expected_means = jointVectorToValues(robot, means_vector); + initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means); + EXPECT_DOUBLES_EQUAL(0.0, joint_priors.error(initial), tol) +} + +TEST(Slice, PoseGoalObjectives) { + const Robot panda = + CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); + const Robot robot = panda.fixLink("link0"); + + // Instantiate kinematics algorithms + KinematicsParameters parameters; + parameters.method = OptimizationParameters::Method::AUGMENTED_LAGRANGIAN; + Kinematics kinematics(parameters); + + // Add prior to pose + gtsam::Rot3 sR7{{0.98161623, 0.13223102, -0.13763916}, + {0.08587125, -0.9499891, -0.30024463}, + {-0.17045735, 0.28290575, -0.94387956}}; + gtsam::Pose3 sT7(sR7, Point3(0.323914, 0.167266, 0.905973)); + gtsam::Values goal_poses; + goal_poses.insert(PoseKey(7, k), sT7); + auto pose_priors = kinematics.poseGoalObjectives(kSlice, robot, goal_poses); + + gtsam::Vector7 initial; + initial << 0.1, 0.2, 0.3, -0.4, 0.5, 0.6, 0.7; + gtsam::Values initial_joints = jointVectorToValues(robot, initial); + auto initial_values = + kinematics.initialValues(kSlice, robot, 0.0, initial_joints); + double tol = 1e-4; + GTSAM_PRINT(initial_values.at(PoseKey(7, k))); + EXPECT(assert_equal(sT7, initial_values.at(PoseKey(7, k)), tol)) + EXPECT_DOUBLES_EQUAL(0, pose_priors.error(initial_values), tol) } TEST(Slice, panda_constraints) { @@ -204,20 +244,24 @@ TEST(Slice, PandaIK) { auto graph = kinematics.graph(kSlice, robot); EXPECT_LONGS_EQUAL(7, graph.size()); - // We should only have 8 unknown links and 7 unknown joint angles, i.e., 15 - // values: - auto initial_values = kinematics.initialValues(kSlice, robot); - EXPECT_LONGS_EQUAL(15, initial_values.size()); - - Rot3 sR7 {{ 0.98161623, 0.13223102, -0.13763916}, - { 0.08587125, -0.9499891 , -0.30024463}, - {-0.17045735, 0.28290575, -0.94387956}}; + // Define the goal pose and add it to a values container + // This is the FK solution of {0.1,0.2,0.3,-0.4,0.5,0.6,0.7} + gtsam::Values goal_poses; + Rot3 sR7{{0.98161623, 0.13223102, -0.13763916}, + {0.08587125, -0.9499891, -0.30024463}, + {-0.17045735, 0.28290575, -0.94387956}}; Pose3 sT7(sR7, Point3(0.323914, 0.167266, 0.905973)); - Vector7 initial; + goal_poses.insert(PoseKey(7, k), sT7); + + // Give a noisy estimate of the original point + Vector7 initial, noise; initial << 0.1, 0.2, 0.3, -0.4, 0.5, 0.6, 0.7; - Vector7 noise; noise << 0.04, -0.1, 0.07, 0.14, -0.05, 0.02, 0.1; - auto values = inverseWithPose(&kinematics, kSlice, robot, {{7, sT7}}, initial+noise); + gtsam::Values initial_joints = jointVectorToValues(robot, noise + initial); + + // Call IK solver + auto values = + kinematics.inverseWithPose(kSlice, robot, goal_poses, initial_joints); // Check that base link did not budge (much) auto base_link = robot.link("link0"); @@ -232,7 +276,7 @@ TEST(Slice, PandaIK) { Vector7 optimal_q; for (size_t j = 0; j < 7; j++) optimal_q[j] = values.at(JointAngleKey(j, k)); - tol = 0.001; + tol = 0.1; EXPECT(assert_equal(initial, optimal_q, tol)); } From 9a555805437989db84be35a2457fae76fd7f179f Mon Sep 17 00:00:00 2001 From: Joobz Date: Mon, 7 Mar 2022 20:18:37 +0100 Subject: [PATCH 07/12] joint angle limits --- gtdynamics/kinematics/Kinematics.h | 10 +++++ gtdynamics/kinematics/KinematicsInterval.cpp | 16 +++++-- gtdynamics/kinematics/KinematicsSlice.cpp | 18 ++++++++ tests/testKinematicsSlice.cpp | 46 ++++++++++++++++++-- 4 files changed, 83 insertions(+), 7 deletions(-) diff --git a/gtdynamics/kinematics/Kinematics.h b/gtdynamics/kinematics/Kinematics.h index 4cf00b8c..321a010f 100644 --- a/gtdynamics/kinematics/Kinematics.h +++ b/gtdynamics/kinematics/Kinematics.h @@ -154,6 +154,16 @@ class Kinematics : public Optimizer { const CONTEXT& context, const Robot& robot, const gtsam::Values& joint_priors = gtsam::Values()) const; + /** + * @fn Factors that enforce joint angle limits. + * @param context Slice or Interval instance. + * @param robot Robot specification from URDF/SDF. + * @return graph with prior factors on joint angles. + */ + template + gtsam::NonlinearFactorGraph jointAngleLimits(const CONTEXT& context, + const Robot& robot) const; + /** * @fn Initialize kinematics. * diff --git a/gtdynamics/kinematics/KinematicsInterval.cpp b/gtdynamics/kinematics/KinematicsInterval.cpp index e132c67c..aa7bcb8c 100644 --- a/gtdynamics/kinematics/KinematicsInterval.cpp +++ b/gtdynamics/kinematics/KinematicsInterval.cpp @@ -86,9 +86,19 @@ NonlinearFactorGraph Kinematics::jointAngleObjectives( } template <> -Values Kinematics::initialValues(const Interval& interval, - const Robot& robot, - double gaussian_noise, const gtsam::Values& joint_priors) const { +NonlinearFactorGraph Kinematics::jointAngleLimits( + const Interval& interval, const Robot& robot) const { + NonlinearFactorGraph graph; + for (size_t k = interval.k_start; k <= interval.k_end; k++) { + graph.add(jointAngleLimits(Slice(k), robot)); + } + return graph; +} + +template <> +Values Kinematics::initialValues( + const Interval& interval, const Robot& robot, double gaussian_noise, + const gtsam::Values& joint_priors) const { Values values; for (size_t k = interval.k_start; k <= interval.k_end; k++) { values.insert(initialValues(Slice(k), robot, gaussian_noise, joint_priors)); diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index 7bc3cce4..9d9df9b2 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -11,6 +11,7 @@ * @author: Frank Dellaert */ +#include #include #include #include @@ -155,6 +156,20 @@ NonlinearFactorGraph Kinematics::jointAngleObjectives( return graph; } +template <> +NonlinearFactorGraph Kinematics::jointAngleLimits( + const Slice& slice, const Robot& robot) const { + NonlinearFactorGraph graph; + for (auto&& joint : robot.joints()) { + graph.add(JointLimitFactor( + JointAngleKey(joint->id(), slice.k), p_.prior_q_cost_model, + joint->parameters().scalar_limits.value_lower_limit, + joint->parameters().scalar_limits.value_upper_limit, + joint->parameters().scalar_limits.value_limit_threshold)); + } + return graph; +} + template <> Values Kinematics::initialValues( const Slice& slice, const Robot& robot, double gaussian_noise, @@ -237,6 +252,9 @@ gtsam::Values Kinematics::inverseWithPose( // Add priors on link poses with desired poses from argument graph.add(this->poseGoalObjectives(slice, robot, goal_poses)); + // Add joint angle limits factors + graph.add(this->jointAngleLimits(slice, robot)); + // Robot kinematics constraints auto constraints = this->constraints(slice, robot); diff --git a/tests/testKinematicsSlice.cpp b/tests/testKinematicsSlice.cpp index a275b7af..b84bfbd4 100644 --- a/tests/testKinematicsSlice.cpp +++ b/tests/testKinematicsSlice.cpp @@ -142,7 +142,7 @@ TEST(Slice, JointAngleObjectives) { auto joint_priors = kinematics.jointAngleObjectives(kSlice, robot); EXPECT_LONGS_EQUAL(7, joint_priors.size()) - // Check means at 0 + // Check that error from priors evaluated at 0 is 0 gtsam::Vector7 means_vector; means_vector << 0, 0, 0, 0, 0, 0, 0; gtsam::Values expected_means = jointVectorToValues(robot, means_vector); @@ -151,7 +151,7 @@ TEST(Slice, JointAngleObjectives) { double tol = 1e-5; EXPECT_DOUBLES_EQUAL(0.0, joint_priors.error(initial), tol) - // Define some means different than 0 + // Define some prior means different than 0 gtsam::Values means; means.insert(JointAngleKey(0, k), 1.0); means.insert(JointAngleKey(2, k), 1.0); @@ -160,7 +160,11 @@ TEST(Slice, JointAngleObjectives) { joint_priors = kinematics.jointAngleObjectives(kSlice, robot, means); EXPECT_LONGS_EQUAL(7, joint_priors.size()) - // check means + // check that error at 0 is now not 0 + initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means); + EXPECT(tol < joint_priors.error(initial)) + + // Check that the evaluated error at the expected means is 0 means_vector << 1, 0, 1, 0, 1, 0, 1; expected_means = jointVectorToValues(robot, means_vector); initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means); @@ -173,13 +177,47 @@ TEST(Slice, JointAngleObjectives) { joint_priors = kinematics.jointAngleObjectives(kSlice, robot, means); EXPECT_LONGS_EQUAL(7, joint_priors.size()) - // check means + // Check that the evaluated error at the expected means is 0 means_vector << 1, 0.5, 1, -1, 1, 0.5, 1; expected_means = jointVectorToValues(robot, means_vector); initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means); EXPECT_DOUBLES_EQUAL(0.0, joint_priors.error(initial), tol) } +TEST(Slice, jointAngleLimits) { + const Robot panda = + CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); + const Robot robot = panda.fixLink("link0"); + + KinematicsParameters parameters; + parameters.method = OptimizationParameters::Method::AUGMENTED_LAGRANGIAN; + Kinematics kinematics(parameters); + + auto jointLimits = kinematics.jointAngleLimits(kSlice, robot); + + // get joint limits + gtsam::Vector7 lower_limits, upper_limits; + for(auto&& joint : robot.joints()){ + auto scalar_values = joint->parameters().scalar_limits; + lower_limits(joint->id()) = scalar_values.value_lower_limit; + upper_limits(joint->id()) = scalar_values.value_upper_limit; + } + auto ones = gtsam::Vector7::Ones(); + + const double tol = 1e-5; + // if lower than lower_limit, error must be greater than 0 + auto values = jointVectorToValues(robot, lower_limits-0.1*ones); + EXPECT(tol < jointLimits.error(values)) + + // if inside the limits, the error must be 0 + values = jointVectorToValues(robot, (lower_limits+upper_limits)/2); + EXPECT_DOUBLES_EQUAL(0.0, jointLimits.error(values), tol) + + // if upper than upper_limit, error must be greater than 0 + values = jointVectorToValues(robot, upper_limits+0.1*ones); + EXPECT(tol < jointLimits.error(values)) +} + TEST(Slice, PoseGoalObjectives) { const Robot panda = CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); From 9c70904e66d4be233e4bd4e6dd6c71ca386831fa Mon Sep 17 00:00:00 2001 From: Joobz Date: Wed, 9 Mar 2022 00:28:32 +0100 Subject: [PATCH 08/12] Adressed comments --- gtdynamics/kinematics/Kinematics.h | 4 +- gtdynamics/kinematics/KinematicsInterval.cpp | 4 +- gtdynamics/kinematics/KinematicsSlice.cpp | 46 ++++++++++--------- .../kinematics/KinematicsTrajectory.cpp | 4 +- tests/testKinematicsSlice.cpp | 16 +++---- 5 files changed, 38 insertions(+), 36 deletions(-) diff --git a/gtdynamics/kinematics/Kinematics.h b/gtdynamics/kinematics/Kinematics.h index 321a010f..723ccbb2 100644 --- a/gtdynamics/kinematics/Kinematics.h +++ b/gtdynamics/kinematics/Kinematics.h @@ -183,7 +183,7 @@ class Kinematics : public Optimizer { template gtsam::Values initialValues( const CONTEXT& context, const Robot& robot, double gaussian_noise = 0.1, - const gtsam::Values& initial_joints = gtsam::Values()) const; + const gtsam::Values& initial_joints = gtsam::Values(), bool use_fk = false) const; /** * @fn Inverse kinematics given a set of contact goals. @@ -210,7 +210,7 @@ class Kinematics : public Optimizer { * @return values with poses and joint angles */ template - gtsam::Values inverseWithPose( + gtsam::Values inverse( const CONTEXT& context, const Robot& robot, const gtsam::Values& goal_poses, const gtsam::Values& joint_priors = gtsam::Values()) const; diff --git a/gtdynamics/kinematics/KinematicsInterval.cpp b/gtdynamics/kinematics/KinematicsInterval.cpp index aa7bcb8c..b814820c 100644 --- a/gtdynamics/kinematics/KinematicsInterval.cpp +++ b/gtdynamics/kinematics/KinematicsInterval.cpp @@ -98,10 +98,10 @@ NonlinearFactorGraph Kinematics::jointAngleLimits( template <> Values Kinematics::initialValues( const Interval& interval, const Robot& robot, double gaussian_noise, - const gtsam::Values& joint_priors) const { + const gtsam::Values& joint_priors, bool use_fk) const { Values values; for (size_t k = interval.k_start; k <= interval.k_end; k++) { - values.insert(initialValues(Slice(k), robot, gaussian_noise, joint_priors)); + values.insert(initialValues(Slice(k), robot, gaussian_noise, joint_priors, use_fk)); } return values; } diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index 9d9df9b2..4d548015 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -73,13 +73,13 @@ EqualityConstraints Kinematics::constraints(const Slice& slice, // Get an expression for the unknown link pose. Pose3_ bTcom(PoseKey(link->id(), slice.k)); - // Kust make sure it does not move from its original rest pose + // Just make sure it does not move from its original rest pose Pose3_ bMcom(link->bMcom()); // Create expression to calculate the error in tangent space auto constraint_expr = gtsam::logmap(bTcom, bMcom); - // Add constriant + // Add constraint constraints.emplace_shared>(constraint_expr, tolerance); } @@ -126,13 +126,20 @@ NonlinearFactorGraph Kinematics::poseGoalObjectives( const gtsam::Values& goal_poses) const { gtsam::NonlinearFactorGraph graph; + for (const gtsam::Key& key : goal_poses.keys()) { + const gtsam::Pose3& desired_pose = goal_poses.at(key); + // TODO(toni): use poseprior from unstable gtsam slam or create new + // factors, to add pose from link7 + graph.addPrior(key, desired_pose, p_.p_cost_model); + } + // Add priors on link poses with desired poses from argument for (auto&& link : robot.links()) { auto pose_key = PoseKey(link->id(), slice.k); if (goal_poses.exists(pose_key)) { const gtsam::Pose3& desired_pose = goal_poses.at(pose_key); - // TODO: use poseprior from unstable gtsam slam or create new factors, to - // add pose from link7 + // TODO(toni): use poseprior from unstable gtsam slam or create new + // factors, to add pose from link7 graph.addPrior(pose_key, desired_pose, p_.p_cost_model); } } @@ -148,9 +155,8 @@ NonlinearFactorGraph Kinematics::jointAngleObjectives( // Minimize the joint angles. for (auto&& joint : robot.joints()) { const gtsam::Key key = JointAngleKey(joint->id(), slice.k); - double joint_mean = 0.0; - if (mean.exists(key)) joint_mean = mean.at(key); - graph.addPrior(key, joint_mean, p_.prior_q_cost_model); + graph.addPrior(key, (mean.exists(key) ? mean.at(key) : 0.0), + p_.prior_q_cost_model); } return graph; @@ -171,9 +177,10 @@ NonlinearFactorGraph Kinematics::jointAngleLimits( } template <> -Values Kinematics::initialValues( - const Slice& slice, const Robot& robot, double gaussian_noise, - const gtsam::Values& initial_joints) const { +Values Kinematics::initialValues(const Slice& slice, const Robot& robot, + double gaussian_noise, + const gtsam::Values& initial_joints, + bool use_fk) const { Values values; auto sampler_noise_model = @@ -181,21 +188,16 @@ Values Kinematics::initialValues( gtsam::Sampler sampler(sampler_noise_model); // Initialize all joint angles. - bool any_value = false; for (auto&& joint : robot.joints()) { auto key = JointAngleKey(joint->id(), slice.k); - double value; - if (initial_joints.exists(key)) { - value = initial_joints.at(key); - any_value = true; - } else - value = sampler.sample()[0]; - InsertJointAngle(&values, joint->id(), slice.k, value); + double angle = initial_joints.exists(key) ? initial_joints.at(key) + : sampler.sample()[0]; + InsertJointAngle(&values, joint->id(), slice.k, angle); } // Maybe fk takes a long time, so only compute it if there was a given initial // joint value in this slice - if (any_value) { + if (use_fk) { // Initialize poses with fk of initialized values auto fk = robot.forwardKinematics(values, slice.k); for (auto&& link : robot.links()) { @@ -241,12 +243,12 @@ Values Kinematics::inverse(const Slice& slice, const Robot& robot, } template <> -gtsam::Values Kinematics::inverseWithPose( +gtsam::Values Kinematics::inverse( const Slice& slice, const Robot& robot, const gtsam::Values& goal_poses, const gtsam::Values& joint_priors) const { auto graph = this->graph(slice, robot); - // Add prior on joint angles to constrain the solution + // Add prior on joint angles to prefer solution close to our initial estimates graph.add(this->jointAngleObjectives(slice, robot, joint_priors)); // Add priors on link poses with desired poses from argument @@ -258,7 +260,7 @@ gtsam::Values Kinematics::inverseWithPose( // Robot kinematics constraints auto constraints = this->constraints(slice, robot); - auto initial_values = this->initialValues(slice, robot, 0.1, joint_priors); + auto initial_values = this->initialValues(slice, robot, 0.1, joint_priors, true); return this->optimize(graph, constraints, initial_values); } diff --git a/gtdynamics/kinematics/KinematicsTrajectory.cpp b/gtdynamics/kinematics/KinematicsTrajectory.cpp index d632ab92..a35ee108 100644 --- a/gtdynamics/kinematics/KinematicsTrajectory.cpp +++ b/gtdynamics/kinematics/KinematicsTrajectory.cpp @@ -78,11 +78,11 @@ NonlinearFactorGraph Kinematics::jointAngleObjectives( template <> Values Kinematics::initialValues( const Trajectory& trajectory, const Robot& robot, double gaussian_noise, - const gtsam::Values& initial_joints) const { + const gtsam::Values& initial_joints, bool use_fk) const { Values values; for (auto&& phase : trajectory.phases()) { values.insert( - initialValues(phase, robot, gaussian_noise, initial_joints)); + initialValues(phase, robot, gaussian_noise, initial_joints, use_fk)); } return values; } diff --git a/tests/testKinematicsSlice.cpp b/tests/testKinematicsSlice.cpp index b84bfbd4..d7b56bcb 100644 --- a/tests/testKinematicsSlice.cpp +++ b/tests/testKinematicsSlice.cpp @@ -94,7 +94,7 @@ gtsam::Values jointVectorToValues(const Robot& robot, return joint_values; } -TEST(Slice, initial_values) { +TEST(Slice, initialValues) { // Load robot from urdf file const Robot panda = CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); @@ -108,7 +108,7 @@ TEST(Slice, initial_values) { gtsam::Values initial_joints = jointVectorToValues(robot, initial); gtsam::Values initial_values = - kinematics.initialValues(kSlice, robot, 0.0, initial_joints); + kinematics.initialValues(kSlice, robot, 0.0, initial_joints, true); // We should only have 7 values for joints and 8 for link poses EXPECT_LONGS_EQUAL(15, initial_values.size()) @@ -147,7 +147,7 @@ TEST(Slice, JointAngleObjectives) { means_vector << 0, 0, 0, 0, 0, 0, 0; gtsam::Values expected_means = jointVectorToValues(robot, means_vector); gtsam::Values initial = - kinematics.initialValues(kSlice, robot, 0.0, expected_means); + kinematics.initialValues(kSlice, robot, 0.0, expected_means, true); double tol = 1e-5; EXPECT_DOUBLES_EQUAL(0.0, joint_priors.error(initial), tol) @@ -161,13 +161,13 @@ TEST(Slice, JointAngleObjectives) { EXPECT_LONGS_EQUAL(7, joint_priors.size()) // check that error at 0 is now not 0 - initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means); + initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means, true); EXPECT(tol < joint_priors.error(initial)) // Check that the evaluated error at the expected means is 0 means_vector << 1, 0, 1, 0, 1, 0, 1; expected_means = jointVectorToValues(robot, means_vector); - initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means); + initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means, true); EXPECT_DOUBLES_EQUAL(0.0, joint_priors.error(initial), tol) // Define means of all joints different than 0 @@ -180,7 +180,7 @@ TEST(Slice, JointAngleObjectives) { // Check that the evaluated error at the expected means is 0 means_vector << 1, 0.5, 1, -1, 1, 0.5, 1; expected_means = jointVectorToValues(robot, means_vector); - initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means); + initial = kinematics.initialValues(kSlice, robot, 0.0, expected_means, true); EXPECT_DOUBLES_EQUAL(0.0, joint_priors.error(initial), tol) } @@ -241,7 +241,7 @@ TEST(Slice, PoseGoalObjectives) { initial << 0.1, 0.2, 0.3, -0.4, 0.5, 0.6, 0.7; gtsam::Values initial_joints = jointVectorToValues(robot, initial); auto initial_values = - kinematics.initialValues(kSlice, robot, 0.0, initial_joints); + kinematics.initialValues(kSlice, robot, 0.0, initial_joints, true); double tol = 1e-4; GTSAM_PRINT(initial_values.at(PoseKey(7, k))); EXPECT(assert_equal(sT7, initial_values.at(PoseKey(7, k)), tol)) @@ -299,7 +299,7 @@ TEST(Slice, PandaIK) { // Call IK solver auto values = - kinematics.inverseWithPose(kSlice, robot, goal_poses, initial_joints); + kinematics.inverse(kSlice, robot, goal_poses, initial_joints); // Check that base link did not budge (much) auto base_link = robot.link("link0"); From d2226e4f7826ef45472e50f90f6d39e80d35717c Mon Sep 17 00:00:00 2001 From: Joobz Date: Wed, 9 Mar 2022 01:45:00 +0100 Subject: [PATCH 09/12] PoseGoals and addressed more comments --- gtdynamics/kinematics/Kinematics.h | 67 +++++++++++++++++--- gtdynamics/kinematics/KinematicsInterval.cpp | 6 +- gtdynamics/kinematics/KinematicsSlice.cpp | 45 +++++++------ tests/testKinematicsSlice.cpp | 25 ++++---- 4 files changed, 99 insertions(+), 44 deletions(-) diff --git a/gtdynamics/kinematics/Kinematics.h b/gtdynamics/kinematics/Kinematics.h index 723ccbb2..4668fe8f 100644 --- a/gtdynamics/kinematics/Kinematics.h +++ b/gtdynamics/kinematics/Kinematics.h @@ -17,6 +17,7 @@ #include #include #include +#include #include #include #include @@ -65,6 +66,56 @@ struct ContactGoal { ///< Map of link name to ContactGoal using ContactGoals = std::vector; +/** + * Similar to the previous struct ContactGoal but with poses instead of + * points. + * Desired world pose for a end-effector pose. + * + * This simple struct stores the robot link that holds the end-effector, the + * end-effector's pose in the final link's CoM frame, and a `goal_pose` in + * world coordinate frames. The goal is satisfied iff + * `pose_on_link.predict(values, k) == goal_pose`. + */ +struct PoseGoal { + LinkSharedPtr ee_link; ///< Link that hold end-effector + gtsam::Pose3 comTee; ///< In link's CoM frame. + gtsam::Pose3 goal_pose; ///< In world frame. + + /// Constructor + PoseGoal(const LinkSharedPtr& ee_link, const gtsam::Pose3& comTee, + const gtsam::Pose3& goal_pose) + : ee_link(ee_link), comTee(comTee), goal_pose(goal_pose) {} + + /// Return link associated with contact pose. + const LinkSharedPtr& link() const { return ee_link; } + + /// Return goal pose in ee_link COM frame. + const gtsam::Pose3& poseInCoM() const { return comTee; } + + /// Return CoM pose needed to achieve goal pose. + const gtsam::Pose3 sTcom() const { + return goal_pose.compose(poseInCoM().inverse()); + } + + /// Print to stream. + friend std::ostream& operator<<(std::ostream& os, const PoseGoal& cg); + + /// GTSAM-style print, works with wrapper. + void print(const std::string& s) const; + + /** + * @fn Check that the contact goal has been achieved for given values. + * @param values a GTSAM Values instance that should contain link pose. + * @param k time step to check (default 0). + * @param tol tolerance in 3D (default 1e-9). + */ + bool satisfied(const gtsam::Values& values, size_t k = 0, + double tol = 1e-9) const; +}; + +///< Map of time to PoseGoal +using PoseGoals = std::map; + /// Noise models etc specific to Kinematics class struct KinematicsParameters : public OptimizationParameters { using Isotropic = gtsam::noiseModel::Isotropic; @@ -132,21 +183,21 @@ class Kinematics : public Optimizer { const CONTEXT& context, const ContactGoals& contact_goals) const; /** - * @fn Create pose goal objectives. + * @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 * @returns graph with pose goal factors. */ template gtsam::NonlinearFactorGraph poseGoalObjectives( - const CONTEXT& context, const Robot& robot, - const gtsam::Values& pose_goals) const; + const CONTEXT& context, const PoseGoals& pose_goals) const; /** * @fn Factors that minimize joint angles. * @param context Slice or Interval instance. * @param robot Robot specification from URDF/SDF. - * @param joint_priors Values where the mean of the priors is specified + * @param joint_priors Values where the mean of the priors is specified. The + * default is an empty Values, meaning that no means are specified. * @returns graph with prior factors on joint angles. */ template @@ -183,7 +234,8 @@ class Kinematics : public Optimizer { template gtsam::Values initialValues( const CONTEXT& context, const Robot& robot, double gaussian_noise = 0.1, - const gtsam::Values& initial_joints = gtsam::Values(), bool use_fk = false) const; + const gtsam::Values& initial_joints = gtsam::Values(), + bool use_fk = false) const; /** * @fn Inverse kinematics given a set of contact goals. @@ -204,15 +256,14 @@ class Kinematics : public Optimizer { * @fn This function does inverse kinematics separately on each slice * @param context Slice or Interval instance * @param robot Robot specification from URDF/SDF - * @param goal_poses goals for EE poses + * @param pose_goals goals for EE poses * @param joint_priors optional argument to put priors centered on given * values. If empty, the priors will be centered on the origin * @return values with poses and joint angles */ template gtsam::Values inverse( - const CONTEXT& context, const Robot& robot, - const gtsam::Values& goal_poses, + const CONTEXT& context, const Robot& robot, const PoseGoals& pose_goals, const gtsam::Values& joint_priors = gtsam::Values()) const; /** diff --git a/gtdynamics/kinematics/KinematicsInterval.cpp b/gtdynamics/kinematics/KinematicsInterval.cpp index b814820c..e31c440a 100644 --- a/gtdynamics/kinematics/KinematicsInterval.cpp +++ b/gtdynamics/kinematics/KinematicsInterval.cpp @@ -46,11 +46,11 @@ EqualityConstraints Kinematics::constraints( template <> NonlinearFactorGraph Kinematics::poseGoalObjectives( - const Interval& interval, const Robot& robot, - const gtsam::Values& goal_poses) const { + const Interval& interval, + const PoseGoals& pose_goals) const { NonlinearFactorGraph graph; for (size_t k = interval.k_start; k <= interval.k_end; k++) { - graph.add(poseGoalObjectives(Slice(k), robot, goal_poses)); + graph.add(poseGoalObjectives(Slice(k), pose_goals)); } return graph; } diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index 4d548015..230809fc 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -36,6 +36,21 @@ void ContactGoal::print(const std::string& s) const { std::cout << (s.empty() ? s : s + " ") << *this; } +std::ostream& operator<<(std::ostream& os, const PoseGoal& pg) { + os << "{" << pg.link()->name() << ", [" << pg.comTee << "], [" << pg.goal_pose << "]}"; + return os; +} + +void PoseGoal::print(const std::string& s) const { + std::cout << (s.empty() ? s : s + " ") << *this; +} + +bool PoseGoal::satisfied(const gtsam::Values& values, size_t k, + double tol) const { + return gtsam::equal( + Pose(values, link()->id(), k).compose(poseInCoM()), goal_pose, tol); +} + template <> NonlinearFactorGraph Kinematics::graph(const Slice& slice, const Robot& robot) const { @@ -122,26 +137,15 @@ EqualityConstraints Kinematics::pointGoalConstraints( template <> NonlinearFactorGraph Kinematics::poseGoalObjectives( - const Slice& slice, const Robot& robot, - const gtsam::Values& goal_poses) const { + const Slice& slice, const PoseGoals& pose_goals) const { gtsam::NonlinearFactorGraph graph; - for (const gtsam::Key& key : goal_poses.keys()) { - const gtsam::Pose3& desired_pose = goal_poses.at(key); + if (pose_goals.find(slice.k) != pose_goals.end()) { + auto pose_key = PoseKey(pose_goals.at(slice.k).link()->id(), slice.k); + const gtsam::Pose3& desired_pose = pose_goals.at(slice.k).sTcom(); // TODO(toni): use poseprior from unstable gtsam slam or create new // factors, to add pose from link7 - graph.addPrior(key, desired_pose, p_.p_cost_model); - } - - // Add priors on link poses with desired poses from argument - for (auto&& link : robot.links()) { - auto pose_key = PoseKey(link->id(), slice.k); - if (goal_poses.exists(pose_key)) { - const gtsam::Pose3& desired_pose = goal_poses.at(pose_key); - // TODO(toni): use poseprior from unstable gtsam slam or create new - // factors, to add pose from link7 - graph.addPrior(pose_key, desired_pose, p_.p_cost_model); - } + graph.addPrior(pose_key, desired_pose, p_.p_cost_model); } return graph; @@ -230,7 +234,7 @@ Values Kinematics::inverse(const Slice& slice, const Robot& robot, graph.add(pointGoalObjectives(slice, contact_goals)); } - // Traget joint angles. + // Target joint angles. graph.add(jointAngleObjectives(slice, robot)); // TODO(frank): allo pose prior as well. @@ -244,7 +248,7 @@ Values Kinematics::inverse(const Slice& slice, const Robot& robot, template <> gtsam::Values Kinematics::inverse( - const Slice& slice, const Robot& robot, const gtsam::Values& goal_poses, + const Slice& slice, const Robot& robot, const PoseGoals& pose_goals, const gtsam::Values& joint_priors) const { auto graph = this->graph(slice, robot); @@ -252,7 +256,7 @@ gtsam::Values Kinematics::inverse( graph.add(this->jointAngleObjectives(slice, robot, joint_priors)); // Add priors on link poses with desired poses from argument - graph.add(this->poseGoalObjectives(slice, robot, goal_poses)); + graph.add(this->poseGoalObjectives(slice, pose_goals)); // Add joint angle limits factors graph.add(this->jointAngleLimits(slice, robot)); @@ -260,7 +264,8 @@ gtsam::Values Kinematics::inverse( // Robot kinematics constraints auto constraints = this->constraints(slice, robot); - auto initial_values = this->initialValues(slice, robot, 0.1, joint_priors, true); + auto initial_values = + this->initialValues(slice, robot, 0.1, joint_priors, true); return this->optimize(graph, constraints, initial_values); } diff --git a/tests/testKinematicsSlice.cpp b/tests/testKinematicsSlice.cpp index d7b56bcb..9c9a4cfc 100644 --- a/tests/testKinematicsSlice.cpp +++ b/tests/testKinematicsSlice.cpp @@ -188,7 +188,7 @@ TEST(Slice, jointAngleLimits) { const Robot panda = CreateRobotFromFile(kUrdfPath + std::string("panda/panda.urdf")); const Robot robot = panda.fixLink("link0"); - + KinematicsParameters parameters; parameters.method = OptimizationParameters::Method::AUGMENTED_LAGRANGIAN; Kinematics kinematics(parameters); @@ -197,7 +197,7 @@ TEST(Slice, jointAngleLimits) { // get joint limits gtsam::Vector7 lower_limits, upper_limits; - for(auto&& joint : robot.joints()){ + for (auto&& joint : robot.joints()) { auto scalar_values = joint->parameters().scalar_limits; lower_limits(joint->id()) = scalar_values.value_lower_limit; upper_limits(joint->id()) = scalar_values.value_upper_limit; @@ -206,15 +206,15 @@ TEST(Slice, jointAngleLimits) { const double tol = 1e-5; // if lower than lower_limit, error must be greater than 0 - auto values = jointVectorToValues(robot, lower_limits-0.1*ones); + auto values = jointVectorToValues(robot, lower_limits - 0.1 * ones); EXPECT(tol < jointLimits.error(values)) // if inside the limits, the error must be 0 - values = jointVectorToValues(robot, (lower_limits+upper_limits)/2); + values = jointVectorToValues(robot, (lower_limits + upper_limits) / 2); EXPECT_DOUBLES_EQUAL(0.0, jointLimits.error(values), tol) // if upper than upper_limit, error must be greater than 0 - values = jointVectorToValues(robot, upper_limits+0.1*ones); + values = jointVectorToValues(robot, upper_limits + 0.1 * ones); EXPECT(tol < jointLimits.error(values)) } @@ -233,9 +233,9 @@ TEST(Slice, PoseGoalObjectives) { {0.08587125, -0.9499891, -0.30024463}, {-0.17045735, 0.28290575, -0.94387956}}; gtsam::Pose3 sT7(sR7, Point3(0.323914, 0.167266, 0.905973)); - gtsam::Values goal_poses; - goal_poses.insert(PoseKey(7, k), sT7); - auto pose_priors = kinematics.poseGoalObjectives(kSlice, robot, goal_poses); + PoseGoals pose_goals = { + {k, PoseGoal(robot.links()[7], gtsam::Pose3(), sT7)}}; + auto pose_priors = kinematics.poseGoalObjectives(kSlice, pose_goals); gtsam::Vector7 initial; initial << 0.1, 0.2, 0.3, -0.4, 0.5, 0.6, 0.7; @@ -284,13 +284,13 @@ TEST(Slice, PandaIK) { // Define the goal pose and add it to a values container // This is the FK solution of {0.1,0.2,0.3,-0.4,0.5,0.6,0.7} - gtsam::Values goal_poses; Rot3 sR7{{0.98161623, 0.13223102, -0.13763916}, {0.08587125, -0.9499891, -0.30024463}, {-0.17045735, 0.28290575, -0.94387956}}; Pose3 sT7(sR7, Point3(0.323914, 0.167266, 0.905973)); - goal_poses.insert(PoseKey(7, k), sT7); - + PoseGoals pose_goals = { + {k, PoseGoal(robot.links()[7], gtsam::Pose3(), sT7)}}; + // Give a noisy estimate of the original point Vector7 initial, noise; initial << 0.1, 0.2, 0.3, -0.4, 0.5, 0.6, 0.7; @@ -298,8 +298,7 @@ TEST(Slice, PandaIK) { gtsam::Values initial_joints = jointVectorToValues(robot, noise + initial); // Call IK solver - auto values = - kinematics.inverse(kSlice, robot, goal_poses, initial_joints); + auto values = kinematics.inverse(kSlice, robot, pose_goals, initial_joints); // Check that base link did not budge (much) auto base_link = robot.link("link0"); From fdb0b632e9d2c6779c98a9f37173c7d14cb880fb Mon Sep 17 00:00:00 2001 From: Joobz Date: Wed, 9 Mar 2022 17:45:14 +0100 Subject: [PATCH 10/12] Adressed comments --- gtdynamics/kinematics/Kinematics.h | 24 ++++++++++------------- gtdynamics/kinematics/KinematicsSlice.cpp | 15 ++++++++------ 2 files changed, 19 insertions(+), 20 deletions(-) diff --git a/gtdynamics/kinematics/Kinematics.h b/gtdynamics/kinematics/Kinematics.h index 4668fe8f..159a9c73 100644 --- a/gtdynamics/kinematics/Kinematics.h +++ b/gtdynamics/kinematics/Kinematics.h @@ -17,7 +17,6 @@ #include #include #include -#include #include #include #include @@ -72,29 +71,26 @@ using ContactGoals = std::vector; * Desired world pose for a end-effector pose. * * This simple struct stores the robot link that holds the end-effector, the - * end-effector's pose in the final link's CoM frame, and a `goal_pose` in + * end-effector's pose in the final link's CoM frame, and a `goal_pose` in * world coordinate frames. The goal is satisfied iff * `pose_on_link.predict(values, k) == goal_pose`. */ struct PoseGoal { - LinkSharedPtr ee_link; ///< Link that hold end-effector - gtsam::Pose3 comTee; ///< In link's CoM frame. - gtsam::Pose3 goal_pose; ///< In world frame. + LinkSharedPtr ee_link; ///< Link that hold end-effector + gtsam::Pose3 comTgoal; ///< Goal pose in link's CoM frame. + gtsam::Pose3 wTgoal; ///< Goal pose in world frame. /// Constructor - PoseGoal(const LinkSharedPtr& ee_link, const gtsam::Pose3& comTee, - const gtsam::Pose3& goal_pose) - : ee_link(ee_link), comTee(comTee), goal_pose(goal_pose) {} + PoseGoal(const LinkSharedPtr& ee_link, const gtsam::Pose3& comTgoal, + const gtsam::Pose3& wTgoal) + : ee_link(ee_link), comTgoal(comTgoal), wTgoal(wTgoal) {} /// Return link associated with contact pose. const LinkSharedPtr& link() const { return ee_link; } - /// Return goal pose in ee_link COM frame. - const gtsam::Pose3& poseInCoM() const { return comTee; } - /// Return CoM pose needed to achieve goal pose. - const gtsam::Pose3 sTcom() const { - return goal_pose.compose(poseInCoM().inverse()); + const gtsam::Pose3 wTcom() const { + return wTgoal.compose(comTgoal.inverse()); } /// Print to stream. @@ -197,7 +193,7 @@ class Kinematics : public Optimizer { * @param context Slice or Interval instance. * @param robot Robot specification from URDF/SDF. * @param joint_priors Values where the mean of the priors is specified. The - * default is an empty Values, meaning that no means are specified. + * default is an empty Values, meaning that the means will default to 0. * @returns graph with prior factors on joint angles. */ template diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index 230809fc..377b6ad1 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -37,7 +37,8 @@ void ContactGoal::print(const std::string& s) const { } std::ostream& operator<<(std::ostream& os, const PoseGoal& pg) { - os << "{" << pg.link()->name() << ", [" << pg.comTee << "], [" << pg.goal_pose << "]}"; + os << "{" << pg.link()->name() << ", [" << pg.comTgoal << "], [" << pg.wTgoal + << "]}"; return os; } @@ -47,8 +48,8 @@ void PoseGoal::print(const std::string& s) const { bool PoseGoal::satisfied(const gtsam::Values& values, size_t k, double tol) const { - return gtsam::equal( - Pose(values, link()->id(), k).compose(poseInCoM()), goal_pose, tol); + return gtsam::equal(Pose(values, link()->id(), k), wTcom(), + tol); } template <> @@ -140,9 +141,11 @@ NonlinearFactorGraph Kinematics::poseGoalObjectives( const Slice& slice, const PoseGoals& pose_goals) const { gtsam::NonlinearFactorGraph graph; - if (pose_goals.find(slice.k) != pose_goals.end()) { - auto pose_key = PoseKey(pose_goals.at(slice.k).link()->id(), slice.k); - const gtsam::Pose3& desired_pose = pose_goals.at(slice.k).sTcom(); + auto it = pose_goals.find(slice.k); // short for "iterator" + if (it != pose_goals.end()) { + const auto& pose_goal = it->second; + auto pose_key = PoseKey(pose_goal.link()->id(), slice.k); + const gtsam::Pose3& desired_pose = pose_goal.wTcom(); // TODO(toni): use poseprior from unstable gtsam slam or create new // factors, to add pose from link7 graph.addPrior(pose_key, desired_pose, p_.p_cost_model); From 0cf03b550c8661b10f4c885a8500efdf2db9d177 Mon Sep 17 00:00:00 2001 From: lprice8 Date: Mon, 16 May 2022 11:23:42 -0400 Subject: [PATCH 11/12] added gpmp and changed kinematics --- gtdynamics/kinematics/Kinematics.h | 8 +- gtdynamics/kinematics/KinematicsSlice.cpp | 4 +- gtdynamics/pandarobot/CMakeLists.txt | 2 +- gtdynamics/pandarobot/gpmp/CMakeLists.txt | 0 .../pandarobot/gpmp/GaussianProcessPrior.h | 114 ++++++++++++++++++ gtdynamics/pandarobot/ikfast/PandaIKFast.cpp | 3 +- gtdynamics/pandarobot/ikfast/PandaIKFast.h | 2 +- .../pandarobot/ikfast/ikfast61_panda.cpp | 47 -------- models/urdfs/panda/panda.urdf | 14 +-- 9 files changed, 130 insertions(+), 64 deletions(-) create mode 100644 gtdynamics/pandarobot/gpmp/CMakeLists.txt create mode 100644 gtdynamics/pandarobot/gpmp/GaussianProcessPrior.h diff --git a/gtdynamics/kinematics/Kinematics.h b/gtdynamics/kinematics/Kinematics.h index 159a9c73..059200d8 100644 --- a/gtdynamics/kinematics/Kinematics.h +++ b/gtdynamics/kinematics/Kinematics.h @@ -120,10 +120,10 @@ struct KinematicsParameters : public OptimizationParameters { prior_q_cost_model; // joint angle prior factor // TODO(yetong): replace noise model with tolerance. - KinematicsParameters() - : p_cost_model(Isotropic::Sigma(6, 1e-4)), - g_cost_model(Isotropic::Sigma(3, 0.01)), - prior_q_cost_model(Isotropic::Sigma(1, 0.5)) {} + KinematicsParameters(double p_cost_model_sigma = 1e-4, double g_cost_model_sigma = 1e-2, double prior_q_cost_model_sigma = 0.5) + : p_cost_model(Isotropic::Sigma(6, p_cost_model_sigma)), + g_cost_model(Isotropic::Sigma(3, g_cost_model_sigma)), + prior_q_cost_model(Isotropic::Sigma(1, prior_q_cost_model_sigma)) {} }; /// All things kinematics, zero velocities/twists, and no forces. diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index 377b6ad1..f1448c80 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -175,10 +175,10 @@ NonlinearFactorGraph Kinematics::jointAngleLimits( NonlinearFactorGraph graph; for (auto&& joint : robot.joints()) { graph.add(JointLimitFactor( - JointAngleKey(joint->id(), slice.k), p_.prior_q_cost_model, + JointAngleKey(joint->id(), slice.k), gtsam::noiseModel::Isotropic::Sigma(1, 0.00001), joint->parameters().scalar_limits.value_lower_limit, joint->parameters().scalar_limits.value_upper_limit, - joint->parameters().scalar_limits.value_limit_threshold)); + 0.04)); } return graph; } diff --git a/gtdynamics/pandarobot/CMakeLists.txt b/gtdynamics/pandarobot/CMakeLists.txt index 565781db..2d34e3e1 100644 --- a/gtdynamics/pandarobot/CMakeLists.txt +++ b/gtdynamics/pandarobot/CMakeLists.txt @@ -1,5 +1,5 @@ # add cablerobot subfolders to gtdynamics' SOURCE_SUBDIRS list -list(APPEND SOURCE_SUBDIRS pandarobot/ikfast) +list(APPEND SOURCE_SUBDIRS pandarobot/ikfast pandarobot/gpmp) set(SOURCE_SUBDIRS ${SOURCE_SUBDIRS} PARENT_SCOPE) # add wrapper interface file diff --git a/gtdynamics/pandarobot/gpmp/CMakeLists.txt b/gtdynamics/pandarobot/gpmp/CMakeLists.txt new file mode 100644 index 00000000..e69de29b diff --git a/gtdynamics/pandarobot/gpmp/GaussianProcessPrior.h b/gtdynamics/pandarobot/gpmp/GaussianProcessPrior.h new file mode 100644 index 00000000..afd5319c --- /dev/null +++ b/gtdynamics/pandarobot/gpmp/GaussianProcessPrior.h @@ -0,0 +1,114 @@ +#pragma once + +#include +#include +#include + +#include +namespace gtdynamics { +class GaussianProcessPrior + : public gtsam::NoiseModelFactor4 { + private: + double delta_t_; + + using This = GaussianProcessPrior; + using Base = gtsam::NoiseModelFactor4; + + public: + GaussianProcessPrior() {} /* Default constructor only for serialization */ + + /// Constructor + /// @param delta_t is the time between the two states + GaussianProcessPrior(gtsam::Key thetaKey1, gtsam::Key thetadKey1, + gtsam::Key thetaKey2, gtsam::Key thetadKey2, + double delta_t, double Qc_model) + : // TODO:define calcQ + Base(gtsam::noiseModel::Gaussian::Covariance(calcQ(Qc_model, delta_t)), + thetaKey1, thetadKey1, thetaKey2, thetadKey2), + delta_t_(delta_t) {} + + virtual ~GaussianProcessPrior() {} + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// factor error function + gtsam::Vector evaluateError( + const double& theta1, const double& thetad1, const double& theta2, + const double& thetad2, boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none) const override { + // using namespace gtsam; + + // state vector + gtsam::Vector2 error; + error << -(theta2 - theta1) + delta_t_ * thetad1, -(thetad2 - thetad1); + + // Jacobians + if (H1) *H1 = (gtsam::Matrix(2, 1) << 1, 0).finished(); + if (H2) *H2 = (gtsam::Matrix(2, 1) << delta_t_, 1).finished(); + if (H3) *H3 = (gtsam::Matrix(2, 1) << -1, 0).finished(); + if (H4) *H4 = (gtsam::Matrix(2, 1) << 0, -1).finished(); + + // transition matrix & error + return error; + } + + // double calcF(gtsam::Matrix error, gtsam::Matrix Qinv) { + // return (1/2 * error.transposed() * Qinv * error); + // } + + gtsam::Matrix calcQ(double Qc, double delta_t) { + return (gtsam::Matrix(2, 2) << 1.0 / 3 * pow(delta_t, 3.0) * Qc, + 1.0 / 2 * pow(delta_t, 2.0) * Qc, 1.0 / 2 * pow(delta_t, 2.0) * Qc, + delta_t * Qc) + .finished(); + } + + // gtsam::Matrix calcQ_inv(double Qc, double delta_t) { + // return (gtsam::Matrix(2*Qc.rows(), 2*Qc.rows()) << + // 12.0 * pow(delta_t, -3.0) / Qc_inv, (-6.0) * pow(delta_t, -2.0) / + // Qc_inv, + // (-6.0) * pow(delta_t, -2.0) / Qc_inv, 4.0 * pow(delta_t, -1.0) / + // Qc_inv).finished(); + // } + + /** demensions */ + size_t dim() const override { return 1; } + + /** number of variables attached to this factor */ + size_t size() const { return 4; } + + /** equals specialized to this factor */ + virtual bool equals(const gtsam::NonlinearFactor& expected, + double tol = 1e-9) const override { + const This* e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) && + fabs(this->delta_t_ - e->delta_t_) < tol; + } + + /** print contents */ + void print(const std::string& s = "", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const override { + std::cout << s << "4-way Gaussian Process Factor Linear(" << 1 << ")" + << std::endl; + Base::print("", keyFormatter); + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(delta_t_); + } + +}; // GaussianProcessPrior + +} // namespace gtdynamics \ No newline at end of file diff --git a/gtdynamics/pandarobot/ikfast/PandaIKFast.cpp b/gtdynamics/pandarobot/ikfast/PandaIKFast.cpp index 5629de7b..fed0987c 100644 --- a/gtdynamics/pandarobot/ikfast/PandaIKFast.cpp +++ b/gtdynamics/pandarobot/ikfast/PandaIKFast.cpp @@ -10,7 +10,6 @@ //----------------------------------------------------------------------------// #define IKFAST_HAS_LIBRARY // Build IKFast with API functions -#define IKFAST_NO_MAIN // Don't include main() from IKFast #define IKFAST_NAMESPACE panda_internal #define IK_VERSION 61 @@ -64,7 +63,7 @@ std::vector PandaIKFast::inverse(const Pose3& bTe, double theta7) { &theta7, solutions); if (!success) { - fprintf(stderr, "Error: (inverse PandaIKFast) failed to get ik solution\n"); + //fprintf(stderr, "Error: (inverse PandaIKFast) failed to get ik solution\n"); return std::vector(); } diff --git a/gtdynamics/pandarobot/ikfast/PandaIKFast.h b/gtdynamics/pandarobot/ikfast/PandaIKFast.h index 4e0fafd4..440085c2 100644 --- a/gtdynamics/pandarobot/ikfast/PandaIKFast.h +++ b/gtdynamics/pandarobot/ikfast/PandaIKFast.h @@ -44,7 +44,7 @@ class PandaIKFast { * @return std::vector -- std vector containing the different * solutions for joint angles' values */ - static std::vector inverse(const gtsam::Pose3& bRe, + static std::vector inverse(const gtsam::Pose3& bTe, double theta7); }; diff --git a/gtdynamics/pandarobot/ikfast/ikfast61_panda.cpp b/gtdynamics/pandarobot/ikfast/ikfast61_panda.cpp index d1141150..34851e91 100644 --- a/gtdynamics/pandarobot/ikfast/ikfast61_panda.cpp +++ b/gtdynamics/pandarobot/ikfast/ikfast61_panda.cpp @@ -12769,50 +12769,3 @@ IKFAST_API const char* GetIkFastVersion() { return "0x1000004a"; } #ifdef IKFAST_NAMESPACE } // end namespace #endif - -#ifndef IKFAST_NO_MAIN -#include -#include -#ifdef IKFAST_NAMESPACE -using namespace IKFAST_NAMESPACE; -#endif -int main(int argc, char** argv) -{ - if( argc != 12+GetNumFreeParameters()+1 ) { - printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n" - "Returns the ik solutions given the transformation of the end effector specified by\n" - "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n" - "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters()); - return 1; - } - - IkSolutionList solutions; - std::vector vfree(GetNumFreeParameters()); - IkReal eerot[9],eetrans[3]; - eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]); - eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]); - eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]); - for(std::size_t i = 0; i < vfree.size(); ++i) - vfree[i] = atof(argv[13+i]); - bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions); - - if( !bSuccess ) { - fprintf(stderr,"Failed to get ik solution\n"); - return -1; - } - - printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions()); - std::vector solvalues(GetNumJoints()); - for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) { - const IkSolutionBase& sol = solutions.GetSolution(i); - printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size()); - std::vector vsolfree(sol.GetFree().size()); - sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL); - for( std::size_t j = 0; j < solvalues.size(); ++j) - printf("%.15f, ", solvalues[j]); - printf("\n"); - } - return 0; -} - -#endif diff --git a/models/urdfs/panda/panda.urdf b/models/urdfs/panda/panda.urdf index a24f1480..b1878770 100644 --- a/models/urdfs/panda/panda.urdf +++ b/models/urdfs/panda/panda.urdf @@ -48,7 +48,7 @@ - + @@ -78,7 +78,7 @@ - + @@ -108,7 +108,7 @@ - + @@ -138,7 +138,7 @@ - + @@ -168,7 +168,7 @@ - + @@ -198,7 +198,7 @@ - + @@ -228,7 +228,7 @@ - + From 54c72df3420ffdc7fb4ec8b0cea9c4d45e8a8057 Mon Sep 17 00:00:00 2001 From: Joobz Date: Wed, 6 Jul 2022 11:51:05 +0200 Subject: [PATCH 12/12] small update --- gtdynamics/kinematics/Kinematics.h | 3 +- gtdynamics/kinematics/KinematicsSlice.cpp | 4 +- gtdynamics/pandarobot/CMakeLists.txt | 2 +- .../tests/testGaussianProcessPrior.cpp | 87 +++++++++++++++++++ 4 files changed, 92 insertions(+), 4 deletions(-) create mode 100644 gtdynamics/pandarobot/tests/testGaussianProcessPrior.cpp diff --git a/gtdynamics/kinematics/Kinematics.h b/gtdynamics/kinematics/Kinematics.h index 059200d8..0197a341 100644 --- a/gtdynamics/kinematics/Kinematics.h +++ b/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 diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index f1448c80..9a01cb11 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -175,10 +175,10 @@ NonlinearFactorGraph Kinematics::jointAngleLimits( 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; } diff --git a/gtdynamics/pandarobot/CMakeLists.txt b/gtdynamics/pandarobot/CMakeLists.txt index 2d34e3e1..a8d48936 100644 --- a/gtdynamics/pandarobot/CMakeLists.txt +++ b/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) diff --git a/gtdynamics/pandarobot/tests/testGaussianProcessPrior.cpp b/gtdynamics/pandarobot/tests/testGaussianProcessPrior.cpp new file mode 100644 index 00000000..bdc456c7 --- /dev/null +++ b/gtdynamics/pandarobot/tests/testGaussianProcessPrior.cpp @@ -0,0 +1,87 @@ + +/** +* @file testGaussianProcessPrior.cpp +* @author Wanli Qian, Toni Jubes, Frank Dellaert +**/ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + + +#include + +#include + +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); +} \ No newline at end of file