diff --git a/gtdynamics/kinematics/Kinematics.h b/gtdynamics/kinematics/Kinematics.h index 4d8d681f..0197a341 100644 --- a/gtdynamics/kinematics/Kinematics.h +++ b/gtdynamics/kinematics/Kinematics.h @@ -65,6 +65,53 @@ 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 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& 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 CoM pose needed to achieve goal pose. + const gtsam::Pose3 wTcom() const { + return wTgoal.compose(comTgoal.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; @@ -73,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. @@ -131,29 +178,61 @@ class Kinematics : public Optimizer { EqualityConstraints pointGoalConstraints( const CONTEXT& context, const ContactGoals& contact_goals) const; + /** + * @fn Create a pose prior for a given link for each given pose. + * @param context Slice or Interval instance. + * @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 + gtsam::NonlinearFactorGraph poseGoalObjectives( + 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. The + * default is an empty Values, meaning that the means will default to 0. * @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 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. * - * 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(), + bool use_fk = false) const; /** * @fn Inverse kinematics given a set of contact goals. @@ -169,6 +248,21 @@ 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 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 PoseGoals& pose_goals, + 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..e31c440a 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 PoseGoals& pose_goals) const { + NonlinearFactorGraph graph; + for (size_t k = interval.k_start; k <= interval.k_end; k++) { + graph.add(poseGoalObjectives(Slice(k), pose_goals)); + } + return graph; +} + template <> NonlinearFactorGraph Kinematics::pointGoalObjectives( const Interval& interval, const ContactGoals& contact_goals) const { @@ -66,21 +77,31 @@ EqualityConstraints Kinematics::pointGoalConstraints( template <> NonlinearFactorGraph Kinematics::jointAngleObjectives( + 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, mean)); + } + return graph; +} + +template <> +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(jointAngleObjectives(Slice(k), robot)); + graph.add(jointAngleLimits(Slice(k), robot)); } return graph; } template <> -Values Kinematics::initialValues(const Interval& interval, - const Robot& robot, - double gaussian_noise) const { +Values Kinematics::initialValues( + const Interval& interval, const Robot& robot, double gaussian_noise, + 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)); + 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 852ba885..9a01cb11 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -11,6 +11,7 @@ * @author: Frank Dellaert */ +#include #include #include #include @@ -18,6 +19,7 @@ #include #include #include +#include namespace gtdynamics { @@ -34,6 +36,22 @@ 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.comTgoal << "], [" << pg.wTgoal + << "]}"; + 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), wTcom(), + tol); +} + template <> NonlinearFactorGraph Kinematics::graph(const Slice& slice, const Robot& robot) const { @@ -63,6 +81,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)); + + // 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 constraint + constraints.emplace_shared>(constraint_expr, + tolerance); + } + } + return constraints; } @@ -98,23 +136,58 @@ EqualityConstraints Kinematics::pointGoalConstraints( return constraints; } +template <> +NonlinearFactorGraph Kinematics::poseGoalObjectives( + const Slice& slice, const PoseGoals& pose_goals) const { + gtsam::NonlinearFactorGraph graph; + + 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); + } + + 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); + graph.addPrior(key, (mean.exists(key) ? mean.at(key) : 0.0), + p_.prior_q_cost_model); } 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), gtsam::noiseModel::Isotropic::Sigma(1, 0.001), + joint->parameters().scalar_limits.value_lower_limit, + joint->parameters().scalar_limits.value_upper_limit, + 0.04));//joint->parameters().scalar_limits.value_limit_threshold)); + } + return graph; +} + template <> Values Kinematics::initialValues(const Slice& slice, const Robot& robot, - double gaussian_noise) const { + double gaussian_noise, + const gtsam::Values& initial_joints, + bool use_fk) const { Values values; auto sampler_noise_model = @@ -123,13 +196,27 @@ Values Kinematics::initialValues(const Slice& slice, const Robot& robot, // Initialize all joint angles. for (auto&& joint : robot.joints()) { - InsertJointAngle(&values, joint->id(), slice.k, sampler.sample()[0]); + auto key = JointAngleKey(joint->id(), slice.k); + double angle = initial_joints.exists(key) ? initial_joints.at(key) + : sampler.sample()[0]; + InsertJointAngle(&values, joint->id(), slice.k, angle); } - // 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 (use_fk) { + // 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; @@ -150,7 +237,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. @@ -161,4 +248,28 @@ Values Kinematics::inverse(const Slice& slice, const Robot& robot, return optimize(graph, constraints, initial_values); } + +template <> +gtsam::Values Kinematics::inverse( + const Slice& slice, const Robot& robot, const PoseGoals& pose_goals, + const gtsam::Values& joint_priors) const { + auto graph = this->graph(slice, robot); + + // 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 + graph.add(this->poseGoalObjectives(slice, pose_goals)); + + // Add joint angle limits factors + graph.add(this->jointAngleLimits(slice, robot)); + + // Robot kinematics constraints + auto constraints = this->constraints(slice, robot); + + auto initial_values = + this->initialValues(slice, robot, 0.1, joint_priors, true); + + return this->optimize(graph, constraints, initial_values); +} } // namespace gtdynamics diff --git a/gtdynamics/kinematics/KinematicsTrajectory.cpp b/gtdynamics/kinematics/KinematicsTrajectory.cpp index 83a9ae37..a35ee108 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, bool use_fk) const { Values values; for (auto&& phase : trajectory.phases()) { - values.insert(initialValues(phase, robot, gaussian_noise)); + values.insert( + initialValues(phase, robot, gaussian_noise, initial_joints, use_fk)); } return values; } diff --git a/gtdynamics/pandarobot/CMakeLists.txt b/gtdynamics/pandarobot/CMakeLists.txt index 565781db..a8d48936 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) +# add pandarobot subfolders to gtdynamics' SOURCE_SUBDIRS list +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/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 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); } /* ************************************************************************* */ 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; 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 @@ - + diff --git a/tests/testKinematicsSlice.cpp b/tests/testKinematicsSlice.cpp index 4b329809..9c9a4cfc 100644 --- a/tests/testKinematicsSlice.cpp +++ b/tests/testKinematicsSlice.cpp @@ -14,6 +14,8 @@ #include #include #include +#include +#include #include "contactGoalsExample.h" @@ -23,21 +25,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? @@ -56,16 +58,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); @@ -81,6 +83,240 @@ TEST(Slice, InverseKinematics) { } } +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)); + } + + return joint_values; +} + +TEST(Slice, initialValues) { + // Load robot from urdf file + const Robot panda = + 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 = + 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()) + + // 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, 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 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); + gtsam::Values initial = + kinematics.initialValues(kSlice, robot, 0.0, expected_means, true); + double tol = 1e-5; + EXPECT_DOUBLES_EQUAL(0.0, joint_priors.error(initial), tol) + + // Define some prior 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 that error at 0 is now not 0 + 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, true); + 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 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, true); + 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")); + 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)); + 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; + gtsam::Values initial_joints = jointVectorToValues(robot, initial); + auto initial_values = + 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)) + EXPECT_DOUBLES_EQUAL(0, pose_priors.error(initial_values), tol) +} + +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) { + 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"); + + // 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()); + + // 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} + 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)); + 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; + noise << 0.04, -0.1, 0.07, 0.14, -0.05, 0.02, 0.1; + gtsam::Values initial_joints = jointVectorToValues(robot, noise + initial); + + // Call IK solver + auto values = kinematics.inverse(kSlice, robot, pose_goals, initial_joints); + + // Check that base link did not budge (much) + auto base_link = robot.link("link0"); + const Pose3 sM0 = base_link->bMcom(); + double tol = 1e-5; + EXPECT(assert_equal(sM0, values.at(PoseKey(0, k)), tol)); + + // Check that desired pose was achieved + EXPECT(assert_equal(sT7, values.at(PoseKey(7, k)), tol)); + + // 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)); + tol = 0.1; + EXPECT(assert_equal(initial, optimal_q, tol)); +} + int main() { TestResult tr; return TestRegistry::runAllTests(tr);