From bceec882374039a0be1129458c1dbd4cdad34e82 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 15 Oct 2021 21:29:43 -0400 Subject: [PATCH 01/14] c++ compiles (but not correct due to commenting-out) for remove JointTyped --- gtdynamics/factors/JointMeasurementFactor.h | 36 +- gtdynamics/factors/PoseFactor.h | 3 +- gtdynamics/factors/TorqueFactor.h | 11 +- gtdynamics/factors/TwistAccelFactor.h | 17 +- gtdynamics/factors/TwistFactor.h | 13 +- gtdynamics/factors/WrenchEquivalenceFactor.h | 15 +- gtdynamics/universal_robot/Joint.cpp | 238 +++++++- gtdynamics/universal_robot/Joint.h | 172 +++--- gtdynamics/universal_robot/JointTyped.h | 484 ++++++++-------- gtdynamics/universal_robot/PrismaticJoint.h | 18 +- gtdynamics/universal_robot/RevoluteJoint.h | 15 +- gtdynamics/universal_robot/Robot.cpp | 9 +- gtdynamics/universal_robot/ScrewJoint.h | 13 +- gtdynamics/universal_robot/ScrewJointBase.cpp | 530 +++++++++--------- gtdynamics/universal_robot/ScrewJointBase.h | 332 +++++------ gtdynamics/utils/JsonSaver.h | 2 +- tests/make_joint.h | 9 +- tests/testJointMeasurementFactor.cpp | 21 +- tests/testPoseFactor.cpp | 3 +- tests/testRevoluteJoint.cpp | 12 +- tests/testScrewJoint.cpp | 4 - 21 files changed, 1061 insertions(+), 896 deletions(-) diff --git a/gtdynamics/factors/JointMeasurementFactor.h b/gtdynamics/factors/JointMeasurementFactor.h index 57ce6e3b..fac74b3f 100644 --- a/gtdynamics/factors/JointMeasurementFactor.h +++ b/gtdynamics/factors/JointMeasurementFactor.h @@ -23,10 +23,7 @@ namespace gtdynamics { /** * @brief A 2-way factor to relate the parent and child links of a joint given * the joint coordinate as a measurement. - * - * @tparam JOINT The type of joint which is being constrained. */ -template class JointMeasurementFactor : public gtsam::NoiseModelFactor2 { private: @@ -34,8 +31,7 @@ class JointMeasurementFactor using Base = gtsam::NoiseModelFactor2; JointConstSharedPtr joint_; - typename JOINT::JointCoordinate joint_coordinate_; - size_t k_; + double joint_angle_; public: /** @@ -48,15 +44,13 @@ class JointMeasurementFactor * @param joint_coordinate The coordinates of the joint motion. * @param k The time index. */ - JointMeasurementFactor( - gtsam::Key wTp_key, gtsam::Key wTc_key, - const gtsam::noiseModel::Base::shared_ptr& model, - const JointConstSharedPtr joint, - const typename JOINT::JointCoordinate& joint_coordinate, size_t k) + JointMeasurementFactor(gtsam::Key wTp_key, gtsam::Key wTc_key, + const gtsam::noiseModel::Base::shared_ptr& model, + const JointConstSharedPtr joint, + double joint_coordinate) : Base(model, wTp_key, wTc_key), joint_(joint), - joint_coordinate_(joint_coordinate), - k_(k) {} + joint_angle_(joint_coordinate) {} /** * @brief Convenience constructor @@ -66,26 +60,21 @@ class JointMeasurementFactor * @param joint_coordinate The coordinates of the joint motion. * @param k The time index. */ - JointMeasurementFactor( - const gtsam::noiseModel::Base::shared_ptr& model, - const JointConstSharedPtr joint, - const typename JOINT::JointCoordinate& joint_coordinate, size_t k) + JointMeasurementFactor(const gtsam::noiseModel::Base::shared_ptr& model, + const JointConstSharedPtr joint, + double joint_coordinate, size_t k) : Base(model, internal::PoseKey(joint->parent()->id(), k), internal::PoseKey(joint->child()->id(), k)), joint_(joint), - joint_coordinate_(joint_coordinate), - k_(k) {} + joint_angle_(joint_coordinate) {} gtsam::Vector evaluateError( const gtsam::Pose3& wTp, const gtsam::Pose3& wTc, boost::optional H_wTp = boost::none, boost::optional H_wTc = boost::none) const override { - gtsam::Values joint_angles; - InsertJointAngle(&joint_angles, joint_->id(), k_, joint_coordinate_); - gtsam::Matrix6 H; gtsam::Pose3 wTc_hat = - joint_->poseOf(joint_->child(), wTp, joint_angles, k_, H_wTp); + joint_->poseOf(joint_->child(), wTp, joint_angle_, H_wTp); gtsam::Vector6 error = wTc.logmap(wTc_hat, H_wTc, H_wTp ? &H : 0); if (H_wTp) { @@ -100,8 +89,7 @@ class JointMeasurementFactor gtsam::DefaultKeyFormatter) const override { std::cout << s << "JointMeasurementFactor(" << keyFormatter(key1()) << "," << keyFormatter(key2()) << ")\n"; - gtsam::traits::Print(joint_coordinate_, - " measured: "); + gtsam::traits::Print(joint_angle_, " measured: "); this->noiseModel_->print(" noise model: "); } }; diff --git a/gtdynamics/factors/PoseFactor.h b/gtdynamics/factors/PoseFactor.h index 4e9c72ad..aa679e42 100644 --- a/gtdynamics/factors/PoseFactor.h +++ b/gtdynamics/factors/PoseFactor.h @@ -106,7 +106,8 @@ class PoseFactor : public gtsam::NoiseModelFactor { boost::optional wTc_hat_H_q_ref; if (H) wTc_hat_H_q_ref = wTc_hat_H_q; - auto wTc_hat = joint_->poseOf(joint_->child(), wTp, x, t_, + auto wTc_hat = joint_->poseOf(joint_->child(), wTp, + JointAngle(x, joint_->id(), t_), H ? &wTc_hat_H_wTp : 0, wTc_hat_H_q_ref); gtsam::Vector6 error = wTc.logmap(wTc_hat, H ? &H_wTc : 0, H ? &H_wTc_hat : 0); diff --git a/gtdynamics/factors/TorqueFactor.h b/gtdynamics/factors/TorqueFactor.h index 2b0a9a91..db532aff 100644 --- a/gtdynamics/factors/TorqueFactor.h +++ b/gtdynamics/factors/TorqueFactor.h @@ -31,13 +31,10 @@ namespace gtdynamics { * TorqueFactor is a two-way nonlinear factor which enforces relation between * wrench and torque on each link */ -class TorqueFactor - : public gtsam::NoiseModelFactor2 { +class TorqueFactor : public gtsam::NoiseModelFactor2 { private: - using JointTorque = typename JointTyped::JointTorque; using This = TorqueFactor; - using Base = gtsam::NoiseModelFactor2; + using Base = gtsam::NoiseModelFactor2; using MyJointConstSharedPtr = boost::shared_ptr; MyJointConstSharedPtr joint_; @@ -72,11 +69,11 @@ class TorqueFactor * @param torque torque on this link joint */ gtsam::Vector evaluateError( - const gtsam::Vector6 &wrench, const JointTorque &torque, + const gtsam::Vector6 &wrench, const double &torque, boost::optional H_wrench = boost::none, boost::optional H_torque = boost::none) const override { if (H_torque) { - *H_torque = -JointTyped::MatrixN::Identity(); + *H_torque = -gtsam::Matrix1::Identity(); } // TODO(G+S): next PR will generalize this from Vector1 return gtsam::Vector1( diff --git a/gtdynamics/factors/TwistAccelFactor.h b/gtdynamics/factors/TwistAccelFactor.h index 19bf21f9..5f02d2a4 100644 --- a/gtdynamics/factors/TwistAccelFactor.h +++ b/gtdynamics/factors/TwistAccelFactor.h @@ -32,18 +32,13 @@ namespace gtdynamics { * between acceleration on previous link and this link. */ class TwistAccelFactor - : public gtsam::NoiseModelFactor6< - gtsam::Vector6, gtsam::Vector6, gtsam::Vector6, - JointTyped::JointCoordinate, JointTyped::JointVelocity, - JointTyped::JointAcceleration> { + : public gtsam::NoiseModelFactor6 { private: - using JointCoordinate = JointTyped::JointCoordinate; - using JointVelocity = JointTyped::JointVelocity; - using JointAcceleration = JointTyped::JointVelocity; using This = TwistAccelFactor; using Base = gtsam::NoiseModelFactor6; + gtsam::Vector6, double, + double, double>; using JointTypedConstSharedPtr = boost::shared_ptr; JointTypedConstSharedPtr joint_; @@ -80,8 +75,8 @@ class TwistAccelFactor */ gtsam::Vector evaluateError( const gtsam::Vector6 &twist_c, const gtsam::Vector6 &twistAccel_p, - const gtsam::Vector6 &twistAccel_c, const JointCoordinate &q, - const JointAcceleration &qVel, const JointAcceleration &qAccel, + const gtsam::Vector6 &twistAccel_c, const double &q, + const double &qVel, const double &qAccel, boost::optional H_twist_c = boost::none, boost::optional H_twistAccel_p = boost::none, boost::optional H_twistAccel_c = boost::none, diff --git a/gtdynamics/factors/TwistFactor.h b/gtdynamics/factors/TwistFactor.h index 497dbb3d..a14c8042 100644 --- a/gtdynamics/factors/TwistFactor.h +++ b/gtdynamics/factors/TwistFactor.h @@ -30,15 +30,12 @@ namespace gtdynamics { * between twist on previous link and this link */ class TwistFactor - : public gtsam::NoiseModelFactor4 { + : public gtsam::NoiseModelFactor4 { private: - using JointCoordinate = typename JointTyped::JointCoordinate; - using JointVelocity = typename JointTyped::JointVelocity; using This = TwistFactor; - using Base = gtsam::NoiseModelFactor4; + using Base = + gtsam::NoiseModelFactor4; gtsam::Pose3 cMp_; JointConstSharedPtr joint_; @@ -70,7 +67,7 @@ class TwistFactor */ gtsam::Vector evaluateError( const gtsam::Vector6 &twist_p, const gtsam::Vector6 &twist_c, - const JointCoordinate &q, const JointVelocity &qVel, + const double &q, const double &qVel, boost::optional H_twist_p = boost::none, boost::optional H_twist_c = boost::none, boost::optional H_q = boost::none, diff --git a/gtdynamics/factors/WrenchEquivalenceFactor.h b/gtdynamics/factors/WrenchEquivalenceFactor.h index d719f19c..86acb779 100644 --- a/gtdynamics/factors/WrenchEquivalenceFactor.h +++ b/gtdynamics/factors/WrenchEquivalenceFactor.h @@ -33,13 +33,10 @@ namespace gtdynamics { /** WrenchEquivalenceFactor is a 3-way nonlinear factor which enforces * relation between wrench expressed in two link frames*/ class WrenchEquivalenceFactor - : public gtsam::NoiseModelFactor3 { + : public gtsam::NoiseModelFactor3 { private: - using JointCoordinate = typename JointTyped::JointCoordinate; using This = WrenchEquivalenceFactor; - using Base = - gtsam::NoiseModelFactor3; + using Base = gtsam::NoiseModelFactor3; using JointTypedConstSharedPtr = boost::shared_ptr; JointTypedConstSharedPtr joint_; @@ -75,7 +72,7 @@ class WrenchEquivalenceFactor */ gtsam::Vector evaluateError( const gtsam::Vector6 &wrench_1, const gtsam::Vector6 &wrench_2, - const JointCoordinate &q, + const double &q, boost::optional H_wrench_1 = boost::none, boost::optional H_wrench_2 = boost::none, boost::optional H_q = boost::none) const override { @@ -91,9 +88,9 @@ class WrenchEquivalenceFactor } if (H_q) { // TODO(frank): really, child? Double-check derivatives - *H_q = - joint_->AdjointMapJacobianJointAngle(joint_->child(), q).transpose() * - wrench_2; + // *H_q = + // joint_->AdjointMapJacobianJointAngle(joint_->child(), q).transpose() * + // wrench_2; } return error; } diff --git a/gtdynamics/universal_robot/Joint.cpp b/gtdynamics/universal_robot/Joint.cpp index 568378f2..35943211 100644 --- a/gtdynamics/universal_robot/Joint.cpp +++ b/gtdynamics/universal_robot/Joint.cpp @@ -20,16 +20,21 @@ #include "gtdynamics/universal_robot/Link.h" +using gtsam::Pose3; +using gtsam::Vector6; + namespace gtdynamics { /* ************************************************************************* */ Joint::Joint(uint8_t id, const std::string &name, const Pose3 &bTj, const LinkSharedPtr &parent_link, const LinkSharedPtr &child_link, - const JointParams ¶meters) + const Vector6 &jScrewAxis, const JointParams ¶meters) : id_(id), name_(name), parent_link_(parent_link), child_link_(child_link), + pScrewAxis_(-jMp_.inverse().AdjointMap() * jScrewAxis), + cScrewAxis_(jMc_.inverse().AdjointMap() * jScrewAxis), parameters_(parameters) { jMp_ = bTj.inverse() * parent_link_->bMcom(); jMc_ = bTj.inverse() * child_link_->bMcom(); @@ -43,10 +48,239 @@ bool Joint::isChildLink(const LinkSharedPtr &link) const { return link == child_link_; } + +/* ************************************************************************* */ +Pose3 Joint::parentTchild( + double q, gtsam::OptionalJacobian<6, 1> pMc_H_q) const { + // Calculate pose of child in parent link, at rest. + // TODO(dellaert): store `pMj_` rather than `jMp_`. + // NOTE(dellaert): Only if this is called more often than childTparent. + const Pose3 pMc = jMp_.inverse() * jMc_; + + // Multiply screw axis with joint angle to get a finite 6D screw. + const Vector6 screw = cScrewAxis_ * q; + + // Calculate the actual relative pose taking into account the joint angle. + // TODO(dellaert): use formula `pMj_ * screw_around_Z * jMc_`. + gtsam::Matrix6 exp_H_screw; + const Pose3 exp = Pose3::Expmap(screw, pMc_H_q ? &exp_H_screw : 0); + if (pMc_H_q) { + *pMc_H_q = exp_H_screw * cScrewAxis_; + } + return pMc * exp; // Note: derivative of compose in exp is identity. +} + +/* ************************************************************************* */ +Pose3 Joint::childTparent( + double q, gtsam::OptionalJacobian<6, 1> cMp_H_q) const { + // TODO(frank): don't go via inverse, specialize in base class + Vector6 pMc_H_q; + Pose3 pMc = parentTchild(q, cMp_H_q ? &pMc_H_q : 0); // pMc(q) -> pMc_H_q + gtsam::Matrix6 cMp_H_pMc; + Pose3 cMp = pMc.inverse(cMp_H_q ? &cMp_H_pMc : 0); // cMp(pMc) -> cMp_H_pMc + if (cMp_H_q) { + *cMp_H_q = cMp_H_pMc * pMc_H_q; + } + return cMp; +} + +/* ************************************************************************* */ +Vector6 Joint::transformTwistTo( + const LinkSharedPtr &link, double q, double q_dot, + boost::optional other_twist, + gtsam::OptionalJacobian<6, 1> H_q, + gtsam::OptionalJacobian<6, 1> H_q_dot, + gtsam::OptionalJacobian<6, 6> H_other_twist) const { + Vector6 other_twist_ = other_twist ? *other_twist : Vector6::Zero(); + + auto other = otherLink(link); + auto this_ad_other = relativePoseOf(other, q).AdjointMap(); + + if (H_q) { + // TODO(frank): really, zero below? Check derivatives + *H_q = AdjointMapJacobianQ(q, relativePoseOf(other, 0.0), screwAxis(link)) * + other_twist_; + } + if (H_q_dot) { + *H_q_dot = screwAxis(link); + } + if (H_other_twist) { + *H_other_twist = this_ad_other; + } + + return this_ad_other * other_twist_ + screwAxis(link) * q_dot; +} + +/* ************************************************************************* */ +Vector6 Joint::transformTwistAccelTo( + const LinkSharedPtr &link, double q, double q_dot, double q_ddot, + boost::optional this_twist, + boost::optional other_twist_accel, + gtsam::OptionalJacobian<6, 1> H_q, + gtsam::OptionalJacobian<6, 1> H_q_dot, + gtsam::OptionalJacobian<6, 1> H_q_ddot, + gtsam::OptionalJacobian<6, 6> H_this_twist, + gtsam::OptionalJacobian<6, 6> H_other_twist_accel) const { + Vector6 this_twist_ = this_twist ? *this_twist : Vector6::Zero(); + Vector6 other_twist_accel_ = + other_twist_accel ? *other_twist_accel : Vector6::Zero(); + Vector6 screw_axis_ = isChildLink(link) ? cScrewAxis_ : pScrewAxis_; + + // i = other link + // j = this link + auto other = otherLink(link); + Pose3 jTi = relativePoseOf(other, q); + + Vector6 this_twist_accel = + jTi.AdjointMap() * other_twist_accel_ + + Pose3::adjoint(this_twist_, screw_axis_ * q_dot, H_this_twist) + + screw_axis_ * q_ddot; + + if (H_other_twist_accel) { + *H_other_twist_accel = jTi.AdjointMap(); + } + if (H_q) { + // TODO(frank): really, zero below? Check derivatives. Also, copy/pasta from + // above? + *H_q = AdjointMapJacobianQ(q, relativePoseOf(other, 0.0), screw_axis_) * + other_twist_accel_; + } + if (H_q_dot) { + *H_q_dot = Pose3::adjointMap(this_twist_) * screw_axis_; + } + if (H_q_ddot) { + *H_q_ddot = screw_axis_; + } + + return this_twist_accel; +} + +/* ************************************************************************* */ +double Joint::transformWrenchToTorque( + const LinkSharedPtr &link, boost::optional wrench, + gtsam::OptionalJacobian<1, 6> H_wrench) const { + auto screw_axis_ = screwAxis(link); + if (H_wrench) { + *H_wrench = screw_axis_.transpose(); + } + return screw_axis_.transpose() * (wrench ? *wrench : Vector6::Zero()); +} + +/* ************************************************************************* */ +gtsam::GaussianFactorGraph Joint::linearFDPriors( + size_t t, const gtsam::Values &known_values, + const OptimizerSetting &opt) const { + gtsam::GaussianFactorGraph priors; + gtsam::Vector1 rhs(Torque(known_values, id(), t)); + // TODO(alej`andro): use optimizer settings + priors.add(internal::TorqueKey(id(), t), gtsam::I_1x1, rhs, + gtsam::noiseModel::Constrained::All(1)); + return priors; +} + +/* ************************************************************************* */ +gtsam::GaussianFactorGraph Joint::linearAFactors( + size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, + const boost::optional &planar_axis) const { + gtsam::GaussianFactorGraph graph; + + const Pose3 T_wi1 = Pose(known_values, parent()->id(), t); + const Pose3 T_wi2 = Pose(known_values, child()->id(), t); + const Pose3 T_i2i1 = T_wi2.inverse() * T_wi1; + const Vector6 V_i2 = Twist(known_values, child()->id(), t); + const Vector6 S_i2_j = screwAxis(child_link_); + const double v_j = JointVel(known_values, id(), t); + + // twist acceleration factor + // A_i2 - Ad(T_21) * A_i1 - S_i2_j * a_j = ad(V_i2) * S_i2_j * v_j + Vector6 rhs_tw = Pose3::adjointMap(V_i2) * S_i2_j * v_j; + graph.add(internal::TwistAccelKey(child()->id(), t), gtsam::I_6x6, + internal::TwistAccelKey(parent()->id(), t), -T_i2i1.AdjointMap(), + internal::JointAccelKey(id(), t), -S_i2_j, rhs_tw, + gtsam::noiseModel::Constrained::All(6)); + + return graph; +} + +/* ************************************************************************* */ +gtsam::GaussianFactorGraph Joint::linearDynamicsFactors( + size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, + const boost::optional &planar_axis) const { + gtsam::GaussianFactorGraph graph; + + const Pose3 T_wi1 = Pose(known_values, parent()->id(), t); + const Pose3 T_wi2 = Pose(known_values, child()->id(), t); + const Pose3 T_i2i1 = T_wi2.inverse() * T_wi1; + const Vector6 S_i2_j = screwAxis(child_link_); + + // torque factor + // S_i_j^T * F_i_j - tau = 0 + gtsam::Vector1 rhs_torque = gtsam::Vector1::Zero(); + graph.add(internal::WrenchKey(child()->id(), id(), t), S_i2_j.transpose(), + internal::TorqueKey(id(), t), -gtsam::I_1x1, rhs_torque, + gtsam::noiseModel::Constrained::All(1)); + + // wrench equivalence factor + // F_i1_j + Ad(T_i2i1)^T F_i2_j = 0 + Vector6 rhs_weq = Vector6::Zero(); + graph.add(internal::WrenchKey(parent()->id(), id(), t), gtsam::I_6x6, + internal::WrenchKey(child()->id(), id(), t), + T_i2i1.AdjointMap().transpose(), rhs_weq, + gtsam::noiseModel::Constrained::All(6)); + + // wrench planar factor + if (planar_axis) { + gtsam::Matrix36 J_wrench = getPlanarJacobian(*planar_axis); + graph.add(internal::WrenchKey(child()->id(), id(), t), J_wrench, + gtsam::Vector3::Zero(), gtsam::noiseModel::Constrained::All(3)); + } + + return graph; +} + +/* ************************************************************************* */ +Vector6 Joint::childTwist(double q_dot) const { return cScrewAxis_ * q_dot; } + +/* ************************************************************************* */ +Vector6 Joint::parentTwist(double q_dot) const { return pScrewAxis_ * q_dot; } + +/* ************************************************************************* */ +gtsam::NonlinearFactorGraph Joint::jointLimitFactors( + size_t t, const OptimizerSetting &opt) const { + gtsam::NonlinearFactorGraph graph; + auto id = this->id(); + // Add joint angle limit factor. + // graph.emplace_shared( + // internal::JointAngleKey(id, t), opt.jl_cost_model, + // parameters().scalar_limits.value_lower_limit, + // parameters().scalar_limits.value_upper_limit, + // parameters().scalar_limits.value_limit_threshold); + + // // Add joint velocity limit factors. + // graph.emplace_shared( + // internal::JointVelKey(id, t), opt.jl_cost_model, + // -parameters().velocity_limit, parameters().velocity_limit, + // parameters().velocity_limit_threshold); + + // // Add joint acceleration limit factors. + // graph.emplace_shared( + // internal::JointAccelKey(id, t), opt.jl_cost_model, + // -parameters().acceleration_limit, parameters().acceleration_limit, + // parameters().acceleration_limit_threshold); + + // // Add joint torque limit factors. + // graph.emplace_shared( + // internal::TorqueKey(id, t), opt.jl_cost_model, -parameters().torque_limit, + // parameters().torque_limit, parameters().torque_limit_threshold); + return graph; +} + +/* ************************************************************************* */ std::ostream &Joint::to_stream(std::ostream &os) const { os << name_ << "\n\tid=" << size_t(id_) << "\n\tparent link: " << parent()->name() - << "\n\tchild link: " << child()->name(); + << "\n\tchild link: " << child()->name() + << "\n\tscrew axis (parent): " << screwAxis(parent()).transpose(); return os; } diff --git a/gtdynamics/universal_robot/Joint.h b/gtdynamics/universal_robot/Joint.h index cc5ca67b..e1a34844 100644 --- a/gtdynamics/universal_robot/Joint.h +++ b/gtdynamics/universal_robot/Joint.h @@ -81,6 +81,7 @@ class Joint : public boost::enable_shared_from_this { protected: using Pose3 = gtsam::Pose3; + using Vector6 = gtsam::Vector6; public: /** @@ -114,6 +115,10 @@ class Joint : public boost::enable_shared_from_this { LinkSharedPtr parent_link_; LinkSharedPtr child_link_; + // Screw axis in parent and child CoM frames. + Vector6 pScrewAxis_; + Vector6 cScrewAxis_; + /// Joint parameters struct. JointParams parameters_; @@ -132,10 +137,12 @@ class Joint : public boost::enable_shared_from_this { * @param[in] bTj joint pose expressed in base frame * @param[in] parent_link Shared pointer to the parent Link. * @param[in] child_link Shared pointer to the child Link. + * @param[in] jScrewAxis Screw axis in the joint frame * @param[in] parameters The joint parameters. */ Joint(uint8_t id, const std::string &name, const Pose3 &bTj, const LinkSharedPtr &parent_link, const LinkSharedPtr &child_link, + const Vector6 &jScrewAxis, const JointParams ¶meters = JointParams()); /** @@ -158,6 +165,11 @@ class Joint : public boost::enable_shared_from_this { /// Transform from the joint frame to the child's center of mass. const Pose3 &jMc() const { return jMc_; } + /// Return screw axis expressed in the specified link frame + const Vector6 screwAxis(const LinkSharedPtr &link) const { + return isChildLink(link) ? cScrewAxis_ : pScrewAxis_; + } + /// Get a gtsam::Key for this joint gtsam::Key key() const { return gtsam::Key(id()); } @@ -214,80 +226,78 @@ class Joint : public boost::enable_shared_from_this { */ virtual Type type() const = 0; + /**@}*/ + /** - * Abstract method. Return the pose of the child link CoM in the parent link - * CoM frame, given a Values object containing the joint coordinate. + * Return transform of child link CoM frame w.r.t parent link CoM frame */ - virtual Pose3 parentTchild( - const gtsam::Values &q, size_t t = 0, - boost::optional H_q = boost::none) const = 0; + Pose3 parentTchild(double q, + gtsam::OptionalJacobian<6, 1> pMc_H_q = boost::none) const; /** - * Abstract method. Return the pose of the parent link in the child link - * frame, given a Values object containing the joint coordinate. + * Return transform of parent link CoM frame w.r.t child link CoM frame */ - virtual Pose3 childTparent( - const gtsam::Values &q, size_t t = 0, - boost::optional H_q = boost::none) const = 0; + Pose3 childTparent(double q, + gtsam::OptionalJacobian<6, 1> cMp_H_q = boost::none) const; /** - * Abstract method. Return the relative pose of the specified link [link2] in - * the other link's [link1] reference frame. + * Return the relative pose of the specified link [link2] in the other link's + * [link1] reference frame. */ - virtual Pose3 relativePoseOf( - const LinkSharedPtr &link2, const gtsam::Values &q, size_t t = 0, - boost::optional H_q = boost::none) const = 0; + Pose3 relativePoseOf(const LinkSharedPtr &link2, double q, + gtsam::OptionalJacobian<6, 1> H_q = boost::none) const { + return isChildLink(link2) ? parentTchild(q, H_q) : childTparent(q, H_q); + } /** * Return the world pose of the specified link [link2], given * the world pose of the other link [link1]. */ - Pose3 poseOf(const LinkSharedPtr &link2, const Pose3 &wT1, - const gtsam::Values &q, size_t t = 0, + Pose3 poseOf(const LinkSharedPtr &link2, const Pose3 &wT1, double q, gtsam::OptionalJacobian<6, 6> H_wT1 = boost::none, - boost::optional H_q = boost::none) const { - auto T12 = relativePoseOf(link2, q, t, H_q); + gtsam::OptionalJacobian<6, 1> H_q = boost::none) const { + auto T12 = relativePoseOf(link2, q, H_q); return wT1.compose(T12, H_wT1); // H_wT2_T12 is identity } /** Abstract method. Return the twist of the other link given this link's * twist and a Values object containing this joint's angle Value. */ - virtual gtsam::Vector6 transformTwistTo( - size_t t, const LinkSharedPtr &link, const gtsam::Values &q_and_q_dot, + gtsam::Vector6 transformTwistTo( + const LinkSharedPtr &link, double q, double q_dot, boost::optional other_twist = boost::none, - boost::optional H_q = boost::none, - boost::optional H_q_dot = boost::none, - gtsam::OptionalJacobian<6, 6> H_other_twist = boost::none) const = 0; + gtsam::OptionalJacobian<6, 1> H_q = boost::none, + gtsam::OptionalJacobian<6, 1> H_q_dot = boost::none, + gtsam::OptionalJacobian<6, 6> H_other_twist = boost::none) const; /** * Return the twist acceleration of the other link given this link's twist * accel and a Values object containing this joint's angle Value and * derivatives. */ - virtual gtsam::Vector6 transformTwistAccelTo( - size_t t, const LinkSharedPtr &link, - const gtsam::Values &q_and_q_dot_and_q_ddot, + gtsam::Vector6 transformTwistAccelTo( + const LinkSharedPtr &link, double q, double q_dot, double q_ddot, boost::optional this_twist = boost::none, boost::optional other_twist_accel = boost::none, - boost::optional H_q = boost::none, - boost::optional H_q_dot = boost::none, - boost::optional H_q_ddot = boost::none, + gtsam::OptionalJacobian<6, 1> H_q = boost::none, + gtsam::OptionalJacobian<6, 1> H_q_dot = boost::none, + gtsam::OptionalJacobian<6, 1> H_q_ddot = boost::none, gtsam::OptionalJacobian<6, 6> H_this_twist = boost::none, - gtsam::OptionalJacobian<6, 6> H_other_twist_accel = - boost::none) const = 0; - - /// Abstract method. Returns forward dynamics priors on torque - virtual gtsam::GaussianFactorGraph linearFDPriors( - size_t t, const gtsam::Values &torques, - const OptimizerSetting &opt) const { - throw std::runtime_error( - "linearFDPriors not implemented for the desired " - "joint type. A linearized version may not be possible."); - } + gtsam::OptionalJacobian<6, 6> H_other_twist_accel = boost::none) const; + + /// Return the torque on this joint given the wrench + double transformWrenchToTorque( + const LinkSharedPtr &link, + boost::optional wrench = boost::none, + gtsam::OptionalJacobian<1, 6> H_wrench = boost::none) const; + + /// Returns forward dynamics priors on torque + gtsam::GaussianFactorGraph linearFDPriors(size_t t, + const gtsam::Values &torques, + const OptimizerSetting &opt) const; /** - * @fn (ABSTRACT) Return linear accel factors in the dynamics graph. + * Returns linear acceleration factors in the dynamics graph. * * @param[in] t The timestep for which to generate factors. * @param[in] known_values Link poses, twists, Joint angles, Joint velocities. @@ -295,16 +305,12 @@ class Joint : public boost::enable_shared_from_this { * @param[in] planar_axis Optional planar axis. * @return linear accel factors. */ - virtual gtsam::GaussianFactorGraph linearAFactors( + gtsam::GaussianFactorGraph linearAFactors( size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, - const boost::optional &planar_axis = boost::none) const { - throw std::runtime_error( - "linearAFactors not implemented for the desired " - "joint type. A linearized version may not be possible."); - } + const boost::optional &planar_axis = boost::none) const; /** - * @fn (ABSTRACT) Return linear dynamics factors in the dynamics graph. + * Returns linear dynamics factors in the dynamics graph. * * @param[in] t The timestep for which to generate factors. * @param[in] known_values Link poses, twists, Joint angles, Joint @@ -314,38 +320,9 @@ class Joint : public boost::enable_shared_from_this { * @param[in] planar_axis Optional planar axis. * @return linear dynamics factors. */ - virtual gtsam::GaussianFactorGraph linearDynamicsFactors( + gtsam::GaussianFactorGraph linearDynamicsFactors( size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, - const boost::optional &planar_axis = boost::none) const { - throw std::runtime_error( - "linearDynamicsFactors not implemented for the " - "desired joint type. A linearized version may not be possible."); - } - - /** - * @fn (ABSTRACT) Return linear dynamics factors in the dynamics graph. - * - * @param[in] t The timestep for which to generate factors. - * @param[in] poses Link poses. - * @param[in] twists Link twists. - * @param[in] joint_angles Joint angles. - * @param[in] joint_vels Joint velocities. - * @param[in] opt OptimizerSetting object containing NoiseModels - * for factors. - * @param[in] planar_axis Optional planar axis. - * @return linear dynamics factors. - */ - virtual gtsam::GaussianFactorGraph linearDynamicsFactors( - size_t t, const std::map &poses, - const std::map &twists, - const std::map &joint_angles, - const std::map &joint_vels, - const OptimizerSetting &opt, - const boost::optional &planar_axis = boost::none) const { - throw std::runtime_error( - "linearDynamicsFactors not implemented for the " - "desired joint type. A linearized version may not be possible."); - } + const boost::optional &planar_axis = boost::none) const; /** * @fn (ABSTRACT) Return joint limit factors in the dynamics graph. @@ -354,44 +331,41 @@ class Joint : public boost::enable_shared_from_this { * @param[in] opt OptimizerSetting object containing NoiseModels for factors. * @return joint limit factors. */ - virtual gtsam::NonlinearFactorGraph jointLimitFactors( - size_t t, const OptimizerSetting &opt) const = 0; - - /**@}*/ + gtsam::NonlinearFactorGraph jointLimitFactors( + size_t t, const OptimizerSetting &opt) const; /// Joint-induced twist in child frame - virtual gtsam::Vector6 childTwist(const gtsam::Values &values, - size_t t = 0) const = 0; + Vector6 childTwist(double q_dot) const; /// Joint-induced twist in parent frame - virtual gtsam::Vector6 parentTwist(const gtsam::Values &values, - size_t t = 0) const = 0; + Vector6 parentTwist(double q_dot) const; /// Calculate pose/twist of child given parent pose/twist std::pair childPoseTwist( const gtsam::Pose3 &wTp, const gtsam::Vector6 &Vp, - const gtsam::Values &known_values, size_t t = 0) const { - const gtsam::Pose3 pTc = parentTchild(known_values, t); - return {wTp * pTc, pTc.inverse().Adjoint(Vp) + childTwist(known_values, t)}; + double q, double q_dot) const { + const gtsam::Pose3 pTc = parentTchild(q); + return {wTp * pTc, pTc.inverse().Adjoint(Vp) + childTwist(q_dot)}; } /// Calculate pose/twist of parent given child pose/twist std::pair parentPoseTwist( - const gtsam::Pose3 &wTc, const gtsam::Vector6 &Vc, - const gtsam::Values &known_values, size_t t = 0) const { - const gtsam::Pose3 pTc = parentTchild(known_values, t); + const gtsam::Pose3 &wTc, const gtsam::Vector6 &Vc, double q, + double q_dot) const { + const gtsam::Pose3 pTc = parentTchild(q); return {wTc * pTc.inverse(), - pTc.Adjoint(Vc) + parentTwist(known_values, t)}; + pTc.Adjoint(Vc) + parentTwist(q_dot)}; } /// Given link pose/twist, calculate pose/twist of other link std::pair otherPoseTwist( const LinkSharedPtr &link, const gtsam::Pose3 &wTl, - const gtsam::Vector6 &Vl, const gtsam::Values &known_values, - size_t t = 0) const { - return isChildLink(link) ? parentPoseTwist(wTl, Vl, known_values, t) - : childPoseTwist(wTl, Vl, known_values, t); + const gtsam::Vector6 &Vl, double q, double q_dot) const { + return isChildLink(link) ? parentPoseTwist(wTl, Vl, q, q_dot) + : childPoseTwist(wTl, Vl, q, q_dot); } }; +using JointTyped = Joint; + } // namespace gtdynamics diff --git a/gtdynamics/universal_robot/JointTyped.h b/gtdynamics/universal_robot/JointTyped.h index 46dcef73..5dcab5ca 100644 --- a/gtdynamics/universal_robot/JointTyped.h +++ b/gtdynamics/universal_robot/JointTyped.h @@ -1,243 +1,245 @@ -/* ---------------------------------------------------------------------------- - * GTDynamics Copyright 2020, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * See LICENSE for the license information - * -------------------------------------------------------------------------- */ - -/** - * @file JointTyped.h - * @brief Specialized form of Joint for specific joint angle types - * @author: Frank Dellaert - * @author: Mandy Xie - * @author: Alejandro Escontrela - * @author: Yetong Zhang - * @author: Varun Agrawal - */ - -#pragma once +// /* ---------------------------------------------------------------------------- +// * GTDynamics Copyright 2020, Georgia Tech Research Corporation, +// * Atlanta, Georgia 30332-0415 +// * All Rights Reserved +// * See LICENSE for the license information +// * -------------------------------------------------------------------------- */ + +// /** +// * @file JointTyped.h +// * @brief Specialized form of Joint for specific joint angle types +// * @author: Frank Dellaert +// * @author: Mandy Xie +// * @author: Alejandro Escontrela +// * @author: Yetong Zhang +// * @author: Varun Agrawal +// */ #include "gtdynamics/universal_robot/Joint.h" -#include "gtdynamics/utils/values.h" - -namespace gtdynamics { - -/** - * JointTyped is a convenience class that inherits from Joint which wraps - * abstract transformXXX that take in joint type arguments into transformXXX - * from Joint which take in gtsam::Values object - */ - -// TODO(Gerry) JointTyped was an intermediate step towards adding ball and -// sphere joints but we never finished it because for other joint types, -// relativePoseOf can't just use a double as the joint angle -// argument, they need Unit3 or Rot3 - -class JointTyped : public Joint { -public: - using This = JointTyped; - - using JointCoordinateType = double; // standin for template - using JointCoordinateTangentType = double; // standin for template - using JointCoordinate = JointCoordinateType; - using JointVelocity = JointCoordinateTangentType; - using JointAcceleration = JointCoordinateTangentType; - using JointTorque = JointCoordinateTangentType; - - enum { N = gtsam::traits::dimension }; - using VectorN = Eigen::Matrix; - using MatrixN = Eigen::Matrix; - -public: - /// Inherit constructors - using Joint::Joint; - - /** - * @name Abstract - * Joint classes must implement these methods. - */ - ///@{ - - /** - * Abstract method. Return the pose of the child link CoM in the parent link - * CoM frame, given the joint coordinate. - */ - virtual Pose3 parentTchild( - JointCoordinate q, - gtsam::OptionalJacobian<6, N> H_q = boost::none) const = 0; - - /** - * Abstract method. Return the pose of the parent link in the child link - * frame, given the joint coordinate. - */ - virtual Pose3 - childTparent(JointCoordinate q, - gtsam::OptionalJacobian<6, N> H_q = boost::none) const = 0; - - /// Joint-induced twist in child frame - virtual gtsam::Vector6 childTwist(JointVelocity q_dot) const = 0; - - /// Joint-induced twist in parent frame - virtual gtsam::Vector6 parentTwist(JointVelocity q_dot) const = 0; - - /** - * Abstract method. Return the twist of this link given the other link's twist - * and joint angle. - */ - virtual gtsam::Vector6 transformTwistTo( - const LinkSharedPtr &link, JointCoordinate q, JointVelocity q_dot, - boost::optional other_twist = boost::none, - gtsam::OptionalJacobian<6, N> H_q = boost::none, - gtsam::OptionalJacobian<6, N> H_q_dot = boost::none, - gtsam::OptionalJacobian<6, 6> H_other_twist = boost::none) const = 0; - - /** Abstract method. Return the twist acceleration of this link given the - * other link's twist acceleration, both links' twists, and joint angle. - */ - virtual gtsam::Vector6 transformTwistAccelTo( - const LinkSharedPtr &link, JointCoordinate q, JointVelocity q_dot, - JointAcceleration q_ddot, - boost::optional this_twist = boost::none, - boost::optional other_twist_accel = boost::none, - gtsam::OptionalJacobian<6, N> H_q = boost::none, - gtsam::OptionalJacobian<6, N> H_q_dot = boost::none, - gtsam::OptionalJacobian<6, N> H_q_ddot = boost::none, - gtsam::OptionalJacobian<6, 6> H_this_twist = boost::none, - gtsam::OptionalJacobian<6, 6> H_other_twist_accel = - boost::none) const = 0; - - /// Abstract method. Return the torque on this joint given the wrench - virtual JointTorque transformWrenchToTorque( - const LinkSharedPtr &link, - boost::optional wrench = boost::none, - gtsam::OptionalJacobian H_wrench = boost::none) const = 0; - - /// Calculate AdjointMap jacobian w.r.t. joint coordinate q. - /// TODO(gerry + stephanie): change to calculate the jacobian of Ad_T(v) wrt T - /// rather than jacobian of Ad_T wrt q (and put in utils or PR to GTSAM) - virtual gtsam::Matrix6 - AdjointMapJacobianJointAngle(const LinkSharedPtr &link, - JointCoordinate q) const = 0; - - ///@} - /** - * @name generic - * These are methods that can be implemented here in terms of abstract methods. - */ - ///@{ - - /** - * Return the relative pose of the specified link [link2] in the other link's - * [link1] reference frame. - */ - Pose3 relativePoseOf(const LinkSharedPtr &link2, JointCoordinate q, - gtsam::OptionalJacobian<6, 1> H_q = boost::none) const { - return isChildLink(link2) ? parentTchild(q, H_q) : childTparent(q, H_q); - } - - /** - * Return the world pose of the specified link [link2], given - * the world pose of the other link [link1]. - */ - Pose3 poseOf(const LinkSharedPtr &link2, const Pose3 &wT1, JointCoordinate q, - gtsam::OptionalJacobian<6, 6> H_wT1 = boost::none, - gtsam::OptionalJacobian<6, N> H_q = boost::none) const { - auto T12 = relativePoseOf(link2, q, H_q); - return wT1.compose(T12, H_wT1); // H_wT2_T12 is identity - } - - ///@} - /** - * @name valueBased - * Methods that extract coordinates/velocities from Values and pass on to - * abstract methods above. - */ - ///@{ - - /** - * Return the pose of the child link CoM in the parent link CoM frame, given a - * Values object containing the joint coordinate. - */ - Pose3 parentTchild( - const gtsam::Values &q, size_t t = 0, - boost::optional H_q = boost::none) const override { - return parentTchild(JointAngle(q, id(), t), H_q); - } - - /** - * Return the pose of the parent link in the child link frame, given a Values - * object containing the joint coordinate. - */ - Pose3 childTparent( - const gtsam::Values &q, size_t t = 0, - boost::optional H_q = boost::none) const override { - return childTparent(JointAngle(q, id(), t), H_q); - } - - /** - * Return the relative pose of the specified link [link2] in - * the other link's [link1] reference frame. - * @throw KeyDoesNotExist if the appropriate key is missing from values - */ - Pose3 relativePoseOf( - const LinkSharedPtr &link2, const gtsam::Values &q, size_t t = 0, - boost::optional H_q = boost::none) const override { - return relativePoseOf(link2, JointAngle(q, id(), t), H_q); - } - - /// Joint-induced twist in child frame - gtsam::Vector6 childTwist(const gtsam::Values &values, - size_t t = 0) const override { - return childTwist(JointVel(values, id(), t)); - } - - /// Joint-induced twist in parent frame - gtsam::Vector6 parentTwist(const gtsam::Values &values, - size_t t = 0) const override { - return parentTwist(JointVel(values, id(), t)); - } - - /** - * Return the twist of this link given the other link's twist and a Values - * object containing this joint's angle Value. - * @param values containing q, q_dot - * @throw ValuesKeyDoesNotExist if the appropriate key is missing from values - */ - gtsam::Vector6 transformTwistTo( - size_t t, const LinkSharedPtr &link, const gtsam::Values &values, - boost::optional other_twist = boost::none, - boost::optional H_q = boost::none, - boost::optional H_q_dot = boost::none, - gtsam::OptionalJacobian<6, 6> H_other_twist = boost::none) // - const override { - return transformTwistTo(link, JointAngle(values, id(), t), - JointVel(values, id(), t), - other_twist, H_q, H_q_dot, H_other_twist); - } - - /** Return the twist acceleration of the other link given this link's twist - * accel and a Values object containing this joint's angle value and - * derivatives. - * @param values containing q, q_dot, and q_ddot - * @throw ValuesKeyDoesNotExist if the appropriate key is missing from values - */ - gtsam::Vector6 transformTwistAccelTo( - size_t t, const LinkSharedPtr &link, const gtsam::Values &values, - boost::optional this_twist = boost::none, - boost::optional other_twist_accel = boost::none, - boost::optional H_q = boost::none, - boost::optional H_q_dot = boost::none, - boost::optional H_q_ddot = boost::none, - gtsam::OptionalJacobian<6, 6> H_this_twist = boost::none, - gtsam::OptionalJacobian<6, 6> H_other_twist_accel = - boost::none) const override { - return transformTwistAccelTo(link, - JointAngle(values, id(), t), - JointVel(values, id(), t), - JointAccel(values, id(), t), - this_twist, other_twist_accel, H_q, H_q_dot, - H_q_ddot, H_this_twist, H_other_twist_accel); - } -}; - -} // namespace gtdynamics + +// #pragma once + +// #include "gtdynamics/universal_robot/Joint.h" +// #include "gtdynamics/utils/values.h" + +// namespace gtdynamics { + +// /** +// * JointTyped is a convenience class that inherits from Joint which wraps +// * abstract transformXXX that take in joint type arguments into transformXXX +// * from Joint which take in gtsam::Values object +// */ + +// // TODO(Gerry) JointTyped was an intermediate step towards adding ball and +// // sphere joints but we never finished it because for other joint types, +// // relativePoseOf can't just use a double as the joint angle +// // argument, they need Unit3 or Rot3 + +// class JointTyped : public Joint { +// public: +// using This = JointTyped; + +// using JointCoordinateType = double; // standin for template +// using JointCoordinateTangentType = double; // standin for template +// using JointCoordinate = JointCoordinateType; +// using JointVelocity = JointCoordinateTangentType; +// using JointAcceleration = JointCoordinateTangentType; +// using JointTorque = JointCoordinateTangentType; + +// enum { N = gtsam::traits::dimension }; +// using VectorN = Eigen::Matrix; +// using MatrixN = Eigen::Matrix; + +// public: +// /// Inherit constructors +// using Joint::Joint; + +// /** +// * @name Abstract +// * Joint classes must implement these methods. +// */ +// ///@{ + +// /** +// * Abstract method. Return the pose of the child link CoM in the parent link +// * CoM frame, given the joint coordinate. +// */ +// virtual Pose3 parentTchild( +// JointCoordinate q, +// gtsam::OptionalJacobian<6, N> H_q = boost::none) const = 0; + +// /** +// * Abstract method. Return the pose of the parent link in the child link +// * frame, given the joint coordinate. +// */ +// virtual Pose3 +// childTparent(JointCoordinate q, +// gtsam::OptionalJacobian<6, N> H_q = boost::none) const = 0; + +// /// Joint-induced twist in child frame +// virtual gtsam::Vector6 childTwist(JointVelocity q_dot) const = 0; + +// /// Joint-induced twist in parent frame +// virtual gtsam::Vector6 parentTwist(JointVelocity q_dot) const = 0; + +// /** +// * Abstract method. Return the twist of this link given the other link's twist +// * and joint angle. +// */ +// virtual gtsam::Vector6 transformTwistTo( +// const LinkSharedPtr &link, JointCoordinate q, JointVelocity q_dot, +// boost::optional other_twist = boost::none, +// gtsam::OptionalJacobian<6, N> H_q = boost::none, +// gtsam::OptionalJacobian<6, N> H_q_dot = boost::none, +// gtsam::OptionalJacobian<6, 6> H_other_twist = boost::none) const = 0; + +// /** Abstract method. Return the twist acceleration of this link given the +// * other link's twist acceleration, both links' twists, and joint angle. +// */ +// virtual gtsam::Vector6 transformTwistAccelTo( +// const LinkSharedPtr &link, JointCoordinate q, JointVelocity q_dot, +// JointAcceleration q_ddot, +// boost::optional this_twist = boost::none, +// boost::optional other_twist_accel = boost::none, +// gtsam::OptionalJacobian<6, N> H_q = boost::none, +// gtsam::OptionalJacobian<6, N> H_q_dot = boost::none, +// gtsam::OptionalJacobian<6, N> H_q_ddot = boost::none, +// gtsam::OptionalJacobian<6, 6> H_this_twist = boost::none, +// gtsam::OptionalJacobian<6, 6> H_other_twist_accel = +// boost::none) const = 0; + +// /// Abstract method. Return the torque on this joint given the wrench +// virtual JointTorque transformWrenchToTorque( +// const LinkSharedPtr &link, +// boost::optional wrench = boost::none, +// gtsam::OptionalJacobian H_wrench = boost::none) const = 0; + +// /// Calculate AdjointMap jacobian w.r.t. joint coordinate q. +// /// TODO(gerry + stephanie): change to calculate the jacobian of Ad_T(v) wrt T +// /// rather than jacobian of Ad_T wrt q (and put in utils or PR to GTSAM) +// virtual gtsam::Matrix6 +// AdjointMapJacobianJointAngle(const LinkSharedPtr &link, +// JointCoordinate q) const = 0; + +// ///@} +// /** +// * @name generic +// * These are methods that can be implemented here in terms of abstract methods. +// */ +// ///@{ + +// /** +// * Return the relative pose of the specified link [link2] in the other link's +// * [link1] reference frame. +// */ +// Pose3 relativePoseOf(const LinkSharedPtr &link2, JointCoordinate q, +// gtsam::OptionalJacobian<6, 1> H_q = boost::none) const { +// return isChildLink(link2) ? parentTchild(q, H_q) : childTparent(q, H_q); +// } + +// /** +// * Return the world pose of the specified link [link2], given +// * the world pose of the other link [link1]. +// */ +// Pose3 poseOf(const LinkSharedPtr &link2, const Pose3 &wT1, JointCoordinate q, +// gtsam::OptionalJacobian<6, 6> H_wT1 = boost::none, +// gtsam::OptionalJacobian<6, N> H_q = boost::none) const { +// auto T12 = relativePoseOf(link2, q, H_q); +// return wT1.compose(T12, H_wT1); // H_wT2_T12 is identity +// } + +// ///@} +// /** +// * @name valueBased +// * Methods that extract coordinates/velocities from Values and pass on to +// * abstract methods above. +// */ +// ///@{ + +// /** +// * Return the pose of the child link CoM in the parent link CoM frame, given a +// * Values object containing the joint coordinate. +// */ +// Pose3 parentTchild( +// const gtsam::Values &q, size_t t = 0, +// boost::optional H_q = boost::none) const override { +// return parentTchild(JointAngle(q, id(), t), H_q); +// } + +// /** +// * Return the pose of the parent link in the child link frame, given a Values +// * object containing the joint coordinate. +// */ +// Pose3 childTparent( +// const gtsam::Values &q, size_t t = 0, +// boost::optional H_q = boost::none) const override { +// return childTparent(JointAngle(q, id(), t), H_q); +// } + +// /** +// * Return the relative pose of the specified link [link2] in +// * the other link's [link1] reference frame. +// * @throw KeyDoesNotExist if the appropriate key is missing from values +// */ +// Pose3 relativePoseOf( +// const LinkSharedPtr &link2, const gtsam::Values &q, size_t t = 0, +// boost::optional H_q = boost::none) const override { +// return relativePoseOf(link2, JointAngle(q, id(), t), H_q); +// } + +// /// Joint-induced twist in child frame +// gtsam::Vector6 childTwist(const gtsam::Values &values, +// size_t t = 0) const override { +// return childTwist(JointVel(values, id(), t)); +// } + +// /// Joint-induced twist in parent frame +// gtsam::Vector6 parentTwist(const gtsam::Values &values, +// size_t t = 0) const override { +// return parentTwist(JointVel(values, id(), t)); +// } + +// /** +// * Return the twist of this link given the other link's twist and a Values +// * object containing this joint's angle Value. +// * @param values containing q, q_dot +// * @throw ValuesKeyDoesNotExist if the appropriate key is missing from values +// */ +// gtsam::Vector6 transformTwistTo( +// size_t t, const LinkSharedPtr &link, const gtsam::Values &values, +// boost::optional other_twist = boost::none, +// boost::optional H_q = boost::none, +// boost::optional H_q_dot = boost::none, +// gtsam::OptionalJacobian<6, 6> H_other_twist = boost::none) // +// const override { +// return transformTwistTo(link, JointAngle(values, id(), t), +// JointVel(values, id(), t), +// other_twist, H_q, H_q_dot, H_other_twist); +// } + +// /** Return the twist acceleration of the other link given this link's twist +// * accel and a Values object containing this joint's angle value and +// * derivatives. +// * @param values containing q, q_dot, and q_ddot +// * @throw ValuesKeyDoesNotExist if the appropriate key is missing from values +// */ +// gtsam::Vector6 transformTwistAccelTo( +// size_t t, const LinkSharedPtr &link, const gtsam::Values &values, +// boost::optional this_twist = boost::none, +// boost::optional other_twist_accel = boost::none, +// boost::optional H_q = boost::none, +// boost::optional H_q_dot = boost::none, +// boost::optional H_q_ddot = boost::none, +// gtsam::OptionalJacobian<6, 6> H_this_twist = boost::none, +// gtsam::OptionalJacobian<6, 6> H_other_twist_accel = +// boost::none) const override { +// return transformTwistAccelTo(link, +// JointAngle(values, id(), t), +// JointVel(values, id(), t), +// JointAccel(values, id(), t), +// this_twist, other_twist_accel, H_q, H_q_dot, +// H_q_ddot, H_this_twist, H_other_twist_accel); +// } +// }; + +// } // namespace gtdynamics diff --git a/gtdynamics/universal_robot/PrismaticJoint.h b/gtdynamics/universal_robot/PrismaticJoint.h index 091c74dc..1375e712 100644 --- a/gtdynamics/universal_robot/PrismaticJoint.h +++ b/gtdynamics/universal_robot/PrismaticJoint.h @@ -7,7 +7,7 @@ /** * @file PrismaticJoint.h - * @brief Representation of PrismaticJoint that inherits from ScrewJointBase + * @brief Representation of PrismaticJoint that inherits from Joint * @author Frank Dellaert * @author Mandy Xie * @author Alejandro Escontrela @@ -18,16 +18,16 @@ #pragma once -#include "gtdynamics/universal_robot/ScrewJointBase.h" +#include "gtdynamics/universal_robot/Joint.h" namespace gtdynamics { /** - * @class PrismaticJoint is an implementation of the ScrewJointBase class - * which represents a prismatic joint and contains all necessary factor - * construction methods. + * @class PrismaticJoint is an implementation of the Joint class which + * represents a prismatic joint and contains all necessary factor construction + * methods. */ -class PrismaticJoint : public ScrewJointBase { +class PrismaticJoint : public Joint { protected: /// Returns the screw axis in the joint frame given the joint axis gtsam::Vector6 getScrewAxis(const gtsam::Vector3 &axis) { @@ -53,11 +53,11 @@ class PrismaticJoint : public ScrewJointBase { const LinkSharedPtr &parent_link, const LinkSharedPtr &child_link, const gtsam::Vector3 &axis, const JointParams ¶meters = JointParams()) - : ScrewJointBase(id, name, bTj, parent_link, child_link, axis, - getScrewAxis(axis), parameters) {} + : Joint(id, name, bTj, parent_link, child_link, getScrewAxis(axis), + parameters) {} /// Return joint type for use in reconstructing robot from JointParams. - Type type() const override { return Type::Prismatic; } + Type type() const final override { return Type::Prismatic; } }; } // namespace gtdynamics diff --git a/gtdynamics/universal_robot/RevoluteJoint.h b/gtdynamics/universal_robot/RevoluteJoint.h index d0be1aaa..1675bcea 100644 --- a/gtdynamics/universal_robot/RevoluteJoint.h +++ b/gtdynamics/universal_robot/RevoluteJoint.h @@ -18,16 +18,15 @@ #pragma once -#include "gtdynamics/universal_robot/ScrewJointBase.h" +#include "gtdynamics/universal_robot/Joint.h" namespace gtdynamics { /** - * @class RevoluteJoint is an implementation of the ScrewJointBase class - * which represents a revolute joint and contains all necessary factor - * construction methods. + * @class RevoluteJoint is an implementation of the Joint class which represents + * a revolute joint and contains all necessary factor construction methods. */ -class RevoluteJoint : public ScrewJointBase { +class RevoluteJoint : public Joint { protected: /// Returns the screw axis in the joint frame given the joint axis gtsam::Vector6 getScrewAxis(const gtsam::Vector3 &axis) { @@ -53,11 +52,11 @@ class RevoluteJoint : public ScrewJointBase { const LinkSharedPtr &parent_link, const LinkSharedPtr &child_link, const gtsam::Vector3 &axis, const JointParams ¶meters = JointParams()) - : ScrewJointBase(id, name, bTj, parent_link, child_link, axis, - getScrewAxis(axis), parameters) {} + : Joint(id, name, bTj, parent_link, child_link, getScrewAxis(axis), + parameters) {} /// Return joint type for use in reconstructing robot from JointParams. - Type type() const override { return Type::Revolute; } + Type type() const final override { return Type::Revolute; } }; } // namespace gtdynamics diff --git a/gtdynamics/universal_robot/Robot.cpp b/gtdynamics/universal_robot/Robot.cpp index a3aa2cc7..c26c6b25 100644 --- a/gtdynamics/universal_robot/Robot.cpp +++ b/gtdynamics/universal_robot/Robot.cpp @@ -145,10 +145,7 @@ void Robot::print(const std::string &s) const { for (const auto &joint : sorted_joints) { cout << joint << endl; - gtsam::Values joint_angles; - InsertJointAngle(&joint_angles, joint->id(), 0.0); - - auto pTc = joint->parentTchild(joint_angles); + auto pTc = joint->parentTchild(0.0); cout << "\tpMc: " << pTc.rotation().rpy().transpose() << ", " << pTc.translation().transpose() << "\n"; } @@ -259,7 +256,9 @@ gtsam::Values Robot::forwardKinematics( // Loop through all joints to find the pose and twist of child links. for (auto &&joint : link1->joints()) { InsertZeroDefaults(joint->id(), t, &values); - const auto poseTwist = joint->otherPoseTwist(link1, T_w1, V_1, values, t); + const auto poseTwist = joint->otherPoseTwist( + link1, T_w1, V_1, JointAngle(values, joint->id(), t), + JointVel(values, joint->id(), t)); const auto link2 = joint->otherLink(link1); if (InsertWithCheck(link2->id(), t, poseTwist, &values)) { q.push(link2); diff --git a/gtdynamics/universal_robot/ScrewJoint.h b/gtdynamics/universal_robot/ScrewJoint.h index 5601449f..a5df892f 100644 --- a/gtdynamics/universal_robot/ScrewJoint.h +++ b/gtdynamics/universal_robot/ScrewJoint.h @@ -18,7 +18,7 @@ #pragma once -#include "gtdynamics/universal_robot/ScrewJointBase.h" +#include "gtdynamics/universal_robot/Joint.h" namespace gtdynamics { @@ -27,7 +27,7 @@ namespace gtdynamics { * which represents a screw joint and contains all necessary factor * construction methods. */ -class ScrewJoint : public ScrewJointBase { +class ScrewJoint : public Joint { protected: /** * Returns the screw axis in the joint frame given the joint axis and thread @@ -57,11 +57,14 @@ class ScrewJoint : public ScrewJointBase { const LinkSharedPtr &parent_link, const LinkSharedPtr &child_link, const gtsam::Vector3 &axis, double thread_pitch, const JointParams ¶meters = JointParams()) - : ScrewJointBase(id, name, bTj, parent_link, child_link, axis, - getScrewAxis(axis, thread_pitch), parameters) {} + : Joint(id, name, bTj, parent_link, child_link, + getScrewAxis(axis, thread_pitch), parameters) {} + + /// Constructor directly from screwAxis + using Joint::Joint; /// Return joint type for use in reconstructing robot from JointParams. - Type type() const override { return Type::Screw; } + Type type() const final override { return Type::Screw; } }; } // namespace gtdynamics diff --git a/gtdynamics/universal_robot/ScrewJointBase.cpp b/gtdynamics/universal_robot/ScrewJointBase.cpp index ab43f658..fd9ac1c8 100644 --- a/gtdynamics/universal_robot/ScrewJointBase.cpp +++ b/gtdynamics/universal_robot/ScrewJointBase.cpp @@ -1,265 +1,265 @@ -/* ---------------------------------------------------------------------------- - * GTDynamics Copyright 2020, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * See LICENSE for the license information - * -------------------------------------------------------------------------- */ - -/** - * @file ScrewJointBase.cpp - * @brief Representation of screw-type robot joints. Revolute, Prismatic, and - * Screw subclasses - * @author Frank Dellaert - * @author Mandy Xie - * @author Alejandro Escontrela - * @author Yetong Zhang - * @author Stephanie McCormick - * @author Gerry Chen - * @author Varun Agrawal - */ - - -#include "gtdynamics/universal_robot/ScrewJointBase.h" - -#include "gtdynamics/factors/JointLimitFactor.h" -#include "gtdynamics/universal_robot/Link.h" -#include "gtdynamics/utils/utils.h" -#include "gtdynamics/utils/values.h" - -#include - -#include -#include -#include - -using namespace gtsam; - -namespace gtdynamics { - -/* ************************************************************************* */ -Pose3 ScrewJointBase::parentTchild( - double q, gtsam::OptionalJacobian<6, 1> pMc_H_q) const { - // Calculate pose of child in parent link, at rest. - // TODO(dellaert): store `pMj_` rather than `jMp_`. - // NOTE(dellaert): Only if this is called more often than childTparent. - const Pose3 pMc = jMp_.inverse() * jMc_; - - // Multiply screw axis with joint angle to get a finite 6D screw. - const Vector6 screw = cScrewAxis_ * q; - - // Calculate the actual relative pose taking into account the joint angle. - // TODO(dellaert): use formula `pMj_ * screw_around_Z * jMc_`. - gtsam::Matrix6 exp_H_screw; - const Pose3 exp = Pose3::Expmap(screw, pMc_H_q ? &exp_H_screw : 0); - if (pMc_H_q) { - *pMc_H_q = exp_H_screw * cScrewAxis_; - } - return pMc * exp; // Note: derivative of compose in exp is identity. -} - -/* ************************************************************************* */ -Pose3 ScrewJointBase::childTparent( - double q, gtsam::OptionalJacobian<6, 1> cMp_H_q) const { - // TODO(frank): don't go via inverse, specialize in base class - Vector6 pMc_H_q; - Pose3 pMc = parentTchild(q, cMp_H_q ? &pMc_H_q : 0); // pMc(q) -> pMc_H_q - gtsam::Matrix6 cMp_H_pMc; - Pose3 cMp = pMc.inverse(cMp_H_q ? &cMp_H_pMc : 0); // cMp(pMc) -> cMp_H_pMc - if (cMp_H_q) { - *cMp_H_q = cMp_H_pMc * pMc_H_q; - } - return cMp; -} - -/* ************************************************************************* */ -Vector6 ScrewJointBase::transformTwistTo( - const LinkSharedPtr &link, double q, double q_dot, - boost::optional other_twist, - gtsam::OptionalJacobian<6, 1> H_q, - gtsam::OptionalJacobian<6, 1> H_q_dot, - gtsam::OptionalJacobian<6, 6> H_other_twist) const { - Vector6 other_twist_ = other_twist ? *other_twist : Vector6::Zero(); - - auto other = otherLink(link); - auto this_ad_other = relativePoseOf(other, q).AdjointMap(); - - if (H_q) { - // TODO(frank): really, zero below? Check derivatives - *H_q = AdjointMapJacobianQ(q, relativePoseOf(other, 0.0), screwAxis(link)) * - other_twist_; - } - if (H_q_dot) { - *H_q_dot = screwAxis(link); - } - if (H_other_twist) { - *H_other_twist = this_ad_other; - } - - return this_ad_other * other_twist_ + screwAxis(link) * q_dot; -} - -/* ************************************************************************* */ -Vector6 ScrewJointBase::transformTwistAccelTo( - const LinkSharedPtr &link, double q, double q_dot, double q_ddot, - boost::optional this_twist, - boost::optional other_twist_accel, - gtsam::OptionalJacobian<6, 1> H_q, - gtsam::OptionalJacobian<6, 1> H_q_dot, - gtsam::OptionalJacobian<6, 1> H_q_ddot, - gtsam::OptionalJacobian<6, 6> H_this_twist, - gtsam::OptionalJacobian<6, 6> H_other_twist_accel) const { - Vector6 this_twist_ = this_twist ? *this_twist : Vector6::Zero(); - Vector6 other_twist_accel_ = - other_twist_accel ? *other_twist_accel : Vector6::Zero(); - Vector6 screw_axis_ = isChildLink(link) ? cScrewAxis_ : pScrewAxis_; - - // i = other link - // j = this link - auto other = otherLink(link); - Pose3 jTi = relativePoseOf(other, q); - - Vector6 this_twist_accel = - jTi.AdjointMap() * other_twist_accel_ + - Pose3::adjoint(this_twist_, screw_axis_ * q_dot, H_this_twist) + - screw_axis_ * q_ddot; - - if (H_other_twist_accel) { - *H_other_twist_accel = jTi.AdjointMap(); - } - if (H_q) { - // TODO(frank): really, zero below? Check derivatives. Also, copy/pasta from - // above? - *H_q = AdjointMapJacobianQ(q, relativePoseOf(other, 0.0), screw_axis_) * - other_twist_accel_; - } - if (H_q_dot) { - *H_q_dot = Pose3::adjointMap(this_twist_) * screw_axis_; - } - if (H_q_ddot) { - *H_q_ddot = screw_axis_; - } - - return this_twist_accel; -} - -/* ************************************************************************* */ -ScrewJointBase::JointTorque ScrewJointBase::transformWrenchToTorque( - const LinkSharedPtr &link, boost::optional wrench, - gtsam::OptionalJacobian<1, 6> H_wrench) const { - auto screw_axis_ = screwAxis(link); - if (H_wrench) { - *H_wrench = screw_axis_.transpose(); - } - return screw_axis_.transpose() * (wrench ? *wrench : Vector6::Zero()); -} - -/* ************************************************************************* */ -gtsam::GaussianFactorGraph ScrewJointBase::linearFDPriors( - size_t t, const gtsam::Values &known_values, - const OptimizerSetting &opt) const { - gtsam::GaussianFactorGraph priors; - gtsam::Vector1 rhs(Torque(known_values, id(), t)); - // TODO(alej`andro): use optimizer settings - priors.add(internal::TorqueKey(id(), t), gtsam::I_1x1, rhs, - gtsam::noiseModel::Constrained::All(1)); - return priors; -} - -/* ************************************************************************* */ -gtsam::GaussianFactorGraph ScrewJointBase::linearAFactors( - size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, - const boost::optional &planar_axis) const { - gtsam::GaussianFactorGraph graph; - - const Pose3 T_wi1 = Pose(known_values, parent()->id(), t); - const Pose3 T_wi2 = Pose(known_values, child()->id(), t); - const Pose3 T_i2i1 = T_wi2.inverse() * T_wi1; - const Vector6 V_i2 = Twist(known_values, child()->id(), t); - const Vector6 S_i2_j = screwAxis(child_link_); - const double v_j = JointVel(known_values, id(), t); - - // twist acceleration factor - // A_i2 - Ad(T_21) * A_i1 - S_i2_j * a_j = ad(V_i2) * S_i2_j * v_j - Vector6 rhs_tw = Pose3::adjointMap(V_i2) * S_i2_j * v_j; - graph.add(internal::TwistAccelKey(child()->id(), t), gtsam::I_6x6, - internal::TwistAccelKey(parent()->id(), t), -T_i2i1.AdjointMap(), - internal::JointAccelKey(id(), t), -S_i2_j, rhs_tw, - gtsam::noiseModel::Constrained::All(6)); - - return graph; -} - -/* ************************************************************************* */ -gtsam::GaussianFactorGraph ScrewJointBase::linearDynamicsFactors( - size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, - const boost::optional &planar_axis) const { - gtsam::GaussianFactorGraph graph; - - const Pose3 T_wi1 = Pose(known_values, parent()->id(), t); - const Pose3 T_wi2 = Pose(known_values, child()->id(), t); - const Pose3 T_i2i1 = T_wi2.inverse() * T_wi1; - const Vector6 S_i2_j = screwAxis(child_link_); - - // torque factor - // S_i_j^T * F_i_j - tau = 0 - gtsam::Vector1 rhs_torque = gtsam::Vector1::Zero(); - graph.add(internal::WrenchKey(child()->id(), id(), t), S_i2_j.transpose(), - internal::TorqueKey(id(), t), -gtsam::I_1x1, rhs_torque, - gtsam::noiseModel::Constrained::All(1)); - - // wrench equivalence factor - // F_i1_j + Ad(T_i2i1)^T F_i2_j = 0 - Vector6 rhs_weq = Vector6::Zero(); - graph.add(internal::WrenchKey(parent()->id(), id(), t), gtsam::I_6x6, - internal::WrenchKey(child()->id(), id(), t), - T_i2i1.AdjointMap().transpose(), rhs_weq, - gtsam::noiseModel::Constrained::All(6)); - - // wrench planar factor - if (planar_axis) { - gtsam::Matrix36 J_wrench = getPlanarJacobian(*planar_axis); - graph.add(internal::WrenchKey(child()->id(), id(), t), J_wrench, - gtsam::Vector3::Zero(), gtsam::noiseModel::Constrained::All(3)); - } - - return graph; -} - -/* ************************************************************************* */ -gtsam::NonlinearFactorGraph ScrewJointBase::jointLimitFactors( - size_t t, const OptimizerSetting &opt) const { - gtsam::NonlinearFactorGraph graph; - auto id = this->id(); - // Add joint angle limit factor. - graph.emplace_shared( - internal::JointAngleKey(id, t), opt.jl_cost_model, - parameters().scalar_limits.value_lower_limit, - parameters().scalar_limits.value_upper_limit, - parameters().scalar_limits.value_limit_threshold); - - // Add joint velocity limit factors. - graph.emplace_shared( - internal::JointVelKey(id, t), opt.jl_cost_model, - -parameters().velocity_limit, parameters().velocity_limit, - parameters().velocity_limit_threshold); - - // Add joint acceleration limit factors. - graph.emplace_shared( - internal::JointAccelKey(id, t), opt.jl_cost_model, - -parameters().acceleration_limit, parameters().acceleration_limit, - parameters().acceleration_limit_threshold); - - // Add joint torque limit factors. - graph.emplace_shared( - internal::TorqueKey(id, t), opt.jl_cost_model, -parameters().torque_limit, - parameters().torque_limit, parameters().torque_limit_threshold); - return graph; -} - -std::ostream &ScrewJointBase::to_stream(std::ostream &os) const { - Joint::to_stream(os); - os << "\n\tscrew axis (parent): " << screwAxis(parent()).transpose(); - return os; -} - -} // namespace gtdynamics +// /* ---------------------------------------------------------------------------- +// * GTDynamics Copyright 2020, Georgia Tech Research Corporation, +// * Atlanta, Georgia 30332-0415 +// * All Rights Reserved +// * See LICENSE for the license information +// * -------------------------------------------------------------------------- */ + +// /** +// * @file ScrewJointBase.cpp +// * @brief Representation of screw-type robot joints. Revolute, Prismatic, and +// * Screw subclasses +// * @author Frank Dellaert +// * @author Mandy Xie +// * @author Alejandro Escontrela +// * @author Yetong Zhang +// * @author Stephanie McCormick +// * @author Gerry Chen +// * @author Varun Agrawal +// */ + + +// #include "gtdynamics/universal_robot/ScrewJointBase.h" + +// #include "gtdynamics/factors/JointLimitFactor.h" +// #include "gtdynamics/universal_robot/Link.h" +// #include "gtdynamics/utils/utils.h" +// #include "gtdynamics/utils/values.h" + +// #include + +// #include +// #include +// #include + +// using namespace gtsam; + +// namespace gtdynamics { + +// /* ************************************************************************* */ +// Pose3 ScrewJointBase::parentTchild( +// double q, gtsam::OptionalJacobian<6, 1> pMc_H_q) const { +// // Calculate pose of child in parent link, at rest. +// // TODO(dellaert): store `pMj_` rather than `jMp_`. +// // NOTE(dellaert): Only if this is called more often than childTparent. +// const Pose3 pMc = jMp_.inverse() * jMc_; + +// // Multiply screw axis with joint angle to get a finite 6D screw. +// const Vector6 screw = cScrewAxis_ * q; + +// // Calculate the actual relative pose taking into account the joint angle. +// // TODO(dellaert): use formula `pMj_ * screw_around_Z * jMc_`. +// gtsam::Matrix6 exp_H_screw; +// const Pose3 exp = Pose3::Expmap(screw, pMc_H_q ? &exp_H_screw : 0); +// if (pMc_H_q) { +// *pMc_H_q = exp_H_screw * cScrewAxis_; +// } +// return pMc * exp; // Note: derivative of compose in exp is identity. +// } + +// /* ************************************************************************* */ +// Pose3 ScrewJointBase::childTparent( +// double q, gtsam::OptionalJacobian<6, 1> cMp_H_q) const { +// // TODO(frank): don't go via inverse, specialize in base class +// Vector6 pMc_H_q; +// Pose3 pMc = parentTchild(q, cMp_H_q ? &pMc_H_q : 0); // pMc(q) -> pMc_H_q +// gtsam::Matrix6 cMp_H_pMc; +// Pose3 cMp = pMc.inverse(cMp_H_q ? &cMp_H_pMc : 0); // cMp(pMc) -> cMp_H_pMc +// if (cMp_H_q) { +// *cMp_H_q = cMp_H_pMc * pMc_H_q; +// } +// return cMp; +// } + +// /* ************************************************************************* */ +// Vector6 ScrewJointBase::transformTwistTo( +// const LinkSharedPtr &link, double q, double q_dot, +// boost::optional other_twist, +// gtsam::OptionalJacobian<6, 1> H_q, +// gtsam::OptionalJacobian<6, 1> H_q_dot, +// gtsam::OptionalJacobian<6, 6> H_other_twist) const { +// Vector6 other_twist_ = other_twist ? *other_twist : Vector6::Zero(); + +// auto other = otherLink(link); +// auto this_ad_other = relativePoseOf(other, q).AdjointMap(); + +// if (H_q) { +// // TODO(frank): really, zero below? Check derivatives +// *H_q = AdjointMapJacobianQ(q, relativePoseOf(other, 0.0), screwAxis(link)) * +// other_twist_; +// } +// if (H_q_dot) { +// *H_q_dot = screwAxis(link); +// } +// if (H_other_twist) { +// *H_other_twist = this_ad_other; +// } + +// return this_ad_other * other_twist_ + screwAxis(link) * q_dot; +// } + +// /* ************************************************************************* */ +// Vector6 ScrewJointBase::transformTwistAccelTo( +// const LinkSharedPtr &link, double q, double q_dot, double q_ddot, +// boost::optional this_twist, +// boost::optional other_twist_accel, +// gtsam::OptionalJacobian<6, 1> H_q, +// gtsam::OptionalJacobian<6, 1> H_q_dot, +// gtsam::OptionalJacobian<6, 1> H_q_ddot, +// gtsam::OptionalJacobian<6, 6> H_this_twist, +// gtsam::OptionalJacobian<6, 6> H_other_twist_accel) const { +// Vector6 this_twist_ = this_twist ? *this_twist : Vector6::Zero(); +// Vector6 other_twist_accel_ = +// other_twist_accel ? *other_twist_accel : Vector6::Zero(); +// Vector6 screw_axis_ = isChildLink(link) ? cScrewAxis_ : pScrewAxis_; + +// // i = other link +// // j = this link +// auto other = otherLink(link); +// Pose3 jTi = relativePoseOf(other, q); + +// Vector6 this_twist_accel = +// jTi.AdjointMap() * other_twist_accel_ + +// Pose3::adjoint(this_twist_, screw_axis_ * q_dot, H_this_twist) + +// screw_axis_ * q_ddot; + +// if (H_other_twist_accel) { +// *H_other_twist_accel = jTi.AdjointMap(); +// } +// if (H_q) { +// // TODO(frank): really, zero below? Check derivatives. Also, copy/pasta from +// // above? +// *H_q = AdjointMapJacobianQ(q, relativePoseOf(other, 0.0), screw_axis_) * +// other_twist_accel_; +// } +// if (H_q_dot) { +// *H_q_dot = Pose3::adjointMap(this_twist_) * screw_axis_; +// } +// if (H_q_ddot) { +// *H_q_ddot = screw_axis_; +// } + +// return this_twist_accel; +// } + +// /* ************************************************************************* */ +// ScrewJointBase::JointTorque ScrewJointBase::transformWrenchToTorque( +// const LinkSharedPtr &link, boost::optional wrench, +// gtsam::OptionalJacobian<1, 6> H_wrench) const { +// auto screw_axis_ = screwAxis(link); +// if (H_wrench) { +// *H_wrench = screw_axis_.transpose(); +// } +// return screw_axis_.transpose() * (wrench ? *wrench : Vector6::Zero()); +// } + +// /* ************************************************************************* */ +// gtsam::GaussianFactorGraph ScrewJointBase::linearFDPriors( +// size_t t, const gtsam::Values &known_values, +// const OptimizerSetting &opt) const { +// gtsam::GaussianFactorGraph priors; +// gtsam::Vector1 rhs(Torque(known_values, id(), t)); +// // TODO(alej`andro): use optimizer settings +// priors.add(internal::TorqueKey(id(), t), gtsam::I_1x1, rhs, +// gtsam::noiseModel::Constrained::All(1)); +// return priors; +// } + +// /* ************************************************************************* */ +// gtsam::GaussianFactorGraph ScrewJointBase::linearAFactors( +// size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, +// const boost::optional &planar_axis) const { +// gtsam::GaussianFactorGraph graph; + +// const Pose3 T_wi1 = Pose(known_values, parent()->id(), t); +// const Pose3 T_wi2 = Pose(known_values, child()->id(), t); +// const Pose3 T_i2i1 = T_wi2.inverse() * T_wi1; +// const Vector6 V_i2 = Twist(known_values, child()->id(), t); +// const Vector6 S_i2_j = screwAxis(child_link_); +// const double v_j = JointVel(known_values, id(), t); + +// // twist acceleration factor +// // A_i2 - Ad(T_21) * A_i1 - S_i2_j * a_j = ad(V_i2) * S_i2_j * v_j +// Vector6 rhs_tw = Pose3::adjointMap(V_i2) * S_i2_j * v_j; +// graph.add(internal::TwistAccelKey(child()->id(), t), gtsam::I_6x6, +// internal::TwistAccelKey(parent()->id(), t), -T_i2i1.AdjointMap(), +// internal::JointAccelKey(id(), t), -S_i2_j, rhs_tw, +// gtsam::noiseModel::Constrained::All(6)); + +// return graph; +// } + +// /* ************************************************************************* */ +// gtsam::GaussianFactorGraph ScrewJointBase::linearDynamicsFactors( +// size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, +// const boost::optional &planar_axis) const { +// gtsam::GaussianFactorGraph graph; + +// const Pose3 T_wi1 = Pose(known_values, parent()->id(), t); +// const Pose3 T_wi2 = Pose(known_values, child()->id(), t); +// const Pose3 T_i2i1 = T_wi2.inverse() * T_wi1; +// const Vector6 S_i2_j = screwAxis(child_link_); + +// // torque factor +// // S_i_j^T * F_i_j - tau = 0 +// gtsam::Vector1 rhs_torque = gtsam::Vector1::Zero(); +// graph.add(internal::WrenchKey(child()->id(), id(), t), S_i2_j.transpose(), +// internal::TorqueKey(id(), t), -gtsam::I_1x1, rhs_torque, +// gtsam::noiseModel::Constrained::All(1)); + +// // wrench equivalence factor +// // F_i1_j + Ad(T_i2i1)^T F_i2_j = 0 +// Vector6 rhs_weq = Vector6::Zero(); +// graph.add(internal::WrenchKey(parent()->id(), id(), t), gtsam::I_6x6, +// internal::WrenchKey(child()->id(), id(), t), +// T_i2i1.AdjointMap().transpose(), rhs_weq, +// gtsam::noiseModel::Constrained::All(6)); + +// // wrench planar factor +// if (planar_axis) { +// gtsam::Matrix36 J_wrench = getPlanarJacobian(*planar_axis); +// graph.add(internal::WrenchKey(child()->id(), id(), t), J_wrench, +// gtsam::Vector3::Zero(), gtsam::noiseModel::Constrained::All(3)); +// } + +// return graph; +// } + +// /* ************************************************************************* */ +// gtsam::NonlinearFactorGraph ScrewJointBase::jointLimitFactors( +// size_t t, const OptimizerSetting &opt) const { +// gtsam::NonlinearFactorGraph graph; +// auto id = this->id(); +// // Add joint angle limit factor. +// graph.emplace_shared( +// internal::JointAngleKey(id, t), opt.jl_cost_model, +// parameters().scalar_limits.value_lower_limit, +// parameters().scalar_limits.value_upper_limit, +// parameters().scalar_limits.value_limit_threshold); + +// // Add joint velocity limit factors. +// graph.emplace_shared( +// internal::JointVelKey(id, t), opt.jl_cost_model, +// -parameters().velocity_limit, parameters().velocity_limit, +// parameters().velocity_limit_threshold); + +// // Add joint acceleration limit factors. +// graph.emplace_shared( +// internal::JointAccelKey(id, t), opt.jl_cost_model, +// -parameters().acceleration_limit, parameters().acceleration_limit, +// parameters().acceleration_limit_threshold); + +// // Add joint torque limit factors. +// graph.emplace_shared( +// internal::TorqueKey(id, t), opt.jl_cost_model, -parameters().torque_limit, +// parameters().torque_limit, parameters().torque_limit_threshold); +// return graph; +// } + +// std::ostream &ScrewJointBase::to_stream(std::ostream &os) const { +// Joint::to_stream(os); +// os << "\n\tscrew axis (parent): " << screwAxis(parent()).transpose(); +// return os; +// } + +// } // namespace gtdynamics diff --git a/gtdynamics/universal_robot/ScrewJointBase.h b/gtdynamics/universal_robot/ScrewJointBase.h index d7e7501a..f0e689c4 100644 --- a/gtdynamics/universal_robot/ScrewJointBase.h +++ b/gtdynamics/universal_robot/ScrewJointBase.h @@ -1,166 +1,166 @@ -/* ---------------------------------------------------------------------------- - * GTDynamics Copyright 2020, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * See LICENSE for the license information - * -------------------------------------------------------------------------- */ - -/** - * @file ScrewJointBase.h - * @brief Representation of screw-type robot joints. Revolute, Prismatic, and - * Screw subclasses - * @author Frank Dellaert - * @author Mandy Xie - * @author Alejandro Escontrela - * @author Yetong Zhang - * @author Stephanie McCormick - * @author Gerry Chen - * @author Varun Agrawal - */ - -#pragma once - -#include - -#include -#include -#include - -#include "gtdynamics/factors/JointLimitFactor.h" -#include "gtdynamics/universal_robot/JointTyped.h" -#include "gtdynamics/utils/utils.h" -#include "gtdynamics/utils/values.h" - -namespace gtdynamics { -/** - * @class ScrewJointBase is an implementation of the abstract Joint class - * which represents a screw-type joint and contains all necessary factor - * construction methods. - * It is the base class for RevoluteJoint, PrismaticJoint, and ScrewJoint. - */ -class ScrewJointBase : public JointTyped { - using Pose3 = gtsam::Pose3; - using Vector6 = gtsam::Vector6; - - protected: - gtsam::Vector3 axis_; - - // Screw axis in parent and child CoM frames. - Vector6 pScrewAxis_; - Vector6 cScrewAxis_; - - public: - /// Return transform of child link CoM frame w.r.t parent link CoM frame - Pose3 parentTchild(double q, gtsam::OptionalJacobian<6, 1> pMc_H_q = - boost::none) const override; - - protected: - /// Return transform of parent link CoM frame w.r.t child link CoM frame - Pose3 childTparent(double q, gtsam::OptionalJacobian<6, 1> cMp_H_q = - boost::none) const override; - - /** - * Return the joint axis in the joint frame. Rotational axis for revolute and - * translation direction for prismatic in the joint frame. - */ - const gtsam::Vector3 &axis() const { return axis_; } - - public: - /** - * Constructor using JointParams, joint name, bTj, screw axes, - * and parent and child links. - */ - ScrewJointBase(uint8_t id, const std::string &name, const Pose3 &bTj, - const LinkSharedPtr &parent_link, - const LinkSharedPtr &child_link, const gtsam::Vector3 &axis, - const Vector6 &jScrewAxis, - const JointParams ¶meters = JointParams()) - : JointTyped(id, name, bTj, parent_link, child_link, parameters), - axis_(axis), - pScrewAxis_(-jMp_.inverse().AdjointMap() * jScrewAxis), - cScrewAxis_(jMc_.inverse().AdjointMap() * jScrewAxis) {} - - /// Return joint type for use in reconstructing robot from JointParams. - Type type() const override { return Type::ScrewAxis; } - - /// Return screw axis expressed in the specified link frame - const Vector6 screwAxis(const LinkSharedPtr &link) const { - return isChildLink(link) ? cScrewAxis_ : pScrewAxis_; - } - - // inherit overloads - using JointTyped::poseOf; - using JointTyped::relativePoseOf; - using JointTyped::transformTwistAccelTo; - using JointTyped::transformTwistTo; - - /** - * Return the twist of this link given the other link's twist and joint angle. - */ - Vector6 transformTwistTo( - const LinkSharedPtr &link, double q, double q_dot, - boost::optional other_twist = boost::none, - gtsam::OptionalJacobian<6, 1> H_q = boost::none, - gtsam::OptionalJacobian<6, 1> H_q_dot = boost::none, - gtsam::OptionalJacobian<6, 6> H_other_twist = boost::none) const override; - - /** - * Return the twist acceleration of this link given the other link's twist - * acceleration, twist, and joint angle and this link's twist. - */ - Vector6 transformTwistAccelTo( - const LinkSharedPtr &link, double q, double q_dot, double q_ddot, - boost::optional this_twist = boost::none, - boost::optional other_twist_accel = boost::none, - gtsam::OptionalJacobian<6, 1> H_q = boost::none, - gtsam::OptionalJacobian<6, 1> H_q_dot = boost::none, - gtsam::OptionalJacobian<6, 1> H_q_ddot = boost::none, - gtsam::OptionalJacobian<6, 6> H_this_twist = boost::none, - gtsam::OptionalJacobian<6, 6> H_other_twist_accel = - boost::none) const override; - - JointTorque transformWrenchToTorque( - const LinkSharedPtr &link, boost::optional wrench = boost::none, - gtsam::OptionalJacobian<1, 6> H_wrench = boost::none) const override; - - // TODO(frank): document and possibly eliminate - gtsam::Matrix6 AdjointMapJacobianJointAngle(const LinkSharedPtr &link, - double q) const override { - return AdjointMapJacobianQ(q, relativePoseOf(otherLink(link), 0), - screwAxis(link)); - } - - /// Return forward dynamics priors on torque. - gtsam::GaussianFactorGraph linearFDPriors( - size_t t, const gtsam::Values &known_values, - const OptimizerSetting &opt) const override; - - /// Return linearized acceleration factors. - gtsam::GaussianFactorGraph linearAFactors( - size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, - const boost::optional &planar_axis) const override; - - /// Return linearized dynamics factors. - gtsam::GaussianFactorGraph linearDynamicsFactors( - size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, - const boost::optional &planar_axis) const override; - - /// Return joint limit factors. - gtsam::NonlinearFactorGraph jointLimitFactors( - size_t t, const OptimizerSetting &opt) const override; - - /// Joint-induced twist in child frame - gtsam::Vector6 childTwist(double q_dot) const override { - return cScrewAxis_ * q_dot; - } - - /// Joint-induced twist in parent frame - gtsam::Vector6 parentTwist(double q_dot) const override { - return pScrewAxis_ * q_dot; - } - - /// Helper function for overloading stream operator - virtual std::ostream &to_stream(std::ostream &os) const override; -}; - -} // namespace gtdynamics +// /* ---------------------------------------------------------------------------- +// * GTDynamics Copyright 2020, Georgia Tech Research Corporation, +// * Atlanta, Georgia 30332-0415 +// * All Rights Reserved +// * See LICENSE for the license information +// * -------------------------------------------------------------------------- */ + +// /** +// * @file ScrewJointBase.h +// * @brief Representation of screw-type robot joints. Revolute, Prismatic, and +// * Screw subclasses +// * @author Frank Dellaert +// * @author Mandy Xie +// * @author Alejandro Escontrela +// * @author Yetong Zhang +// * @author Stephanie McCormick +// * @author Gerry Chen +// * @author Varun Agrawal +// */ + +// #pragma once + +// #include + +// #include +// #include +// #include + +// #include "gtdynamics/factors/JointLimitFactor.h" +// #include "gtdynamics/universal_robot/JointTyped.h" +// #include "gtdynamics/utils/utils.h" +// #include "gtdynamics/utils/values.h" + +// namespace gtdynamics { +// /** +// * @class ScrewJointBase is an implementation of the abstract Joint class +// * which represents a screw-type joint and contains all necessary factor +// * construction methods. +// * It is the base class for RevoluteJoint, PrismaticJoint, and ScrewJoint. +// */ +// class ScrewJointBase : public Joint { +// using Pose3 = gtsam::Pose3; +// using Vector6 = gtsam::Vector6; + +// protected: +// gtsam::Vector3 axis_; + +// // Screw axis in parent and child CoM frames. +// Vector6 pScrewAxis_; +// Vector6 cScrewAxis_; + +// public: +// /// Return transform of child link CoM frame w.r.t parent link CoM frame +// Pose3 parentTchild(double q, gtsam::OptionalJacobian<6, 1> pMc_H_q = +// boost::none) const override; + +// protected: +// /// Return transform of parent link CoM frame w.r.t child link CoM frame +// Pose3 childTparent(double q, gtsam::OptionalJacobian<6, 1> cMp_H_q = +// boost::none) const override; + +// /** +// * Return the joint axis in the joint frame. Rotational axis for revolute and +// * translation direction for prismatic in the joint frame. +// */ +// const gtsam::Vector3 &axis() const { return axis_; } + +// public: +// /** +// * Constructor using JointParams, joint name, bTj, screw axes, +// * and parent and child links. +// */ +// ScrewJointBase(uint8_t id, const std::string &name, const Pose3 &bTj, +// const LinkSharedPtr &parent_link, +// const LinkSharedPtr &child_link, const gtsam::Vector3 &axis, +// const Vector6 &jScrewAxis, +// const JointParams ¶meters = JointParams()) +// : JointTyped(id, name, bTj, parent_link, child_link, parameters), +// axis_(axis), +// pScrewAxis_(-jMp_.inverse().AdjointMap() * jScrewAxis), +// cScrewAxis_(jMc_.inverse().AdjointMap() * jScrewAxis) {} + +// /// Return joint type for use in reconstructing robot from JointParams. +// Type type() const override { return Type::ScrewAxis; } + +// /// Return screw axis expressed in the specified link frame +// const Vector6 screwAxis(const LinkSharedPtr &link) const { +// return isChildLink(link) ? cScrewAxis_ : pScrewAxis_; +// } + +// // inherit overloads +// using JointTyped::poseOf; +// using JointTyped::relativePoseOf; +// using JointTyped::transformTwistAccelTo; +// using JointTyped::transformTwistTo; + +// /** +// * Return the twist of this link given the other link's twist and joint angle. +// */ +// Vector6 transformTwistTo( +// const LinkSharedPtr &link, double q, double q_dot, +// boost::optional other_twist = boost::none, +// gtsam::OptionalJacobian<6, 1> H_q = boost::none, +// gtsam::OptionalJacobian<6, 1> H_q_dot = boost::none, +// gtsam::OptionalJacobian<6, 6> H_other_twist = boost::none) const override; + +// /** +// * Return the twist acceleration of this link given the other link's twist +// * acceleration, twist, and joint angle and this link's twist. +// */ +// Vector6 transformTwistAccelTo( +// const LinkSharedPtr &link, double q, double q_dot, double q_ddot, +// boost::optional this_twist = boost::none, +// boost::optional other_twist_accel = boost::none, +// gtsam::OptionalJacobian<6, 1> H_q = boost::none, +// gtsam::OptionalJacobian<6, 1> H_q_dot = boost::none, +// gtsam::OptionalJacobian<6, 1> H_q_ddot = boost::none, +// gtsam::OptionalJacobian<6, 6> H_this_twist = boost::none, +// gtsam::OptionalJacobian<6, 6> H_other_twist_accel = +// boost::none) const override; + +// JointTorque transformWrenchToTorque( +// const LinkSharedPtr &link, boost::optional wrench = boost::none, +// gtsam::OptionalJacobian<1, 6> H_wrench = boost::none) const override; + +// // TODO(frank): document and possibly eliminate +// gtsam::Matrix6 AdjointMapJacobianJointAngle(const LinkSharedPtr &link, +// double q) const override { +// return AdjointMapJacobianQ(q, relativePoseOf(otherLink(link), 0), +// screwAxis(link)); +// } + +// /// Return forward dynamics priors on torque. +// gtsam::GaussianFactorGraph linearFDPriors( +// size_t t, const gtsam::Values &known_values, +// const OptimizerSetting &opt) const override; + +// /// Return linearized acceleration factors. +// gtsam::GaussianFactorGraph linearAFactors( +// size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, +// const boost::optional &planar_axis) const override; + +// /// Return linearized dynamics factors. +// gtsam::GaussianFactorGraph linearDynamicsFactors( +// size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, +// const boost::optional &planar_axis) const override; + +// /// Return joint limit factors. +// gtsam::NonlinearFactorGraph jointLimitFactors( +// size_t t, const OptimizerSetting &opt) const override; + +// /// Joint-induced twist in child frame +// gtsam::Vector6 childTwist(double q_dot) const override { +// return cScrewAxis_ * q_dot; +// } + +// /// Joint-induced twist in parent frame +// gtsam::Vector6 parentTwist(double q_dot) const override { +// return pScrewAxis_ * q_dot; +// } + +// /// Helper function for overloading stream operator +// virtual std::ostream &to_stream(std::ostream &os) const override; +// }; + +// } // namespace gtdynamics diff --git a/gtdynamics/utils/JsonSaver.h b/gtdynamics/utils/JsonSaver.h index 6dab8339..a77c4eb4 100644 --- a/gtdynamics/utils/JsonSaver.h +++ b/gtdynamics/utils/JsonSaver.h @@ -201,7 +201,7 @@ class JsonSaver { std::stringstream ss; if (const TorqueFactor* f = dynamic_cast(&(*factor))) { auto joint = f->getJoint(); - ss << GetVector(boost::static_pointer_cast(joint) + ss << GetVector(boost::static_pointer_cast(joint) ->screwAxis(joint->child()) .transpose()); } else if (const gtsam::PriorFactor* f = diff --git a/tests/make_joint.h b/tests/make_joint.h index 1c7d2e6d..6fa27159 100644 --- a/tests/make_joint.h +++ b/tests/make_joint.h @@ -14,12 +14,11 @@ #pragma once #include "gtdynamics/universal_robot/Link.h" -#include "gtdynamics/universal_robot/ScrewJointBase.h" +#include "gtdynamics/universal_robot/ScrewJoint.h" namespace gtdynamics { /// Create a joint with given rest transform cMp and screw-axis in child frame. -boost::shared_ptr make_joint(gtsam::Pose3 cMp, - gtsam::Vector6 cScrewAxis) { +JointConstSharedPtr make_joint(gtsam::Pose3 cMp, gtsam::Vector6 cScrewAxis) { // create links std::string name = "l1"; double mass = 100; @@ -41,7 +40,7 @@ boost::shared_ptr make_joint(gtsam::Pose3 cMp, gtsam::Pose3 jMc = bMj.inverse() * l2->bMcom(); gtsam::Vector6 jScrewAxis = jMc.AdjointMap() * cScrewAxis; - return boost::make_shared(ScrewJointBase( - 1, "j1", bMj, l1, l2, jScrewAxis.head<3>(), jScrewAxis, joint_params)); + return boost::make_shared( + 1, "j1", bMj, l1, l2, jScrewAxis, joint_params); } } // namespace gtdynamics diff --git a/tests/testJointMeasurementFactor.cpp b/tests/testJointMeasurementFactor.cpp index 678b5668..8f413c18 100644 --- a/tests/testJointMeasurementFactor.cpp +++ b/tests/testJointMeasurementFactor.cpp @@ -32,9 +32,6 @@ using namespace gtdynamics; using namespace gtsam; using gtsam::assert_equal; -const Key key0 = gtdynamics::internal::PoseKey(0), - key1 = gtdynamics::internal::PoseKey(1); - auto kModel = noiseModel::Isotropic::Sigma(6, 0.1); auto robot = simple_rr::getRobot(); @@ -42,13 +39,11 @@ size_t t = 0; // Test should not throw an exception. TEST(JointMeasurementFactor, Constructor) { - JointMeasurementFactor(key0, key1, kModel, robot.joints()[0], - 0.0, t); + JointMeasurementFactor(kModel, robot.joints()[0], 0.0, t); } TEST(JointMeasurementFactor, Error) { - JointMeasurementFactor factor(key0, key1, kModel, - robot.joints()[0], 0.0, t); + JointMeasurementFactor factor(kModel, robot.joints()[0], 0.0, t); auto link0 = robot.links()[0]; auto link1 = robot.links()[1]; @@ -58,8 +53,7 @@ TEST(JointMeasurementFactor, Error) { EXPECT(assert_equal(Vector::Zero(6), error, 1e-9)); // Error when the elbow is bent to 90 degrees - JointMeasurementFactor factor2(key0, key1, kModel, - robot.joints()[0], M_PI, t); + JointMeasurementFactor factor2(kModel, robot.joints()[0], M_PI, t); Pose3 wTl1(Rot3::Rz(M_PI), gtsam::Point3(0, 0, 0.5)); Vector error2 = factor2.evaluateError(link0->bMcom(), wTl1); @@ -68,8 +62,7 @@ TEST(JointMeasurementFactor, Error) { } TEST(JointMeasurementFactor, Jacobians) { - JointMeasurementFactor factor(key0, key1, kModel, - robot.joints()[0], 0.0, t); + JointMeasurementFactor factor(kModel, robot.joints()[0], 0.0, t); auto link0 = robot.links()[0]; auto link1 = robot.links()[1]; @@ -85,8 +78,7 @@ TEST(JointMeasurementFactor, Jacobians) { // Non-trivial joint angle double angle = M_PI; - JointMeasurementFactor factor2(key0, key1, kModel, - robot.joints()[0], angle, t); + JointMeasurementFactor factor2(kModel, robot.joints()[0], angle, t); wTl1 = Pose3(Rot3::Rz(angle), gtsam::Point3(0, 0, 0.5)); values.clear(); InsertPose(&values, link0->id(), wTl0); @@ -106,8 +98,7 @@ TEST(JointMeasurementFactor, ArbitraryTime) { // Non-trivial joint angle double angle = M_PI; - JointMeasurementFactor factor(key0, key1, kModel, - robot.joints()[0], angle, t); + JointMeasurementFactor factor(kModel, robot.joints()[0], angle, t); Pose3 wTl0 = link0->bMcom(); Pose3 wTl1 = Pose3(Rot3::Rz(angle), gtsam::Point3(0, 0, 0.5)); diff --git a/tests/testPoseFactor.cpp b/tests/testPoseFactor.cpp index 3ad31ebf..607a31cf 100644 --- a/tests/testPoseFactor.cpp +++ b/tests/testPoseFactor.cpp @@ -108,8 +108,7 @@ TEST(PoseFactor, breaking_rr) { auto l1 = robot.link("l1"); auto l2 = robot.link("l2"); - auto j1 = boost::dynamic_pointer_cast( - robot.joint("j1")); + const auto& j1 = robot.joint("j1"); Vector6 screw_axis = (Vector6() << 1, 0, 0, 0, -1, 0).finished(); Pose3 cMp = j1->relativePoseOf(l1, 0.0); diff --git a/tests/testRevoluteJoint.cpp b/tests/testRevoluteJoint.cpp index 31d0717b..b3f5d15c 100644 --- a/tests/testRevoluteJoint.cpp +++ b/tests/testRevoluteJoint.cpp @@ -159,15 +159,12 @@ TEST(Joint, ValuesRelativePoseOf) { RevoluteJoint j1(1, "j1", Pose3(Rot3(), Point3(0, 0, 2)), l1, l2, axis, parameters); - Values values; const size_t t = 777; // Rotating joint by -M_PI / 2 double q = -M_PI / 2; Pose3 T12(Rot3::Rx(q), Point3(0, 1, 1)); Pose3 T21(Rot3::Rx(-q), Point3(0, 1, -1)); - InsertJointAngle(&values, 1, t, q); - // Calculate numerical derivatives of relativePoseOf with respect to other // link pose. auto f1 = [&](double q) { return j1.relativePoseOf(l1, q); }; @@ -176,8 +173,8 @@ TEST(Joint, ValuesRelativePoseOf) { Matrix61 numericalH2 = numericalDerivative11(f2, q); Matrix H1v, H2v; - EXPECT(assert_equal(T21, j1.relativePoseOf(l1, values, t, H1v))); - EXPECT(assert_equal(T12, j1.relativePoseOf(l2, values, t, H2v))); + EXPECT(assert_equal(T21, j1.relativePoseOf(l1, q, H1v))); + EXPECT(assert_equal(T12, j1.relativePoseOf(l2, q, H2v))); EXPECT(assert_equal(numericalH1, H1v)); EXPECT(assert_equal(numericalH2, H2v)); @@ -211,10 +208,7 @@ TEST(RevoluteJoint, ParentTchild) { auto robot = simple_urdf::getRobot(); auto j1 = robot.joint("j1"); - Values joint_angles; - InsertJointAngle(&joint_angles, j1->id(), M_PI_2); - - Pose3 pTc = j1->parentTchild(joint_angles); + Pose3 pTc = j1->parentTchild(M_PI_2); // Rotate around the x axis for arm point up. // This means the second link bends to the right. Pose3 expected_pTc(Rot3::Rx(M_PI_2), Point3(0, -1, 1)); diff --git a/tests/testScrewJoint.cpp b/tests/testScrewJoint.cpp index dda92873..c66b41fd 100644 --- a/tests/testScrewJoint.cpp +++ b/tests/testScrewJoint.cpp @@ -76,10 +76,6 @@ TEST(Joint, params_constructor) { EXPECT(assert_equal(T_12com, j1->relativePoseOf(l2, -M_PI / 2))); EXPECT(assert_equal(T_21com, j1->relativePoseOf(l1, -M_PI / 2))); - // should throw error - CHECK_EXCEPTION(j1->relativePoseOf(l2, gtsam::Values()), - gtsam::ValuesKeyDoesNotExist); - // links auto links = j1->links(); EXPECT(links[0] == l1); From 562b575190b35090d55efba3bf4cca9f76791d61 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 15 Oct 2021 21:31:48 -0400 Subject: [PATCH 02/14] update python wrapper for JointTyped removal --- gtdynamics.i | 22 +++++----------------- 1 file changed, 5 insertions(+), 17 deletions(-) diff --git a/gtdynamics.i b/gtdynamics.i index adb9860d..1bbf82f7 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -16,24 +16,19 @@ const gtsam::KeyFormatter GTDKeyFormatter; /********************** factors **********************/ #include -template class JointMeasurementFactor : gtsam::NonlinearFactor { JointMeasurementFactor(gtsam::Key wTp_key, gtsam::Key wTc_key, const gtsam::noiseModel::Base *cost_model, const gtdynamics::Joint *joint, - const JOINT::JointCoordinate &joint_coordinate, - size_t k); + double joint_coordinate); JointMeasurementFactor(const gtsam::noiseModel::Base::shared_ptr &model, const gtdynamics::Joint *joint, - const JOINT::JointCoordinate &joint_coordinate, - size_t k); + double joint_coordinate, size_t k); void print(const string &s = "", const gtsam::KeyFormatter &keyFormatter = gtdynamics::GTDKeyFormatter); }; -typedef gtdynamics::JointMeasurementFactor RevoluteJointMeasurementFactor; - #include class PoseFactor : gtsam::NonlinearFactor { PoseFactor(gtsam::Key wTp_key, gtsam::Key wTc_key, gtsam::Key q_key, @@ -231,8 +226,6 @@ class Link { /********************** joint **********************/ #include -#include -#include #include #include #include @@ -259,12 +252,7 @@ virtual class Joint { gtdynamics::Link* child() const; }; -virtual class JointTyped : gtdynamics::Joint { -}; - -virtual class ScrewJointBase : gtdynamics::JointTyped {}; - -virtual class RevoluteJoint : gtdynamics::ScrewJointBase { +virtual class RevoluteJoint : gtdynamics::Joint { RevoluteJoint( int id, const string &name, const gtsam::Pose3 &wTj, const gtdynamics::Link *parent_link, const gtdynamics::Link *child_link, @@ -273,7 +261,7 @@ virtual class RevoluteJoint : gtdynamics::ScrewJointBase { void print(const string &s = "") const; }; -virtual class PrismaticJoint : gtdynamics::ScrewJointBase { +virtual class PrismaticJoint : gtdynamics::Joint { PrismaticJoint( int id, const string &name, const gtsam::Pose3 &wTj, const gtdynamics::Link *parent_link, const gtdynamics::Link *child_link, @@ -282,7 +270,7 @@ virtual class PrismaticJoint : gtdynamics::ScrewJointBase { void print(const string &s = "") const; }; -virtual class ScrewJoint : gtdynamics::ScrewJointBase { +virtual class ScrewJoint : gtdynamics::Joint { ScrewJoint( int id, const string &name, const gtsam::Pose3 &wTj, const gtdynamics::Link *parent_link, const gtdynamics::Link *child_link, From 1bc9d70def71396970e470a76e94ce27f898aaf9 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 15 Oct 2021 21:41:01 -0400 Subject: [PATCH 03/14] update Joint::Type enum --- gtdynamics/universal_robot/Joint.h | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/gtdynamics/universal_robot/Joint.h b/gtdynamics/universal_robot/Joint.h index e1a34844..a4ffc265 100644 --- a/gtdynamics/universal_robot/Joint.h +++ b/gtdynamics/universal_robot/Joint.h @@ -41,6 +41,13 @@ class Link; // forward declaration LINK_TYPEDEF_CLASS_POINTER(Link); LINK_TYPEDEF_CLASS_POINTER(Joint); +/** + * Joint effort types + * + * Actuated: motor powered + * Unactuated: not powered, free to move, exert zero torque + * Impedance: with spring resistance + */ enum JointEffortType { Actuated, Unactuated, Impedance }; /** @@ -84,19 +91,11 @@ class Joint : public boost::enable_shared_from_this { using Vector6 = gtsam::Vector6; public: - /** - * Joint effort types - * - * Actuated: motor powered - * Unactuated: not powered, free to move, exert zero torque - * Impedance: with spring resistance - */ - + /// Joint Type (see Lee09mmt, 2.1 paragraph 2) enum Type : char { Revolute = 'R', Prismatic = 'P', - Screw = 'C', - ScrewAxis = 'A' + Screw = 'H', }; protected: From 8880aaf753bdfc1aa9b77f4cec32d36915d73234 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 15 Oct 2021 22:27:13 -0400 Subject: [PATCH 04/14] cleanup --- gtdynamics/universal_robot/Joint.h | 4 +++- gtdynamics/utils/JsonSaver.h | 4 +--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtdynamics/universal_robot/Joint.h b/gtdynamics/universal_robot/Joint.h index a4ffc265..8643d7c2 100644 --- a/gtdynamics/universal_robot/Joint.h +++ b/gtdynamics/universal_robot/Joint.h @@ -324,11 +324,13 @@ class Joint : public boost::enable_shared_from_this { const boost::optional &planar_axis = boost::none) const; /** - * @fn (ABSTRACT) Return joint limit factors in the dynamics graph. + * Return joint limit factors in the dynamics graph. * * @param[in] t The timestep for which to generate joint limit factors. * @param[in] opt OptimizerSetting object containing NoiseModels for factors. * @return joint limit factors. + * + * TODO(gerry): remove this out of Joint and into DynamicsGraph */ gtsam::NonlinearFactorGraph jointLimitFactors( size_t t, const OptimizerSetting &opt) const; diff --git a/gtdynamics/utils/JsonSaver.h b/gtdynamics/utils/JsonSaver.h index a77c4eb4..3b2d969f 100644 --- a/gtdynamics/utils/JsonSaver.h +++ b/gtdynamics/utils/JsonSaver.h @@ -201,9 +201,7 @@ class JsonSaver { std::stringstream ss; if (const TorqueFactor* f = dynamic_cast(&(*factor))) { auto joint = f->getJoint(); - ss << GetVector(boost::static_pointer_cast(joint) - ->screwAxis(joint->child()) - .transpose()); + ss << GetVector(joint->screwAxis(joint->child()).transpose()); } else if (const gtsam::PriorFactor* f = dynamic_cast*>( &(*factor))) { From 749532bbac31898dd3ce7703b6501b8153531417 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 15 Oct 2021 22:27:48 -0400 Subject: [PATCH 05/14] fix failing unit tests --- gtdynamics/factors/JointLimitFactor.h | 17 +++---- gtdynamics/factors/WrenchEquivalenceFactor.h | 9 ++-- gtdynamics/universal_robot/Joint.cpp | 52 ++++++++++---------- tests/testJointMeasurementFactor.cpp | 4 +- 4 files changed, 40 insertions(+), 42 deletions(-) diff --git a/gtdynamics/factors/JointLimitFactor.h b/gtdynamics/factors/JointLimitFactor.h index 4cbc5638..712e451a 100644 --- a/gtdynamics/factors/JointLimitFactor.h +++ b/gtdynamics/factors/JointLimitFactor.h @@ -24,7 +24,7 @@ #include #include -#include "gtdynamics/universal_robot/JointTyped.h" +#include "gtdynamics/universal_robot/Joint.h" namespace gtdynamics { @@ -32,13 +32,11 @@ namespace gtdynamics { * JointLimitFactor is a class which enforces joint angle, velocity, * acceleration and torque value to be within limi */ -class JointLimitFactor - : public gtsam::NoiseModelFactor1 { +class JointLimitFactor : public gtsam::NoiseModelFactor1 { private: - using JointCoordinateType = JointTyped::JointCoordinateType; using This = JointLimitFactor; - using Base = gtsam::NoiseModelFactor1; - JointCoordinateType low_, high_; + using Base = gtsam::NoiseModelFactor1; + double low_, high_; public: /** @@ -51,9 +49,8 @@ class JointLimitFactor */ JointLimitFactor(gtsam::Key q_key, const gtsam::noiseModel::Base::shared_ptr &cost_model, - JointCoordinateType lower_limit, - JointCoordinateType upper_limit, - JointCoordinateType limit_threshold) + double lower_limit, double upper_limit, + double limit_threshold) : Base(cost_model, q_key), low_(lower_limit + limit_threshold), high_(upper_limit - limit_threshold) {} @@ -72,7 +69,7 @@ class JointLimitFactor * @param q joint value */ gtsam::Vector evaluateError( - const JointCoordinateType &q, + const double &q, boost::optional H_q = boost::none) const override { if (q < low_) { if (H_q) *H_q = -gtsam::I_1x1; diff --git a/gtdynamics/factors/WrenchEquivalenceFactor.h b/gtdynamics/factors/WrenchEquivalenceFactor.h index 86acb779..d34b6dd1 100644 --- a/gtdynamics/factors/WrenchEquivalenceFactor.h +++ b/gtdynamics/factors/WrenchEquivalenceFactor.h @@ -76,7 +76,7 @@ class WrenchEquivalenceFactor boost::optional H_wrench_1 = boost::none, boost::optional H_wrench_2 = boost::none, boost::optional H_q = boost::none) const override { - gtsam::Pose3 T_21 = joint_->relativePoseOf(joint_->parent(), q); + gtsam::Pose3 T_21 = joint_->childTparent(q); gtsam::Matrix6 Ad_21_T = T_21.AdjointMap().transpose(); gtsam::Vector6 error = wrench_1 + Ad_21_T * wrench_2; @@ -88,9 +88,10 @@ class WrenchEquivalenceFactor } if (H_q) { // TODO(frank): really, child? Double-check derivatives - // *H_q = - // joint_->AdjointMapJacobianJointAngle(joint_->child(), q).transpose() * - // wrench_2; + *H_q = AdjointMapJacobianQ(q, joint_->childTparent(0.0), + joint_->screwAxis(joint_->child())) + .transpose() * + wrench_2; } return error; } diff --git a/gtdynamics/universal_robot/Joint.cpp b/gtdynamics/universal_robot/Joint.cpp index 35943211..75a90f99 100644 --- a/gtdynamics/universal_robot/Joint.cpp +++ b/gtdynamics/universal_robot/Joint.cpp @@ -19,6 +19,7 @@ #include #include "gtdynamics/universal_robot/Link.h" +#include "gtdynamics/factors/JointLimitFactor.h" using gtsam::Pose3; using gtsam::Vector6; @@ -33,12 +34,11 @@ Joint::Joint(uint8_t id, const std::string &name, const Pose3 &bTj, name_(name), parent_link_(parent_link), child_link_(child_link), + jMp_(bTj.inverse() * parent_link->bMcom()), + jMc_(bTj.inverse() * child_link->bMcom()), pScrewAxis_(-jMp_.inverse().AdjointMap() * jScrewAxis), cScrewAxis_(jMc_.inverse().AdjointMap() * jScrewAxis), - parameters_(parameters) { - jMp_ = bTj.inverse() * parent_link_->bMcom(); - jMc_ = bTj.inverse() * child_link_->bMcom(); -} + parameters_(parameters) {} /* ************************************************************************* */ bool Joint::isChildLink(const LinkSharedPtr &link) const { @@ -250,28 +250,28 @@ gtsam::NonlinearFactorGraph Joint::jointLimitFactors( gtsam::NonlinearFactorGraph graph; auto id = this->id(); // Add joint angle limit factor. - // graph.emplace_shared( - // internal::JointAngleKey(id, t), opt.jl_cost_model, - // parameters().scalar_limits.value_lower_limit, - // parameters().scalar_limits.value_upper_limit, - // parameters().scalar_limits.value_limit_threshold); - - // // Add joint velocity limit factors. - // graph.emplace_shared( - // internal::JointVelKey(id, t), opt.jl_cost_model, - // -parameters().velocity_limit, parameters().velocity_limit, - // parameters().velocity_limit_threshold); - - // // Add joint acceleration limit factors. - // graph.emplace_shared( - // internal::JointAccelKey(id, t), opt.jl_cost_model, - // -parameters().acceleration_limit, parameters().acceleration_limit, - // parameters().acceleration_limit_threshold); - - // // Add joint torque limit factors. - // graph.emplace_shared( - // internal::TorqueKey(id, t), opt.jl_cost_model, -parameters().torque_limit, - // parameters().torque_limit, parameters().torque_limit_threshold); + graph.emplace_shared( + internal::JointAngleKey(id, t), opt.jl_cost_model, + parameters().scalar_limits.value_lower_limit, + parameters().scalar_limits.value_upper_limit, + parameters().scalar_limits.value_limit_threshold); + + // Add joint velocity limit factors. + graph.emplace_shared( + internal::JointVelKey(id, t), opt.jl_cost_model, + -parameters().velocity_limit, parameters().velocity_limit, + parameters().velocity_limit_threshold); + + // Add joint acceleration limit factors. + graph.emplace_shared( + internal::JointAccelKey(id, t), opt.jl_cost_model, + -parameters().acceleration_limit, parameters().acceleration_limit, + parameters().acceleration_limit_threshold); + + // Add joint torque limit factors. + graph.emplace_shared( + internal::TorqueKey(id, t), opt.jl_cost_model, -parameters().torque_limit, + parameters().torque_limit, parameters().torque_limit_threshold); return graph; } diff --git a/tests/testJointMeasurementFactor.cpp b/tests/testJointMeasurementFactor.cpp index 8f413c18..62ff85a4 100644 --- a/tests/testJointMeasurementFactor.cpp +++ b/tests/testJointMeasurementFactor.cpp @@ -104,8 +104,8 @@ TEST(JointMeasurementFactor, ArbitraryTime) { Pose3 wTl1 = Pose3(Rot3::Rz(angle), gtsam::Point3(0, 0, 0.5)); Values values; - InsertPose(&values, link0->id(), wTl0); - InsertPose(&values, link1->id(), wTl1); + InsertPose(&values, link0->id(), t, wTl0); + InsertPose(&values, link1->id(), t, wTl1); Vector error = factor.evaluateError(wTl0, wTl1); EXPECT(assert_equal(Vector::Zero(6), error, 1e-9)); From fe68291d8dcc9750937a7053fe37938202fdd4f1 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 15 Oct 2021 22:36:48 -0400 Subject: [PATCH 06/14] kill JointTyped.h and ScrewJointBase.h files --- .../factors/PreintegratedContactFactors.h | 2 +- gtdynamics/factors/TorqueFactor.h | 11 +- gtdynamics/factors/TwistAccelFactor.h | 7 +- gtdynamics/factors/TwistFactor.h | 9 +- gtdynamics/factors/WrenchEquivalenceFactor.h | 9 +- gtdynamics/factors/WrenchPlanarFactor.h | 2 +- gtdynamics/universal_robot/Joint.h | 2 - gtdynamics/universal_robot/JointTyped.h | 245 ---------------- gtdynamics/universal_robot/Robot.cpp | 1 - gtdynamics/universal_robot/ScrewJoint.h | 5 +- gtdynamics/universal_robot/ScrewJointBase.cpp | 265 ------------------ gtdynamics/universal_robot/ScrewJointBase.h | 166 ----------- gtdynamics/universal_robot/sdf.cpp | 1 - gtdynamics/universal_robot/sdf.h | 1 - gtdynamics/universal_robot/sdf_internal.h | 1 - gtdynamics/utils/JsonSaver.h | 2 +- tests/testRevoluteJoint.cpp | 1 - 17 files changed, 21 insertions(+), 709 deletions(-) delete mode 100644 gtdynamics/universal_robot/JointTyped.h delete mode 100644 gtdynamics/universal_robot/ScrewJointBase.cpp delete mode 100644 gtdynamics/universal_robot/ScrewJointBase.h diff --git a/gtdynamics/factors/PreintegratedContactFactors.h b/gtdynamics/factors/PreintegratedContactFactors.h index da5891cf..7b1d4c3b 100644 --- a/gtdynamics/factors/PreintegratedContactFactors.h +++ b/gtdynamics/factors/PreintegratedContactFactors.h @@ -24,7 +24,7 @@ #include #include -#include "gtdynamics/universal_robot/JointTyped.h" +#include "gtdynamics/universal_robot/Joint.h" #include "gtdynamics/universal_robot/Link.h" namespace gtdynamics { diff --git a/gtdynamics/factors/TorqueFactor.h b/gtdynamics/factors/TorqueFactor.h index db532aff..4375b6e8 100644 --- a/gtdynamics/factors/TorqueFactor.h +++ b/gtdynamics/factors/TorqueFactor.h @@ -21,7 +21,7 @@ #include #include -#include "gtdynamics/universal_robot/JointTyped.h" +#include "gtdynamics/universal_robot/Joint.h" #include "gtdynamics/universal_robot/Link.h" #include "gtdynamics/utils/values.h" @@ -35,13 +35,12 @@ class TorqueFactor : public gtsam::NoiseModelFactor2 { private: using This = TorqueFactor; using Base = gtsam::NoiseModelFactor2; - using MyJointConstSharedPtr = boost::shared_ptr; - MyJointConstSharedPtr joint_; + JointConstSharedPtr joint_; /// Private constructor with arbitrary keys TorqueFactor(const gtsam::noiseModel::Base::shared_ptr &cost_model, gtsam::Key wrench_key, gtsam::Key torque_key, - const MyJointConstSharedPtr &joint) + const JointConstSharedPtr &joint) : Base(cost_model, wrench_key, torque_key), joint_(joint) {} public: @@ -55,7 +54,7 @@ class TorqueFactor : public gtsam::NoiseModelFactor2 { * @param joint JointConstSharedPtr to the joint */ TorqueFactor(const gtsam::noiseModel::Base::shared_ptr &cost_model, - const MyJointConstSharedPtr &joint, size_t k = 0) + const JointConstSharedPtr &joint, size_t k = 0) : TorqueFactor(cost_model, internal::WrenchKey(joint->child()->id(), joint->id(), k), internal::TorqueKey(joint->id(), k), joint) {} @@ -82,7 +81,7 @@ class TorqueFactor : public gtsam::NoiseModelFactor2 { } /// Returns the joint - MyJointConstSharedPtr getJoint() const { return joint_; } + JointConstSharedPtr getJoint() const { return joint_; } //// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { diff --git a/gtdynamics/factors/TwistAccelFactor.h b/gtdynamics/factors/TwistAccelFactor.h index 5f02d2a4..d7c42006 100644 --- a/gtdynamics/factors/TwistAccelFactor.h +++ b/gtdynamics/factors/TwistAccelFactor.h @@ -23,7 +23,7 @@ #include #include -#include "gtdynamics/universal_robot/JointTyped.h" +#include "gtdynamics/universal_robot/Joint.h" namespace gtdynamics { @@ -39,8 +39,7 @@ class TwistAccelFactor using Base = gtsam::NoiseModelFactor6; - using JointTypedConstSharedPtr = boost::shared_ptr; - JointTypedConstSharedPtr joint_; + JointConstSharedPtr joint_; public: /** @@ -56,7 +55,7 @@ class TwistAccelFactor gtsam::Key twistAccel_key_c, gtsam::Key q_key, gtsam::Key qVel_key, gtsam::Key qAccel_key, const gtsam::noiseModel::Base::shared_ptr &cost_model, - JointTypedConstSharedPtr joint) + JointConstSharedPtr joint) : Base(cost_model, twist_key_c, twistAccel_key_p, twistAccel_key_c, q_key, qVel_key, qAccel_key), joint_(joint) {} diff --git a/gtdynamics/factors/TwistFactor.h b/gtdynamics/factors/TwistFactor.h index a14c8042..a4ef254a 100644 --- a/gtdynamics/factors/TwistFactor.h +++ b/gtdynamics/factors/TwistFactor.h @@ -21,7 +21,7 @@ #include #include -#include "gtdynamics/universal_robot/JointTyped.h" +#include "gtdynamics/universal_robot/Joint.h" namespace gtdynamics { @@ -72,10 +72,9 @@ class TwistFactor boost::optional H_twist_c = boost::none, boost::optional H_q = boost::none, boost::optional H_qVel = boost::none) const override { - auto error = - boost::static_pointer_cast(joint_)->transformTwistTo( - joint_->child(), q, qVel, twist_p, H_q, H_qVel, H_twist_p) - - twist_c; + auto error = joint_->transformTwistTo(joint_->child(), q, qVel, twist_p, + H_q, H_qVel, H_twist_p) - + twist_c; if (H_twist_c) { *H_twist_c = -gtsam::I_6x6; diff --git a/gtdynamics/factors/WrenchEquivalenceFactor.h b/gtdynamics/factors/WrenchEquivalenceFactor.h index d34b6dd1..3e0020e3 100644 --- a/gtdynamics/factors/WrenchEquivalenceFactor.h +++ b/gtdynamics/factors/WrenchEquivalenceFactor.h @@ -24,7 +24,7 @@ #include #include -#include "gtdynamics/universal_robot/JointTyped.h" +#include "gtdynamics/universal_robot/Joint.h" #include "gtdynamics/universal_robot/Link.h" #include "gtdynamics/utils/values.h" @@ -38,14 +38,13 @@ class WrenchEquivalenceFactor using This = WrenchEquivalenceFactor; using Base = gtsam::NoiseModelFactor3; - using JointTypedConstSharedPtr = boost::shared_ptr; - JointTypedConstSharedPtr joint_; + JointConstSharedPtr joint_; /// Private constructor with arbitrary keys WrenchEquivalenceFactor(const gtsam::noiseModel::Base::shared_ptr &cost_model, gtsam::Key wrench_key_1, gtsam::Key wrench_key_2, gtsam::Key q_key, - const JointTypedConstSharedPtr &joint) + const JointConstSharedPtr &joint) : Base(cost_model, wrench_key_1, wrench_key_2, q_key), joint_(joint) {} public: @@ -54,7 +53,7 @@ class WrenchEquivalenceFactor * @param joint JointConstSharedPtr to the joint */ WrenchEquivalenceFactor(const gtsam::noiseModel::Base::shared_ptr &cost_model, - const JointTypedConstSharedPtr &joint, size_t k = 0) + const JointConstSharedPtr &joint, size_t k = 0) : WrenchEquivalenceFactor( cost_model, internal::WrenchKey(joint->parent()->id(), joint->id(), k), diff --git a/gtdynamics/factors/WrenchPlanarFactor.h b/gtdynamics/factors/WrenchPlanarFactor.h index e94537dd..b4440cf9 100644 --- a/gtdynamics/factors/WrenchPlanarFactor.h +++ b/gtdynamics/factors/WrenchPlanarFactor.h @@ -20,7 +20,7 @@ #include #include -#include "gtdynamics/universal_robot/JointTyped.h" +#include "gtdynamics/universal_robot/Joint.h" #include "gtdynamics/universal_robot/Link.h" #include "gtdynamics/utils/utils.h" #include "gtdynamics/utils/values.h" diff --git a/gtdynamics/universal_robot/Joint.h b/gtdynamics/universal_robot/Joint.h index 8643d7c2..f3fe46c8 100644 --- a/gtdynamics/universal_robot/Joint.h +++ b/gtdynamics/universal_robot/Joint.h @@ -367,6 +367,4 @@ class Joint : public boost::enable_shared_from_this { } }; -using JointTyped = Joint; - } // namespace gtdynamics diff --git a/gtdynamics/universal_robot/JointTyped.h b/gtdynamics/universal_robot/JointTyped.h deleted file mode 100644 index 5dcab5ca..00000000 --- a/gtdynamics/universal_robot/JointTyped.h +++ /dev/null @@ -1,245 +0,0 @@ -// /* ---------------------------------------------------------------------------- -// * GTDynamics Copyright 2020, Georgia Tech Research Corporation, -// * Atlanta, Georgia 30332-0415 -// * All Rights Reserved -// * See LICENSE for the license information -// * -------------------------------------------------------------------------- */ - -// /** -// * @file JointTyped.h -// * @brief Specialized form of Joint for specific joint angle types -// * @author: Frank Dellaert -// * @author: Mandy Xie -// * @author: Alejandro Escontrela -// * @author: Yetong Zhang -// * @author: Varun Agrawal -// */ - -#include "gtdynamics/universal_robot/Joint.h" - -// #pragma once - -// #include "gtdynamics/universal_robot/Joint.h" -// #include "gtdynamics/utils/values.h" - -// namespace gtdynamics { - -// /** -// * JointTyped is a convenience class that inherits from Joint which wraps -// * abstract transformXXX that take in joint type arguments into transformXXX -// * from Joint which take in gtsam::Values object -// */ - -// // TODO(Gerry) JointTyped was an intermediate step towards adding ball and -// // sphere joints but we never finished it because for other joint types, -// // relativePoseOf can't just use a double as the joint angle -// // argument, they need Unit3 or Rot3 - -// class JointTyped : public Joint { -// public: -// using This = JointTyped; - -// using JointCoordinateType = double; // standin for template -// using JointCoordinateTangentType = double; // standin for template -// using JointCoordinate = JointCoordinateType; -// using JointVelocity = JointCoordinateTangentType; -// using JointAcceleration = JointCoordinateTangentType; -// using JointTorque = JointCoordinateTangentType; - -// enum { N = gtsam::traits::dimension }; -// using VectorN = Eigen::Matrix; -// using MatrixN = Eigen::Matrix; - -// public: -// /// Inherit constructors -// using Joint::Joint; - -// /** -// * @name Abstract -// * Joint classes must implement these methods. -// */ -// ///@{ - -// /** -// * Abstract method. Return the pose of the child link CoM in the parent link -// * CoM frame, given the joint coordinate. -// */ -// virtual Pose3 parentTchild( -// JointCoordinate q, -// gtsam::OptionalJacobian<6, N> H_q = boost::none) const = 0; - -// /** -// * Abstract method. Return the pose of the parent link in the child link -// * frame, given the joint coordinate. -// */ -// virtual Pose3 -// childTparent(JointCoordinate q, -// gtsam::OptionalJacobian<6, N> H_q = boost::none) const = 0; - -// /// Joint-induced twist in child frame -// virtual gtsam::Vector6 childTwist(JointVelocity q_dot) const = 0; - -// /// Joint-induced twist in parent frame -// virtual gtsam::Vector6 parentTwist(JointVelocity q_dot) const = 0; - -// /** -// * Abstract method. Return the twist of this link given the other link's twist -// * and joint angle. -// */ -// virtual gtsam::Vector6 transformTwistTo( -// const LinkSharedPtr &link, JointCoordinate q, JointVelocity q_dot, -// boost::optional other_twist = boost::none, -// gtsam::OptionalJacobian<6, N> H_q = boost::none, -// gtsam::OptionalJacobian<6, N> H_q_dot = boost::none, -// gtsam::OptionalJacobian<6, 6> H_other_twist = boost::none) const = 0; - -// /** Abstract method. Return the twist acceleration of this link given the -// * other link's twist acceleration, both links' twists, and joint angle. -// */ -// virtual gtsam::Vector6 transformTwistAccelTo( -// const LinkSharedPtr &link, JointCoordinate q, JointVelocity q_dot, -// JointAcceleration q_ddot, -// boost::optional this_twist = boost::none, -// boost::optional other_twist_accel = boost::none, -// gtsam::OptionalJacobian<6, N> H_q = boost::none, -// gtsam::OptionalJacobian<6, N> H_q_dot = boost::none, -// gtsam::OptionalJacobian<6, N> H_q_ddot = boost::none, -// gtsam::OptionalJacobian<6, 6> H_this_twist = boost::none, -// gtsam::OptionalJacobian<6, 6> H_other_twist_accel = -// boost::none) const = 0; - -// /// Abstract method. Return the torque on this joint given the wrench -// virtual JointTorque transformWrenchToTorque( -// const LinkSharedPtr &link, -// boost::optional wrench = boost::none, -// gtsam::OptionalJacobian H_wrench = boost::none) const = 0; - -// /// Calculate AdjointMap jacobian w.r.t. joint coordinate q. -// /// TODO(gerry + stephanie): change to calculate the jacobian of Ad_T(v) wrt T -// /// rather than jacobian of Ad_T wrt q (and put in utils or PR to GTSAM) -// virtual gtsam::Matrix6 -// AdjointMapJacobianJointAngle(const LinkSharedPtr &link, -// JointCoordinate q) const = 0; - -// ///@} -// /** -// * @name generic -// * These are methods that can be implemented here in terms of abstract methods. -// */ -// ///@{ - -// /** -// * Return the relative pose of the specified link [link2] in the other link's -// * [link1] reference frame. -// */ -// Pose3 relativePoseOf(const LinkSharedPtr &link2, JointCoordinate q, -// gtsam::OptionalJacobian<6, 1> H_q = boost::none) const { -// return isChildLink(link2) ? parentTchild(q, H_q) : childTparent(q, H_q); -// } - -// /** -// * Return the world pose of the specified link [link2], given -// * the world pose of the other link [link1]. -// */ -// Pose3 poseOf(const LinkSharedPtr &link2, const Pose3 &wT1, JointCoordinate q, -// gtsam::OptionalJacobian<6, 6> H_wT1 = boost::none, -// gtsam::OptionalJacobian<6, N> H_q = boost::none) const { -// auto T12 = relativePoseOf(link2, q, H_q); -// return wT1.compose(T12, H_wT1); // H_wT2_T12 is identity -// } - -// ///@} -// /** -// * @name valueBased -// * Methods that extract coordinates/velocities from Values and pass on to -// * abstract methods above. -// */ -// ///@{ - -// /** -// * Return the pose of the child link CoM in the parent link CoM frame, given a -// * Values object containing the joint coordinate. -// */ -// Pose3 parentTchild( -// const gtsam::Values &q, size_t t = 0, -// boost::optional H_q = boost::none) const override { -// return parentTchild(JointAngle(q, id(), t), H_q); -// } - -// /** -// * Return the pose of the parent link in the child link frame, given a Values -// * object containing the joint coordinate. -// */ -// Pose3 childTparent( -// const gtsam::Values &q, size_t t = 0, -// boost::optional H_q = boost::none) const override { -// return childTparent(JointAngle(q, id(), t), H_q); -// } - -// /** -// * Return the relative pose of the specified link [link2] in -// * the other link's [link1] reference frame. -// * @throw KeyDoesNotExist if the appropriate key is missing from values -// */ -// Pose3 relativePoseOf( -// const LinkSharedPtr &link2, const gtsam::Values &q, size_t t = 0, -// boost::optional H_q = boost::none) const override { -// return relativePoseOf(link2, JointAngle(q, id(), t), H_q); -// } - -// /// Joint-induced twist in child frame -// gtsam::Vector6 childTwist(const gtsam::Values &values, -// size_t t = 0) const override { -// return childTwist(JointVel(values, id(), t)); -// } - -// /// Joint-induced twist in parent frame -// gtsam::Vector6 parentTwist(const gtsam::Values &values, -// size_t t = 0) const override { -// return parentTwist(JointVel(values, id(), t)); -// } - -// /** -// * Return the twist of this link given the other link's twist and a Values -// * object containing this joint's angle Value. -// * @param values containing q, q_dot -// * @throw ValuesKeyDoesNotExist if the appropriate key is missing from values -// */ -// gtsam::Vector6 transformTwistTo( -// size_t t, const LinkSharedPtr &link, const gtsam::Values &values, -// boost::optional other_twist = boost::none, -// boost::optional H_q = boost::none, -// boost::optional H_q_dot = boost::none, -// gtsam::OptionalJacobian<6, 6> H_other_twist = boost::none) // -// const override { -// return transformTwistTo(link, JointAngle(values, id(), t), -// JointVel(values, id(), t), -// other_twist, H_q, H_q_dot, H_other_twist); -// } - -// /** Return the twist acceleration of the other link given this link's twist -// * accel and a Values object containing this joint's angle value and -// * derivatives. -// * @param values containing q, q_dot, and q_ddot -// * @throw ValuesKeyDoesNotExist if the appropriate key is missing from values -// */ -// gtsam::Vector6 transformTwistAccelTo( -// size_t t, const LinkSharedPtr &link, const gtsam::Values &values, -// boost::optional this_twist = boost::none, -// boost::optional other_twist_accel = boost::none, -// boost::optional H_q = boost::none, -// boost::optional H_q_dot = boost::none, -// boost::optional H_q_ddot = boost::none, -// gtsam::OptionalJacobian<6, 6> H_this_twist = boost::none, -// gtsam::OptionalJacobian<6, 6> H_other_twist_accel = -// boost::none) const override { -// return transformTwistAccelTo(link, -// JointAngle(values, id(), t), -// JointVel(values, id(), t), -// JointAccel(values, id(), t), -// this_twist, other_twist_accel, H_q, H_q_dot, -// H_q_ddot, H_this_twist, H_other_twist_accel); -// } -// }; - -// } // namespace gtdynamics diff --git a/gtdynamics/universal_robot/Robot.cpp b/gtdynamics/universal_robot/Robot.cpp index c26c6b25..635f7fb5 100644 --- a/gtdynamics/universal_robot/Robot.cpp +++ b/gtdynamics/universal_robot/Robot.cpp @@ -21,7 +21,6 @@ #include "gtdynamics/universal_robot/Joint.h" #include "gtdynamics/universal_robot/RobotTypes.h" -#include "gtdynamics/universal_robot/ScrewJointBase.h" #include "gtdynamics/utils/utils.h" #include "gtdynamics/utils/values.h" diff --git a/gtdynamics/universal_robot/ScrewJoint.h b/gtdynamics/universal_robot/ScrewJoint.h index a5df892f..b8eecd02 100644 --- a/gtdynamics/universal_robot/ScrewJoint.h +++ b/gtdynamics/universal_robot/ScrewJoint.h @@ -23,9 +23,8 @@ namespace gtdynamics { /** - * @class ScrewJoint is an implementation of the ScrewJointBase class - * which represents a screw joint and contains all necessary factor - * construction methods. + * @class ScrewJoint is an implementation of the Joint class which represents a + * screw joint and contains all necessary factor construction methods. */ class ScrewJoint : public Joint { protected: diff --git a/gtdynamics/universal_robot/ScrewJointBase.cpp b/gtdynamics/universal_robot/ScrewJointBase.cpp deleted file mode 100644 index fd9ac1c8..00000000 --- a/gtdynamics/universal_robot/ScrewJointBase.cpp +++ /dev/null @@ -1,265 +0,0 @@ -// /* ---------------------------------------------------------------------------- -// * GTDynamics Copyright 2020, Georgia Tech Research Corporation, -// * Atlanta, Georgia 30332-0415 -// * All Rights Reserved -// * See LICENSE for the license information -// * -------------------------------------------------------------------------- */ - -// /** -// * @file ScrewJointBase.cpp -// * @brief Representation of screw-type robot joints. Revolute, Prismatic, and -// * Screw subclasses -// * @author Frank Dellaert -// * @author Mandy Xie -// * @author Alejandro Escontrela -// * @author Yetong Zhang -// * @author Stephanie McCormick -// * @author Gerry Chen -// * @author Varun Agrawal -// */ - - -// #include "gtdynamics/universal_robot/ScrewJointBase.h" - -// #include "gtdynamics/factors/JointLimitFactor.h" -// #include "gtdynamics/universal_robot/Link.h" -// #include "gtdynamics/utils/utils.h" -// #include "gtdynamics/utils/values.h" - -// #include - -// #include -// #include -// #include - -// using namespace gtsam; - -// namespace gtdynamics { - -// /* ************************************************************************* */ -// Pose3 ScrewJointBase::parentTchild( -// double q, gtsam::OptionalJacobian<6, 1> pMc_H_q) const { -// // Calculate pose of child in parent link, at rest. -// // TODO(dellaert): store `pMj_` rather than `jMp_`. -// // NOTE(dellaert): Only if this is called more often than childTparent. -// const Pose3 pMc = jMp_.inverse() * jMc_; - -// // Multiply screw axis with joint angle to get a finite 6D screw. -// const Vector6 screw = cScrewAxis_ * q; - -// // Calculate the actual relative pose taking into account the joint angle. -// // TODO(dellaert): use formula `pMj_ * screw_around_Z * jMc_`. -// gtsam::Matrix6 exp_H_screw; -// const Pose3 exp = Pose3::Expmap(screw, pMc_H_q ? &exp_H_screw : 0); -// if (pMc_H_q) { -// *pMc_H_q = exp_H_screw * cScrewAxis_; -// } -// return pMc * exp; // Note: derivative of compose in exp is identity. -// } - -// /* ************************************************************************* */ -// Pose3 ScrewJointBase::childTparent( -// double q, gtsam::OptionalJacobian<6, 1> cMp_H_q) const { -// // TODO(frank): don't go via inverse, specialize in base class -// Vector6 pMc_H_q; -// Pose3 pMc = parentTchild(q, cMp_H_q ? &pMc_H_q : 0); // pMc(q) -> pMc_H_q -// gtsam::Matrix6 cMp_H_pMc; -// Pose3 cMp = pMc.inverse(cMp_H_q ? &cMp_H_pMc : 0); // cMp(pMc) -> cMp_H_pMc -// if (cMp_H_q) { -// *cMp_H_q = cMp_H_pMc * pMc_H_q; -// } -// return cMp; -// } - -// /* ************************************************************************* */ -// Vector6 ScrewJointBase::transformTwistTo( -// const LinkSharedPtr &link, double q, double q_dot, -// boost::optional other_twist, -// gtsam::OptionalJacobian<6, 1> H_q, -// gtsam::OptionalJacobian<6, 1> H_q_dot, -// gtsam::OptionalJacobian<6, 6> H_other_twist) const { -// Vector6 other_twist_ = other_twist ? *other_twist : Vector6::Zero(); - -// auto other = otherLink(link); -// auto this_ad_other = relativePoseOf(other, q).AdjointMap(); - -// if (H_q) { -// // TODO(frank): really, zero below? Check derivatives -// *H_q = AdjointMapJacobianQ(q, relativePoseOf(other, 0.0), screwAxis(link)) * -// other_twist_; -// } -// if (H_q_dot) { -// *H_q_dot = screwAxis(link); -// } -// if (H_other_twist) { -// *H_other_twist = this_ad_other; -// } - -// return this_ad_other * other_twist_ + screwAxis(link) * q_dot; -// } - -// /* ************************************************************************* */ -// Vector6 ScrewJointBase::transformTwistAccelTo( -// const LinkSharedPtr &link, double q, double q_dot, double q_ddot, -// boost::optional this_twist, -// boost::optional other_twist_accel, -// gtsam::OptionalJacobian<6, 1> H_q, -// gtsam::OptionalJacobian<6, 1> H_q_dot, -// gtsam::OptionalJacobian<6, 1> H_q_ddot, -// gtsam::OptionalJacobian<6, 6> H_this_twist, -// gtsam::OptionalJacobian<6, 6> H_other_twist_accel) const { -// Vector6 this_twist_ = this_twist ? *this_twist : Vector6::Zero(); -// Vector6 other_twist_accel_ = -// other_twist_accel ? *other_twist_accel : Vector6::Zero(); -// Vector6 screw_axis_ = isChildLink(link) ? cScrewAxis_ : pScrewAxis_; - -// // i = other link -// // j = this link -// auto other = otherLink(link); -// Pose3 jTi = relativePoseOf(other, q); - -// Vector6 this_twist_accel = -// jTi.AdjointMap() * other_twist_accel_ + -// Pose3::adjoint(this_twist_, screw_axis_ * q_dot, H_this_twist) + -// screw_axis_ * q_ddot; - -// if (H_other_twist_accel) { -// *H_other_twist_accel = jTi.AdjointMap(); -// } -// if (H_q) { -// // TODO(frank): really, zero below? Check derivatives. Also, copy/pasta from -// // above? -// *H_q = AdjointMapJacobianQ(q, relativePoseOf(other, 0.0), screw_axis_) * -// other_twist_accel_; -// } -// if (H_q_dot) { -// *H_q_dot = Pose3::adjointMap(this_twist_) * screw_axis_; -// } -// if (H_q_ddot) { -// *H_q_ddot = screw_axis_; -// } - -// return this_twist_accel; -// } - -// /* ************************************************************************* */ -// ScrewJointBase::JointTorque ScrewJointBase::transformWrenchToTorque( -// const LinkSharedPtr &link, boost::optional wrench, -// gtsam::OptionalJacobian<1, 6> H_wrench) const { -// auto screw_axis_ = screwAxis(link); -// if (H_wrench) { -// *H_wrench = screw_axis_.transpose(); -// } -// return screw_axis_.transpose() * (wrench ? *wrench : Vector6::Zero()); -// } - -// /* ************************************************************************* */ -// gtsam::GaussianFactorGraph ScrewJointBase::linearFDPriors( -// size_t t, const gtsam::Values &known_values, -// const OptimizerSetting &opt) const { -// gtsam::GaussianFactorGraph priors; -// gtsam::Vector1 rhs(Torque(known_values, id(), t)); -// // TODO(alej`andro): use optimizer settings -// priors.add(internal::TorqueKey(id(), t), gtsam::I_1x1, rhs, -// gtsam::noiseModel::Constrained::All(1)); -// return priors; -// } - -// /* ************************************************************************* */ -// gtsam::GaussianFactorGraph ScrewJointBase::linearAFactors( -// size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, -// const boost::optional &planar_axis) const { -// gtsam::GaussianFactorGraph graph; - -// const Pose3 T_wi1 = Pose(known_values, parent()->id(), t); -// const Pose3 T_wi2 = Pose(known_values, child()->id(), t); -// const Pose3 T_i2i1 = T_wi2.inverse() * T_wi1; -// const Vector6 V_i2 = Twist(known_values, child()->id(), t); -// const Vector6 S_i2_j = screwAxis(child_link_); -// const double v_j = JointVel(known_values, id(), t); - -// // twist acceleration factor -// // A_i2 - Ad(T_21) * A_i1 - S_i2_j * a_j = ad(V_i2) * S_i2_j * v_j -// Vector6 rhs_tw = Pose3::adjointMap(V_i2) * S_i2_j * v_j; -// graph.add(internal::TwistAccelKey(child()->id(), t), gtsam::I_6x6, -// internal::TwistAccelKey(parent()->id(), t), -T_i2i1.AdjointMap(), -// internal::JointAccelKey(id(), t), -S_i2_j, rhs_tw, -// gtsam::noiseModel::Constrained::All(6)); - -// return graph; -// } - -// /* ************************************************************************* */ -// gtsam::GaussianFactorGraph ScrewJointBase::linearDynamicsFactors( -// size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, -// const boost::optional &planar_axis) const { -// gtsam::GaussianFactorGraph graph; - -// const Pose3 T_wi1 = Pose(known_values, parent()->id(), t); -// const Pose3 T_wi2 = Pose(known_values, child()->id(), t); -// const Pose3 T_i2i1 = T_wi2.inverse() * T_wi1; -// const Vector6 S_i2_j = screwAxis(child_link_); - -// // torque factor -// // S_i_j^T * F_i_j - tau = 0 -// gtsam::Vector1 rhs_torque = gtsam::Vector1::Zero(); -// graph.add(internal::WrenchKey(child()->id(), id(), t), S_i2_j.transpose(), -// internal::TorqueKey(id(), t), -gtsam::I_1x1, rhs_torque, -// gtsam::noiseModel::Constrained::All(1)); - -// // wrench equivalence factor -// // F_i1_j + Ad(T_i2i1)^T F_i2_j = 0 -// Vector6 rhs_weq = Vector6::Zero(); -// graph.add(internal::WrenchKey(parent()->id(), id(), t), gtsam::I_6x6, -// internal::WrenchKey(child()->id(), id(), t), -// T_i2i1.AdjointMap().transpose(), rhs_weq, -// gtsam::noiseModel::Constrained::All(6)); - -// // wrench planar factor -// if (planar_axis) { -// gtsam::Matrix36 J_wrench = getPlanarJacobian(*planar_axis); -// graph.add(internal::WrenchKey(child()->id(), id(), t), J_wrench, -// gtsam::Vector3::Zero(), gtsam::noiseModel::Constrained::All(3)); -// } - -// return graph; -// } - -// /* ************************************************************************* */ -// gtsam::NonlinearFactorGraph ScrewJointBase::jointLimitFactors( -// size_t t, const OptimizerSetting &opt) const { -// gtsam::NonlinearFactorGraph graph; -// auto id = this->id(); -// // Add joint angle limit factor. -// graph.emplace_shared( -// internal::JointAngleKey(id, t), opt.jl_cost_model, -// parameters().scalar_limits.value_lower_limit, -// parameters().scalar_limits.value_upper_limit, -// parameters().scalar_limits.value_limit_threshold); - -// // Add joint velocity limit factors. -// graph.emplace_shared( -// internal::JointVelKey(id, t), opt.jl_cost_model, -// -parameters().velocity_limit, parameters().velocity_limit, -// parameters().velocity_limit_threshold); - -// // Add joint acceleration limit factors. -// graph.emplace_shared( -// internal::JointAccelKey(id, t), opt.jl_cost_model, -// -parameters().acceleration_limit, parameters().acceleration_limit, -// parameters().acceleration_limit_threshold); - -// // Add joint torque limit factors. -// graph.emplace_shared( -// internal::TorqueKey(id, t), opt.jl_cost_model, -parameters().torque_limit, -// parameters().torque_limit, parameters().torque_limit_threshold); -// return graph; -// } - -// std::ostream &ScrewJointBase::to_stream(std::ostream &os) const { -// Joint::to_stream(os); -// os << "\n\tscrew axis (parent): " << screwAxis(parent()).transpose(); -// return os; -// } - -// } // namespace gtdynamics diff --git a/gtdynamics/universal_robot/ScrewJointBase.h b/gtdynamics/universal_robot/ScrewJointBase.h deleted file mode 100644 index f0e689c4..00000000 --- a/gtdynamics/universal_robot/ScrewJointBase.h +++ /dev/null @@ -1,166 +0,0 @@ -// /* ---------------------------------------------------------------------------- -// * GTDynamics Copyright 2020, Georgia Tech Research Corporation, -// * Atlanta, Georgia 30332-0415 -// * All Rights Reserved -// * See LICENSE for the license information -// * -------------------------------------------------------------------------- */ - -// /** -// * @file ScrewJointBase.h -// * @brief Representation of screw-type robot joints. Revolute, Prismatic, and -// * Screw subclasses -// * @author Frank Dellaert -// * @author Mandy Xie -// * @author Alejandro Escontrela -// * @author Yetong Zhang -// * @author Stephanie McCormick -// * @author Gerry Chen -// * @author Varun Agrawal -// */ - -// #pragma once - -// #include - -// #include -// #include -// #include - -// #include "gtdynamics/factors/JointLimitFactor.h" -// #include "gtdynamics/universal_robot/JointTyped.h" -// #include "gtdynamics/utils/utils.h" -// #include "gtdynamics/utils/values.h" - -// namespace gtdynamics { -// /** -// * @class ScrewJointBase is an implementation of the abstract Joint class -// * which represents a screw-type joint and contains all necessary factor -// * construction methods. -// * It is the base class for RevoluteJoint, PrismaticJoint, and ScrewJoint. -// */ -// class ScrewJointBase : public Joint { -// using Pose3 = gtsam::Pose3; -// using Vector6 = gtsam::Vector6; - -// protected: -// gtsam::Vector3 axis_; - -// // Screw axis in parent and child CoM frames. -// Vector6 pScrewAxis_; -// Vector6 cScrewAxis_; - -// public: -// /// Return transform of child link CoM frame w.r.t parent link CoM frame -// Pose3 parentTchild(double q, gtsam::OptionalJacobian<6, 1> pMc_H_q = -// boost::none) const override; - -// protected: -// /// Return transform of parent link CoM frame w.r.t child link CoM frame -// Pose3 childTparent(double q, gtsam::OptionalJacobian<6, 1> cMp_H_q = -// boost::none) const override; - -// /** -// * Return the joint axis in the joint frame. Rotational axis for revolute and -// * translation direction for prismatic in the joint frame. -// */ -// const gtsam::Vector3 &axis() const { return axis_; } - -// public: -// /** -// * Constructor using JointParams, joint name, bTj, screw axes, -// * and parent and child links. -// */ -// ScrewJointBase(uint8_t id, const std::string &name, const Pose3 &bTj, -// const LinkSharedPtr &parent_link, -// const LinkSharedPtr &child_link, const gtsam::Vector3 &axis, -// const Vector6 &jScrewAxis, -// const JointParams ¶meters = JointParams()) -// : JointTyped(id, name, bTj, parent_link, child_link, parameters), -// axis_(axis), -// pScrewAxis_(-jMp_.inverse().AdjointMap() * jScrewAxis), -// cScrewAxis_(jMc_.inverse().AdjointMap() * jScrewAxis) {} - -// /// Return joint type for use in reconstructing robot from JointParams. -// Type type() const override { return Type::ScrewAxis; } - -// /// Return screw axis expressed in the specified link frame -// const Vector6 screwAxis(const LinkSharedPtr &link) const { -// return isChildLink(link) ? cScrewAxis_ : pScrewAxis_; -// } - -// // inherit overloads -// using JointTyped::poseOf; -// using JointTyped::relativePoseOf; -// using JointTyped::transformTwistAccelTo; -// using JointTyped::transformTwistTo; - -// /** -// * Return the twist of this link given the other link's twist and joint angle. -// */ -// Vector6 transformTwistTo( -// const LinkSharedPtr &link, double q, double q_dot, -// boost::optional other_twist = boost::none, -// gtsam::OptionalJacobian<6, 1> H_q = boost::none, -// gtsam::OptionalJacobian<6, 1> H_q_dot = boost::none, -// gtsam::OptionalJacobian<6, 6> H_other_twist = boost::none) const override; - -// /** -// * Return the twist acceleration of this link given the other link's twist -// * acceleration, twist, and joint angle and this link's twist. -// */ -// Vector6 transformTwistAccelTo( -// const LinkSharedPtr &link, double q, double q_dot, double q_ddot, -// boost::optional this_twist = boost::none, -// boost::optional other_twist_accel = boost::none, -// gtsam::OptionalJacobian<6, 1> H_q = boost::none, -// gtsam::OptionalJacobian<6, 1> H_q_dot = boost::none, -// gtsam::OptionalJacobian<6, 1> H_q_ddot = boost::none, -// gtsam::OptionalJacobian<6, 6> H_this_twist = boost::none, -// gtsam::OptionalJacobian<6, 6> H_other_twist_accel = -// boost::none) const override; - -// JointTorque transformWrenchToTorque( -// const LinkSharedPtr &link, boost::optional wrench = boost::none, -// gtsam::OptionalJacobian<1, 6> H_wrench = boost::none) const override; - -// // TODO(frank): document and possibly eliminate -// gtsam::Matrix6 AdjointMapJacobianJointAngle(const LinkSharedPtr &link, -// double q) const override { -// return AdjointMapJacobianQ(q, relativePoseOf(otherLink(link), 0), -// screwAxis(link)); -// } - -// /// Return forward dynamics priors on torque. -// gtsam::GaussianFactorGraph linearFDPriors( -// size_t t, const gtsam::Values &known_values, -// const OptimizerSetting &opt) const override; - -// /// Return linearized acceleration factors. -// gtsam::GaussianFactorGraph linearAFactors( -// size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, -// const boost::optional &planar_axis) const override; - -// /// Return linearized dynamics factors. -// gtsam::GaussianFactorGraph linearDynamicsFactors( -// size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, -// const boost::optional &planar_axis) const override; - -// /// Return joint limit factors. -// gtsam::NonlinearFactorGraph jointLimitFactors( -// size_t t, const OptimizerSetting &opt) const override; - -// /// Joint-induced twist in child frame -// gtsam::Vector6 childTwist(double q_dot) const override { -// return cScrewAxis_ * q_dot; -// } - -// /// Joint-induced twist in parent frame -// gtsam::Vector6 parentTwist(double q_dot) const override { -// return pScrewAxis_ * q_dot; -// } - -// /// Helper function for overloading stream operator -// virtual std::ostream &to_stream(std::ostream &os) const override; -// }; - -// } // namespace gtdynamics diff --git a/gtdynamics/universal_robot/sdf.cpp b/gtdynamics/universal_robot/sdf.cpp index b164263b..3dfceb45 100644 --- a/gtdynamics/universal_robot/sdf.cpp +++ b/gtdynamics/universal_robot/sdf.cpp @@ -21,7 +21,6 @@ #include "gtdynamics/universal_robot/PrismaticJoint.h" #include "gtdynamics/universal_robot/RevoluteJoint.h" #include "gtdynamics/universal_robot/ScrewJoint.h" -#include "gtdynamics/universal_robot/ScrewJointBase.h" #include "gtdynamics/universal_robot/sdf_internal.h" namespace gtdynamics { diff --git a/gtdynamics/universal_robot/sdf.h b/gtdynamics/universal_robot/sdf.h index 4d32ab7f..63eeb9b4 100644 --- a/gtdynamics/universal_robot/sdf.h +++ b/gtdynamics/universal_robot/sdf.h @@ -16,7 +16,6 @@ #include #include "gtdynamics/universal_robot/Robot.h" -#include "gtdynamics/universal_robot/ScrewJointBase.h" namespace gtdynamics { diff --git a/gtdynamics/universal_robot/sdf_internal.h b/gtdynamics/universal_robot/sdf_internal.h index 4a6f9dc9..16f66c00 100644 --- a/gtdynamics/universal_robot/sdf_internal.h +++ b/gtdynamics/universal_robot/sdf_internal.h @@ -18,7 +18,6 @@ #include #include "gtdynamics/universal_robot/Robot.h" -#include "gtdynamics/universal_robot/ScrewJointBase.h" namespace gtdynamics { diff --git a/gtdynamics/utils/JsonSaver.h b/gtdynamics/utils/JsonSaver.h index 3b2d969f..16e9aa83 100644 --- a/gtdynamics/utils/JsonSaver.h +++ b/gtdynamics/utils/JsonSaver.h @@ -34,7 +34,7 @@ #include "gtdynamics/factors/WrenchEquivalenceFactor.h" #include "gtdynamics/factors/WrenchFactor.h" #include "gtdynamics/factors/WrenchPlanarFactor.h" -#include "gtdynamics/universal_robot/ScrewJointBase.h" +#include "gtdynamics/universal_robot/Joint.h" #include "gtdynamics/utils/utils.h" #define kQuote_ "\"" diff --git a/tests/testRevoluteJoint.cpp b/tests/testRevoluteJoint.cpp index b3f5d15c..bd5825f3 100644 --- a/tests/testRevoluteJoint.cpp +++ b/tests/testRevoluteJoint.cpp @@ -159,7 +159,6 @@ TEST(Joint, ValuesRelativePoseOf) { RevoluteJoint j1(1, "j1", Pose3(Rot3(), Point3(0, 0, 2)), l1, l2, axis, parameters); - const size_t t = 777; // Rotating joint by -M_PI / 2 double q = -M_PI / 2; Pose3 T12(Rot3::Rx(q), Point3(0, 1, 1)); From c4e5eb73266ac9f4cecd60a41e60fd83827cf9a6 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 15 Oct 2021 22:39:50 -0400 Subject: [PATCH 07/14] fix python unit test for RevoluteJointMeasurementFactor --- python/tests/test_factors.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/python/tests/test_factors.py b/python/tests/test_factors.py index 632b08c2..338c1ca9 100644 --- a/python/tests/test_factors.py +++ b/python/tests/test_factors.py @@ -30,9 +30,7 @@ def setUp(self): def test_revolute_joint_measurement_factor(self): """Test RevoluteJointMeasurementFactor.""" - factor = gtd.RevoluteJointMeasurementFactor( - self.wTp_key, self.wTc_key, - gtsam.noiseModel.Isotropic.Sigma(6, 0.1), - self.robot.joint("joint_1"), np.pi / 4, self.k) + factor = gtd.JointMeasurementFactor(gtsam.noiseModel.Isotropic.Sigma(6, 0.1), + self.robot.joint("joint_1"), np.pi / 4, self.k) - self.assertTrue(isinstance(factor, gtd.RevoluteJointMeasurementFactor)) + self.assertTrue(isinstance(factor, gtd.JointMeasurementFactor)) From bb809558ddc540df13414086edf32677ffb8728e Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sat, 16 Oct 2021 14:34:01 -0400 Subject: [PATCH 08/14] values.h functions are no longer templated on JointCoordinate --- gtdynamics.i | 36 +++++---------- gtdynamics/dynamics/Simulator.h | 6 +-- gtdynamics/factors/PoseFactor.h | 6 +-- gtdynamics/utils/values.cpp | 50 ++++++++++++++++++++- gtdynamics/utils/values.h | 80 +++++++-------------------------- 5 files changed, 83 insertions(+), 95 deletions(-) diff --git a/gtdynamics.i b/gtdynamics.i index 1bbf82f7..117d3d05 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -708,49 +708,37 @@ gtdynamics::DynamicsSymbol PhaseKey(int k); gtdynamics::DynamicsSymbol TimeKey(int t); ///////////////////// Key Methods ///////////////////// -template -void InsertJointAngle(gtsam::Values@ values, int j, int t, T value); +void InsertJointAngle(gtsam::Values@ values, int j, int t, double value); -template -void InsertJointAngle(gtsam::Values @values, int j, T value); +void InsertJointAngle(gtsam::Values @values, int j, double value); gtsam::Vector JointAngle(const gtsam::VectorValues &values, int j, int t); -template -T JointAngle(const gtsam::Values &values, int j, int t); +double JointAngle(const gtsam::Values &values, int j, int t); -template -void InsertJointVel(gtsam::Values @values, int j, int t, T value); +void InsertJointVel(gtsam::Values @values, int j, int t, double value); -template -void InsertJointVel(gtsam::Values @values, int j, T value); +void InsertJointVel(gtsam::Values @values, int j, double value); gtsam::Vector JointVel(const gtsam::VectorValues &values, int j, int t); -template -T JointVel(const gtsam::Values &values, int j, int t); +double JointVel(const gtsam::Values &values, int j, int t); -template -void InsertJointAccel(gtsam::Values @values, int j, int t, T value); +void InsertJointAccel(gtsam::Values @values, int j, int t, double value); -template -void InsertJointAccel(gtsam::Values @values, int j, T value); +void InsertJointAccel(gtsam::Values @values, int j, double value); gtsam::Vector JointAccel(const gtsam::VectorValues &values, int j, int t); -template -T JointAccel(const gtsam::Values &values, int j, int t); +double JointAccel(const gtsam::Values &values, int j, int t); -template -void InsertTorque(gtsam::Values @values, int j, int t, T value); +void InsertTorque(gtsam::Values @values, int j, int t, double value); -template -void InsertTorque(gtsam::Values @values, int j, T value); +void InsertTorque(gtsam::Values @values, int j, double value); gtsam::Vector Torque(const gtsam::VectorValues &values, int j, int t); -template -T Torque(const gtsam::Values &values, int j, int t); +double Torque(const gtsam::Values &values, int j, int t); void InsertPose(gtsam::Values @values, int i, int t, gtsam::Pose3 value); diff --git a/gtdynamics/dynamics/Simulator.h b/gtdynamics/dynamics/Simulator.h index 65da17ef..4fe5a8d1 100644 --- a/gtdynamics/dynamics/Simulator.h +++ b/gtdynamics/dynamics/Simulator.h @@ -93,9 +93,9 @@ class Simulator { const double dt2 = std::pow(dt, 2); for (auto &&joint : robot_.joints()) { auto j = joint->id(); - const double q = JointAngle(current_values_, j); - const double v = JointVel(current_values_, j); - const double a = JointAccel(current_values_, j); + const double q = JointAngle(current_values_, j); + const double v = JointVel(current_values_, j); + const double a = JointAccel(current_values_, j); // TODO(frank): one could use t values and save entire simulation. const double v_new = v + dt * a; diff --git a/gtdynamics/factors/PoseFactor.h b/gtdynamics/factors/PoseFactor.h index aa679e42..6a2a172b 100644 --- a/gtdynamics/factors/PoseFactor.h +++ b/gtdynamics/factors/PoseFactor.h @@ -106,9 +106,9 @@ class PoseFactor : public gtsam::NoiseModelFactor { boost::optional wTc_hat_H_q_ref; if (H) wTc_hat_H_q_ref = wTc_hat_H_q; - auto wTc_hat = joint_->poseOf(joint_->child(), wTp, - JointAngle(x, joint_->id(), t_), - H ? &wTc_hat_H_wTp : 0, wTc_hat_H_q_ref); + auto wTc_hat = + joint_->poseOf(joint_->child(), wTp, JointAngle(x, joint_->id(), t_), + H ? &wTc_hat_H_wTp : 0, wTc_hat_H_q_ref); gtsam::Vector6 error = wTc.logmap(wTc_hat, H ? &H_wTc : 0, H ? &H_wTc_hat : 0); if (H) { diff --git a/gtdynamics/utils/values.cpp b/gtdynamics/utils/values.cpp index 4eacb46c..3538148a 100644 --- a/gtdynamics/utils/values.cpp +++ b/gtdynamics/utils/values.cpp @@ -32,26 +32,74 @@ const char *KeyDoesNotExist::what() const noexcept { } /* ************************************************************************* */ +void InsertJointAngle(Values *values, int j, int t, double value) { + values->insert(internal::JointAngleKey(j, t), value); +} + +void InsertJointAngle(Values *values, int j, double value) { + values->insert(internal::JointAngleKey(j), value); +} + Vector JointAngle(const VectorValues &values, int j, int t) { return values.at(internal::JointAngleKey(j, t)); } +double JointAngle(const Values &values, int j, int t) { + return internal::at(values, internal::JointAngleKey(j, t)); +} + /* ************************************************************************* */ +void InsertJointVel(Values *values, int j, int t, double value) { + values->insert(internal::JointVelKey(j, t), value); +} + +void InsertJointVel(Values *values, int j, double value) { + values->insert(internal::JointVelKey(j), value); +} + Vector JointVel(const VectorValues &values, int j, int t) { return values.at(internal::JointVelKey(j, t)); } +double JointVel(const Values &values, int j, int t) { + return internal::at(values, internal::JointVelKey(j, t)); +} + /* ************************************************************************* */ +void InsertJointAccel(Values *values, int j, int t, double value) { + values->insert(internal::JointAccelKey(j, t), value); +} + +void InsertJointAccel(Values *values, int j, double value) { + values->insert(internal::JointAccelKey(j), value); +} + Vector JointAccel(const VectorValues &values, int j, int t) { return values.at(internal::JointAccelKey(j, t)); } +double JointAccel(const Values &values, int j, int t) { + return internal::at(values, internal::JointAccelKey(j, t)); +} + + /* ************************************************************************* */ -/// Retrieve torque on the j-th joint at time t. +void InsertTorque(Values *values, int j, int t, double value) { + values->insert(internal::TorqueKey(j, t), value); +} + +void InsertTorque(Values *values, int j, double value) { + values->insert(internal::TorqueKey(j), value); +} + Vector Torque(const VectorValues &values, int j, int t) { return values.at(internal::TorqueKey(j, t)); }; +double Torque(const Values &values, int j, int t) { + return internal::at(values, internal::TorqueKey(j, t)); +}; + /* ************************************************************************* */ /// Insert pose for i-th link at time t. void InsertPose(Values *values, int i, int t, Pose3 value) { diff --git a/gtdynamics/utils/values.h b/gtdynamics/utils/values.h index 35cab883..081b9588 100644 --- a/gtdynamics/utils/values.h +++ b/gtdynamics/utils/values.h @@ -98,29 +98,21 @@ T at(const gtsam::Values &values, size_t key) { /** * @brief Insert j-th joint angle at time t. * - * @tparam T Joint angle type (default `double`). * @param values Values pointer to insert joint angle into. * @param j The joint id. * @param t Time step. * @param value The joint angle value. */ -template -void InsertJointAngle(gtsam::Values *values, int j, int t, T value) { - values->insert(internal::JointAngleKey(j, t), value); -} +void InsertJointAngle(gtsam::Values *values, int j, int t, double value); /** * @brief Insert j-th joint angle at time 0. * - * @tparam T Joint angle type (default `double`). * @param values Values pointer to insert joint angle into. * @param j The joint id. * @param value The joint angle value. */ -template -void InsertJointAngle(gtsam::Values *values, int j, T value) { - values->insert(internal::JointAngleKey(j), value); -} +void InsertJointAngle(gtsam::Values *values, int j, double value); /** * @brief Retrieve j-th joint angle at time t as a Vector. @@ -136,16 +128,12 @@ gtsam::Vector JointAngle(const gtsam::VectorValues &values, int j, int t = 0); /** * @brief Retrieve j-th joint angle at time t. * - * @tparam T Joint angle type (default is `double`). * @param values Values dictionary containing the joint angle. * @param j The joint id. * @param t Time step. - * @return T + * @return The joint angle */ -template -T JointAngle(const gtsam::Values &values, int j, int t = 0) { - return internal::at(values, internal::JointAngleKey(j, t)); -} +double JointAngle(const gtsam::Values &values, int j, int t = 0); /* ************************************************************************* Functions for Joint Velocities. @@ -154,29 +142,21 @@ T JointAngle(const gtsam::Values &values, int j, int t = 0) { /** * @brief Insert j-th joint velocity at time t. * - * @tparam T Joint velocity type (default `double`). * @param values Values dictionary pointer to insert joint velocity into. * @param j The joint id. * @param t Time step. * @param value The joint velocity value. */ -template -void InsertJointVel(gtsam::Values *values, int j, int t, T value) { - values->insert(internal::JointVelKey(j, t), value); -} +void InsertJointVel(gtsam::Values *values, int j, int t, double value); /** * @brief Insert j-th joint velocity at time 0. * - * @tparam T Joint velocity type (default `double`). * @param values Values pointer to insert joint velocity into. * @param j The joint id. * @param value The joint velocity value. */ -template -void InsertJointVel(gtsam::Values *values, int j, T value) { - values->insert(internal::JointVelKey(j), value); -} +void InsertJointVel(gtsam::Values *values, int j, double value); /** * @brief Retrieve j-th joint velocity at time t. @@ -192,16 +172,12 @@ gtsam::Vector JointVel(const gtsam::VectorValues &values, int j, int t = 0); /** * @brief Retrieve j-th joint velocity at time t. * - * @tparam T Joint velocity type (default is `double`). * @param values Values dictionary containing the joint velocity. * @param j The joint id. * @param t Time step. - * @return T + * @return The joint velocity */ -template -T JointVel(const gtsam::Values &values, int j, int t = 0) { - return internal::at(values, internal::JointVelKey(j, t)); -} +double JointVel(const gtsam::Values &values, int j, int t = 0); /* ************************************************************************* Functions for Joint Accelerations. @@ -210,29 +186,21 @@ T JointVel(const gtsam::Values &values, int j, int t = 0) { /** * @brief Insert j-th joint acceleration at time t. * - * @tparam T Joint acceleration type (default `double`). * @param values Values dictionary pointer to insert joint acceleration into. * @param j The joint id. * @param t Time step. * @param value The joint acceleration value. */ -template -void InsertJointAccel(gtsam::Values *values, int j, int t, T value) { - values->insert(internal::JointAccelKey(j, t), value); -} +void InsertJointAccel(gtsam::Values *values, int j, int t, double value); /** * @brief Insert j-th joint acceleration at time 0. * - * @tparam T Joint acceleration type (default `double`). * @param values Values pointer to insert joint acceleration into. * @param j The joint id. * @param value The joint acceleration value. */ -template -void InsertJointAccel(gtsam::Values *values, int j, T value) { - values->insert(internal::JointAccelKey(j), value); -} +void InsertJointAccel(gtsam::Values *values, int j, double value); /** * @brief Retrieve j-th joint acceleration at time t. @@ -248,16 +216,12 @@ gtsam::Vector JointAccel(const gtsam::VectorValues &values, int j, int t = 0); /** * @brief Retrieve j-th joint acceleration at time t. * - * @tparam T Joint acceleration type (default is `double`). * @param values Values dictionary containing the joint acceleration. * @param j The joint id. * @param t Time step. - * @return T + * @return The joint acceleration */ -template -T JointAccel(const gtsam::Values &values, int j, int t = 0) { - return internal::at(values, internal::JointAccelKey(j, t)); -} +double JointAccel(const gtsam::Values &values, int j, int t = 0); /* ************************************************************************* Functions for Torques. @@ -266,29 +230,21 @@ T JointAccel(const gtsam::Values &values, int j, int t = 0) { /** * @brief Insert torque on the j-th joint at time t. * - * @tparam T Torque type (default `double`). * @param values Values dictionary pointer to insert torque into. * @param j The joint id. * @param t Time step. * @param value The torque value. */ -template -void InsertTorque(gtsam::Values *values, int j, int t, T value) { - values->insert(internal::TorqueKey(j, t), value); -} +void InsertTorque(gtsam::Values *values, int j, int t, double value); /** * @brief Insert torque on the j-th joint at time 0. * - * @tparam T Torque type (default `double`). * @param values Values pointer to insert torque into. * @param j The joint id. * @param value The torque value. */ -template -void InsertTorque(gtsam::Values *values, int j, T value) { - values->insert(internal::TorqueKey(j), value); -} +void InsertTorque(gtsam::Values *values, int j, double value); /** * @brief Retrieve torque on the j-th joint at time t. @@ -304,16 +260,12 @@ gtsam::Vector Torque(const gtsam::VectorValues &values, int j, int t = 0); /** * @brief Retrieve torque on the j-th joint at time t. * - * @tparam T Joint torque type (default is `double`). * @param values Values dictionary containing the joint torque. * @param j The joint id. * @param t Time step. - * @return T + * @return The joint torque */ -template -T Torque(const gtsam::Values &values, int j, int t = 0) { - return internal::at(values, internal::TorqueKey(j, t)); -}; +double Torque(const gtsam::Values &values, int j, int t = 0); /* ************************************************************************* Functions for Poses. From 60122c1c3f958b94f3720a63b9d80657abea809d Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sat, 16 Oct 2021 14:41:02 -0400 Subject: [PATCH 09/14] update python to not use "Double" for values --- .../inverted_pendulum.py | 8 +-- .../example_jumping_robot/src/jr_simulator.py | 10 ++-- .../example_jumping_robot/src/jr_values.py | 20 +++---- .../src/jr_visualizer.py | 6 +-- .../tests/test_jr_simulator.py | 8 +-- .../tests/test_jumping_robot.py | 4 +- gtdynamics/cablerobot/src/cdpr_planar_sim.py | 10 ++-- .../cablerobot/src/gerry00_planar_sim.py | 8 +-- gtdynamics/cablerobot/src/test_cdpr_planar.py | 22 ++++---- .../src/test_cdpr_planar_controller.py | 2 +- .../cablerobot/src/test_cdpr_planar_sim.py | 8 +-- gtdynamics/cablerobot/src/utils.py | 6 +-- python/gtdynamics/__init__.py | 52 +++++++++++++++++++ python/tests/test_forward_kinematics.py | 2 +- python/tests/test_four_bar.py | 6 +-- python/tests/test_print.py | 2 +- python/tests/test_robot.py | 2 +- python/tests/test_simulator.py | 8 +-- 18 files changed, 118 insertions(+), 66 deletions(-) diff --git a/examples/example_inverted_pendulum_trajectory_optimization/inverted_pendulum.py b/examples/example_inverted_pendulum_trajectory_optimization/inverted_pendulum.py index ed0dc2e7..e9019f83 100644 --- a/examples/example_inverted_pendulum_trajectory_optimization/inverted_pendulum.py +++ b/examples/example_inverted_pendulum_trajectory_optimization/inverted_pendulum.py @@ -112,10 +112,10 @@ def time(values, id, t): key: [fn(results, j1_id, t) for t in range(t_steps + 1)] for (key, fn) in [ ("t", time), # - ("theta", gtd.JointAngleDouble), - ("dtheta", gtd.JointVelDouble), - ("ddtheta", gtd.JointAccelDouble), - ("tau", gtd.TorqueDouble) + ("theta", gtd.JointAngle), + ("dtheta", gtd.JointVel), + ("ddtheta", gtd.JointAccel), + ("tau", gtd.Torque) ] } df = pd.DataFrame(data) diff --git a/examples/example_jumping_robot/src/jr_simulator.py b/examples/example_jumping_robot/src/jr_simulator.py index 59e9697c..e3243cbd 100644 --- a/examples/example_jumping_robot/src/jr_simulator.py +++ b/examples/example_jumping_robot/src/jr_simulator.py @@ -134,7 +134,7 @@ def step_robot_dynamics_by_layer(self, k, values): for joint in self.jr.robot.joints(): j = joint.id() q_key = gtd.internal.JointAngleKey(j, k).key() - graph_q.add(gtd.PriorFactorDouble(q_key, gtd.JointAngleDouble(values, j, k), opt.prior_q_cost_model)) + graph_q.add(gtd.PriorFactorDouble(q_key, gtd.JointAngle(values, j, k), opt.prior_q_cost_model)) init_values_q = gtd.ExtractValues(init_values, graph_q.keys()) results_q = self.optimize(graph_q, init_values_q) @@ -153,7 +153,7 @@ def step_robot_dynamics_by_layer(self, k, values): for joint in self.jr.robot.joints(): j = joint.id() v_key = gtd.internal.JointVelKey(j, k).key() - graph_v.add(gtd.PriorFactorDouble(v_key, gtd.JointVelDouble(values, j, k), opt.prior_qv_cost_model)) + graph_v.add(gtd.PriorFactorDouble(v_key, gtd.JointVel(values, j, k), opt.prior_qv_cost_model)) init_values_v = gtd.ExtractValues(init_values, graph_v.keys()) results_v = self.optimize(graph_v, init_values_v) @@ -230,9 +230,9 @@ def step_robot_dynamics(self, k, values): for joint in self.jr.robot.joints(): j = joint.id() q_key = gtd.internal.JointAngleKey(j, k).key() - graph.add(gtd.PriorFactorDouble(q_key, gtd.JointAngleDouble(values, j, k), opt.prior_q_cost_model)) + graph.add(gtd.PriorFactorDouble(q_key, gtd.JointAngle(values, j, k), opt.prior_q_cost_model)) v_key = gtd.internal.JointVelKey(j, k).key() - graph.add(gtd.PriorFactorDouble(v_key, gtd.JointVelDouble(values, j, k), opt.prior_v_cost_model)) + graph.add(gtd.PriorFactorDouble(v_key, gtd.JointVel(values, j, k), opt.prior_v_cost_model)) # construct initial values if k==0: @@ -347,7 +347,7 @@ def simulate_with_torque_seq(self, num_steps, dt, torques_seq): self.step_integration(k, dt, values, False) for joint in self.jr.robot.joints(): j = joint.id() - gtd.InsertTorqueDouble(values, j, k, torques_seq[k][j]) + gtd.InsertTorque(values, j, k, torques_seq[k][j]) self.step_robot_dynamics_by_layer(k, values) phase = self.step_phase_change(k, phase, values) step_phases.append(phase) diff --git a/examples/example_jumping_robot/src/jr_values.py b/examples/example_jumping_robot/src/jr_values.py index 9c03414d..2cac38ce 100644 --- a/examples/example_jumping_robot/src/jr_values.py +++ b/examples/example_jumping_robot/src/jr_values.py @@ -232,8 +232,8 @@ def init_values_from_prev_robot(robot, k, values): v_key = gtd.internal.JointVelKey(j, k).key() JRValues.copy_value(values, init_values, q_key) JRValues.copy_value(values, init_values, v_key) - gtd.InsertJointAccelDouble( - init_values, j, k, gtd.JointAccelDouble(values, j, k-1)) + gtd.InsertJointAccel( + init_values, j, k, gtd.JointAccel(values, j, k-1)) i1 = joint.parent().id() i2 = joint.child().id() gtd.InsertWrench(init_values, i1, j, k, @@ -300,17 +300,17 @@ def init_values_from_fk_robot(jr, k, values): v = values.atDouble(v_key) init_values.insert(q_key, q) init_values.insert(v_key, v) - gtd.InsertJointAccelDouble(init_values, j, k, 0.0) + gtd.InsertJointAccel(init_values, j, k, 0.0) i1 = joint.parent().id() i2 = joint.child().id() gtd.InsertWrench(init_values, i1, j, k, np.zeros(6)) gtd.InsertWrench(init_values, i2, j, k, np.zeros(6)) torque_key = gtd.internal.TorqueKey(j, k).key() if values.exists(torque_key): - gtd.InsertTorqueDouble( + gtd.InsertTorque( init_values, j, k, values.atDouble(torque_key)) else: - gtd.InsertTorqueDouble(init_values, j, k, 0.0) + gtd.InsertTorque(init_values, j, k, 0.0) return init_values @@ -319,13 +319,13 @@ def integrate_joints(jr, values, k, dt): """ Integrate joint angle and velocity and add to values. """ for joint in jr.robot.joints(): j = joint.id() - q_prev = gtd.JointAngleDouble(values, j, k-1) - v_prev = gtd.JointVelDouble(values, j, k-1) - a_prev = gtd.JointAccelDouble(values, j, k-1) + q_prev = gtd.JointAngle(values, j, k-1) + v_prev = gtd.JointVel(values, j, k-1) + a_prev = gtd.JointAccel(values, j, k-1) v_curr = v_prev + a_prev * dt q_curr = q_prev + v_prev * dt + 0.5 * a_prev * dt * dt - gtd.InsertJointAngleDouble(values, j, k, q_curr) - gtd.InsertJointVelDouble(values, j, k, v_curr) + gtd.InsertJointAngle(values, j, k, q_curr) + gtd.InsertJointVel(values, j, k, v_curr) @staticmethod def integrate_torso(jr, values, k, dt): diff --git a/examples/example_jumping_robot/src/jr_visualizer.py b/examples/example_jumping_robot/src/jr_visualizer.py index 781806b2..287b2036 100644 --- a/examples/example_jumping_robot/src/jr_visualizer.py +++ b/examples/example_jumping_robot/src/jr_visualizer.py @@ -119,9 +119,9 @@ def make_plot(values, jr, num_steps): for k in range(num_steps): for name in joint_names: j = jr.robot.joint(name).id() - qs_dict[name].append(gtd.JointAngleDouble(values, j, k)) - vs_dict[name].append(gtd.JointVelDouble(values, j, k)) - torques_dict[name].append(gtd.TorqueDouble(values, j, k)) + qs_dict[name].append(gtd.JointAngle(values, j, k)) + vs_dict[name].append(gtd.JointVel(values, j, k)) + torques_dict[name].append(gtd.Torque(values, j, k)) pressures_dict[name].append(values.atDouble(Actuator.PressureKey(j, k))) masses_dict[name].append(values.atDouble(Actuator.MassKey(j, k))) mdots_dict[name].append(values.atDouble(Actuator.MassRateActualKey(j, k))) diff --git a/examples/example_jumping_robot/tests/test_jr_simulator.py b/examples/example_jumping_robot/tests/test_jr_simulator.py index 2fb6ad44..2c7aff72 100644 --- a/examples/example_jumping_robot/tests/test_jr_simulator.py +++ b/examples/example_jumping_robot/tests/test_jr_simulator.py @@ -74,9 +74,9 @@ def test_robot_forward_dynamics(self): k = 0 for joint in self.robot().joints(): j = joint.id() - gtd.InsertTorqueDouble(values, j, k, torques[j]) - gtd.InsertJointAngleDouble(values, j, k, qs[j]) - gtd.InsertJointVelDouble(values, j, k, vs[j]) + gtd.InsertTorque(values, j, k, torques[j]) + gtd.InsertJointAngle(values, j, k, qs[j]) + gtd.InsertJointVel(values, j, k, vs[j]) torso_pose = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0, 0, 0.55)) torso_i = self.robot().link("torso").id() gtd.InsertPose(values, torso_i, k, torso_pose) @@ -114,7 +114,7 @@ def test_actuation_forward_dynamics(self): torques = [] for actuator in self.jr_simulator.jr.actuators: j = actuator.j - torque = gtd.TorqueDouble(values, j, k) + torque = gtd.Torque(values, j, k) torques.append(torque) self.assertAlmostEqual(torque, 0, places=7) # TODO(yetong): check torques, pressures, etc diff --git a/examples/example_jumping_robot/tests/test_jumping_robot.py b/examples/example_jumping_robot/tests/test_jumping_robot.py index 30e279e2..4d910262 100644 --- a/examples/example_jumping_robot/tests/test_jumping_robot.py +++ b/examples/example_jumping_robot/tests/test_jumping_robot.py @@ -48,8 +48,8 @@ def test_forward_kinematics(self): qs = [-theta, 2 * theta, -theta, -theta, 2 * theta, -theta] for joint in self.jr.robot.joints(): j = joint.id() - gtd.InsertJointAngleDouble(values, j, k, qs[j]) - gtd.InsertJointVelDouble(values, j, k, 0.) + gtd.InsertJointAngle(values, j, k, qs[j]) + gtd.InsertJointVel(values, j, k, 0.) fk_results = self.jr.robot.forwardKinematics(values, k) torso_i = self.jr.robot.link("torso").id() diff --git a/gtdynamics/cablerobot/src/cdpr_planar_sim.py b/gtdynamics/cablerobot/src/cdpr_planar_sim.py index ad6349ca..ad6846f6 100644 --- a/gtdynamics/cablerobot/src/cdpr_planar_sim.py +++ b/gtdynamics/cablerobot/src/cdpr_planar_sim.py @@ -76,8 +76,8 @@ def update_kinematics(cdpr, fg, x, k): Vs=[gtd.Twist(x, cdpr.ee_id(), k)])) # IK initial estimate for j in range(4): - gtd.InsertJointAngleDouble(x, j, k, 0) - gtd.InsertJointVelDouble(x, j, k, 0) + gtd.InsertJointAngle(x, j, k, 0) + gtd.InsertJointVel(x, j, k, 0) # IK solve result = gtsam.LevenbergMarquardtOptimizer(fg, x).optimize() assert abs(fg.error(result)) < 1e-20, "inverse kinematics didn't converge" @@ -108,10 +108,10 @@ def update_dynamics(cdpr, fg, x, u, k, dt): fg.push_back(cdpr.collocation_factors(ks=[k], dt=dt)) # ID priors (torque inputs) fg.push_back( - cdpr.priors_id(ks=[k], torquess=[[gtd.TorqueDouble(u, ji, k) for ji in range(4)]])) + cdpr.priors_id(ks=[k], torquess=[[gtd.Torque(u, ji, k) for ji in range(4)]])) # ID initial guess for ji in range(4): - gtd.InsertTorqueDouble(x, ji, k, gtd.TorqueDouble(u, ji, k)) + gtd.InsertTorque(x, ji, k, gtd.Torque(u, ji, k)) gtd.InsertWrench(x, cdpr.ee_id(), ji, k, np.zeros(6)) gtd.InsertPose(x, cdpr.ee_id(), k+1, gtsam.Pose3(gtsam.Rot3(), (1.5, 0, 1.5))) gtd.InsertTwist(x, cdpr.ee_id(), k+1, np.zeros(6)) @@ -144,7 +144,7 @@ def step(self, verbose=False): u = self.controller.update(self.x, self.k) if verbose: print('control torques: {:.2e}, {:.2e}, {:.2e}, {:.2e}'.format( - *[gtd.TorqueDouble(u, ji, self.k) for ji in range(4)])) + *[gtd.Torque(u, ji, self.k) for ji in range(4)])) self.update_dynamics(self.cdpr, self.fg, self.x, u, self.k, self.dt) self.k += 1 return self.x diff --git a/gtdynamics/cablerobot/src/gerry00_planar_sim.py b/gtdynamics/cablerobot/src/gerry00_planar_sim.py index c633e1c3..84b7f989 100644 --- a/gtdynamics/cablerobot/src/gerry00_planar_sim.py +++ b/gtdynamics/cablerobot/src/gerry00_planar_sim.py @@ -26,10 +26,10 @@ def __init__(self, N): self.N = N def update(self, values, t): tau = gtsam.Values() - gtd.InsertTorqueDouble(tau, 0, t, 3 * np.cos(2. * t / self.N * np.pi)) - gtd.InsertTorqueDouble(tau, 1, t, 3 * np.cos(2. * t / self.N * np.pi + np.pi/2)) - gtd.InsertTorqueDouble(tau, 2, t, 3 * np.cos(2. * t / self.N * np.pi + np.pi)) - gtd.InsertTorqueDouble(tau, 3, t, 3 * np.cos(2. * t / self.N * np.pi + 3*np.pi/2)) + gtd.InsertTorque(tau, 0, t, 3 * np.cos(2. * t / self.N * np.pi)) + gtd.InsertTorque(tau, 1, t, 3 * np.cos(2. * t / self.N * np.pi + np.pi/2)) + gtd.InsertTorque(tau, 2, t, 3 * np.cos(2. * t / self.N * np.pi + np.pi)) + gtd.InsertTorque(tau, 3, t, 3 * np.cos(2. * t / self.N * np.pi + 3*np.pi/2)) return tau def main(): diff --git a/gtdynamics/cablerobot/src/test_cdpr_planar.py b/gtdynamics/cablerobot/src/test_cdpr_planar.py index 44fc3095..310fabfd 100644 --- a/gtdynamics/cablerobot/src/test_cdpr_planar.py +++ b/gtdynamics/cablerobot/src/test_cdpr_planar.py @@ -32,16 +32,16 @@ def testKinematics(self): def zeroValues(): values = gtsam.Values() for j in range(4): - gtd.InsertJointAngleDouble(values, j, 0, 3.0) - gtd.InsertJointVelDouble(values, j, 0, 17.0 + np.random.rand(1)) + gtd.InsertJointAngle(values, j, 0, 3.0) + gtd.InsertJointVel(values, j, 0, 17.0 + np.random.rand(1)) gtd.InsertPose(values, cdpr.ee_id(), 0, Pose3()) gtd.InsertTwist(values, cdpr.ee_id(), 0, (0,0,0,0,0,1.4142)) return values # things needed to define FK values = gtsam.Values() for j, l, ldot in zip(range(4), [1.35 * np.sqrt(2),]*4, [-1, 1, 1, -1.]): - gtd.InsertJointAngleDouble(values, j, 0, l) - gtd.InsertJointVelDouble(values, j, 0, ldot) + gtd.InsertJointAngle(values, j, 0, l) + gtd.InsertJointVel(values, j, 0, ldot) # things needed to define IK gtd.InsertPose(values, cdpr.ee_id(), 0, Pose3(Rot3(), (1.5, 0, 1.5))) gtd.InsertTwist(values, cdpr.ee_id(), 0, (0, 0, 0, 0, 0, -np.sqrt(2))) @@ -56,8 +56,8 @@ def zeroValues(): # try optimizing FK fkgraph = gtsam.NonlinearFactorGraph(kfg) fkgraph.push_back(cdpr.priors_fk([0], - [[gtd.JointAngleDouble(values, ji, 0) for ji in range(4)]], - [[gtd.JointVelDouble(values, ji, 0) for ji in range(4)]])) + [[gtd.JointAngle(values, ji, 0) for ji in range(4)]], + [[gtd.JointVel(values, ji, 0) for ji in range(4)]])) params = gtsam.LevenbergMarquardtParams() params.setAbsoluteErrorTol(1e-20) # FK less sensitive so we need to decrease the tolerance fkres = gtsam.LevenbergMarquardtOptimizer(fkgraph, zeroValues(), params).optimize() @@ -74,7 +74,7 @@ def testDynamicsInstantaneous(self): gtd.InsertTwist(values, cdpr.ee_id(), 0, np.zeros(6)) # things needed to define ID for j, tau in zip(range(4), [1, 0, 0, 1]): - gtd.InsertTorqueDouble(values, j, 0, tau) + gtd.InsertTorque(values, j, 0, tau) # things needed to define FD gtd.InsertTwistAccel(values, cdpr.ee_id(), 0, (0, 0, 0, 0, 0, -np.sqrt(2))) # things needed intermediaries @@ -102,7 +102,7 @@ def testDynamicsInstantaneous(self): init = gtsam.Values(values) for ji in range(4): init.erase(gtd.internal.TorqueKey(ji, 0).key()) - gtd.InsertTorqueDouble(init, ji, 0, -1) + gtd.InsertTorque(init, ji, 0, -1) results = gtsam.LevenbergMarquardtOptimizer(dfg, init).optimize() self.gtsamAssertEquals(results, values) @@ -126,9 +126,9 @@ def testDynamicsCollocation(self): init.insert(0, 0.01) for t in range(3): for j in range(4): - gtd.InsertJointAngleDouble(init, j, t, 1) - gtd.InsertJointVelDouble(init, j, t, 1) - gtd.InsertTorqueDouble(init, j, t, 1) + gtd.InsertJointAngle(init, j, t, 1) + gtd.InsertJointVel(init, j, t, 1) + gtd.InsertTorque(init, j, t, 1) gtd.InsertWrench(init, cdpr.ee_id(), j, t, np.ones(6)) gtd.InsertPose(init, cdpr.ee_id(), t, Pose3(Rot3(), (1.5, 1, 1.5))) gtd.InsertTwist(init, cdpr.ee_id(), t, np.ones(6)) diff --git a/gtdynamics/cablerobot/src/test_cdpr_planar_controller.py b/gtdynamics/cablerobot/src/test_cdpr_planar_controller.py index 6376efe2..7a0e55de 100644 --- a/gtdynamics/cablerobot/src/test_cdpr_planar_controller.py +++ b/gtdynamics/cablerobot/src/test_cdpr_planar_controller.py @@ -45,7 +45,7 @@ def testTrajFollow(self): print(('k: {:d} -- des: {:.3f}, {:.3f}, {:.3f} -- act: {:.3f}, {:.3f}, {:.3f}' + ' -- u: {:.3e}, {:.3e}, {:.3e}, {:.3e}').format( k, *des.translation(), *act.translation(), - *[gtd.TorqueDouble(result, ji, k) for ji in range(4)])) + *[gtd.Torque(result, ji, k) for ji in range(4)])) for k, (des, act) in enumerate(zip(x_des, pAct)): self.gtsamAssertEquals(des, act, tol=1e-2) diff --git a/gtdynamics/cablerobot/src/test_cdpr_planar_sim.py b/gtdynamics/cablerobot/src/test_cdpr_planar_sim.py index 78181247..6307e9fa 100644 --- a/gtdynamics/cablerobot/src/test_cdpr_planar_sim.py +++ b/gtdynamics/cablerobot/src/test_cdpr_planar_sim.py @@ -29,10 +29,10 @@ def testSim(self): class DummyController(CdprControllerBase): def update(self, values, k): tau = gtsam.Values() - gtd.InsertTorqueDouble(tau, 0, k, 1.) - gtd.InsertTorqueDouble(tau, 1, k, 1.) - gtd.InsertTorqueDouble(tau, 2, k, 0.) - gtd.InsertTorqueDouble(tau, 3, k, 0.) + gtd.InsertTorque(tau, 0, k, 1.) + gtd.InsertTorque(tau, 1, k, 1.) + gtd.InsertTorque(tau, 2, k, 0.) + gtd.InsertTorque(tau, 3, k, 0.) return tau Tf = 1 dt = 0.1 diff --git a/gtdynamics/cablerobot/src/utils.py b/gtdynamics/cablerobot/src/utils.py index 547d72d6..2e346702 100644 --- a/gtdynamics/cablerobot/src/utils.py +++ b/gtdynamics/cablerobot/src/utils.py @@ -29,9 +29,9 @@ def zerovalues(lid, ts=[], dt=0.01): zero.insert(0, dt) for t in ts: for j in range(4): - gtd.InsertJointAngleDouble(zero, j, t, 0) - gtd.InsertJointVelDouble(zero, j, t, 0) - gtd.InsertTorqueDouble(zero, j, t, 0) + gtd.InsertJointAngle(zero, j, t, 0) + gtd.InsertJointVel(zero, j, t, 0) + gtd.InsertTorque(zero, j, t, 0) gtd.InsertWrench(zero, lid, j, t, np.zeros(6)) gtd.InsertPose(zero, lid, t, gtsam.Pose3(gtsam.Rot3(), (1.5, 0, 1.5))) gtd.InsertTwist(zero, lid, t, np.zeros(6)) diff --git a/python/gtdynamics/__init__.py b/python/gtdynamics/__init__.py index 44ea44b5..a81117ea 100644 --- a/python/gtdynamics/__init__.py +++ b/python/gtdynamics/__init__.py @@ -17,3 +17,55 @@ class Values(_GtdKeyFormatter, gtsam.Values): class NonlinearFactorGraph(_GtdKeyFormatter, gtsam.NonlinearFactorGraph): pass + + +# Deprecate templated JointXXXDouble and InsertJointXXXDouble functions +import warnings + + +def InsertJointAngleDouble(*args, **kwargs): + warnings.warn("InsertJointAngleDouble is deprecated. Please use InsertJointAngle instead", + category=DeprecationWarning) + InsertJointAngle(*args, **kwargs) + + +def JointAngleDouble(*args, **kwargs): + warnings.warn("JointAngleDouble is deprecated. Please use JointAngle instead", + category=DeprecationWarning) + JointAngle(*args, **kwargs) + + +def InsertJointVelDouble(*args, **kwargs): + warnings.warn("InsertJointVelDouble is deprecated. Please use InsertJointVel instead", + category=DeprecationWarning) + InsertJointVel(*args, **kwargs) + + +def JointVelDouble(*args, **kwargs): + warnings.warn("JointVelDouble is deprecated. Please use JointVel instead", + category=DeprecationWarning) + JointVel(*args, **kwargs) + + +def InsertJointAccelDouble(*args, **kwargs): + warnings.warn("InsertJointAccelDouble is deprecated. Please use InsertJointAccel instead", + category=DeprecationWarning) + InsertJointAccel(*args, **kwargs) + + +def JointAccelDouble(*args, **kwargs): + warnings.warn("JointAccelDouble is deprecated. Please use JointAccel instead", + category=DeprecationWarning) + JointAccel(*args, **kwargs) + + +def InsertTorqueDouble(*args, **kwargs): + warnings.warn("InsertTorqueDouble is deprecated. Please use InsertTorque instead", + category=DeprecationWarning) + InsertTorque(*args, **kwargs) + + +def TorqueDouble(*args, **kwargs): + warnings.warn("TorqueDouble is deprecated. Please use Torque instead", + category=DeprecationWarning) + Torque(*args, **kwargs) diff --git a/python/tests/test_forward_kinematics.py b/python/tests/test_forward_kinematics.py index 11c85964..8950a863 100644 --- a/python/tests/test_forward_kinematics.py +++ b/python/tests/test_forward_kinematics.py @@ -45,7 +45,7 @@ def setUp(self): 11: -1.852, } for i, angle in angles.items(): - gtd.InsertJointAngleDouble(self.joint_angles, i, 0, angle) + gtd.InsertJointAngle(self.joint_angles, i, 0, angle) def test_forward_kinematics_factor(self): """Test if forward kinematics are correct.""" diff --git a/python/tests/test_four_bar.py b/python/tests/test_four_bar.py index 25b86119..371ffcfb 100644 --- a/python/tests/test_four_bar.py +++ b/python/tests/test_four_bar.py @@ -85,11 +85,11 @@ def test_four_bar(self): joint_vels = np.array([0, 0, 0, 0]) torques = np.array([1, 0, 0, 0]) for idx, joint in enumerate(robot.joints()): - gtd.InsertJointAngleDouble(known_values, joint.id(), 0, + gtd.InsertJointAngle(known_values, joint.id(), 0, joint_angles[idx]) - gtd.InsertJointVelDouble(known_values, joint.id(), 0, + gtd.InsertJointVel(known_values, joint.id(), 0, joint_vels[idx]) - gtd.InsertTorqueDouble(known_values, joint.id(), 0, torques[idx]) + gtd.InsertTorque(known_values, joint.id(), 0, torques[idx]) prior_graph = graph_builder.forwardDynamicsPriors( robot, 0, known_values) diff --git a/python/tests/test_print.py b/python/tests/test_print.py index dfd81b89..4f3b38e6 100644 --- a/python/tests/test_print.py +++ b/python/tests/test_print.py @@ -23,7 +23,7 @@ class TestPrint(unittest.TestCase): def test_values(self): """Checks that printing Values uses the GTDKeyFormatter instead of gtsam's default""" v = gtd.Values() - gtd.InsertJointAngleDouble(v, 0, 1, 2) + gtd.InsertJointAngle(v, 0, 1, 2) self.assertTrue('q(0)1' in v.__repr__()) def test_nonlinear_factor_graph(self): diff --git a/python/tests/test_robot.py b/python/tests/test_robot.py index 57642447..d39ec6ab 100644 --- a/python/tests/test_robot.py +++ b/python/tests/test_robot.py @@ -46,7 +46,7 @@ def test_forward_kinematics(self): for idx, joint in enumerate(joints): th[joint] = joint_angles[idx] - gtd.InsertJointAngleDouble(joint_angles_values, + gtd.InsertJointAngle(joint_angles_values, robot.joint(joint).id(), joint_angles[idx]) diff --git a/python/tests/test_simulator.py b/python/tests/test_simulator.py index 0f818147..5e8b97b6 100644 --- a/python/tests/test_simulator.py +++ b/python/tests/test_simulator.py @@ -39,7 +39,7 @@ def test_simulator(self): initial_values = Values() torques = Values() - gtd.InsertTorqueDouble(torques, 0, 1.0) + gtd.InsertTorque(torques, 0, 1.0) simulator = gtd.Simulator(robot, initial_values, gravity, planar_axis) @@ -52,9 +52,9 @@ def test_simulator(self): expected_qAccel = acceleration expected_qVel = acceleration * dt expected_qAngle = acceleration * 0.5 * dt * dt - self.assertEqual(expected_qAngle, gtd.JointAngleDouble(results, 0, 0)) - self.assertEqual(expected_qVel, gtd.JointVelDouble(results, 0, 0)) - self.assertEqual(expected_qAccel, gtd.JointAccelDouble(results, 0, 0)) + self.assertEqual(expected_qAngle, gtd.JointAngle(results, 0, 0)) + self.assertEqual(expected_qVel, gtd.JointVel(results, 0, 0)) + self.assertEqual(expected_qAccel, gtd.JointAccel(results, 0, 0)) if __name__ == "__main__": From 44d89d15a8f2fce50867574b49342ed1a7bc16b7 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sat, 16 Oct 2021 15:06:52 -0400 Subject: [PATCH 10/14] rename measured joint angle --- gtdynamics/factors/JointMeasurementFactor.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gtdynamics/factors/JointMeasurementFactor.h b/gtdynamics/factors/JointMeasurementFactor.h index fac74b3f..79a2711e 100644 --- a/gtdynamics/factors/JointMeasurementFactor.h +++ b/gtdynamics/factors/JointMeasurementFactor.h @@ -31,7 +31,7 @@ class JointMeasurementFactor using Base = gtsam::NoiseModelFactor2; JointConstSharedPtr joint_; - double joint_angle_; + double measured_joint_coordinate_; public: /** @@ -50,7 +50,7 @@ class JointMeasurementFactor double joint_coordinate) : Base(model, wTp_key, wTc_key), joint_(joint), - joint_angle_(joint_coordinate) {} + measured_joint_coordinate_(joint_coordinate) {} /** * @brief Convenience constructor @@ -66,7 +66,7 @@ class JointMeasurementFactor : Base(model, internal::PoseKey(joint->parent()->id(), k), internal::PoseKey(joint->child()->id(), k)), joint_(joint), - joint_angle_(joint_coordinate) {} + measured_joint_coordinate_(joint_coordinate) {} gtsam::Vector evaluateError( const gtsam::Pose3& wTp, const gtsam::Pose3& wTc, @@ -74,7 +74,7 @@ class JointMeasurementFactor boost::optional H_wTc = boost::none) const override { gtsam::Matrix6 H; gtsam::Pose3 wTc_hat = - joint_->poseOf(joint_->child(), wTp, joint_angle_, H_wTp); + joint_->poseOf(joint_->child(), wTp, measured_joint_coordinate_, H_wTp); gtsam::Vector6 error = wTc.logmap(wTc_hat, H_wTc, H_wTp ? &H : 0); if (H_wTp) { @@ -89,7 +89,7 @@ class JointMeasurementFactor gtsam::DefaultKeyFormatter) const override { std::cout << s << "JointMeasurementFactor(" << keyFormatter(key1()) << "," << keyFormatter(key2()) << ")\n"; - gtsam::traits::Print(joint_angle_, " measured: "); + gtsam::traits::Print(measured_joint_coordinate_, " measured: "); this->noiseModel_->print(" noise model: "); } }; From 938aa6971afd8adecc56025ba02e2c6ba5bab5a2 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sat, 16 Oct 2021 15:13:58 -0400 Subject: [PATCH 11/14] rename ScrewJoint to HelicalJoint --- gtdynamics.i | 6 +++--- .../universal_robot/{ScrewJoint.h => HelicalJoint.h} | 12 ++++++------ gtdynamics/universal_robot/sdf.cpp | 4 ++-- tests/make_joint.h | 4 ++-- tests/{testScrewJoint.cpp => testHelicalJoint.cpp} | 8 ++++---- tests/testSdf.cpp | 4 ++-- 6 files changed, 19 insertions(+), 19 deletions(-) rename gtdynamics/universal_robot/{ScrewJoint.h => HelicalJoint.h} (84%) rename tests/{testScrewJoint.cpp => testHelicalJoint.cpp} (95%) diff --git a/gtdynamics.i b/gtdynamics.i index 117d3d05..aba582cf 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -228,7 +228,7 @@ class Link { #include #include #include -#include +#include class JointParams { JointParams(); double velocity_limit; @@ -270,8 +270,8 @@ virtual class PrismaticJoint : gtdynamics::Joint { void print(const string &s = "") const; }; -virtual class ScrewJoint : gtdynamics::Joint { - ScrewJoint( +virtual class HelicalJoint : gtdynamics::Joint { + HelicalJoint( int id, const string &name, const gtsam::Pose3 &wTj, const gtdynamics::Link *parent_link, const gtdynamics::Link *child_link, const Vector &axis, double thread_pitch, diff --git a/gtdynamics/universal_robot/ScrewJoint.h b/gtdynamics/universal_robot/HelicalJoint.h similarity index 84% rename from gtdynamics/universal_robot/ScrewJoint.h rename to gtdynamics/universal_robot/HelicalJoint.h index b8eecd02..fdcc1e72 100644 --- a/gtdynamics/universal_robot/ScrewJoint.h +++ b/gtdynamics/universal_robot/HelicalJoint.h @@ -6,7 +6,7 @@ * -------------------------------------------------------------------------- */ /** - * @file ScrewJoint.h + * @file HelicalJoint.h * @brief Representation of screw joint. * @author Frank Dellaert * @author Mandy Xie @@ -23,10 +23,10 @@ namespace gtdynamics { /** - * @class ScrewJoint is an implementation of the Joint class which represents a - * screw joint and contains all necessary factor construction methods. + * @class HelicalJoint is an implementation of the Joint class which represents + * a helical joint and contains all necessary factor construction methods. */ -class ScrewJoint : public Joint { +class HelicalJoint : public Joint { protected: /** * Returns the screw axis in the joint frame given the joint axis and thread @@ -40,7 +40,7 @@ class ScrewJoint : public Joint { public: /** - * @brief Create ScrewJoint using JointParams, joint name, joint pose in + * @brief Create HelicalJoint using JointParams, joint name, joint pose in * world frame, screw axes, and parent and child links. * * @param[in] id id for keys @@ -52,7 +52,7 @@ class ScrewJoint : public Joint { * @param[in] thread_pitch joint's thread pitch in dist per rev * @param[in] parameters JointParams struct. */ - ScrewJoint(uint8_t id, const std::string &name, const gtsam::Pose3 &bTj, + HelicalJoint(uint8_t id, const std::string &name, const gtsam::Pose3 &bTj, const LinkSharedPtr &parent_link, const LinkSharedPtr &child_link, const gtsam::Vector3 &axis, double thread_pitch, const JointParams ¶meters = JointParams()) diff --git a/gtdynamics/universal_robot/sdf.cpp b/gtdynamics/universal_robot/sdf.cpp index 3dfceb45..f686867a 100644 --- a/gtdynamics/universal_robot/sdf.cpp +++ b/gtdynamics/universal_robot/sdf.cpp @@ -20,7 +20,7 @@ #include "gtdynamics/universal_robot/Link.h" #include "gtdynamics/universal_robot/PrismaticJoint.h" #include "gtdynamics/universal_robot/RevoluteJoint.h" -#include "gtdynamics/universal_robot/ScrewJoint.h" +#include "gtdynamics/universal_robot/HelicalJoint.h" #include "gtdynamics/universal_robot/sdf_internal.h" namespace gtdynamics { @@ -187,7 +187,7 @@ JointSharedPtr JointFromSdf(uint8_t id, const LinkSharedPtr &parent_link, child_link, axis, parameters); break; case sdf::JointType::SCREW: - joint = boost::make_shared( + joint = boost::make_shared( id, name, bMj, parent_link, child_link, axis, sdf_joint.ThreadPitch(), parameters); break; diff --git a/tests/make_joint.h b/tests/make_joint.h index 6fa27159..818963d7 100644 --- a/tests/make_joint.h +++ b/tests/make_joint.h @@ -14,7 +14,7 @@ #pragma once #include "gtdynamics/universal_robot/Link.h" -#include "gtdynamics/universal_robot/ScrewJoint.h" +#include "gtdynamics/universal_robot/HelicalJoint.h" namespace gtdynamics { /// Create a joint with given rest transform cMp and screw-axis in child frame. @@ -40,7 +40,7 @@ JointConstSharedPtr make_joint(gtsam::Pose3 cMp, gtsam::Vector6 cScrewAxis) { gtsam::Pose3 jMc = bMj.inverse() * l2->bMcom(); gtsam::Vector6 jScrewAxis = jMc.AdjointMap() * cScrewAxis; - return boost::make_shared( + return boost::make_shared( 1, "j1", bMj, l1, l2, jScrewAxis, joint_params); } } // namespace gtdynamics diff --git a/tests/testScrewJoint.cpp b/tests/testHelicalJoint.cpp similarity index 95% rename from tests/testScrewJoint.cpp rename to tests/testHelicalJoint.cpp index c66b41fd..11376767 100644 --- a/tests/testScrewJoint.cpp +++ b/tests/testHelicalJoint.cpp @@ -6,8 +6,8 @@ * -------------------------------------------------------------------------- */ /** - * @file testScrewJoint.cpp - * @brief Test ScrewJoint class. + * @file testHelicalJoint.cpp + * @brief Test HelicalJoint class. * @author Frank Dellaert, Mandy Xie, Alejandro Escontrela, and Yetong Zhang */ @@ -17,7 +17,7 @@ #include "gtdynamics/universal_robot/Link.h" #include "gtdynamics/universal_robot/RobotModels.h" -#include "gtdynamics/universal_robot/ScrewJoint.h" +#include "gtdynamics/universal_robot/HelicalJoint.h" #include "gtdynamics/universal_robot/sdf.h" #include "gtdynamics/utils/utils.h" @@ -40,7 +40,7 @@ TEST(Joint, params_constructor) { parameters.scalar_limits.value_upper_limit = 1.57; parameters.scalar_limits.value_limit_threshold = 0; - auto j1 = boost::make_shared( + auto j1 = boost::make_shared( 123, "j1", Pose3(Rot3(), Point3(0, 0, 2)), l1, l2, gtsam::Vector3(1, 0, 0), 0.5, parameters); diff --git a/tests/testSdf.cpp b/tests/testSdf.cpp index c26e7a67..497e7443 100644 --- a/tests/testSdf.cpp +++ b/tests/testSdf.cpp @@ -20,7 +20,7 @@ #include "gtdynamics/universal_robot/Link.h" #include "gtdynamics/universal_robot/PrismaticJoint.h" #include "gtdynamics/universal_robot/RevoluteJoint.h" -#include "gtdynamics/universal_robot/ScrewJoint.h" +#include "gtdynamics/universal_robot/HelicalJoint.h" #include "gtdynamics/universal_robot/sdf.h" #include "gtdynamics/universal_robot/sdf_internal.h" #include "gtdynamics/utils/utils.h" @@ -535,7 +535,7 @@ TEST(Sdf, sdf_constructor_screw) { j1_parameters.effort_type = JointEffortType::Actuated; const gtsam::Vector3 j1_axis = GetSdfAxis(*model.JointByName("joint_1")); - auto j1 = boost::make_shared( + auto j1 = boost::make_shared( 1, "joint_1", bTj, l0, l1, j1_axis, model.JointByName("joint_1")->ThreadPitch(), j1_parameters); From 1432238c45f175b937ccc208517faa7c0c75d813 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 26 Oct 2021 03:47:40 -0400 Subject: [PATCH 12/14] fix straggler template --- scripts/gerry00_ForwardDynamicsPrismatic.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/scripts/gerry00_ForwardDynamicsPrismatic.cpp b/scripts/gerry00_ForwardDynamicsPrismatic.cpp index 8f8a290d..849ff0c7 100644 --- a/scripts/gerry00_ForwardDynamicsPrismatic.cpp +++ b/scripts/gerry00_ForwardDynamicsPrismatic.cpp @@ -47,12 +47,12 @@ int main(int argc, char** argv) { int j1 = simple_rpr.joint("joint_1")->id(), j2 = simple_rpr.joint("joint_2")->id(), j3 = simple_rpr.joint("joint_3")->id(); - InsertJointAngle(&theta_and_theta_dot, j1, 0, 0.0); - InsertJointAngle(&theta_and_theta_dot, j2, 0, 0.0); - InsertJointAngle(&theta_and_theta_dot, j3, 0, 0.0); - InsertJointVel(&theta_and_theta_dot, j1, 0, 0.3); - InsertJointVel(&theta_and_theta_dot, j2, 0, 0.1); - InsertJointVel(&theta_and_theta_dot, j3, 0, 0.0); + InsertJointAngle(&theta_and_theta_dot, j1, 0, 0.0); + InsertJointAngle(&theta_and_theta_dot, j2, 0, 0.0); + InsertJointAngle(&theta_and_theta_dot, j3, 0, 0.0); + InsertJointVel(&theta_and_theta_dot, j1, 0, 0.3); + InsertJointVel(&theta_and_theta_dot, j2, 0, 0.1); + InsertJointVel(&theta_and_theta_dot, j3, 0, 0.0); std::vector taus; for (int t = 0; t <= T; t++) { From 7f66e4aeb9f4e1e0ae1bc9a9465b6e8745ed3b89 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 27 Oct 2021 14:49:47 -0400 Subject: [PATCH 13/14] fix rebase mistakes --- gtdynamics.i | 15 ++++++++------- gtdynamics/dynamics/DynamicsGraph.cpp | 14 +++++++------- gtdynamics/factors/WrenchPlanarFactor.h | 3 +-- gtdynamics/statics/StaticsSlice.cpp | 12 ++++-------- 4 files changed, 20 insertions(+), 24 deletions(-) diff --git a/gtdynamics.i b/gtdynamics.i index aba582cf..b6bca487 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -99,10 +99,11 @@ class TwistFactor : gtsam::NonlinearFactor { #include class TwistAccelFactor : gtsam::NonlinearFactor { - TwistAccelFactor(gtsam::Key twist_key_c, gtsam::Key twistAccel_key_p, gtsam::Key twistAccel_key_c, - gtsam::Key q_key, gtsam::Key qVel_key, gtsam::Key qAccel_key, - const gtsam::noiseModel::Base* cost_model, - const gtdynamics::JointTyped* joint); + TwistAccelFactor(gtsam::Key twist_key_c, gtsam::Key twistAccel_key_p, + gtsam::Key twistAccel_key_c, gtsam::Key q_key, + gtsam::Key qVel_key, gtsam::Key qAccel_key, + const gtsam::noiseModel::Base *cost_model, + const gtdynamics::Joint *joint); void print(const string &s="", const gtsam::KeyFormatter &keyFormatter=gtdynamics::GTDKeyFormatter); @@ -111,7 +112,7 @@ class TwistAccelFactor : gtsam::NonlinearFactor { #include class TorqueFactor : gtsam::NonlinearFactor { TorqueFactor(const gtsam::noiseModel::Base *cost_model, - const gtdynamics::JointTyped *joint, size_t k=0); + const gtdynamics::Joint *joint, size_t k=0); void print(const string &s="", const gtsam::KeyFormatter &keyFormatter=gtdynamics::GTDKeyFormatter); @@ -140,7 +141,7 @@ class WrenchFactor : gtsam::NonlinearFactor { #include class WrenchEquivalenceFactor : gtsam::NonlinearFactor{ WrenchEquivalenceFactor(const gtsam::noiseModel::Base *cost_model, - gtdynamics::JointTyped *joint, size_t k = 0); + gtdynamics::Joint *joint, size_t k = 0); void print(const string &s="", const gtsam::KeyFormatter &keyFormatter=gtdynamics::GTDKeyFormatter); }; @@ -148,7 +149,7 @@ class WrenchEquivalenceFactor : gtsam::NonlinearFactor{ #include class WrenchPlanarFactor : gtsam::NonlinearFactor { WrenchPlanarFactor(const gtsam::noiseModel::Base *cost_model, - Vector planar_axis, gtdynamics::JointTyped *joint, size_t k=0); + Vector planar_axis, gtdynamics::Joint *joint, size_t k=0); void print(const string &s="", const gtsam::KeyFormatter &keyFormatter=gtdynamics::GTDKeyFormatter); }; diff --git a/gtdynamics/dynamics/DynamicsGraph.cpp b/gtdynamics/dynamics/DynamicsGraph.cpp index 9c1010e0..448fa182 100644 --- a/gtdynamics/dynamics/DynamicsGraph.cpp +++ b/gtdynamics/dynamics/DynamicsGraph.cpp @@ -269,12 +269,12 @@ gtsam::NonlinearFactorGraph DynamicsGraph::aFactors( gtsam::Z_6x1, opt_.ba_cost_model); for (auto &&joint : robot.joints()) graph.emplace_shared( - internal::TwistKey(joint->child()->id(), t), - internal::TwistAccelKey(joint->parent()->id(), t), - internal::TwistAccelKey(joint->child()->id(), t), - internal::JointAngleKey(joint->id(), t), internal::JointVelKey(joint->id(), t), - internal::JointAccelKey(joint->id(), t), opt_.a_cost_model, - boost::static_pointer_cast(joint)); + internal::TwistKey(joint->child()->id(), t), + internal::TwistAccelKey(joint->parent()->id(), t), + internal::TwistAccelKey(joint->child()->id(), t), + internal::JointAngleKey(joint->id(), t), + internal::JointVelKey(joint->id(), t), + internal::JointAccelKey(joint->id(), t), opt_.a_cost_model, joint); // Add contact factors. if (contact_points) { @@ -351,7 +351,7 @@ gtsam::NonlinearFactorGraph DynamicsGraph::dynamicsFactors( // TODO(frank): sort out const shared ptr mess for (auto &&joint : robot.joints()) { auto j = joint->id(), child_id = joint->child()->id(); - auto const_joint = boost::static_pointer_cast(joint); + auto const_joint = joint; graph.emplace_shared(opt_.f_cost_model, const_joint, k); graph.emplace_shared(opt_.t_cost_model, const_joint, k); diff --git a/gtdynamics/factors/WrenchPlanarFactor.h b/gtdynamics/factors/WrenchPlanarFactor.h index b4440cf9..78810033 100644 --- a/gtdynamics/factors/WrenchPlanarFactor.h +++ b/gtdynamics/factors/WrenchPlanarFactor.h @@ -56,8 +56,7 @@ class WrenchPlanarFactor : public gtsam::NoiseModelFactor1 { */ WrenchPlanarFactor(const gtsam::noiseModel::Base::shared_ptr &cost_model, gtsam::Vector3 planar_axis, - const boost::shared_ptr &joint, - size_t k = 0) + const JointConstSharedPtr &joint, size_t k = 0) : WrenchPlanarFactor( cost_model, planar_axis, internal::WrenchKey(joint->child()->id(), joint->id(), k)) {} diff --git a/gtdynamics/statics/StaticsSlice.cpp b/gtdynamics/statics/StaticsSlice.cpp index 75f70409..92e68b8d 100644 --- a/gtdynamics/statics/StaticsSlice.cpp +++ b/gtdynamics/statics/StaticsSlice.cpp @@ -33,9 +33,8 @@ gtsam::NonlinearFactorGraph Statics::wrenchEquivalenceFactors( const Slice& slice, const Robot& robot) const { gtsam::NonlinearFactorGraph graph; for (auto&& joint : robot.joints()) { - graph.emplace_shared( - p_->f_cost_model, boost::static_pointer_cast(joint), - slice.k); + graph.emplace_shared(p_->f_cost_model, joint, + slice.k); } return graph; } @@ -44,9 +43,7 @@ gtsam::NonlinearFactorGraph Statics::torqueFactors(const Slice& slice, const Robot& robot) const { gtsam::NonlinearFactorGraph graph; for (auto&& joint : robot.joints()) { - graph.emplace_shared( - p_->t_cost_model, boost::static_pointer_cast(joint), - slice.k); + graph.emplace_shared(p_->t_cost_model, joint, slice.k); } return graph; } @@ -57,8 +54,7 @@ gtsam::NonlinearFactorGraph Statics::wrenchPlanarFactors( if (p_->planar_axis) for (auto&& joint : robot.joints()) { graph.emplace_shared( - p_->planar_cost_model, *p_->planar_axis, - boost::static_pointer_cast(joint), slice.k); + p_->planar_cost_model, *p_->planar_axis, joint, slice.k); } return graph; } From d97648a953c8db75e91f8e4c7d2e4732e2becf5d Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 27 Oct 2021 15:05:29 -0400 Subject: [PATCH 14/14] remove python deprecated functions --- python/gtdynamics/__init__.py | 52 ----------------------------------- 1 file changed, 52 deletions(-) diff --git a/python/gtdynamics/__init__.py b/python/gtdynamics/__init__.py index a81117ea..44ea44b5 100644 --- a/python/gtdynamics/__init__.py +++ b/python/gtdynamics/__init__.py @@ -17,55 +17,3 @@ class Values(_GtdKeyFormatter, gtsam.Values): class NonlinearFactorGraph(_GtdKeyFormatter, gtsam.NonlinearFactorGraph): pass - - -# Deprecate templated JointXXXDouble and InsertJointXXXDouble functions -import warnings - - -def InsertJointAngleDouble(*args, **kwargs): - warnings.warn("InsertJointAngleDouble is deprecated. Please use InsertJointAngle instead", - category=DeprecationWarning) - InsertJointAngle(*args, **kwargs) - - -def JointAngleDouble(*args, **kwargs): - warnings.warn("JointAngleDouble is deprecated. Please use JointAngle instead", - category=DeprecationWarning) - JointAngle(*args, **kwargs) - - -def InsertJointVelDouble(*args, **kwargs): - warnings.warn("InsertJointVelDouble is deprecated. Please use InsertJointVel instead", - category=DeprecationWarning) - InsertJointVel(*args, **kwargs) - - -def JointVelDouble(*args, **kwargs): - warnings.warn("JointVelDouble is deprecated. Please use JointVel instead", - category=DeprecationWarning) - JointVel(*args, **kwargs) - - -def InsertJointAccelDouble(*args, **kwargs): - warnings.warn("InsertJointAccelDouble is deprecated. Please use InsertJointAccel instead", - category=DeprecationWarning) - InsertJointAccel(*args, **kwargs) - - -def JointAccelDouble(*args, **kwargs): - warnings.warn("JointAccelDouble is deprecated. Please use JointAccel instead", - category=DeprecationWarning) - JointAccel(*args, **kwargs) - - -def InsertTorqueDouble(*args, **kwargs): - warnings.warn("InsertTorqueDouble is deprecated. Please use InsertTorque instead", - category=DeprecationWarning) - InsertTorque(*args, **kwargs) - - -def TorqueDouble(*args, **kwargs): - warnings.warn("TorqueDouble is deprecated. Please use Torque instead", - category=DeprecationWarning) - Torque(*args, **kwargs)