From 764b1275169f8d276b1b78be3d7c20938abd6cde Mon Sep 17 00:00:00 2001 From: Matt King-Smith Date: Tue, 16 May 2023 17:17:15 -0400 Subject: [PATCH 01/11] initial factor commit --- gpmp2/kinematics/PriorWorkspaceVelocity.h | 108 ++++++++++++++++++++++ 1 file changed, 108 insertions(+) create mode 100644 gpmp2/kinematics/PriorWorkspaceVelocity.h diff --git a/gpmp2/kinematics/PriorWorkspaceVelocity.h b/gpmp2/kinematics/PriorWorkspaceVelocity.h new file mode 100644 index 0000000..235f699 --- /dev/null +++ b/gpmp2/kinematics/PriorWorkspaceVelocity.h @@ -0,0 +1,108 @@ +/** + * @file PriorWorkspaceVelocity.h + * @brief Prior defined on the workspace velocity of final joint of a robot + * given its state in configuration space + * @author Matthew King-Smith + * @date May 16, 2023 + **/ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include + +namespace gpmp2 { + +/** + * binary factor Gaussian prior defined on the workspace vector + */ +class PriorWorkspaceVelocity : public gtsam::NoiseModelFactorN { + private: + // typedefs + typedef PriorWorkspaceVelocity This; + typedef gtsam::NoiseModelFactorN Base; + + // arm + Arm arm_; + + // desired velocity (linear and angular) + gtsam::Vector6 dest_vel_; + + public: + /// shorthand for a smart pointer to a factor + typedef std::shared_ptr shared_ptr; + + /** + * Constructor + * @param cost_model cost function covariance + * @param dest_vel desired velocity (linear and angular) + */ + PriorWorkspaceVelocity(gtsam::Key poseKey, gtsam::Key velKey, + const gtsam::SharedNoiseModel& cost_model, + const Arm& arm, const gtsam::Vector6& dest_vel) + : Base(cost_model, poseKey, velKey), arm_(arm), dest_vel_(dest_vel) {} + + virtual ~PriorWorkspaceVelocity() {} + + /// error function + gtsam::Vector evaluateError( + const gtsam::Vector& conf, gtsam::OptionalMatrixType H1 = nullptr, + gtsam::OptionalMatrixType H2 = nullptr) const override { + using namespace gtsam; + + // fk + std::vector joint_pos; + std::vector joint_vel; + std::vector J_jpx_jp, J_jvx_jv; + arm_.forwardKinematics(conf, ) + + if (H1) *H1 = Matrix::Zero(conf.size(), conf.size()); + Vector err(conf.size()); + for (size_t i = 0; i < (size_t)conf.size(); i++) { + if (H1) { + double Hp; + err(i) = hingeLossJointLimitCost(conf(i), -vel_limit_(i), vel_limit_(i), + limit_thresh_(i), &Hp); + (*H1)(i, i) = Hp; + } else { + err(i) = hingeLossJointLimitCost(conf(i), -vel_limit_(i), vel_limit_(i), + limit_thresh_(i)); + } + } + return err; + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /** print contents */ + void print(const std::string& s = "", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const { + std::cout << s << "VelocityLimitFactorVector :" << std::endl; + Base::print("", keyFormatter); + std::cout << "Limit cost threshold : " << limit_thresh_ << std::endl; + } + + private: +#ifdef GPMP2_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int version) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor1", boost::serialization::base_object(*this)); + } +#endif +}; + +} // namespace gpmp2 From 7cbc76505e3bd1a9fd2d5f8bb1b381a7b7dc99c5 Mon Sep 17 00:00:00 2001 From: Matt King-Smith Date: Tue, 16 May 2023 20:58:49 -0400 Subject: [PATCH 02/11] merged workspace pose and velocity into one factor --- gpmp2/kinematics/PriorWorkspacePoseVelocity.h | 127 ++++++++++++++++++ gpmp2/kinematics/PriorWorkspaceVelocity.h | 108 --------------- 2 files changed, 127 insertions(+), 108 deletions(-) create mode 100644 gpmp2/kinematics/PriorWorkspacePoseVelocity.h delete mode 100644 gpmp2/kinematics/PriorWorkspaceVelocity.h diff --git a/gpmp2/kinematics/PriorWorkspacePoseVelocity.h b/gpmp2/kinematics/PriorWorkspacePoseVelocity.h new file mode 100644 index 0000000..b300713 --- /dev/null +++ b/gpmp2/kinematics/PriorWorkspacePoseVelocity.h @@ -0,0 +1,127 @@ +/** + * @file PriorWorkspacePoseVelocity.h + * @brief Prior defined on the workspace pose and velocity of final joint + *(end-effector) of a robot given its state in configuration space + * @author Matthew King-Smith + * @date May 16, 2023 + **/ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include + +namespace gpmp2 { + +/** + * binary factor Gaussian prior defined on the workspace pose and velocities + */ +class PriorWorkspacePoseVelocity + : public gtsam::NoiseModelFactorN { + private: + // typedefs + typedef PriorWorkspacePoseVelocity This; + typedef gtsam::NoiseModelFactorN Base; + + // arm + Arm arm_; + + // desired workspace pose + gtsam::Pose3 des_pose_; + + // desired velocity (linear and angular) + gtsam::Vector6 des_vel_; + + public: + /// shorthand for a smart pointer to a factor + typedef std::shared_ptr shared_ptr; + + /** + * Constructor + * @param cost_model cost function covariance + * @param dest_vel desired velocity (linear and angular) + */ + PriorWorkspacePoseVelocity(gtsam::Key poseKey, gtsam::Key velKey, + const gtsam::SharedNoiseModel& cost_model, + const Arm& arm, const gtsam::Pose3& des_pose, + const gtsam::Vector6& des_vel) + : Base(cost_model, poseKey, velKey), + arm_(arm), + des_pose_(des_pose), + des_vel_(des_vel) {} + + virtual ~PriorWorkspacePoseVelocity() {} + + /// error function + gtsam::Vector evaluateError( + const gtsam::Vector& joint_conf, const gtsam::Vector& joint_rates, + gtsam::OptionalMatrixType H1 = nullptr, + gtsam::OptionalMatrixType H2 = nullptr) const override { + using namespace gtsam; + + // fk + std::vector joint_pose; + std::vector joint_vel; + std::vector J_jpx_jp, J_jvx_jp, J_jvx_jv; + arm_.forwardKinematics(joint_conf, joint_rates, joint_pose, joint_vel, + &J_jpx_jp, &J_jvx_jp, &J_jvx_jv); + + if (H1) { + Matrix66 H_ep; + Vector pose_error = + des_pose_.logmap(joint_pose[arm_.dof() - 1], {}, H_ep); + // Jacobian for the joint positions + *H1 = (gtsam::Matrix(6, arm_.dof()) << H_ep * J_jpx_jp[arm_.dof() - 1], + J_jvx_jp) + .finished(); + } else { + Vector pose_error = des_pose_.logmap(joint_pose[arm_.dof() - 1]); + } + + if (H2) { + Vector vel_error = des_vel_ - joint_vel[arm_.dof() - 1]; + // Jacobian for the joint rates + *H2 = J_jvx_jv; + } else { + Vector vel_error = des_vel_ - joint_vel[arm_.dof() - 1]; + } + return pose_error + vel_error; + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /** print contents */ + void print(const std::string& s = "", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const { + std::cout << s << "PriorWorkspacePoseVelocity :" << std::endl; + Base::print("", keyFormatter); + std::cout << "desired end-effector pose : "; + des_pose_.print() << std::endl; + std::cout << "desired end-effector velocity : "; + des_vel_.print(); + } + + private: +#ifdef GPMP2_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int version) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor1", boost::serialization::base_object(*this)); + } +#endif +}; + +} \ No newline at end of file diff --git a/gpmp2/kinematics/PriorWorkspaceVelocity.h b/gpmp2/kinematics/PriorWorkspaceVelocity.h deleted file mode 100644 index 235f699..0000000 --- a/gpmp2/kinematics/PriorWorkspaceVelocity.h +++ /dev/null @@ -1,108 +0,0 @@ -/** - * @file PriorWorkspaceVelocity.h - * @brief Prior defined on the workspace velocity of final joint of a robot - * given its state in configuration space - * @author Matthew King-Smith - * @date May 16, 2023 - **/ - -#pragma once - -#include -#include -#include -#include -#include - -#include -#include - -namespace gpmp2 { - -/** - * binary factor Gaussian prior defined on the workspace vector - */ -class PriorWorkspaceVelocity : public gtsam::NoiseModelFactorN { - private: - // typedefs - typedef PriorWorkspaceVelocity This; - typedef gtsam::NoiseModelFactorN Base; - - // arm - Arm arm_; - - // desired velocity (linear and angular) - gtsam::Vector6 dest_vel_; - - public: - /// shorthand for a smart pointer to a factor - typedef std::shared_ptr shared_ptr; - - /** - * Constructor - * @param cost_model cost function covariance - * @param dest_vel desired velocity (linear and angular) - */ - PriorWorkspaceVelocity(gtsam::Key poseKey, gtsam::Key velKey, - const gtsam::SharedNoiseModel& cost_model, - const Arm& arm, const gtsam::Vector6& dest_vel) - : Base(cost_model, poseKey, velKey), arm_(arm), dest_vel_(dest_vel) {} - - virtual ~PriorWorkspaceVelocity() {} - - /// error function - gtsam::Vector evaluateError( - const gtsam::Vector& conf, gtsam::OptionalMatrixType H1 = nullptr, - gtsam::OptionalMatrixType H2 = nullptr) const override { - using namespace gtsam; - - // fk - std::vector joint_pos; - std::vector joint_vel; - std::vector J_jpx_jp, J_jvx_jv; - arm_.forwardKinematics(conf, ) - - if (H1) *H1 = Matrix::Zero(conf.size(), conf.size()); - Vector err(conf.size()); - for (size_t i = 0; i < (size_t)conf.size(); i++) { - if (H1) { - double Hp; - err(i) = hingeLossJointLimitCost(conf(i), -vel_limit_(i), vel_limit_(i), - limit_thresh_(i), &Hp); - (*H1)(i, i) = Hp; - } else { - err(i) = hingeLossJointLimitCost(conf(i), -vel_limit_(i), vel_limit_(i), - limit_thresh_(i)); - } - } - return err; - } - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); - } - - /** print contents */ - void print(const std::string& s = "", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const { - std::cout << s << "VelocityLimitFactorVector :" << std::endl; - Base::print("", keyFormatter); - std::cout << "Limit cost threshold : " << limit_thresh_ << std::endl; - } - - private: -#ifdef GPMP2_ENABLE_BOOST_SERIALIZATION - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int version) { - ar& boost::serialization::make_nvp( - "NoiseModelFactor1", boost::serialization::base_object(*this)); - } -#endif -}; - -} // namespace gpmp2 From 38302aef06254e1ba06a3980e462e12b2ccd9c79 Mon Sep 17 00:00:00 2001 From: Matt King-Smith Date: Tue, 16 May 2023 23:25:55 -0400 Subject: [PATCH 03/11] Added unit test for pose velocity factor --- gpmp2/kinematics/PriorWorkspacePoseVelocity.h | 42 ++++---- .../tests/testPriorWorkspacePoseVelocity.cpp | 97 +++++++++++++++++++ 2 files changed, 116 insertions(+), 23 deletions(-) create mode 100644 gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp diff --git a/gpmp2/kinematics/PriorWorkspacePoseVelocity.h b/gpmp2/kinematics/PriorWorkspacePoseVelocity.h index b300713..b1b1b25 100644 --- a/gpmp2/kinematics/PriorWorkspacePoseVelocity.h +++ b/gpmp2/kinematics/PriorWorkspacePoseVelocity.h @@ -23,11 +23,11 @@ namespace gpmp2 { * binary factor Gaussian prior defined on the workspace pose and velocities */ class PriorWorkspacePoseVelocity - : public gtsam::NoiseModelFactorN { + : public gtsam::NoiseModelFactorN { private: // typedefs typedef PriorWorkspacePoseVelocity This; - typedef gtsam::NoiseModelFactorN Base; + typedef gtsam::NoiseModelFactorN Base; // arm Arm arm_; @@ -42,10 +42,13 @@ class PriorWorkspacePoseVelocity /// shorthand for a smart pointer to a factor typedef std::shared_ptr shared_ptr; + /// Default constructor + PriorWorkspacePoseVelocity() {} + /** * Constructor * @param cost_model cost function covariance - * @param dest_vel desired velocity (linear and angular) + * @param des_vel desired velocity (linear and angular) */ PriorWorkspacePoseVelocity(gtsam::Key poseKey, gtsam::Key velKey, const gtsam::SharedNoiseModel& cost_model, @@ -56,40 +59,34 @@ class PriorWorkspacePoseVelocity des_pose_(des_pose), des_vel_(des_vel) {} - virtual ~PriorWorkspacePoseVelocity() {} + ~PriorWorkspacePoseVelocity() {} /// error function gtsam::Vector evaluateError( const gtsam::Vector& joint_conf, const gtsam::Vector& joint_rates, gtsam::OptionalMatrixType H1 = nullptr, - gtsam::OptionalMatrixType H2 = nullptr) const override { + gtsam::OptionalMatrixType H2 = nullptr) const { using namespace gtsam; // fk std::vector joint_pose; std::vector joint_vel; std::vector J_jpx_jp, J_jvx_jp, J_jvx_jv; - arm_.forwardKinematics(joint_conf, joint_rates, joint_pose, joint_vel, + arm_.forwardKinematics(joint_conf, joint_rates, joint_pose, &joint_vel, &J_jpx_jp, &J_jvx_jp, &J_jvx_jv); + Matrix66 H_ep; + Vector pose_error = des_pose_.logmap(joint_pose[arm_.dof() - 1], {}, H_ep); + Vector vel_error = des_vel_ - joint_vel[arm_.dof() - 1]; if (H1) { - Matrix66 H_ep; - Vector pose_error = - des_pose_.logmap(joint_pose[arm_.dof() - 1], {}, H_ep); // Jacobian for the joint positions - *H1 = (gtsam::Matrix(6, arm_.dof()) << H_ep * J_jpx_jp[arm_.dof() - 1], - J_jvx_jp) - .finished(); - } else { - Vector pose_error = des_pose_.logmap(joint_pose[arm_.dof() - 1]); + *H1 = (Matrix(6, 2 * arm_.dof()) << H_ep * J_jpx_jp[arm_.dof() - 1], + J_jvx_jp[arm_.dof() - 1]) + .finished(); // H_ep * J_jpx_jp[arm_.dof() - 1]; } - if (H2) { - Vector vel_error = des_vel_ - joint_vel[arm_.dof() - 1]; // Jacobian for the joint rates - *H2 = J_jvx_jv; - } else { - Vector vel_error = des_vel_ - joint_vel[arm_.dof() - 1]; + *H2 = J_jvx_jv[arm_.dof() - 1]; } return pose_error + vel_error; } @@ -107,9 +104,8 @@ class PriorWorkspacePoseVelocity std::cout << s << "PriorWorkspacePoseVelocity :" << std::endl; Base::print("", keyFormatter); std::cout << "desired end-effector pose : "; - des_pose_.print() << std::endl; - std::cout << "desired end-effector velocity : "; - des_vel_.print(); + des_pose_.print(); // << std::endl; + std::cout << "desired end-effector velocity : " << des_vel_ << std::endl; } private: @@ -124,4 +120,4 @@ class PriorWorkspacePoseVelocity #endif }; -} \ No newline at end of file +} // namespace gpmp2 \ No newline at end of file diff --git a/gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp b/gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp new file mode 100644 index 0000000..83af64c --- /dev/null +++ b/gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp @@ -0,0 +1,97 @@ +/** + * @file testPriorWorkspacePoseVelocity.cpp + * @author Matthew King-Smith + **/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; +using namespace gpmp2; + +/* ************************************************************************** */ +TEST(PriorWorkspacePoseVelocity, error) { + Vector2 a(1, 1), alpha(M_PI / 2, 0), d(0, 0); + Arm arm = Arm(2, a, alpha, d); + Vector2 q; + Vector2 qdot; + Pose3 des_pose; + Vector6 des_vel; + PriorWorkspacePoseVelocity factor; + Vector actual, expect; + Matrix H_pose_exp, H_pose_act, H_vel_exp, H_vel_act; + noiseModel::Gaussian::shared_ptr cost_model = + noiseModel::Isotropic::Sigma(6, 1.0); + + q = Vector2(M_PI / 4.0, -M_PI / 2); + qdot = Vector2(0, 0); + des_pose = Pose3(); + des_vel = Vector6(0, 0, 0, 0, 0, 0); + factor = PriorWorkspacePoseVelocity(0, 0, cost_model, arm, des_pose, des_vel); + actual = factor.evaluateError(q, qdot, &H_pose_act, &H_vel_act); + expect = (Vector(6) << 0.613943126, 1.48218982, -0.613943126, 1.1609828, + 0.706727485, -0.547039678) + .finished(); + // H_pose_exp = numericalDerivative11( + // std::function( + // std::bind(&PriorWorkspacePoseVelocity::evaluateError, factor, + // std::placeholders::_1, qdot, nullptr, nullptr)), + // q, 1e-6); + + // H_vel_exp = numericalDerivative11( + // std::function( + // std::bind(&PriorWorkspacePoseVelocity::evaluateError, factor, q, + // std::placeholders::_1, nullptr, nullptr)), + // qdot, 1e-6); + EXPECT(assert_equal(expect, actual, 1e-6)); + // EXPECT(assert_equal(H_pose_exp, H_pose_act, 1e-6)); + // EXPECT(assert_equal(H_vel_exp, H_vel_act, 1e-6)); +} + +/* ************************************************************************** */ +// TEST(GaussianPriorWorkspacePoseArm, optimization) { +// noiseModel::Gaussian::shared_ptr cost_model = +// noiseModel::Isotropic::Sigma(6, 0.1); + +// Vector a = (Vector(2) << 1, 1).finished(); +// Vector alpha = (Vector(2) << 0, 0).finished(); +// Vector d = (Vector(2) << 0, 0).finished(); +// ArmModel arm = ArmModel(Arm(2, a, alpha, d), BodySphereVector()); +// Pose3 des = Pose3(Rot3(), Point3(2, 0, 0)); + +// Key qkey = Symbol('x', 0); +// Vector q = (Vector(2) << 0, 0).finished(); +// Vector qinit = (Vector(2) << M_PI / 2, M_PI / 2).finished(); + +// NonlinearFactorGraph graph; +// graph.add(GaussianPriorWorkspacePoseArm(qkey, arm, 1, des, cost_model)); +// Values init_values; +// init_values.insert(qkey, qinit); + +// LevenbergMarquardtParams parameters; +// parameters.setVerbosity("ERROR"); +// parameters.setAbsoluteErrorTol(1e-12); +// LevenbergMarquardtOptimizer optimizer(graph, init_values, parameters); +// optimizer.optimize(); +// Values results = optimizer.values(); + +// EXPECT_DOUBLES_EQUAL(0, graph.error(results), 1e-3); +// EXPECT(assert_equal(q, results.at(qkey), 1e-3)); +// } + +/* ************************************************************************** */ +/* main function */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} From 7f48b89c039a4f6916e552f0559fe01ec5781da4 Mon Sep 17 00:00:00 2001 From: Matt King-Smith Date: Wed, 17 May 2023 08:14:49 -0400 Subject: [PATCH 04/11] Verified C++ unit tests. --- gpmp2/kinematics/PriorWorkspacePoseVelocity.h | 20 ++--- .../tests/testPriorWorkspacePoseVelocity.cpp | 90 ++++++++++--------- 2 files changed, 57 insertions(+), 53 deletions(-) diff --git a/gpmp2/kinematics/PriorWorkspacePoseVelocity.h b/gpmp2/kinematics/PriorWorkspacePoseVelocity.h index b1b1b25..a06eb86 100644 --- a/gpmp2/kinematics/PriorWorkspacePoseVelocity.h +++ b/gpmp2/kinematics/PriorWorkspacePoseVelocity.h @@ -23,11 +23,11 @@ namespace gpmp2 { * binary factor Gaussian prior defined on the workspace pose and velocities */ class PriorWorkspacePoseVelocity - : public gtsam::NoiseModelFactorN { + : public gtsam::NoiseModelFactorN { private: // typedefs typedef PriorWorkspacePoseVelocity This; - typedef gtsam::NoiseModelFactorN Base; + typedef gtsam::NoiseModelFactorN Base; // arm Arm arm_; @@ -62,10 +62,10 @@ class PriorWorkspacePoseVelocity ~PriorWorkspacePoseVelocity() {} /// error function - gtsam::Vector evaluateError( - const gtsam::Vector& joint_conf, const gtsam::Vector& joint_rates, - gtsam::OptionalMatrixType H1 = nullptr, - gtsam::OptionalMatrixType H2 = nullptr) const { + gtsam::Vector evaluateError(const gtsam::Vector& joint_conf, + const gtsam::Vector& joint_rates, + gtsam::OptionalMatrixType H1 = nullptr, + gtsam::OptionalMatrixType H2 = nullptr) const override { using namespace gtsam; // fk @@ -80,19 +80,17 @@ class PriorWorkspacePoseVelocity Vector vel_error = des_vel_ - joint_vel[arm_.dof() - 1]; if (H1) { // Jacobian for the joint positions - *H1 = (Matrix(6, 2 * arm_.dof()) << H_ep * J_jpx_jp[arm_.dof() - 1], - J_jvx_jp[arm_.dof() - 1]) - .finished(); // H_ep * J_jpx_jp[arm_.dof() - 1]; + *H1 = H_ep * J_jpx_jp[arm_.dof() - 1]; } if (H2) { // Jacobian for the joint rates - *H2 = J_jvx_jv[arm_.dof() - 1]; + *H2 = -J_jvx_jv[arm_.dof() - 1]; } return pose_error + vel_error; } /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { + gtsam::NonlinearFactor::shared_ptr clone() const override { return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } diff --git a/gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp b/gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp index 83af64c..48de654 100644 --- a/gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp +++ b/gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp @@ -26,68 +26,74 @@ TEST(PriorWorkspacePoseVelocity, error) { Vector2 q; Vector2 qdot; Pose3 des_pose; - Vector6 des_vel; - PriorWorkspacePoseVelocity factor; + // Vector6 des_vel; + // PriorWorkspacePoseVelocity factor; Vector actual, expect; Matrix H_pose_exp, H_pose_act, H_vel_exp, H_vel_act; noiseModel::Gaussian::shared_ptr cost_model = noiseModel::Isotropic::Sigma(6, 1.0); q = Vector2(M_PI / 4.0, -M_PI / 2); - qdot = Vector2(0, 0); + qdot = (Vector2() << 0, 0).finished(); des_pose = Pose3(); - des_vel = Vector6(0, 0, 0, 0, 0, 0); - factor = PriorWorkspacePoseVelocity(0, 0, cost_model, arm, des_pose, des_vel); + Vector6 des_vel = (Vector6() << 0, 0, 0, 0, 0, 0).finished(); + PriorWorkspacePoseVelocity factor(0, 0, cost_model, arm, des_pose, des_vel); actual = factor.evaluateError(q, qdot, &H_pose_act, &H_vel_act); expect = (Vector(6) << 0.613943126, 1.48218982, -0.613943126, 1.1609828, 0.706727485, -0.547039678) .finished(); - // H_pose_exp = numericalDerivative11( - // std::function( - // std::bind(&PriorWorkspacePoseVelocity::evaluateError, factor, - // std::placeholders::_1, qdot, nullptr, nullptr)), - // q, 1e-6); + H_pose_exp = numericalDerivative11( + std::function( + std::bind(&PriorWorkspacePoseVelocity::evaluateError, factor, + std::placeholders::_1, qdot, nullptr, nullptr)), + q, 1e-6); - // H_vel_exp = numericalDerivative11( - // std::function( - // std::bind(&PriorWorkspacePoseVelocity::evaluateError, factor, q, - // std::placeholders::_1, nullptr, nullptr)), - // qdot, 1e-6); + H_vel_exp = numericalDerivative11( + std::function( + std::bind(&PriorWorkspacePoseVelocity::evaluateError, factor, q, + std::placeholders::_1, nullptr, nullptr)), + qdot, 1e-6); EXPECT(assert_equal(expect, actual, 1e-6)); - // EXPECT(assert_equal(H_pose_exp, H_pose_act, 1e-6)); - // EXPECT(assert_equal(H_vel_exp, H_vel_act, 1e-6)); + EXPECT(assert_equal(H_pose_exp, H_pose_act, 1e-6)); + EXPECT(assert_equal(H_vel_exp, H_vel_act, 1e-6)); } /* ************************************************************************** */ -// TEST(GaussianPriorWorkspacePoseArm, optimization) { -// noiseModel::Gaussian::shared_ptr cost_model = -// noiseModel::Isotropic::Sigma(6, 0.1); - -// Vector a = (Vector(2) << 1, 1).finished(); -// Vector alpha = (Vector(2) << 0, 0).finished(); -// Vector d = (Vector(2) << 0, 0).finished(); -// ArmModel arm = ArmModel(Arm(2, a, alpha, d), BodySphereVector()); -// Pose3 des = Pose3(Rot3(), Point3(2, 0, 0)); +TEST(PriorWorkspacePoseVelocity, optimization) { + noiseModel::Gaussian::shared_ptr cost_model = + noiseModel::Isotropic::Sigma(6, 0.1); -// Key qkey = Symbol('x', 0); -// Vector q = (Vector(2) << 0, 0).finished(); -// Vector qinit = (Vector(2) << M_PI / 2, M_PI / 2).finished(); + Vector a = (Vector(2) << 1, 1).finished(); + Vector alpha = (Vector(2) << 0, 0).finished(); + Vector d = (Vector(2) << 0, 0).finished(); + Arm arm = Arm(2, a, alpha, d); + Pose3 des_pose = Pose3(Rot3(), Point3(2, 0, 0)); + Vector6 des_vel = Vector6(0, 0, 0, 0, 0, 0); + Key qkey = Symbol('x', 0); + Key qdotkey = Symbol('v', 0); + Vector q = (Vector(2) << 0, 0).finished(); + Vector qdot = (Vector(2) << 0, 0).finished(); + Vector qinit = (Vector(2) << M_PI / 2, M_PI / 2).finished(); + Vector qdotinit = (Vector(2) << 0, 0).finished(); -// NonlinearFactorGraph graph; -// graph.add(GaussianPriorWorkspacePoseArm(qkey, arm, 1, des, cost_model)); -// Values init_values; -// init_values.insert(qkey, qinit); + NonlinearFactorGraph graph; + graph.add(PriorWorkspacePoseVelocity(qkey, qdotkey, cost_model, arm, + des_pose, des_vel)); + Values init_values; + init_values.insert(qkey, qinit); + init_values.insert(qdotkey, qdotinit); -// LevenbergMarquardtParams parameters; -// parameters.setVerbosity("ERROR"); -// parameters.setAbsoluteErrorTol(1e-12); -// LevenbergMarquardtOptimizer optimizer(graph, init_values, parameters); -// optimizer.optimize(); -// Values results = optimizer.values(); + LevenbergMarquardtParams parameters; + parameters.setVerbosity("ERROR"); + parameters.setAbsoluteErrorTol(1e-12); + LevenbergMarquardtOptimizer optimizer(graph, init_values, parameters); + optimizer.optimize(); + Values results = optimizer.values(); -// EXPECT_DOUBLES_EQUAL(0, graph.error(results), 1e-3); -// EXPECT(assert_equal(q, results.at(qkey), 1e-3)); -// } + EXPECT_DOUBLES_EQUAL(0, graph.error(results), 1e-3); + EXPECT(assert_equal(q, results.at(qkey), 1e-3)); + EXPECT(assert_equal(qdot, results.at(qdotkey), 1e-3)); +} /* ************************************************************************** */ /* main function */ From 413a11cdd408eac0150525e3adc6792906833920 Mon Sep 17 00:00:00 2001 From: Matt King-Smith Date: Wed, 17 May 2023 08:16:08 -0400 Subject: [PATCH 05/11] Formatting --- gpmp2/kinematics/PriorWorkspacePoseVelocity.h | 8 ++++---- gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/gpmp2/kinematics/PriorWorkspacePoseVelocity.h b/gpmp2/kinematics/PriorWorkspacePoseVelocity.h index a06eb86..d6a4f64 100644 --- a/gpmp2/kinematics/PriorWorkspacePoseVelocity.h +++ b/gpmp2/kinematics/PriorWorkspacePoseVelocity.h @@ -62,10 +62,10 @@ class PriorWorkspacePoseVelocity ~PriorWorkspacePoseVelocity() {} /// error function - gtsam::Vector evaluateError(const gtsam::Vector& joint_conf, - const gtsam::Vector& joint_rates, - gtsam::OptionalMatrixType H1 = nullptr, - gtsam::OptionalMatrixType H2 = nullptr) const override { + gtsam::Vector evaluateError( + const gtsam::Vector& joint_conf, const gtsam::Vector& joint_rates, + gtsam::OptionalMatrixType H1 = nullptr, + gtsam::OptionalMatrixType H2 = nullptr) const override { using namespace gtsam; // fk diff --git a/gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp b/gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp index 48de654..3af8323 100644 --- a/gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp +++ b/gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp @@ -77,8 +77,8 @@ TEST(PriorWorkspacePoseVelocity, optimization) { Vector qdotinit = (Vector(2) << 0, 0).finished(); NonlinearFactorGraph graph; - graph.add(PriorWorkspacePoseVelocity(qkey, qdotkey, cost_model, arm, - des_pose, des_vel)); + graph.add(PriorWorkspacePoseVelocity(qkey, qdotkey, cost_model, arm, des_pose, + des_vel)); Values init_values; init_values.insert(qkey, qinit); init_values.insert(qdotkey, qdotinit); From 81c1bcefc7073b689824a0bbb5987bc20d74a672 Mon Sep 17 00:00:00 2001 From: Matt King-Smith Date: Wed, 17 May 2023 08:26:21 -0400 Subject: [PATCH 06/11] added factor to gpmp2.i --- gpmp2.i | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/gpmp2.i b/gpmp2.i index 23fc6de..37885fc 100644 --- a/gpmp2.i +++ b/gpmp2.i @@ -363,6 +363,14 @@ class PointRobotModel { double sphere_radius(size_t i) const; }; +// desired workspace pose and velocity for last link factor +#include + +virtual class PriorWorkspacePoseVelocity : gtsam::NoiseModelFactor { + PriorWorkspacePoseVelocity(size_t poseKey, size_t velKey, const gtsam::noiseModel::Base* cost_model, + const gpmp2::Arm& arm, const gtsam::Pose3& des_pose, const gtsam::Vector6& des_vel); +}; + // goal destination factor #include From a75e596909e1b83c8b4323ac8f28381a11532c7b Mon Sep 17 00:00:00 2001 From: Matt King-Smith Date: Wed, 17 May 2023 10:17:40 -0400 Subject: [PATCH 07/11] Fixed unit test and implemented MATLAB example --- gpmp2.i | 2 +- gpmp2/kinematics/PriorWorkspacePoseVelocity.h | 2 +- .../tests/testPriorWorkspacePoseVelocity.cpp | 10 +- .../KinovaGen3PoseVelocityConstraintExample.m | 206 ++++++++++++++++++ 4 files changed, 214 insertions(+), 6 deletions(-) create mode 100644 matlab/gpmp2_examples/KinovaGen3PoseVelocityConstraintExample.m diff --git a/gpmp2.i b/gpmp2.i index 37885fc..882f612 100644 --- a/gpmp2.i +++ b/gpmp2.i @@ -368,7 +368,7 @@ class PointRobotModel { virtual class PriorWorkspacePoseVelocity : gtsam::NoiseModelFactor { PriorWorkspacePoseVelocity(size_t poseKey, size_t velKey, const gtsam::noiseModel::Base* cost_model, - const gpmp2::Arm& arm, const gtsam::Pose3& des_pose, const gtsam::Vector6& des_vel); + const gpmp2::Arm& arm, const gtsam::Pose3& des_pose, const gtsam::Vector& des_vel); }; // goal destination factor diff --git a/gpmp2/kinematics/PriorWorkspacePoseVelocity.h b/gpmp2/kinematics/PriorWorkspacePoseVelocity.h index d6a4f64..9cba39c 100644 --- a/gpmp2/kinematics/PriorWorkspacePoseVelocity.h +++ b/gpmp2/kinematics/PriorWorkspacePoseVelocity.h @@ -80,7 +80,7 @@ class PriorWorkspacePoseVelocity Vector vel_error = des_vel_ - joint_vel[arm_.dof() - 1]; if (H1) { // Jacobian for the joint positions - *H1 = H_ep * J_jpx_jp[arm_.dof() - 1]; + *H1 = H_ep * J_jpx_jp[arm_.dof() - 1] - J_jvx_jp[arm_.dof() - 1]; } if (H2) { // Jacobian for the joint rates diff --git a/gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp b/gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp index 3af8323..0ac8e3f 100644 --- a/gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp +++ b/gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp @@ -34,14 +34,16 @@ TEST(PriorWorkspacePoseVelocity, error) { noiseModel::Isotropic::Sigma(6, 1.0); q = Vector2(M_PI / 4.0, -M_PI / 2); - qdot = (Vector2() << 0, 0).finished(); + qdot = (Vector2() << 0.1, 0.1).finished(); des_pose = Pose3(); Vector6 des_vel = (Vector6() << 0, 0, 0, 0, 0, 0).finished(); PriorWorkspacePoseVelocity factor(0, 0, cost_model, arm, des_pose, des_vel); actual = factor.evaluateError(q, qdot, &H_pose_act, &H_vel_act); - expect = (Vector(6) << 0.613943126, 1.48218982, -0.613943126, 1.1609828, - 0.706727485, -0.547039678) + expect = (Vector(6) << 0.613943126, 1.340768463762691, -0.613943126, + 1.090272121881345, 0.777438163118655, -0.647039678) .finished(); + // (original solution wo velocity): 0.613943126, 1.48218982, + // -0.613943126, 1.1609828, 0.706727485, -0.547039678 H_pose_exp = numericalDerivative11( std::function( std::bind(&PriorWorkspacePoseVelocity::evaluateError, factor, @@ -74,7 +76,7 @@ TEST(PriorWorkspacePoseVelocity, optimization) { Vector q = (Vector(2) << 0, 0).finished(); Vector qdot = (Vector(2) << 0, 0).finished(); Vector qinit = (Vector(2) << M_PI / 2, M_PI / 2).finished(); - Vector qdotinit = (Vector(2) << 0, 0).finished(); + Vector qdotinit = (Vector(2) << 0.1, 0.1).finished(); NonlinearFactorGraph graph; graph.add(PriorWorkspacePoseVelocity(qkey, qdotkey, cost_model, arm, des_pose, diff --git a/matlab/gpmp2_examples/KinovaGen3PoseVelocityConstraintExample.m b/matlab/gpmp2_examples/KinovaGen3PoseVelocityConstraintExample.m new file mode 100644 index 0000000..65e6cfb --- /dev/null +++ b/matlab/gpmp2_examples/KinovaGen3PoseVelocityConstraintExample.m @@ -0,0 +1,206 @@ +% 7DOF Kinova Gen3 manipulator example with factor graph in MATLAB +% @author Matthew King-Smith +% @date 05-17-2023 + +close all; +clear; + +% import dynamic libraries +import gtsam.* +import gpmp2.* + +%% Generate dataset +dataset = generate3Ddataset('KinovaBoxDataset'); +origin = [dataset.origin_x, dataset.origin_y, dataset.origin_z]; +origin_point3 = Point3(origin'); +cell_size = dataset.cell_size; + +% sdf +disp('calculating signed distance field ...'); +field = signedDistanceField3D(dataset.map, dataset.cell_size); +disp('calculating signed distance field done'); + +% arm: KinovaGen3 +arm = generateArm('KinovaGen3'); + +% start and goal joint position and velocity configurations +start_conf = zeros(7,1); +start_vel = zeros(7,1); + +% convert feasible terminal configuration to an end-effector SE(3) pose +end_conf = [-pi/4, pi/3, 0, -pi/5, 0, 0, 0]'; +jposes = arm.fk_model().forwardKinematicsPose(end_conf); +ee_des_pose = Pose3(Rot3.Ypr(jposes(1,end),jposes(2,end),jposes(3,end)), ... + Point3(jposes(4,end),jposes(5,end),jposes(6,end))); + +% ee_des_vel = [linear_rate; angular_rate]^T +ee_des_vel = zeros(6,1); + +% plotting colors +green = [0.4667 0.6745 0.1882]; +cyan = [0, 1, 1]; + +% plot problem setting +fig1 = figure(1); +clf; +hold on; +set(fig1,'Name', 'Problem Settings'); +title('Problem Setup') +plotRobotModel(arm, start_conf); +plotRobotModel(arm, end_conf, green); +plotMap3D(dataset.corner_idx, origin, cell_size); +xlabel('x'); +ylabel('y'); +zlabel('z'); +grid on, view(3) +hold off; + +%% settings +total_time_sec = 10; +total_time_step = 50; +total_check_step = 100; +delta_t = total_time_sec / total_time_step; +check_inter = total_check_step / total_time_step - 1; + +% GP +Qc = 1 * eye(7); +Qc_model = noiseModel.Gaussian.Covariance(Qc); + +% obstacle cost and hinge loss distance settings +cost_sigma = 0.02; +epsilon_dist = 0.1; + +% noise model +fix_sigma = 0.001; +pose_fix_model = noiseModel.Isotropic.Sigma(7, fix_sigma); +vel_fix_model = noiseModel.Isotropic.Sigma(7, fix_sigma); + +% noise model for end-effector pose and rates +ee_fix_model = noiseModel.Isotropic.Sigma(6, 5e-3); + +% init sdf +sdf = SignedDistanceField(origin_point3, cell_size, size(field, 1), ... + size(field, 2), size(field, 3)); +for z = 1:size(field, 3) + sdf.initFieldData(z-1, field(:,:,z)'); +end + +%% initial traj +init_values = initArmTrajStraightLine(start_conf, end_conf, total_time_step); + +% plot initial trajectory +for i=0:total_time_step + hold on; + % plot arm + conf = init_values.atVector(symbol('x', i)); + plotRobotModel(arm, conf, cyan); + pause(0.1) +end +hold off; + +%% init optimization +graph = NonlinearFactorGraph; + +for i = 0 : total_time_step + key_pos = symbol('x', i); + key_vel = symbol('v', i); + + % priors + if i==0 + graph.add(PriorFactorVector(key_pos, start_conf, pose_fix_model)); + graph.add(PriorFactorVector(key_vel, start_vel, vel_fix_model)); + elseif i==total_time_step + graph.add(PriorWorkspacePoseVelocity(key_pos, key_vel,... + ee_fix_model, arm.fk_model(),... + ee_des_pose, ee_des_vel)); + end + + % GP priors and cost factor + if i > 0 + key_pos1 = symbol('x', i-1); + key_pos2 = symbol('x', i); + key_vel1 = symbol('v', i-1); + key_vel2 = symbol('v', i); + graph.add(GaussianProcessPriorLinear(key_pos1, key_vel1, ... + key_pos2, key_vel2, delta_t, Qc_model)); + + % cost factor + graph.add(ObstacleSDFFactorArm(... + key_pos, arm, sdf, cost_sigma, epsilon_dist)); + + % GP cost factor + if check_inter > 0 + for j = 1:check_inter + tau = j * (total_time_sec / total_check_step); + graph.add(ObstacleSDFFactorGPArm( ... + key_pos1, key_vel1, key_pos2, key_vel2, ... + arm, sdf, cost_sigma, epsilon_dist, ... + Qc_model, delta_t, tau)); + end + end + end +end + +%% Factor graph optimization +solver = 'LM'; %'DL','GN' + +if strcmp(solver,'LM') + parameters = LevenbergMarquardtParams; + parameters.setVerbosity('ERROR'); + parameters.setlambdaInitial(1000.0); + parameters.setRelativeErrorTol(1e-9); + parameters.setMaxIterations(100000); + parameters.setAbsoluteErrorTol(1e-9); + optimizer = LevenbergMarquardtOptimizer(graph, init_values, parameters); +elseif strcmp(solver,'DL') + parameters = DoglegParams; + parameters.setVerbosity('ERROR'); + parameters.setMaxIterations(1000.0); + optimizer = DoglegOptimizer(graph, init_values, parameters); +elseif strcmp(solver,'GN') + parameters = GaussNewtonParams; + parameters.setVerbosity('ERROR'); + parameters.setMaxIterations(1000.0); + optimizer = GaussNewtonOptimizer(graph, init_values, parameters); +end + + +fprintf('Initial Error = %d\n', graph.error(init_values)); + +optimizer.optimize(); + +result = optimizer.values(); +fprintf(' Final Error = %d\n', graph.error(result)); + +%% Plot results + +% plot final values +fig2 = figure(2); +set(fig2, 'Name', 'Optimization Results'); +clf; +title('Result Values'); +% plot world, initial and terminal configurations +hold on; +plotMap3D(dataset.corner_idx, origin, cell_size); +plotRobotModel(arm, start_conf); +plotRobotModel(arm, end_conf, green); +xlabel('x'); +ylabel('y'); +zlabel('z'); +grid on, view(3); +hold off; +for i=0:total_time_step + hold on; + % plot arm + conf = result.atVector(symbol('x', i)); + plotRobotModel(arm, conf, cyan); + pause(0.1); +end +hold off +joints_pose = arm.fk_model().forwardKinematicsPose(result.atVector(symbol('x', i))); +joints_vel = arm.fk_model().forwardKinematicsVel(result.atVector(symbol('x', i)),result.atVector(symbol('v', i))); + +% Errors +ee_pose_err = [ee_des_pose.rotation.ypr; ee_des_pose.translation] - joints_pose(:,end); +ee_vel_err = ee_des_vel- joints_vel(:,end); + From c2cb57e3b7308509b09c0af5feedb4400c2a1628 Mon Sep 17 00:00:00 2001 From: Matt King-Smith Date: Wed, 17 May 2023 11:06:36 -0400 Subject: [PATCH 08/11] Improved pose velocity workspace factor. --- gpmp2/kinematics/PriorWorkspacePoseVelocity.h | 14 +++++++++----- .../tests/testPriorWorkspacePoseVelocity.cpp | 11 +++++------ .../KinovaGen3PoseVelocityConstraintExample.m | 4 ++-- 3 files changed, 16 insertions(+), 13 deletions(-) diff --git a/gpmp2/kinematics/PriorWorkspacePoseVelocity.h b/gpmp2/kinematics/PriorWorkspacePoseVelocity.h index 9cba39c..174c103 100644 --- a/gpmp2/kinematics/PriorWorkspacePoseVelocity.h +++ b/gpmp2/kinematics/PriorWorkspacePoseVelocity.h @@ -23,11 +23,11 @@ namespace gpmp2 { * binary factor Gaussian prior defined on the workspace pose and velocities */ class PriorWorkspacePoseVelocity - : public gtsam::NoiseModelFactorN { + : public gtsam::NoiseModelFactor2 { private: // typedefs typedef PriorWorkspacePoseVelocity This; - typedef gtsam::NoiseModelFactorN Base; + typedef gtsam::NoiseModelFactor2 Base; // arm Arm arm_; @@ -80,13 +80,17 @@ class PriorWorkspacePoseVelocity Vector vel_error = des_vel_ - joint_vel[arm_.dof() - 1]; if (H1) { // Jacobian for the joint positions - *H1 = H_ep * J_jpx_jp[arm_.dof() - 1] - J_jvx_jp[arm_.dof() - 1]; + *H1 = (Matrix(12, arm_.dof()) << H_ep * J_jpx_jp[arm_.dof() - 1],// + - J_jvx_jp[arm_.dof() - 1]).finished(); } if (H2) { // Jacobian for the joint rates - *H2 = -J_jvx_jv[arm_.dof() - 1]; + *H2 = (Matrix(12, arm_.dof()) << Matrix::Zero(6, arm_.dof()), // + -J_jvx_jv[arm_.dof() - 1]).finished(); } - return pose_error + vel_error; + Vector12 tot_result; + tot_result << pose_error, vel_error; + return tot_result; } /// @return a deep copy of this factor diff --git a/gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp b/gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp index 0ac8e3f..ba1c4d5 100644 --- a/gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp +++ b/gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp @@ -31,7 +31,7 @@ TEST(PriorWorkspacePoseVelocity, error) { Vector actual, expect; Matrix H_pose_exp, H_pose_act, H_vel_exp, H_vel_act; noiseModel::Gaussian::shared_ptr cost_model = - noiseModel::Isotropic::Sigma(6, 1.0); + noiseModel::Isotropic::Sigma(12, 1.0); q = Vector2(M_PI / 4.0, -M_PI / 2); qdot = (Vector2() << 0.1, 0.1).finished(); @@ -39,11 +39,10 @@ TEST(PriorWorkspacePoseVelocity, error) { Vector6 des_vel = (Vector6() << 0, 0, 0, 0, 0, 0).finished(); PriorWorkspacePoseVelocity factor(0, 0, cost_model, arm, des_pose, des_vel); actual = factor.evaluateError(q, qdot, &H_pose_act, &H_vel_act); - expect = (Vector(6) << 0.613943126, 1.340768463762691, -0.613943126, - 1.090272121881345, 0.777438163118655, -0.647039678) + expect = (Vector(12) << 00.613943126, 1.48218982, -0.613943126, 1.1609828, + 0.706727485, -0.547039678, 0, -0.141421356237309, 0, + -0.070710678118655, 0.070710678118655, -0.1000) .finished(); - // (original solution wo velocity): 0.613943126, 1.48218982, - // -0.613943126, 1.1609828, 0.706727485, -0.547039678 H_pose_exp = numericalDerivative11( std::function( std::bind(&PriorWorkspacePoseVelocity::evaluateError, factor, @@ -63,7 +62,7 @@ TEST(PriorWorkspacePoseVelocity, error) { /* ************************************************************************** */ TEST(PriorWorkspacePoseVelocity, optimization) { noiseModel::Gaussian::shared_ptr cost_model = - noiseModel::Isotropic::Sigma(6, 0.1); + noiseModel::Isotropic::Sigma(12, 0.1); Vector a = (Vector(2) << 1, 1).finished(); Vector alpha = (Vector(2) << 0, 0).finished(); diff --git a/matlab/gpmp2_examples/KinovaGen3PoseVelocityConstraintExample.m b/matlab/gpmp2_examples/KinovaGen3PoseVelocityConstraintExample.m index 65e6cfb..f3196d0 100644 --- a/matlab/gpmp2_examples/KinovaGen3PoseVelocityConstraintExample.m +++ b/matlab/gpmp2_examples/KinovaGen3PoseVelocityConstraintExample.m @@ -71,12 +71,12 @@ epsilon_dist = 0.1; % noise model -fix_sigma = 0.001; +fix_sigma = 0.0001; pose_fix_model = noiseModel.Isotropic.Sigma(7, fix_sigma); vel_fix_model = noiseModel.Isotropic.Sigma(7, fix_sigma); % noise model for end-effector pose and rates -ee_fix_model = noiseModel.Isotropic.Sigma(6, 5e-3); +ee_fix_model = noiseModel.Isotropic.Sigma(12, 1e-5); % init sdf sdf = SignedDistanceField(origin_point3, cell_size, size(field, 1), ... From 77461c2fb93a88a24c9b683f169575c9ec162b5f Mon Sep 17 00:00:00 2001 From: Matt King-Smith Date: Thu, 18 May 2023 11:02:23 -0400 Subject: [PATCH 09/11] Improved plotting and comment fixes --- gpmp2/kinematics/ForwardKinematics-inl.h | 2 +- gpmp2/kinematics/ForwardKinematics.h | 4 +- .../KinovaGen3PoseVelocityConstraintExample.m | 201 ++++++++++++++---- 3 files changed, 165 insertions(+), 42 deletions(-) diff --git a/gpmp2/kinematics/ForwardKinematics-inl.h b/gpmp2/kinematics/ForwardKinematics-inl.h index c011b1c..53c827c 100644 --- a/gpmp2/kinematics/ForwardKinematics-inl.h +++ b/gpmp2/kinematics/ForwardKinematics-inl.h @@ -35,7 +35,7 @@ gtsam::Matrix ForwardKinematics::forwardKinematicsPosition( forwardKinematics(jp, {}, jpx, nullptr); // convert vector in matrix - gtsam::Matrix jpx_mat(6, nr_links_); + gtsam::Matrix jpx_mat(3, nr_links_); for (size_t i = 0; i < nr_links_; i++) jpx_mat.col(i) = jpx[i].translation(); return jpx_mat; } diff --git a/gpmp2/kinematics/ForwardKinematics.h b/gpmp2/kinematics/ForwardKinematics.h index 1a2fa70..4289477 100644 --- a/gpmp2/kinematics/ForwardKinematics.h +++ b/gpmp2/kinematics/ForwardKinematics.h @@ -49,7 +49,7 @@ class ForwardKinematics { * @param jp robot pose in config space * @param jv robot velocity in config space * @param jpx link poses in 3D work space - * @param jvx link velocities in 3D work space, no angular rate + * @param jvx link velocities in 3D work space * @param J_jpx_jp et al. optional Jacobians **/ virtual void forwardKinematics( @@ -62,7 +62,7 @@ class ForwardKinematics { /** * Matrix wrapper for forwardKinematics, mainly used by matlab - * each column is a single point / velocity of the joint, size 6xN, 3xN, 3xN + * each column is a single point / velocity of the joint, size 6xN, 3xN, 6xN * No Jacobians provided by this version */ gtsam::Matrix forwardKinematicsPose(const Pose& jp) const; diff --git a/matlab/gpmp2_examples/KinovaGen3PoseVelocityConstraintExample.m b/matlab/gpmp2_examples/KinovaGen3PoseVelocityConstraintExample.m index f3196d0..ea39d7e 100644 --- a/matlab/gpmp2_examples/KinovaGen3PoseVelocityConstraintExample.m +++ b/matlab/gpmp2_examples/KinovaGen3PoseVelocityConstraintExample.m @@ -41,19 +41,19 @@ cyan = [0, 1, 1]; % plot problem setting -fig1 = figure(1); -clf; -hold on; -set(fig1,'Name', 'Problem Settings'); -title('Problem Setup') -plotRobotModel(arm, start_conf); -plotRobotModel(arm, end_conf, green); -plotMap3D(dataset.corner_idx, origin, cell_size); -xlabel('x'); -ylabel('y'); -zlabel('z'); -grid on, view(3) -hold off; +% fig1 = figure(1); +% clf; +% hold on; +% set(fig1,'Name', 'Problem Settings'); +% title('Problem Setup') +% plotRobotModel(arm, start_conf); +% plotRobotModel(arm, end_conf, green); +% plotMap3D(dataset.corner_idx, origin, cell_size); +% xlabel('x'); +% ylabel('y'); +% zlabel('z'); +% grid on, view(3) +% hold off; %% settings total_time_sec = 10; @@ -89,14 +89,14 @@ init_values = initArmTrajStraightLine(start_conf, end_conf, total_time_step); % plot initial trajectory -for i=0:total_time_step - hold on; - % plot arm - conf = init_values.atVector(symbol('x', i)); - plotRobotModel(arm, conf, cyan); - pause(0.1) -end -hold off; +% for i=0:total_time_step +% hold on; +% % plot arm +% conf = init_values.atVector(symbol('x', i)); +% plotRobotModel(arm, conf, cyan); +% pause(0.1) +% end +% hold off; %% init optimization graph = NonlinearFactorGraph; @@ -170,37 +170,160 @@ optimizer.optimize(); result = optimizer.values(); + fprintf(' Final Error = %d\n', graph.error(result)); +%% Save data + +% joint-space positions and rates +init_joint_pos = utilities.extractVectors(init_values,'x')'; +result_joint_pos = utilities.extractVectors(result,'x')'; +result_joint_rates = utilities.extractVectors(result,'v')'; + +numSteps = total_time_step + 1; +% workspace-space joint poses and velocities +joints_poses = nan(6,7,numSteps); +joints_vels = nan(6,7,numSteps); +for i=1:total_time_step+1 + joints_pose(:,:,i) = arm.fk_model().forwardKinematicsPose(result_joint_pos(:,i)); + joints_vel(:,:,i) = arm.fk_model().forwardKinematicsVel(result_joint_pos(:,i),result_joint_rates(:,i)); +end + %% Plot results +import gtsam.* +import gpmp2.* + +% Plotting sizes and settings +if ~strcmp(get(0,'defaultTextInterpreter'),'latex') + set(0,'defaulttextinterpreter','latex'); + set(0,'DefaultLegendInterpreter','latex'); + set(0, 'defaultAxesTickLabelInterpreter','latex'); + set(0,'defaultfigurecolor',[1 1 1]); + set(0,'defaultAxesFontName','Times New Roman'); + set(0,'DefaultAxesXGrid','on'); + set(0,'DefaultAxesYGrid','on'); + set(0,'DefaultAxesZGrid','on'); + set(0,'DefaultAxesBox','on'); + set(0,'defaultAxesFontSize',16); + set(0,'DefaultLegendFontSize', 16); +end % plot final values -fig2 = figure(2); -set(fig2, 'Name', 'Optimization Results'); +fig1 = figure(1); +set(fig1, 'Name', 'Factore Graph Initialization and Optimization Results'); clf; -title('Result Values'); +for kk = 1:1:6 + ax(kk) = subplot(1,6,kk); +end + +subplot(ax(1)); +title('\textbf{Initial Guess Trajectory}'); + % plot world, initial and terminal configurations hold on; plotMap3D(dataset.corner_idx, origin, cell_size); plotRobotModel(arm, start_conf); plotRobotModel(arm, end_conf, green); -xlabel('x'); -ylabel('y'); -zlabel('z'); -grid on, view(3); -hold off; -for i=0:total_time_step +xlabel('x [m]'); +ylabel('y [m]'); +zlabel('z [m]'); +axis([-0.2 0.6 -0.3, 0.5, 0 1.4]) +set(ax(1),'Position',[0.0422 -0.8174 0.2912 2.6390]); +view(140,30); + +subplot(ax(2)); +title('\textbf{Factor Graph Optimization Result}'); +hold on; +plotMap3D(dataset.corner_idx, origin, cell_size); +plotRobotModel(arm, start_conf); +plotRobotModel(arm, end_conf, green); +xlabel('x [m]'); +ylabel('y [m]'); +zlabel('z [m]'); +axis([-0.2 0.6 -0.3, 0.5, 0 1.4]) +set(ax(2),'Position',[0.3866 -0.8236 0.2897 2.6390]); +view(140,30); + +title(ax(3),'\textbf{End-Effector Plots}'); +xlabel(ax(3),'Time [s]'); +ylabel(ax(3),'Euler Angles [deg]') +set(ax(3),'Position',[0.7443 0.79 0.2505 0.18]); + +xlabel(ax(4),'Time [s]'); +ylabel(ax(4),'Postion [m]') +set(ax(4),'Position',[0.7443 0.5427 0.2505 0.18]); + +xlabel(ax(5),'Time [s]'); +ylabel(ax(5),'Angular Velocity [deg/s]') +set(ax(5),'Position',[0.7443 0.3 0.2505 0.18]); + +xlabel(ax(6),'Time [s]'); +ylabel(ax(6),'Linear Velocity [m/s]') +set(ax(6),'Position',[0.7443 0.0562 0.2505 0.18]); + +axis(ax(3), [0, total_time_sec, -10, 185]); +axis(ax(4), [0, total_time_sec, min(min(joints_pose(4:6,end,:))), max(max(joints_pose(4:6,end,:)))]); +axis(ax(5), [0, total_time_sec, min(min(rad2deg(joints_vel(4:6,end,:)))), max(max(rad2deg(joints_vel(4:6,end,:))))]); +axis(ax(6), [0, total_time_sec, min(min(joints_vel(1:3,end,:))), max(max(joints_vel(1:3,end,:)))]); + +ee_ypr_deg = rad2deg(ee_des_pose.rotation.ypr); +ee_pos_m = ee_des_pose.translation; + +clrs = {'r',green,'b'}; +for kk = 1:3 + % terminal waypoint + line(ax(3),total_time_sec,ee_ypr_deg(kk),'marker','o','markerfacecolor',clrs{kk},'markeredgecolor',clrs{kk},'linestyle','none'); + line(ax(4),total_time_sec,ee_pos_m(kk),'marker','o','markerfacecolor',clrs{kk},'markeredgecolor',clrs{kk},'linestyle','none'); + line(ax(5),total_time_sec,ee_des_vel(kk+3),'marker','o','markerfacecolor',clrs{kk},'markeredgecolor',clrs{kk},'linestyle','none'); + line(ax(6),total_time_sec,ee_des_vel(kk),'marker','o','markerfacecolor',clrs{kk},'markeredgecolor',clrs{kk},'linestyle','none'); + + % actual poses and velocites + ee_ypr_h(kk) = line(ax(3),nan(1,1),nan(1,1),'color',clrs{kk}); + ee_pos_h(kk) = line(ax(4),nan(1,1),nan(1,1),'color',clrs{kk}); + ee_angVel_h(kk) = line(ax(5),nan(1,1),nan(1,1),'color',clrs{kk}); + ee_linVel_h(kk) = line(ax(6),nan(1,1),nan(1,1),'color',clrs{kk}); +end + +legend(ax(3),ee_ypr_h, {'$\psi$','$\theta$','$\phi$'}, 'Position', [0.7445,0.875,0.0328,0.0704]); +legend(ax(4),ee_pos_h, {'$x$','$y$','$z$'}, 'Position', [0.7445,0.631,0.0317,0.0704]); +legend(ax(5),ee_angVel_h, {'$\omega_x$','$\omega_y$','$\omega_z$'}, 'Position', [0.7445,0.3,0.0363,0.0704]); +legend(ax(6),ee_linVel_h, {'$\dot{x}$','$\dot{y}$','$\dot{z}$'}, 'Position', [0.7445,0.0562,0.0317,0.0704]); + +t = 0:delta_t:total_time_sec; +for i=1:total_time_step+1 hold on; - % plot arm - conf = result.atVector(symbol('x', i)); - plotRobotModel(arm, conf, cyan); +% joints_pose(:,:,i) = arm.fk_model().forwardKinematicsPose(result_joint_pos(:,i)); +% joints_vel(:,:,i) = arm.fk_model().forwardKinematicsVel(result_joint_pos(:,i),result_joint_rates(:,i)); + subplot(ax(1)); + plotRobotModel(arm, init_joint_pos(:,i), cyan); + subplot(ax(2)); + plotRobotModel(arm, result_joint_pos(:,i), cyan); + for kk = 1:3 + if kk ~= 3 + set(ee_ypr_h(kk),'Xdata',t(1:i),'Ydata',rad2deg(joints_pose(kk,end,1:i))); + else + set(ee_ypr_h(kk),'Xdata',t(1:i),'Ydata',unwrap(rad2deg(joints_pose(kk,end,1:i)))); + end + set(ee_pos_h(kk),'Xdata',t(1:i),'Ydata',joints_pose(kk+3,end,1:i)); + set(ee_angVel_h(kk),'Xdata',t(1:i),'Ydata',rad2deg(joints_vel(kk+3,end,1:i))); + set(ee_linVel_h(kk),'Xdata',t(1:i),'Ydata',joints_vel(kk,end,1:i)); + end pause(0.1); end -hold off -joints_pose = arm.fk_model().forwardKinematicsPose(result.atVector(symbol('x', i))); -joints_vel = arm.fk_model().forwardKinematicsVel(result.atVector(symbol('x', i)),result.atVector(symbol('v', i))); -% Errors -ee_pose_err = [ee_des_pose.rotation.ypr; ee_des_pose.translation] - joints_pose(:,end); -ee_vel_err = ee_des_vel- joints_vel(:,end); + +% for i=1:total_time_step +% hold on; +% % plot arm +% conf = result.atVector(symbol('x', i)); +% plotRobotModel(arm, conf, cyan); +% pause(0.1); +% end +% hold off +% joints_pose = arm.fk_model().forwardKinematicsPose(result.atVector(symbol('x', i))); +% joints_vel = arm.fk_model().forwardKinematicsVel(result.atVector(symbol('x', i)),result.atVector(symbol('v', i))); +% +% % Errors +% ee_pose_err = [ee_des_pose.rotation.ypr; ee_des_pose.translation] - joints_pose(:,end); +% ee_vel_err = ee_des_vel- joints_vel(:,end); From ed1ce4037814b914f1ff14366313d8b5c26b3c7d Mon Sep 17 00:00:00 2001 From: Matt King-Smith Date: Thu, 18 May 2023 12:20:36 -0400 Subject: [PATCH 10/11] code to save videos --- .../KinovaGen3PoseVelocityConstraintExample.m | 93 +++++++------------ 1 file changed, 32 insertions(+), 61 deletions(-) diff --git a/matlab/gpmp2_examples/KinovaGen3PoseVelocityConstraintExample.m b/matlab/gpmp2_examples/KinovaGen3PoseVelocityConstraintExample.m index ea39d7e..5cd9977 100644 --- a/matlab/gpmp2_examples/KinovaGen3PoseVelocityConstraintExample.m +++ b/matlab/gpmp2_examples/KinovaGen3PoseVelocityConstraintExample.m @@ -9,6 +9,9 @@ import gtsam.* import gpmp2.* +% flag for recording a video of animation +rcdVid = false; + %% Generate dataset dataset = generate3Ddataset('KinovaBoxDataset'); origin = [dataset.origin_x, dataset.origin_y, dataset.origin_z]; @@ -36,25 +39,6 @@ % ee_des_vel = [linear_rate; angular_rate]^T ee_des_vel = zeros(6,1); -% plotting colors -green = [0.4667 0.6745 0.1882]; -cyan = [0, 1, 1]; - -% plot problem setting -% fig1 = figure(1); -% clf; -% hold on; -% set(fig1,'Name', 'Problem Settings'); -% title('Problem Setup') -% plotRobotModel(arm, start_conf); -% plotRobotModel(arm, end_conf, green); -% plotMap3D(dataset.corner_idx, origin, cell_size); -% xlabel('x'); -% ylabel('y'); -% zlabel('z'); -% grid on, view(3) -% hold off; - %% settings total_time_sec = 10; total_time_step = 50; @@ -85,20 +69,9 @@ sdf.initFieldData(z-1, field(:,:,z)'); end -%% initial traj +%% initial traj and optimization init_values = initArmTrajStraightLine(start_conf, end_conf, total_time_step); -% plot initial trajectory -% for i=0:total_time_step -% hold on; -% % plot arm -% conf = init_values.atVector(symbol('x', i)); -% plotRobotModel(arm, conf, cyan); -% pause(0.1) -% end -% hold off; - -%% init optimization graph = NonlinearFactorGraph; for i = 0 : total_time_step @@ -189,9 +162,18 @@ joints_vel(:,:,i) = arm.fk_model().forwardKinematicsVel(result_joint_pos(:,i),result_joint_rates(:,i)); end +% correct for discontinuities when using euler angle representation +joints_pose(1:3,end,:) = unwrap(joints_pose(1:3,end,:)); + +% separate out translation and rotation +ee_ypr_deg = rad2deg(ee_des_pose.rotation.ypr); +ee_pos_m = ee_des_pose.translation; + %% Plot results -import gtsam.* -import gpmp2.* + +% plotting colors +green = [0.4667 0.6745 0.1882]; +cyan = [0, 1, 1]; % Plotting sizes and settings if ~strcmp(get(0,'defaultTextInterpreter'),'latex') @@ -266,9 +248,6 @@ axis(ax(5), [0, total_time_sec, min(min(rad2deg(joints_vel(4:6,end,:)))), max(max(rad2deg(joints_vel(4:6,end,:))))]); axis(ax(6), [0, total_time_sec, min(min(joints_vel(1:3,end,:))), max(max(joints_vel(1:3,end,:)))]); -ee_ypr_deg = rad2deg(ee_des_pose.rotation.ypr); -ee_pos_m = ee_des_pose.translation; - clrs = {'r',green,'b'}; for kk = 1:3 % terminal waypoint @@ -290,40 +269,32 @@ legend(ax(6),ee_linVel_h, {'$\dot{x}$','$\dot{y}$','$\dot{z}$'}, 'Position', [0.7445,0.0562,0.0317,0.0704]); t = 0:delta_t:total_time_sec; + +if rcdVid + v = VideoWriter('KinovaGen3_PoseStabilization','Uncompressed AVI'); + open(v); +end + for i=1:total_time_step+1 hold on; -% joints_pose(:,:,i) = arm.fk_model().forwardKinematicsPose(result_joint_pos(:,i)); -% joints_vel(:,:,i) = arm.fk_model().forwardKinematicsVel(result_joint_pos(:,i),result_joint_rates(:,i)); subplot(ax(1)); plotRobotModel(arm, init_joint_pos(:,i), cyan); subplot(ax(2)); plotRobotModel(arm, result_joint_pos(:,i), cyan); for kk = 1:3 - if kk ~= 3 - set(ee_ypr_h(kk),'Xdata',t(1:i),'Ydata',rad2deg(joints_pose(kk,end,1:i))); - else - set(ee_ypr_h(kk),'Xdata',t(1:i),'Ydata',unwrap(rad2deg(joints_pose(kk,end,1:i)))); - end + set(ee_ypr_h(kk),'Xdata',t(1:i),'Ydata',rad2deg(joints_pose(kk,end,1:i))); set(ee_pos_h(kk),'Xdata',t(1:i),'Ydata',joints_pose(kk+3,end,1:i)); set(ee_angVel_h(kk),'Xdata',t(1:i),'Ydata',rad2deg(joints_vel(kk+3,end,1:i))); set(ee_linVel_h(kk),'Xdata',t(1:i),'Ydata',joints_vel(kk,end,1:i)); end - pause(0.1); + if rcdVid + frame = getframe(gcf); + writeVideo(v,frame); + else + pause(0.1); + end +end +hold off; +if rcdVid + close(v); end - - -% for i=1:total_time_step -% hold on; -% % plot arm -% conf = result.atVector(symbol('x', i)); -% plotRobotModel(arm, conf, cyan); -% pause(0.1); -% end -% hold off -% joints_pose = arm.fk_model().forwardKinematicsPose(result.atVector(symbol('x', i))); -% joints_vel = arm.fk_model().forwardKinematicsVel(result.atVector(symbol('x', i)),result.atVector(symbol('v', i))); -% -% % Errors -% ee_pose_err = [ee_des_pose.rotation.ypr; ee_des_pose.translation] - joints_pose(:,end); -% ee_vel_err = ee_des_vel- joints_vel(:,end); - From 33482c58e848c59cf2a708aa03e8b67b2468e2da Mon Sep 17 00:00:00 2001 From: Matt King-Smith Date: Wed, 24 May 2023 10:58:34 -0400 Subject: [PATCH 11/11] Updated name of factor --- gpmp2.i | 6 +++--- ...Velocity.h => WorkspacePoseVelocityPrior.h} | 16 ++++++++-------- ....cpp => testWorkspacePoseVelocityPrior.cpp} | 18 +++++++++--------- .../KinovaGen3PoseVelocityConstraintExample.m | 2 +- 4 files changed, 21 insertions(+), 21 deletions(-) rename gpmp2/kinematics/{PriorWorkspacePoseVelocity.h => WorkspacePoseVelocityPrior.h} (91%) rename gpmp2/kinematics/tests/{testPriorWorkspacePoseVelocity.cpp => testWorkspacePoseVelocityPrior.cpp} (87%) diff --git a/gpmp2.i b/gpmp2.i index 882f612..b54f999 100644 --- a/gpmp2.i +++ b/gpmp2.i @@ -364,10 +364,10 @@ class PointRobotModel { }; // desired workspace pose and velocity for last link factor -#include +#include -virtual class PriorWorkspacePoseVelocity : gtsam::NoiseModelFactor { - PriorWorkspacePoseVelocity(size_t poseKey, size_t velKey, const gtsam::noiseModel::Base* cost_model, +virtual class WorkspacePoseVelocityPrior : gtsam::NoiseModelFactor { + WorkspacePoseVelocityPrior(size_t poseKey, size_t velKey, const gtsam::noiseModel::Base* cost_model, const gpmp2::Arm& arm, const gtsam::Pose3& des_pose, const gtsam::Vector& des_vel); }; diff --git a/gpmp2/kinematics/PriorWorkspacePoseVelocity.h b/gpmp2/kinematics/WorkspacePoseVelocityPrior.h similarity index 91% rename from gpmp2/kinematics/PriorWorkspacePoseVelocity.h rename to gpmp2/kinematics/WorkspacePoseVelocityPrior.h index 174c103..6d8006c 100644 --- a/gpmp2/kinematics/PriorWorkspacePoseVelocity.h +++ b/gpmp2/kinematics/WorkspacePoseVelocityPrior.h @@ -1,5 +1,5 @@ /** - * @file PriorWorkspacePoseVelocity.h + * @file WorkspacePoseVelocityPrior.h * @brief Prior defined on the workspace pose and velocity of final joint *(end-effector) of a robot given its state in configuration space * @author Matthew King-Smith @@ -22,11 +22,11 @@ namespace gpmp2 { /** * binary factor Gaussian prior defined on the workspace pose and velocities */ -class PriorWorkspacePoseVelocity +class WorkspacePoseVelocityPrior : public gtsam::NoiseModelFactor2 { private: // typedefs - typedef PriorWorkspacePoseVelocity This; + typedef WorkspacePoseVelocityPrior This; typedef gtsam::NoiseModelFactor2 Base; // arm @@ -43,14 +43,14 @@ class PriorWorkspacePoseVelocity typedef std::shared_ptr shared_ptr; /// Default constructor - PriorWorkspacePoseVelocity() {} + WorkspacePoseVelocityPrior() {} /** * Constructor * @param cost_model cost function covariance * @param des_vel desired velocity (linear and angular) */ - PriorWorkspacePoseVelocity(gtsam::Key poseKey, gtsam::Key velKey, + WorkspacePoseVelocityPrior(gtsam::Key poseKey, gtsam::Key velKey, const gtsam::SharedNoiseModel& cost_model, const Arm& arm, const gtsam::Pose3& des_pose, const gtsam::Vector6& des_vel) @@ -59,7 +59,7 @@ class PriorWorkspacePoseVelocity des_pose_(des_pose), des_vel_(des_vel) {} - ~PriorWorkspacePoseVelocity() {} + ~WorkspacePoseVelocityPrior() {} /// error function gtsam::Vector evaluateError( @@ -103,7 +103,7 @@ class PriorWorkspacePoseVelocity void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { - std::cout << s << "PriorWorkspacePoseVelocity :" << std::endl; + std::cout << s << "WorkspacePoseVelocityPrior :" << std::endl; Base::print("", keyFormatter); std::cout << "desired end-effector pose : "; des_pose_.print(); // << std::endl; @@ -122,4 +122,4 @@ class PriorWorkspacePoseVelocity #endif }; -} // namespace gpmp2 \ No newline at end of file +} // namespace gpmp2 diff --git a/gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp b/gpmp2/kinematics/tests/testWorkspacePoseVelocityPrior.cpp similarity index 87% rename from gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp rename to gpmp2/kinematics/tests/testWorkspacePoseVelocityPrior.cpp index ba1c4d5..c35123e 100644 --- a/gpmp2/kinematics/tests/testPriorWorkspacePoseVelocity.cpp +++ b/gpmp2/kinematics/tests/testWorkspacePoseVelocityPrior.cpp @@ -1,10 +1,10 @@ /** - * @file testPriorWorkspacePoseVelocity.cpp + * @file testWorkspacePoseVelocityPrior.cpp * @author Matthew King-Smith **/ #include -#include +#include #include #include #include @@ -20,14 +20,14 @@ using namespace gtsam; using namespace gpmp2; /* ************************************************************************** */ -TEST(PriorWorkspacePoseVelocity, error) { +TEST(WorkspacePoseVelocityPrior, error) { Vector2 a(1, 1), alpha(M_PI / 2, 0), d(0, 0); Arm arm = Arm(2, a, alpha, d); Vector2 q; Vector2 qdot; Pose3 des_pose; // Vector6 des_vel; - // PriorWorkspacePoseVelocity factor; + // WorkspacePoseVelocityPrior factor; Vector actual, expect; Matrix H_pose_exp, H_pose_act, H_vel_exp, H_vel_act; noiseModel::Gaussian::shared_ptr cost_model = @@ -37,7 +37,7 @@ TEST(PriorWorkspacePoseVelocity, error) { qdot = (Vector2() << 0.1, 0.1).finished(); des_pose = Pose3(); Vector6 des_vel = (Vector6() << 0, 0, 0, 0, 0, 0).finished(); - PriorWorkspacePoseVelocity factor(0, 0, cost_model, arm, des_pose, des_vel); + WorkspacePoseVelocityPrior factor(0, 0, cost_model, arm, des_pose, des_vel); actual = factor.evaluateError(q, qdot, &H_pose_act, &H_vel_act); expect = (Vector(12) << 00.613943126, 1.48218982, -0.613943126, 1.1609828, 0.706727485, -0.547039678, 0, -0.141421356237309, 0, @@ -45,13 +45,13 @@ TEST(PriorWorkspacePoseVelocity, error) { .finished(); H_pose_exp = numericalDerivative11( std::function( - std::bind(&PriorWorkspacePoseVelocity::evaluateError, factor, + std::bind(&WorkspacePoseVelocityPrior::evaluateError, factor, std::placeholders::_1, qdot, nullptr, nullptr)), q, 1e-6); H_vel_exp = numericalDerivative11( std::function( - std::bind(&PriorWorkspacePoseVelocity::evaluateError, factor, q, + std::bind(&WorkspacePoseVelocityPrior::evaluateError, factor, q, std::placeholders::_1, nullptr, nullptr)), qdot, 1e-6); EXPECT(assert_equal(expect, actual, 1e-6)); @@ -60,7 +60,7 @@ TEST(PriorWorkspacePoseVelocity, error) { } /* ************************************************************************** */ -TEST(PriorWorkspacePoseVelocity, optimization) { +TEST(WorkspacePoseVelocityPrior, optimization) { noiseModel::Gaussian::shared_ptr cost_model = noiseModel::Isotropic::Sigma(12, 0.1); @@ -78,7 +78,7 @@ TEST(PriorWorkspacePoseVelocity, optimization) { Vector qdotinit = (Vector(2) << 0.1, 0.1).finished(); NonlinearFactorGraph graph; - graph.add(PriorWorkspacePoseVelocity(qkey, qdotkey, cost_model, arm, des_pose, + graph.add(WorkspacePoseVelocityPrior(qkey, qdotkey, cost_model, arm, des_pose, des_vel)); Values init_values; init_values.insert(qkey, qinit); diff --git a/matlab/gpmp2_examples/KinovaGen3PoseVelocityConstraintExample.m b/matlab/gpmp2_examples/KinovaGen3PoseVelocityConstraintExample.m index 5cd9977..b364606 100644 --- a/matlab/gpmp2_examples/KinovaGen3PoseVelocityConstraintExample.m +++ b/matlab/gpmp2_examples/KinovaGen3PoseVelocityConstraintExample.m @@ -83,7 +83,7 @@ graph.add(PriorFactorVector(key_pos, start_conf, pose_fix_model)); graph.add(PriorFactorVector(key_vel, start_vel, vel_fix_model)); elseif i==total_time_step - graph.add(PriorWorkspacePoseVelocity(key_pos, key_vel,... + graph.add(WorkspacePoseVelocityPrior(key_pos, key_vel,... ee_fix_model, arm.fk_model(),... ee_des_pose, ee_des_vel)); end