diff --git a/examples/example_a1_walking/main.cpp b/examples/example_a1_walking/main.cpp index 91c1ffb73..68e31f40d 100644 --- a/examples/example_a1_walking/main.cpp +++ b/examples/example_a1_walking/main.cpp @@ -11,6 +11,8 @@ * @author: Dan Barladeanu */ +#include +#include #include #include #include @@ -218,7 +220,7 @@ int oldGraph() { for (auto&& link : robot.links()) { if (link->name().find("trunk") == std::string::npos) { link->setMass(0.0); - link->setInertia(gtsam::Matrix3::Zero()); + link->setInertia(gtsam::Z_3x3); } } @@ -372,7 +374,7 @@ int newGraph() { for (auto&& link : robot.links()) { if (link->name().find("trunk") == std::string::npos) { link->setMass(0.0); - link->setInertia(gtsam::Matrix3::Zero()); + link->setInertia(gtsam::Z_3x3); } } diff --git a/examples/example_a1_walking/main_combined.cpp b/examples/example_a1_walking/main_combined.cpp index debdddac9..7946cf15d 100644 --- a/examples/example_a1_walking/main_combined.cpp +++ b/examples/example_a1_walking/main_combined.cpp @@ -11,6 +11,8 @@ * @author: Dan Barladeanu */ +#include +#include #include #include #include @@ -77,7 +79,7 @@ int CombinedRun(bool add_mass_to_body) { for (auto&& link : robot_massless.links()) { if (link->name().find("trunk") == std::string::npos) { link->setMass(0.0); - link->setInertia(gtsam::Matrix3::Zero()); + link->setInertia(gtsam::Z_3x3); } } diff --git a/examples/example_constraint_manifold/QuadrupedUtils.cpp b/examples/example_constraint_manifold/QuadrupedUtils.cpp index 8294876f7..e00a9204e 100644 --- a/examples/example_constraint_manifold/QuadrupedUtils.cpp +++ b/examples/example_constraint_manifold/QuadrupedUtils.cpp @@ -11,6 +11,7 @@ * @author: Yetong Zhang */ +#include #include "QuadrupedUtils.h" #include @@ -40,7 +41,7 @@ Vector3 get_contact_force(const Pose3 &pose, const Vector6 wrench, Vector3 force_l(wrench(3), wrench(4), wrench(5)); if (H_pose || H_wrench) { Matrix36 J_fl_wrench; - J_fl_wrench << Z_3x3, I_3x3; + J_fl_wrench << gtsam::Z_3x3, gtsam::I_3x3; Matrix36 J_rot_pose; Rot3 rot = pose.rotation(J_rot_pose); diff --git a/examples/example_constraint_manifold/main_arm_kinematic_planning.cpp b/examples/example_constraint_manifold/main_arm_kinematic_planning.cpp index cb027a463..773bcc870 100644 --- a/examples/example_constraint_manifold/main_arm_kinematic_planning.cpp +++ b/examples/example_constraint_manifold/main_arm_kinematic_planning.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include diff --git a/examples/example_constraint_manifold/main_connected_poses.cpp b/examples/example_constraint_manifold/main_connected_poses.cpp index d59a9741a..f3ed5647a 100644 --- a/examples/example_constraint_manifold/main_connected_poses.cpp +++ b/examples/example_constraint_manifold/main_connected_poses.cpp @@ -12,6 +12,7 @@ * @author Yetong Zhang */ +#include #include #include #include @@ -107,14 +108,14 @@ double EvaluatePoseError(const Values& gt, const Values& result) { Pose2 gt_pose = gt.at(A(k)); Pose2 est_pose = result.at(A(k)); Pose2 rel_pose = est_pose.inverse().compose(gt_pose); - Matrix3 diff = rel_pose.matrix() - I_3x3; + Matrix3 diff = rel_pose.matrix() - gtsam::I_3x3; error1 += pow(diff.norm(), 2); } { Pose2 gt_pose = gt.at(B(k)); Pose2 est_pose = result.at(B(k)); Pose2 rel_pose = est_pose.inverse().compose(gt_pose); - Matrix3 diff = rel_pose.matrix() - I_3x3; + Matrix3 diff = rel_pose.matrix() - gtsam::I_3x3; error2 += pow(diff.norm(), 2); } } diff --git a/examples/example_constraint_manifold/main_range_constraint.cpp b/examples/example_constraint_manifold/main_range_constraint.cpp index fba5f2c1d..20356b3e7 100644 --- a/examples/example_constraint_manifold/main_range_constraint.cpp +++ b/examples/example_constraint_manifold/main_range_constraint.cpp @@ -11,6 +11,7 @@ * @author Yetong Zhang */ +#include #include #include #include @@ -159,7 +160,7 @@ double EvaluatePoseError(const Values& gt, const Values& result) { Pose2 gt_pose = gt.at(A(k)); Pose2 est_pose = result.at(A(k)); Pose2 rel_pose = est_pose.inverse().compose(gt_pose); - Matrix3 diff = rel_pose.matrix() - I_3x3; + Matrix3 diff = rel_pose.matrix() - gtsam::I_3x3; // std::cout << diff << "\n"; // std::cout << diff.norm() << "\n"; error1 += pow(diff.norm(), 2); @@ -168,7 +169,7 @@ double EvaluatePoseError(const Values& gt, const Values& result) { Pose2 gt_pose = gt.at(B(k)); Pose2 est_pose = result.at(B(k)); Pose2 rel_pose = est_pose.inverse().compose(gt_pose); - Matrix3 diff = rel_pose.matrix() - I_3x3; + Matrix3 diff = rel_pose.matrix() - gtsam::I_3x3; error2 += pow(diff.norm(), 2); } } diff --git a/examples/example_full_kinodynamic_balancing/main.cpp b/examples/example_full_kinodynamic_balancing/main.cpp index 6abfd74fd..9efd47042 100644 --- a/examples/example_full_kinodynamic_balancing/main.cpp +++ b/examples/example_full_kinodynamic_balancing/main.cpp @@ -11,6 +11,8 @@ * @author Alejandro Escontrela */ +#include +#include #include #include #include diff --git a/examples/example_full_kinodynamic_walking/main_rotate.cpp b/examples/example_full_kinodynamic_walking/main_rotate.cpp index eb4869905..e15d124d6 100644 --- a/examples/example_full_kinodynamic_walking/main_rotate.cpp +++ b/examples/example_full_kinodynamic_walking/main_rotate.cpp @@ -11,6 +11,7 @@ * @Author: Alejandro Escontrela */ +#include #include #include #include diff --git a/examples/example_full_kinodynamic_walking/main_walk_forward.cpp b/examples/example_full_kinodynamic_walking/main_walk_forward.cpp index dfc58be6b..406700af5 100644 --- a/examples/example_full_kinodynamic_walking/main_walk_forward.cpp +++ b/examples/example_full_kinodynamic_walking/main_walk_forward.cpp @@ -11,6 +11,7 @@ * @Author: Alejandro Escontrela */ +#include #include #include #include diff --git a/examples/example_spider_walking/main.cpp b/examples/example_spider_walking/main.cpp index fb0053ec8..bc7fea47b 100644 --- a/examples/example_spider_walking/main.cpp +++ b/examples/example_spider_walking/main.cpp @@ -12,6 +12,7 @@ * Gandhi, Varun Agrawal */ +#include #include #include #include diff --git a/examples/example_spider_walking/main_rotate.cpp b/examples/example_spider_walking/main_rotate.cpp index d4cf271c1..149e91c90 100644 --- a/examples/example_spider_walking/main_rotate.cpp +++ b/examples/example_spider_walking/main_rotate.cpp @@ -14,6 +14,7 @@ * @Author: Tarushree Gandhi */ +#include #include #include #include diff --git a/examples/scripts/alejandro_yetong_01_id_four_bar.cpp b/examples/scripts/alejandro_yetong_01_id_four_bar.cpp index 226536375..c280c895b 100644 --- a/examples/scripts/alejandro_yetong_01_id_four_bar.cpp +++ b/examples/scripts/alejandro_yetong_01_id_four_bar.cpp @@ -11,6 +11,8 @@ * @author Alejandro Escontrela and Yetong Zhang */ +#include +#include #include #include #include diff --git a/gtdynamics.i b/gtdynamics.i index 53e0fc11a..df428896d 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -374,6 +374,8 @@ class KinematicsParameters : gtdynamics::OptimizationParameters { double bv_cost_model_sigma = 1e-4, double v_cost_model_sigma = 1e-4, double cv_cost_model_sigma = 1e-2); + void setJointPriorSigma(gtsam::Key joint_key, double sigma); + void setJointLimit(gtsam::Key joint_key, double lower, double upper); }; class Kinematics { diff --git a/gtdynamics/cablerobot/factors/CableAccelerationFactor.cpp b/gtdynamics/cablerobot/factors/CableAccelerationFactor.cpp index d8c93126a..885b4bca8 100644 --- a/gtdynamics/cablerobot/factors/CableAccelerationFactor.cpp +++ b/gtdynamics/cablerobot/factors/CableAccelerationFactor.cpp @@ -6,6 +6,7 @@ * @author Gerry Chen */ +#include #include "CableAccelerationFactor.h" #include @@ -38,8 +39,8 @@ double CableAccelerationFactor::computeLddot( Vector3 xAb = VAx.bottomRows<3>(); // acceleration of point b in x's frame Vector3 wAb = wTx.rotation().rotate(xAb, H_wTx ? &wAb_H_wRx : nullptr, H_VAx ? &wAb_H_xAb : nullptr); - if (H_wTx) wAb_H_wTx << wAb_H_wRx, Z_3x3; - if (H_VAx) wAb_H_VAx << Z_3x3, wAb_H_xAb; + if (H_wTx) wAb_H_wTx << wAb_H_wRx, gtsam::Z_3x3; + if (H_VAx) wAb_H_VAx << gtsam::Z_3x3, wAb_H_xAb; // cable direction Point3 wPb = wTx.transformFrom(xPb_, H_wTx ? &wPb_H_wTx : nullptr); Vector3 dir = normalize(wPb - wPa_, H_wTx ? &dir_H_wPb : nullptr); diff --git a/gtdynamics/cablerobot/factors/CableAccelerationFactor.h b/gtdynamics/cablerobot/factors/CableAccelerationFactor.h index ea74a51d9..35329ebd3 100644 --- a/gtdynamics/cablerobot/factors/CableAccelerationFactor.h +++ b/gtdynamics/cablerobot/factors/CableAccelerationFactor.h @@ -86,7 +86,7 @@ class CableAccelerationFactor gtsam::OptionalMatrixType H_Vx = nullptr, gtsam::OptionalMatrixType H_VAx = nullptr) const override { double expected_lddot = computeLddot(wTx, Vx, VAx, H_wTx, H_Vx, H_VAx); - if (H_lddot) *H_lddot = gtsam::I_1x1; + if (H_lddot) *H_lddot = gtsam::Matrix1::Identity(); if (H_wTx) *H_wTx = -(*H_wTx); if (H_Vx) *H_Vx = -(*H_Vx); if (H_VAx) *H_VAx = -(*H_VAx); diff --git a/gtdynamics/cablerobot/factors/CableLengthFactor.h b/gtdynamics/cablerobot/factors/CableLengthFactor.h index b6fc0a375..0db86b65a 100644 --- a/gtdynamics/cablerobot/factors/CableLengthFactor.h +++ b/gtdynamics/cablerobot/factors/CableLengthFactor.h @@ -60,7 +60,7 @@ class CableLengthFactor gtsam::Matrix13 H_wPb; auto wPb = wTx.transformFrom(xPb_, H_wTx ? &wPb_H_wTx : 0); double expected_l = gtsam::distance3(wPb, wPa_, H_wTx ? &H_wPb : 0); - if (H_l) *H_l = gtsam::I_1x1; + if (H_l) *H_l = gtsam::Matrix1::Identity(); if (H_wTx) *H_wTx = -H_wPb * wPb_H_wTx; return gtsam::Vector1(l - expected_l); } diff --git a/gtdynamics/cablerobot/factors/CableTensionFactor.cpp b/gtdynamics/cablerobot/factors/CableTensionFactor.cpp index 569de1a7b..d3eab422c 100644 --- a/gtdynamics/cablerobot/factors/CableTensionFactor.cpp +++ b/gtdynamics/cablerobot/factors/CableTensionFactor.cpp @@ -6,6 +6,7 @@ * @author Gerry Chen */ +#include #include "CableTensionFactor.h" #include @@ -41,7 +42,7 @@ Vector6 CableTensionFactor::computeWrench( // force->wrench Vector3 wf = -t * dir; if (H_t) wf_H_t = -dir; - if (H_wTx) wf_H_dir = -t * I_3x3; + if (H_wTx) wf_H_dir = -t * gtsam::I_3x3; Vector3 xf = wTx.rotation().unrotate(wf, // force in the EE frame H_wTx ? &xf_H_wRx : 0, (H_t || H_wTx) ? &xf_H_wf : 0); @@ -50,7 +51,7 @@ Vector6 CableTensionFactor::computeWrench( (H_t || H_wTx) ? &xm_H_xf : 0); Vector6 F = (Vector6() << xm, xf).finished(); - if (H_t || H_wTx) H_xf << xm_H_xf, I_3x3; + if (H_t || H_wTx) H_xf << xm_H_xf, gtsam::I_3x3; if (H_t) *H_t = H_xf * xf_H_wf * wf_H_t; if (H_wTx) { *(H_wTx) = H_xf * xf_H_wf * wf_H_dir * dir_H_wPb * wPb_H_wTx; @@ -77,7 +78,7 @@ Vector6 CableTensionFactor::computeWrenchUsingAdjoint( Pose3 bTx = Pose3(wTx.rotation().inverse(), xPb_).inverse(); Vector6 wF = (Vector6() << 0, 0, 0, -t * dir).finished(); if (H_t) wF_H_t = (Matrix61() << 0, 0, 0, -dir).finished(); - if (H_wTx) wF_H_dir = (Matrix63() << Z_3x3, -t * I_3x3).finished(); + if (H_wTx) wF_H_dir = (Matrix63() << gtsam::Z_3x3, -t * gtsam::I_3x3).finished(); Vector6 F = bTx.AdjointMap().transpose() * wF; // TODO(gerry): find jacobian of adjoint map return F; diff --git a/gtdynamics/cablerobot/factors/CableTensionFactor.h b/gtdynamics/cablerobot/factors/CableTensionFactor.h index 102d68eaa..08d877bdc 100644 --- a/gtdynamics/cablerobot/factors/CableTensionFactor.h +++ b/gtdynamics/cablerobot/factors/CableTensionFactor.h @@ -88,7 +88,7 @@ class CableTensionFactor (Vector6() << Fx - computeWrench(t, wTx, H_t, H_wTx)).finished(); if (H_t) *H_t = -(*H_t); if (H_wTx) *H_wTx = -(*H_wTx); - if (H_Fx) *H_Fx = gtsam::I_6x6; + if (H_Fx) *H_Fx = gtsam::Matrix6::Identity(); return error; } diff --git a/gtdynamics/cablerobot/factors/CableVelocityFactor.cpp b/gtdynamics/cablerobot/factors/CableVelocityFactor.cpp index 80f1a331f..5a82269dc 100644 --- a/gtdynamics/cablerobot/factors/CableVelocityFactor.cpp +++ b/gtdynamics/cablerobot/factors/CableVelocityFactor.cpp @@ -6,6 +6,7 @@ * @author Gerry Chen */ +#include #include "CableVelocityFactor.h" #include @@ -41,7 +42,7 @@ double CableVelocityFactor::computeLdot(const Pose3 &wTx, const Vector6 &Vx, // TODO(gerry): use Adjoint Vector3 eePDOTem = Vx.tail<3>() + cross(Vx.head<3>(), xPb_, H_Vx ? &cross_H_omega : 0); - if (H_Vx) xPDOTb_H_Vx << cross_H_omega, I_3x3; + if (H_Vx) xPDOTb_H_Vx << cross_H_omega, gtsam::I_3x3; Vector3 wPDOTem = wTx.rotation().rotate(eePDOTem, // H_wTx ? &wPDOTb_H_wRx : 0, H_Vx ? &wPDOTb_H_xPDOTb : 0); diff --git a/gtdynamics/cablerobot/factors/CableVelocityFactor.h b/gtdynamics/cablerobot/factors/CableVelocityFactor.h index 871c730ea..fec0aef7e 100644 --- a/gtdynamics/cablerobot/factors/CableVelocityFactor.h +++ b/gtdynamics/cablerobot/factors/CableVelocityFactor.h @@ -74,7 +74,7 @@ class CableVelocityFactor gtsam::OptionalMatrixType H_wTx = nullptr, gtsam::OptionalMatrixType H_Vx = nullptr) const override { double expected_ldot = computeLdot(wTx, Vx, H_wTx, H_Vx); - if (H_ldot) *H_ldot = gtsam::I_1x1; + if (H_ldot) *H_ldot = gtsam::Matrix1::Identity(); if (H_wTx) *H_wTx = -(*H_wTx); if (H_Vx) *H_Vx = -(*H_Vx); return gtsam::Vector1(ldot - expected_ldot); diff --git a/gtdynamics/constrained_optimizer/IPOptOptimizer.cpp b/gtdynamics/constrained_optimizer/IPOptOptimizer.cpp index 61e1dab0d..bf96603f2 100644 --- a/gtdynamics/constrained_optimizer/IPOptOptimizer.cpp +++ b/gtdynamics/constrained_optimizer/IPOptOptimizer.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -16,8 +17,8 @@ Pose3 IFOptTranslator::VecToPose(const Vector6 &vec, OptionalJacobian<6, 6> H) { Vector3 euler_angles(vec(0), vec(1), vec(2)); Vector3 trans(vec(3), vec(4), vec(5)); Matrix36 H_angles_vec, H_trans_vec; - H_angles_vec << I_3x3, Z_3x3; - H_trans_vec << Z_3x3, I_3x3; + H_angles_vec << gtsam::I_3x3, gtsam::Z_3x3; + H_trans_vec << gtsam::Z_3x3, gtsam::I_3x3; Matrix33 H_rot_angles; Rot3 rot = Rot3::RzRyRx(euler_angles, H_rot_angles); Matrix63 H_pose_rot, H_pose_trans; diff --git a/gtdynamics/dynamics/Chain.cpp b/gtdynamics/dynamics/Chain.cpp index 77d6243a0..6f5a76c8c 100644 --- a/gtdynamics/dynamics/Chain.cpp +++ b/gtdynamics/dynamics/Chain.cpp @@ -11,6 +11,7 @@ * @author Dan Barladeanu, Frank Dellaert. */ +#include #include namespace gtdynamics { diff --git a/gtdynamics/dynamics/ChainDynamicsGraph.cpp b/gtdynamics/dynamics/ChainDynamicsGraph.cpp index 3b8b3468a..3e2f319bb 100644 --- a/gtdynamics/dynamics/ChainDynamicsGraph.cpp +++ b/gtdynamics/dynamics/ChainDynamicsGraph.cpp @@ -11,6 +11,7 @@ * @author Dan Barladeanu */ +#include #include "gtdynamics/dynamics/ChainDynamicsGraph.h" #include @@ -192,7 +193,7 @@ gtsam::NonlinearFactorGraph ChainDynamicsGraph::qFactors( chain_joints_[i], base_key, end_effector_key, t); gtsam::ExpressionEqualityConstraint chain_constraint( - chain_expression, gtsam::Vector6::Zero(), tolerance); + chain_expression, gtsam::Z_6x1, tolerance); graph.add(chain_constraint.penaltyFactor(1.0)); } diff --git a/gtdynamics/dynamics/DynamicsGraph.cpp b/gtdynamics/dynamics/DynamicsGraph.cpp index 19fb5d33f..4548554b3 100644 --- a/gtdynamics/dynamics/DynamicsGraph.cpp +++ b/gtdynamics/dynamics/DynamicsGraph.cpp @@ -11,6 +11,8 @@ * @author Yetong Zhang, Alejandro Escontrela */ +#include +#include #include #include #include @@ -33,8 +35,6 @@ using gtsam::Double_; using gtsam::ExpressionFactor; using gtsam::GaussianFactorGraph; -using gtsam::I_1x1; -using gtsam::I_6x6; using gtsam::Key; using gtsam::NonlinearFactorGraph; using gtsam::Pose3; @@ -42,7 +42,6 @@ using gtsam::PriorFactor; using gtsam::Values; using gtsam::Vector; using gtsam::Vector6; -using gtsam::Z_6x1; namespace gtdynamics { @@ -64,7 +63,7 @@ GaussianFactorGraph DynamicsGraph::linearDynamicsGraph( if (link->isFixed()) { // prior on twist acceleration for fixed link // A_i = 0 - graph.add(TwistAccelKey(i, k), I_6x6, Z_6x1, all_constrained); + graph.add(TwistAccelKey(i, k), gtsam::I_6x6, gtsam::Z_6x1, all_constrained); } else { // wrench factor // G_i * A_i - F_i_j1 - .. - F_i_jn = ad(V_i)^T * G_i * V*i + m_i * R_i^T @@ -87,10 +86,10 @@ GaussianFactorGraph DynamicsGraph::linearDynamicsGraph( graph.add(accel_key, G_i, rhs, all_constrained); } else if (connected_joints.size() == 1) { graph.add(accel_key, G_i, WrenchKey(i, connected_joints[0]->id(), k), - -I_6x6, rhs, all_constrained); + -gtsam::I_6x6, rhs, all_constrained); } else if (connected_joints.size() == 2) { graph.add(accel_key, G_i, WrenchKey(i, connected_joints[0]->id(), k), - -I_6x6, WrenchKey(i, connected_joints[1]->id(), k), -I_6x6, + -gtsam::I_6x6, WrenchKey(i, connected_joints[1]->id(), k), -gtsam::I_6x6, rhs, all_constrained); } } @@ -121,7 +120,7 @@ GaussianFactorGraph DynamicsGraph::linearIDPriors( int j = joint->id(); double accel = JointAccel(joint_accels, j, k); gtsam::Vector1 rhs(accel); - graph.add(JointAccelKey(j, k), I_1x1, rhs, all_constrained); + graph.add(JointAccelKey(j, k), gtsam::I_1x1, rhs, all_constrained); } return graph; } diff --git a/gtdynamics/dynamics/DynamicsSlice.cpp b/gtdynamics/dynamics/DynamicsSlice.cpp index 3fb49ee98..e40fd97f2 100644 --- a/gtdynamics/dynamics/DynamicsSlice.cpp +++ b/gtdynamics/dynamics/DynamicsSlice.cpp @@ -10,6 +10,7 @@ * @brief Dynamics factors for a single time slice. */ +#include #include #include #include diff --git a/gtdynamics/dynamics/tests/testChain.cpp b/gtdynamics/dynamics/tests/testChain.cpp index ddd2712f1..d631acbce 100644 --- a/gtdynamics/dynamics/tests/testChain.cpp +++ b/gtdynamics/dynamics/tests/testChain.cpp @@ -11,6 +11,8 @@ * @author: Dan Barladeanu, Frank Dellaert */ +#include +#include #include #include #include @@ -967,10 +969,10 @@ gtsam::Values OldGraphOneLeg() { robot.removeLink(link); } else if (link->name().find("trunk") == std::string::npos) { link->setMass(0.0); - link->setInertia(gtsam::Matrix3::Zero()); + link->setInertia(gtsam::Z_3x3); } else { link->setMass(1.0); - link->setInertia(gtsam::Matrix3::Identity()); + link->setInertia(gtsam::I_3x3); } } @@ -1074,7 +1076,7 @@ gtsam::Values NewGraphOneLeg() { // Add trunk wrench constraint to constraints constraints.emplace_shared< gtsam::ExpressionEqualityConstraint>( - trunk_wrench_constraint, gtsam::Vector6::Zero(), wrench_tolerance); + trunk_wrench_constraint, gtsam::Z_6x1, wrench_tolerance); // Get expression for chain on the leg gtsam::Vector6_ wrench_end_effector = @@ -1181,7 +1183,7 @@ gtsam::Values OldGraphFourLegs() { for (auto&& link : robot.links()) { if (link->name().find("trunk") == std::string::npos) { link->setMass(0.0); - link->setInertia(gtsam::Matrix3::Zero()); + link->setInertia(gtsam::Z_3x3); } } diff --git a/gtdynamics/dynamics/tests/testContactDynamicsFrictionConeFactor.cpp b/gtdynamics/dynamics/tests/testContactDynamicsFrictionConeFactor.cpp index ab9a4f6a3..50e9e4955 100644 --- a/gtdynamics/dynamics/tests/testContactDynamicsFrictionConeFactor.cpp +++ b/gtdynamics/dynamics/tests/testContactDynamicsFrictionConeFactor.cpp @@ -11,6 +11,7 @@ * @author Alejandro Escontrela */ +#include #include #include #include diff --git a/gtdynamics/dynamics/tests/testContactDynamicsMomentFactor.cpp b/gtdynamics/dynamics/tests/testContactDynamicsMomentFactor.cpp index a3587fc3d..9bdae2297 100644 --- a/gtdynamics/dynamics/tests/testContactDynamicsMomentFactor.cpp +++ b/gtdynamics/dynamics/tests/testContactDynamicsMomentFactor.cpp @@ -11,6 +11,7 @@ * @author Alejandro Escontrela */ +#include #include #include #include diff --git a/gtdynamics/dynamics/tests/testContactKinematicsAccelFactor.cpp b/gtdynamics/dynamics/tests/testContactKinematicsAccelFactor.cpp index b787d4928..b767ed7bf 100644 --- a/gtdynamics/dynamics/tests/testContactKinematicsAccelFactor.cpp +++ b/gtdynamics/dynamics/tests/testContactKinematicsAccelFactor.cpp @@ -11,6 +11,7 @@ * @author Alejandro Escontrela */ +#include #include #include #include diff --git a/gtdynamics/dynamics/tests/testDynamicsGraph.cpp b/gtdynamics/dynamics/tests/testDynamicsGraph.cpp index fa90c39ad..a142b18ea 100644 --- a/gtdynamics/dynamics/tests/testDynamicsGraph.cpp +++ b/gtdynamics/dynamics/tests/testDynamicsGraph.cpp @@ -11,6 +11,7 @@ * @authors Yetong Zhang, Alejandro Escontrela */ +#include #include #include #include diff --git a/gtdynamics/dynamics/tests/testTwistAccelFactor.cpp b/gtdynamics/dynamics/tests/testTwistAccelFactor.cpp index c914f5f47..220c76819 100644 --- a/gtdynamics/dynamics/tests/testTwistAccelFactor.cpp +++ b/gtdynamics/dynamics/tests/testTwistAccelFactor.cpp @@ -11,6 +11,7 @@ * @author Frank Dellaert and Mandy Xie */ +#include #include #include #include diff --git a/gtdynamics/dynamics/tests/testWrenchFactor.cpp b/gtdynamics/dynamics/tests/testWrenchFactor.cpp index f9847cf46..63881a6d4 100644 --- a/gtdynamics/dynamics/tests/testWrenchFactor.cpp +++ b/gtdynamics/dynamics/tests/testWrenchFactor.cpp @@ -11,6 +11,8 @@ * @author Yetong Zhang */ +#include +#include #include #include #include @@ -38,7 +40,7 @@ const Matrix6 inertia = link->inertiaMatrix(); const Vector3 gravity(0, -9.8, 0); noiseModel::Gaussian::shared_ptr cost_model = - noiseModel::Gaussian::Covariance(I_6x6); + noiseModel::Gaussian::Covariance(gtsam::I_6x6); } // namespace example @@ -62,7 +64,7 @@ TEST(WrenchFactor, Case1) { InsertPose(&x, id, Pose3(Rot3(), Point3(1, 0, 0))); Vector6 actual_errors = factor->unwhitenedError(x); - Vector6 expected_errors = Z_6x1; + Vector6 expected_errors = gtsam::Z_6x1; EXPECT(assert_equal(expected_errors, actual_errors, 1e-6)); // Make sure linearization is correct EXPECT_CORRECT_FACTOR_JACOBIANS(*factor, x, diffDelta, tol); @@ -86,7 +88,7 @@ TEST(WrenchFactor, Case2) { InsertPose(&x, id, Pose3(Rot3(), Point3(1, 0, 0))); Vector6 actual_errors = factor->unwhitenedError(x); - Vector6 expected_errors = Z_6x1; + Vector6 expected_errors = gtsam::Z_6x1; EXPECT(assert_equal(expected_errors, actual_errors, 1e-6)); // Make sure linearization is correct EXPECT_CORRECT_FACTOR_JACOBIANS(*factor, x, diffDelta, tol); @@ -109,7 +111,7 @@ TEST(WrenchFactor, NonzeroTwistCase) { InsertPose(&x, id, Pose3(Rot3(), Point3(1, 0, 0))); Vector6 actual_errors = factor->unwhitenedError(x); - Vector6 expected_errors = Z_6x1; + Vector6 expected_errors = gtsam::Z_6x1; EXPECT(assert_equal(expected_errors, actual_errors, 1e-6)); // Make sure linearization is correct EXPECT_CORRECT_FACTOR_JACOBIANS(*factor, x, diffDelta, tol); diff --git a/gtdynamics/factors/CollocationFactors.h b/gtdynamics/factors/CollocationFactors.h index be9d4ead7..41cf73d42 100644 --- a/gtdynamics/factors/CollocationFactors.h +++ b/gtdynamics/factors/CollocationFactors.h @@ -85,7 +85,7 @@ class EulerPoseCollocationFactor auto pose_t1_hat = predictPose(pose_t0, twistdt, H_pose_t0, H_twistdt); gtsam::Vector error = pose_t1.logmap(pose_t1_hat); if (H_pose_t1) { - *H_pose_t1 = -gtsam::I_6x6; + *H_pose_t1 = -gtsam::Matrix6::Identity(); } if (H_twist) { *H_twist = H_twistdt * dt; @@ -166,7 +166,7 @@ class TrapezoidalPoseCollocationFactor auto pose_t1_hat = predictPose(pose_t0, twistdt, H_pose_t0, H_twistdt); gtsam::Vector error = pose_t1.logmap(pose_t1_hat); if (H_pose_t1) { - *H_pose_t1 = -gtsam::I_6x6; + *H_pose_t1 = -gtsam::Matrix6::Identity(); } if (H_twist_t0) { *H_twist_t0 = 0.5 * dt * H_twistdt; @@ -248,7 +248,7 @@ class FixTimeEulerPoseCollocationFactor auto pose_t1_hat = predictPose(pose_t0, twistdt, H_pose_t0, H_twistdt); gtsam::Vector error = pose_t1.logmap(pose_t1_hat); if (H_pose_t1) { - *H_pose_t1 = -gtsam::I_6x6; + *H_pose_t1 = -gtsam::Matrix6::Identity(); } if (H_twist_t0) { *H_twist_t0 = dt_ * H_twistdt; @@ -327,7 +327,7 @@ class FixTimeTrapezoidalPoseCollocationFactor auto pose_t1_hat = predictPose(pose_t0, twistdt, H_pose_t0, H_twistdt); gtsam::Vector error = pose_t1.logmap(pose_t1_hat); if (H_pose_t1) { - *H_pose_t1 = -gtsam::I_6x6; + *H_pose_t1 = -gtsam::Matrix6::Identity(); } if (H_twist_t0) { *H_twist_t0 = 0.5 * dt_ * H_twistdt; @@ -401,13 +401,13 @@ class EulerTwistCollocationFactor gtsam::OptionalMatrixType H_dt = nullptr) const override { gtsam::Vector error = twist_t0 + dt * accel - twist_t1; if (H_twist_t1) { - *H_twist_t1 = -gtsam::I_6x6; + *H_twist_t1 = -gtsam::Matrix6::Identity(); } if (H_twist_t0) { - *H_twist_t0 = gtsam::I_6x6; + *H_twist_t0 = gtsam::Matrix6::Identity(); } if (H_accel) { - *H_accel = gtsam::I_6x6 * dt; + *H_accel = gtsam::Matrix6::Identity() * dt; } if (H_dt) { *H_dt = accel; @@ -483,16 +483,16 @@ class TrapezoidalTwistCollocationFactor gtsam::Vector error = twist_t0 + 0.5 * dt * (accel_t0 + accel_t1) - twist_t1; if (H_twist_t1) { - *H_twist_t1 = -gtsam::I_6x6; + *H_twist_t1 = -gtsam::Matrix6::Identity(); } if (H_twist_t0) { - *H_twist_t0 = gtsam::I_6x6; + *H_twist_t0 = gtsam::Matrix6::Identity(); } if (H_accel_t0) { - *H_accel_t0 = 0.5 * dt * gtsam::I_6x6; + *H_accel_t0 = 0.5 * dt * gtsam::Matrix6::Identity(); } if (H_accel_t1) { - *H_accel_t1 = 0.5 * dt * gtsam::I_6x6; + *H_accel_t1 = 0.5 * dt * gtsam::Matrix6::Identity(); } if (H_dt) { *H_dt = 0.5 * (accel_t0 + accel_t1); @@ -564,13 +564,13 @@ class FixTimeEulerTwistCollocationFactor gtsam::OptionalMatrixType H_accel_t0 = nullptr) const override { gtsam::Vector error = twist_t0 + dt_ * accel_t0 - twist_t1; if (H_twist_t1) { - *H_twist_t1 = -gtsam::I_6x6; + *H_twist_t1 = -gtsam::Matrix6::Identity(); } if (H_twist_t0) { - *H_twist_t0 = gtsam::I_6x6; + *H_twist_t0 = gtsam::Matrix6::Identity(); } if (H_accel_t0) { - *H_accel_t0 = dt_ * gtsam::I_6x6; + *H_accel_t0 = dt_ * gtsam::Matrix6::Identity(); } return error; } @@ -644,16 +644,16 @@ class FixTimeTrapezoidalTwistCollocationFactor gtsam::Vector error = twist_t0 + 0.5 * dt_ * (accel_t0 + accel_t1) - twist_t1; if (H_twist_t1) { - *H_twist_t1 = -gtsam::I_6x6; + *H_twist_t1 = -gtsam::Matrix6::Identity(); } if (H_twist_t0) { - *H_twist_t0 = gtsam::I_6x6; + *H_twist_t0 = gtsam::Matrix6::Identity(); } if (H_accel_t0) { - *H_accel_t0 = 0.5 * dt_ * gtsam::I_6x6; + *H_accel_t0 = 0.5 * dt_ * gtsam::Matrix6::Identity(); } if (H_accel_t1) { - *H_accel_t1 = 0.5 * dt_ * gtsam::I_6x6; + *H_accel_t1 = 0.5 * dt_ * gtsam::Matrix6::Identity(); } return error; } diff --git a/gtdynamics/factors/MinTorqueFactor.h b/gtdynamics/factors/MinTorqueFactor.h index 22c4544cf..af2d93246 100644 --- a/gtdynamics/factors/MinTorqueFactor.h +++ b/gtdynamics/factors/MinTorqueFactor.h @@ -52,7 +52,7 @@ class MinTorqueFactor : public gtsam::NoiseModelFactorN { gtsam::Vector error(1); error(0) = torque; - if (H_torque) *H_torque = gtsam::I_1x1; + if (H_torque) *H_torque = gtsam::Matrix1::Identity(); return error; } diff --git a/gtdynamics/factors/ObjectiveFactors.h b/gtdynamics/factors/ObjectiveFactors.h index 9917ec182..2f8bd795f 100644 --- a/gtdynamics/factors/ObjectiveFactors.h +++ b/gtdynamics/factors/ObjectiveFactors.h @@ -31,8 +31,8 @@ namespace gtdynamics { * * Example Usage: * LinkObjectives(graph, id, k).pose(Pose3(), noise) - * .twist(Z_6x1, noise) - * .twistAccel(Z_6x1, noise); + * .twist(gtsam::Vector6::Zero(), noise) + * .twistAccel(gtsam::Vector6::Zero(), noise); */ class LinkObjectives : public gtsam::NonlinearFactorGraph { private: diff --git a/gtdynamics/factors/PreintegratedContactFactors.h b/gtdynamics/factors/PreintegratedContactFactors.h index 50486e90a..488638d1f 100644 --- a/gtdynamics/factors/PreintegratedContactFactors.h +++ b/gtdynamics/factors/PreintegratedContactFactors.h @@ -142,12 +142,12 @@ class PreintegratedPointContactFactor // https://arxiv.org/src/1712.05873v2/anc/icra-supplementary-material.pdf if (H_wTb_i) { gtsam::Matrix36 H; - H << gtsam::SO3::Hat(error), gtsam::Z_3x3; + H << gtsam::SO3::Hat(error), gtsam::Matrix3::Zero(); *H_wTb_i = H; } if (H_wTc_i) { gtsam::Matrix36 H; - H << gtsam::Z_3x3, -gtsam::I_3x3; + H << gtsam::Matrix3::Zero(), -gtsam::Matrix3::Identity(); *H_wTc_i = H; } if (H_wTb_j) { @@ -163,7 +163,7 @@ class PreintegratedPointContactFactor "Body rotation and contact rotation are not equal."); } gtsam::Matrix36 H; - H << gtsam::Z_3x3, + H << gtsam::Matrix3::Zero(), (wTc_i.rotation().inverse() * wTc_j.rotation()).matrix(); *H_wTc_j = H; } @@ -239,7 +239,7 @@ class PreintegratedRigidContactMeasurements { const gtsam::Matrix3 &linearVelocityCovariance, double dt) { gtsam::Matrix6 C; - C << angularVelocityCovariance, gtsam::Z_3x3, gtsam::Z_3x3, + C << angularVelocityCovariance, gtsam::Matrix3::Zero(), gtsam::Matrix3::Zero(), linearVelocityCovariance; preintMeasCov_ += (C * dt * dt); } @@ -251,7 +251,7 @@ class PreintegratedRigidContactMeasurements { * steps. */ void integrateMeasurement(double deltaT) { - preintMeasCov_ << wCov_, gtsam::Z_3x3, gtsam::Z_3x3, vCov_; + preintMeasCov_ << wCov_, gtsam::Matrix3::Zero(), gtsam::Matrix3::Zero(), vCov_; preintMeasCov_ *= deltaT; } diff --git a/gtdynamics/jumpingrobot/factors/JRCollocationFactors.h b/gtdynamics/jumpingrobot/factors/JRCollocationFactors.h index 726ff4114..2be1b27ac 100644 --- a/gtdynamics/jumpingrobot/factors/JRCollocationFactors.h +++ b/gtdynamics/jumpingrobot/factors/JRCollocationFactors.h @@ -29,8 +29,8 @@ namespace gtdynamics { double multDouble1(const double& d1, const double& d2, gtsam::OptionalJacobian<1, 1> H1, gtsam::OptionalJacobian<1, 1> H2) { - if (H1) *H1 = gtsam::I_1x1 * d2; - if (H2) *H2 = gtsam::I_1x1 * d1; + if (H1) *H1 = gtsam::Matrix1::Identity() * d2; + if (H2) *H2 = gtsam::Matrix1::Identity() * d1; return d1 * d2; } diff --git a/gtdynamics/jumpingrobot/factors/PneumaticActuatorFactors.h b/gtdynamics/jumpingrobot/factors/PneumaticActuatorFactors.h index 8395ad59f..8f330547d 100644 --- a/gtdynamics/jumpingrobot/factors/PneumaticActuatorFactors.h +++ b/gtdynamics/jumpingrobot/factors/PneumaticActuatorFactors.h @@ -457,7 +457,7 @@ class ClippingActuatorFactor H_p->setConstant(1, 1, derivative_y); } if (H_f) { - *H_f = -gtsam::I_1x1; + *H_f = -gtsam::Matrix1::Identity(); } return gtsam::Vector1(f_expected - f); } @@ -527,7 +527,7 @@ class ActuatorVolumeFactor : public gtsam::NoiseModelFactorN { gtsam::OptionalMatrixType H_l = nullptr) const override { double expected_v = computeVolume(l, H_l); if (H_v) { - *H_v = -gtsam::I_1x1; + *H_v = -gtsam::Matrix1::Identity(); } return gtsam::Vector1(expected_v - v); } diff --git a/gtdynamics/jumpingrobot/factors/PneumaticFactors.h b/gtdynamics/jumpingrobot/factors/PneumaticFactors.h index 8cc5ba665..a76cb421e 100644 --- a/gtdynamics/jumpingrobot/factors/PneumaticFactors.h +++ b/gtdynamics/jumpingrobot/factors/PneumaticFactors.h @@ -149,7 +149,7 @@ class MassFlowRateFactor double expected_mdot = computeExpectedMassFlow(pm, ps, mdot, H_pm, H_ps, H_mdot); if (H_mdot) { - *H_mdot = *H_mdot - gtsam::I_1x1; + *H_mdot = *H_mdot - gtsam::Matrix1::Identity(); } return gtsam::Vector1(expected_mdot - mdot); } diff --git a/gtdynamics/kinematics/JointLimitFactor.h b/gtdynamics/kinematics/JointLimitFactor.h index e61252017..3eb7a7dae 100644 --- a/gtdynamics/kinematics/JointLimitFactor.h +++ b/gtdynamics/kinematics/JointLimitFactor.h @@ -69,13 +69,13 @@ class JointLimitFactor : public gtsam::NoiseModelFactorN { gtsam::OptionalMatrixType H_q) const override { gtsam::Vector error(1); if (q < low_) { - if (H_q) *H_q = -gtsam::I_1x1; + if (H_q) *H_q = -gtsam::Matrix1::Identity(); error(0) = low_ - q; } else if (q <= high_) { - if (H_q) *H_q = gtsam::Z_1x1; + if (H_q) *H_q = gtsam::Matrix1::Zero(); error(0) = 0.0; } else { - if (H_q) *H_q = gtsam::I_1x1; + if (H_q) *H_q = gtsam::Matrix1::Identity(); error(0) = q - high_; } return error; diff --git a/gtdynamics/kinematics/KinematicsParameters.h b/gtdynamics/kinematics/KinematicsParameters.h index dfa80f587..d3d41d32c 100644 --- a/gtdynamics/kinematics/KinematicsParameters.h +++ b/gtdynamics/kinematics/KinematicsParameters.h @@ -13,8 +13,13 @@ #pragma once #include +#include #include +#include +#include +#include + namespace gtdynamics { /// Noise models etc specific to Kinematics class @@ -22,13 +27,20 @@ struct KinematicsParameters : public OptimizationParameters { using Isotropic = gtsam::noiseModel::Isotropic; gtsam::SharedNoiseModel p_cost_model, // pose factor g_cost_model, // goal point - prior_q_cost_model, // joint angle prior factor + prior_q_cost_model, // joint angle prior factor (default) bp_cost_model, // fixed-link pose prior factor cp_cost_model, // contact-height factor bv_cost_model, // fixed-link twist prior factor v_cost_model, // twist factor cv_cost_model; // contact-twist factor + /// Per-joint joint-angle prior models keyed by joint key; overrides + /// prior_q_cost_model for listed joints (used by kinematics IK only). + std::map joint_prior_overrides; + + /// Per-joint {lower, upper} limit overrides by joint key (IK only). + std::map> joint_limit_overrides; + KinematicsParameters() : KinematicsParameters(1e-4, 1e-2, 0.5, 1e-4, 1e-2, 1e-4, 1e-4, 1e-2) {} @@ -68,6 +80,16 @@ struct KinematicsParameters : public OptimizationParameters { Isotropic::Sigma(6, bv_cost_model_sigma), Isotropic::Sigma(6, v_cost_model_sigma), Isotropic::Sigma(3, cv_cost_model_sigma)) {} + + /// Override the joint-angle prior sigma for one joint by key. + void setJointPriorSigma(gtsam::Key joint_key, double sigma) { + joint_prior_overrides[joint_key] = Isotropic::Sigma(1, sigma); + } + + /// Override the joint-angle limits for one joint by key. + void setJointLimit(gtsam::Key joint_key, double lower, double upper) { + joint_limit_overrides[joint_key] = {lower, upper}; + } }; } // namespace gtdynamics diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index 33c03bd71..dfeeb84c9 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -302,11 +302,15 @@ NonlinearFactorGraph Kinematics::jointAngleObjectives( const Slice& slice, const Robot& robot, const Values& mean) const { NonlinearFactorGraph graph; - // Minimize the joint angles. + // Per-joint override from p_.joint_prior_overrides, else prior_q_cost_model. for (auto&& joint : robot.joints()) { const gtsam::Key key = JointAngleKey(joint->id(), slice.k); + auto it = p_.joint_prior_overrides.find(joint->key()); + const gtsam::SharedNoiseModel cost_model = + (it != p_.joint_prior_overrides.end()) ? it->second + : p_.prior_q_cost_model; graph.addPrior(key, (mean.exists(key) ? mean.at(key) : 0.0), - p_.prior_q_cost_model); + cost_model); } return graph; @@ -317,12 +321,18 @@ NonlinearFactorGraph Kinematics::jointAngleLimits( const Slice& slice, const Robot& robot) const { NonlinearFactorGraph graph; for (auto&& joint : robot.joints()) { - graph.add(JointLimitFactor( - JointAngleKey(joint->id(), slice.k), - gtsam::noiseModel::Isotropic::Sigma(1, 0.001), - joint->parameters().scalar_limits.value_lower_limit, - joint->parameters().scalar_limits.value_upper_limit, - 0.04)); // joint->parameters().scalar_limits.value_limit_threshold)); + // Override URDF limits for joints listed in p_.joint_limit_overrides. + auto it = p_.joint_limit_overrides.find(joint->key()); + const auto& limits = joint->parameters().scalar_limits; + const double lower = + (it != p_.joint_limit_overrides.end()) ? it->second.first + : limits.value_lower_limit; + const double upper = + (it != p_.joint_limit_overrides.end()) ? it->second.second + : limits.value_upper_limit; + graph.add(JointLimitFactor(JointAngleKey(joint->id(), slice.k), + gtsam::noiseModel::Isotropic::Sigma(1, 0.001), + lower, upper, 0.04)); } return graph; } diff --git a/gtdynamics/kinematics/tests/testContactHeightFactor.cpp b/gtdynamics/kinematics/tests/testContactHeightFactor.cpp index 857776d20..af3df8c4a 100644 --- a/gtdynamics/kinematics/tests/testContactHeightFactor.cpp +++ b/gtdynamics/kinematics/tests/testContactHeightFactor.cpp @@ -11,6 +11,7 @@ * @author Alejandro Escontrela */ +#include #include #include #include diff --git a/gtdynamics/kinematics/tests/testContactKinematicsTwistFactor.cpp b/gtdynamics/kinematics/tests/testContactKinematicsTwistFactor.cpp index 5745edd80..3de167ca4 100644 --- a/gtdynamics/kinematics/tests/testContactKinematicsTwistFactor.cpp +++ b/gtdynamics/kinematics/tests/testContactKinematicsTwistFactor.cpp @@ -11,6 +11,7 @@ * @author Alejandro Escontrela */ +#include #include #include #include diff --git a/gtdynamics/kinematics/tests/testKinematicsSlice.cpp b/gtdynamics/kinematics/tests/testKinematicsSlice.cpp index d53207d54..e286d8db6 100644 --- a/gtdynamics/kinematics/tests/testKinematicsSlice.cpp +++ b/gtdynamics/kinematics/tests/testKinematicsSlice.cpp @@ -11,6 +11,7 @@ * @author: Frank Dellaert */ +#include #include #include #include @@ -422,6 +423,54 @@ TEST(Slice, BarLabPoseConstraintIK) { result.at(PoseKey(base_link->id(), k)), tol)); } +// Higher gantry prior sigmas let the gantry absorb more of the motion to goal. +TEST(Slice, BarLabGantryPriorSigma) { + using gtsam::Pose3; + using gtsam::Values; + + const Robot robot = + CreateRobotFromFile(kUrdfPath + std::string("bar_lab.urdf")) + .fixLink("columns"); + auto ee_link = robot.link("robot1_link_6"); + const auto ee_id = ee_link->id(); + const std::vector gantry_joints = { + "bridge1_joint_EA_X", "robot1_joint_EA_Y", "robot1_joint_EA_Z"}; + + // Reachable goal from a config that uses the gantry, so there is redundancy. + Kinematics goal_kinematics; + Values fk = goal_kinematics.initialValues(kSlice, robot, 0.0, + barLabConfig(robot), true); + const Pose3 wTcom_goal = fk.at(PoseKey(ee_id, k)); + PoseGoals pose_goals = {{k, PoseGoal(ee_link, Pose3(), wTcom_goal)}}; + + // Solve IK with the given gantry prior sigma; pose goal is a hard constraint. + auto solve = [&](double gantry_sigma) { + KinematicsParameters params; + params.method = OptimizationParameters::Method::AUGMENTED_LAGRANGIAN; + for (const auto& name : gantry_joints) + params.setJointPriorSigma(robot.joint(name)->key(), gantry_sigma); + return Kinematics(params).inverse(kSlice, robot, pose_goals, gtsam::Values(), + /*pose_goals_as_constraints=*/true); + }; + + // Total absolute travel of the gantry joints from their zero prior mean. + auto gantry_travel = [&](const Values& result) { + double travel = 0.0; + for (const auto& name : gantry_joints) + travel += std::fabs( + result.at(JointAngleKey(robot.joint(name)->id(), k))); + return travel; + }; + + const Values tight = solve(0.1); + const Values loose = solve(10.0); + + // Both reach the goal pose; the looser gantry priors move the gantry more. + EXPECT(assert_equal(wTcom_goal, tight.at(PoseKey(ee_id, k)), 1e-3)); + EXPECT(assert_equal(wTcom_goal, loose.at(PoseKey(ee_id, k)), 1e-3)); + EXPECT(gantry_travel(tight) < gantry_travel(loose)); +} + int main() { TestResult tr; return TestRegistry::runAllTests(tr); diff --git a/gtdynamics/kinematics/tests/testPoseFactor.cpp b/gtdynamics/kinematics/tests/testPoseFactor.cpp index d9528a090..036f10653 100644 --- a/gtdynamics/kinematics/tests/testPoseFactor.cpp +++ b/gtdynamics/kinematics/tests/testPoseFactor.cpp @@ -11,6 +11,8 @@ * @author Frank Dellaert and Mandy Xie */ +#include +#include #include #include #include @@ -34,7 +36,6 @@ using gtsam::Pose3; using gtsam::Rot3; using gtsam::Values; using gtsam::Vector6; -using gtsam::Z_6x1; using gtsam::noiseModel::Gaussian; namespace example { @@ -87,7 +88,7 @@ TEST(PoseFactor, breaking) { InsertPose(&values, 1, Pose3(Rot3(), Point3(1, 0, 0))); InsertPose(&values, 2, Pose3(Rot3(), Point3(3, 0, 0))); InsertJointAngle(&values, 1, 0.0); - EXPECT(assert_equal(Z_6x1, factor->unwhitenedError(values), 1e-6)); + EXPECT(assert_equal(gtsam::Z_6x1, factor->unwhitenedError(values), 1e-6)); } // check prediction at half PI @@ -96,7 +97,7 @@ TEST(PoseFactor, breaking) { InsertPose(&values, 1, Pose3(Rot3(), Point3(1, 0, 0))); InsertPose(&values, 2, Pose3(Rot3::Rz(M_PI / 2), Point3(2, 1, 0))); InsertJointAngle(&values, 1, M_PI / 2); - EXPECT(assert_equal(Z_6x1, factor->unwhitenedError(values), 1e-6)); + EXPECT(assert_equal(gtsam::Z_6x1, factor->unwhitenedError(values), 1e-6)); } } @@ -120,7 +121,7 @@ TEST(PoseFactor, breaking_rr) { InsertPose(&values, 1, Pose3()); InsertPose(&values, 2, j1->relativePoseOf(l2, M_PI / 4)); InsertJointAngle(&values, 1, M_PI / 4); - EXPECT(assert_equal(Z_6x1, factor->unwhitenedError(values), 1e-6)); + EXPECT(assert_equal(gtsam::Z_6x1, factor->unwhitenedError(values), 1e-6)); } // Test non-zero cMp rotation case diff --git a/gtdynamics/kinematics/tests/testTwistFactor.cpp b/gtdynamics/kinematics/tests/testTwistFactor.cpp index ebb872cee..dbc2bf4b4 100644 --- a/gtdynamics/kinematics/tests/testTwistFactor.cpp +++ b/gtdynamics/kinematics/tests/testTwistFactor.cpp @@ -11,6 +11,7 @@ * @author Frank Dellaert and Mandy Xie */ +#include #include #include #include diff --git a/gtdynamics/mechanics/tests/testTorqueFactor.cpp b/gtdynamics/mechanics/tests/testTorqueFactor.cpp index 8dffbb1be..a4f12b2f7 100644 --- a/gtdynamics/mechanics/tests/testTorqueFactor.cpp +++ b/gtdynamics/mechanics/tests/testTorqueFactor.cpp @@ -11,6 +11,7 @@ * @author Frank Dellaert and Mandy Xie */ +#include #include #include #include diff --git a/gtdynamics/mechanics/tests/testWrenchEquivalenceFactor.cpp b/gtdynamics/mechanics/tests/testWrenchEquivalenceFactor.cpp index ab1dc5f43..c64874bd6 100644 --- a/gtdynamics/mechanics/tests/testWrenchEquivalenceFactor.cpp +++ b/gtdynamics/mechanics/tests/testWrenchEquivalenceFactor.cpp @@ -11,6 +11,7 @@ * @author Yetong Zhang */ +#include #include #include #include diff --git a/gtdynamics/mechanics/tests/testWrenchPlanarFactor.cpp b/gtdynamics/mechanics/tests/testWrenchPlanarFactor.cpp index 5e1105126..0096917eb 100644 --- a/gtdynamics/mechanics/tests/testWrenchPlanarFactor.cpp +++ b/gtdynamics/mechanics/tests/testWrenchPlanarFactor.cpp @@ -11,6 +11,8 @@ * @author Yetong Zhang */ +#include +#include #include #include #include diff --git a/gtdynamics/optimizer/ConvexIQPSolver.cpp b/gtdynamics/optimizer/ConvexIQPSolver.cpp index 5ff435800..d70475b56 100644 --- a/gtdynamics/optimizer/ConvexIQPSolver.cpp +++ b/gtdynamics/optimizer/ConvexIQPSolver.cpp @@ -12,6 +12,7 @@ */ #include +#include namespace gtdynamics { diff --git a/gtdynamics/scenarios/IEQuadrupedUtilsCosts.cpp b/gtdynamics/scenarios/IEQuadrupedUtilsCosts.cpp index 231f5115a..70a64c6d3 100644 --- a/gtdynamics/scenarios/IEQuadrupedUtilsCosts.cpp +++ b/gtdynamics/scenarios/IEQuadrupedUtilsCosts.cpp @@ -11,6 +11,7 @@ * @author: Yetong Zhang */ +#include #include #include @@ -33,7 +34,7 @@ Vector3 IEVision60Robot::GetContactForce(const Pose3 &pose, Vector3 force_l(wrench(3), wrench(4), wrench(5)); if (H_pose || H_wrench) { Matrix36 J_fl_wrench; - J_fl_wrench << Z_3x3, I_3x3; + J_fl_wrench << gtsam::Z_3x3, gtsam::I_3x3; Matrix36 J_rot_pose; Rot3 rot = pose.rotation(J_rot_pose); @@ -769,7 +770,7 @@ NoiseModelFactor::shared_ptr IEVision60Robot::statePointVelCostFactor( Pose3 lTc(Rot3::Identity(), point_l); Pose3 cTl = lTc.inverse(); Matrix36 H_vel_c; - H_vel_c << Z_3x3, I_3x3; + H_vel_c << gtsam::Z_3x3, gtsam::I_3x3; H_vel_c = H_vel_c * cTl.AdjointMap(); const std::function f = [H_vel_c](const Vector6 &A) { return H_vel_c * A; }; diff --git a/gtdynamics/statics/Statics.cpp b/gtdynamics/statics/Statics.cpp index fd90114f4..09fe23905 100644 --- a/gtdynamics/statics/Statics.cpp +++ b/gtdynamics/statics/Statics.cpp @@ -11,6 +11,8 @@ * @author Frank Dellaert, Mandy Xie, Yetong Zhang, and Gerry Chen */ +#include +#include #include #include #include diff --git a/gtdynamics/statics/tests/testStaticWrenchFactor.cpp b/gtdynamics/statics/tests/testStaticWrenchFactor.cpp index 1d8aaf814..338cda41c 100644 --- a/gtdynamics/statics/tests/testStaticWrenchFactor.cpp +++ b/gtdynamics/statics/tests/testStaticWrenchFactor.cpp @@ -11,6 +11,8 @@ * @author Yetong Zhang */ +#include +#include #include #include #include @@ -38,7 +40,7 @@ const double mass = robot.links()[0]->mass(); const Vector3 gravity(0, -9.8, 0); noiseModel::Gaussian::shared_ptr cost_model = - noiseModel::Gaussian::Covariance(I_6x6); + noiseModel::Gaussian::Covariance(gtsam::I_6x6); } // namespace example @@ -60,7 +62,7 @@ TEST(StaticWrenchFactor, GravityCompensation) { InsertPose(&x, id, Pose3(Rot3(), Point3(1, 0, 0))); Vector6 actual_errors = factor.unwhitenedError(x); - Vector6 expected_errors = Z_6x1; + Vector6 expected_errors = gtsam::Z_6x1; EXPECT(assert_equal(expected_errors, actual_errors, 1e-6)); // Make sure linearization is correct EXPECT_CORRECT_FACTOR_JACOBIANS(factor, x, diffDelta, tol); diff --git a/gtdynamics/universal_robot/Joint.cpp b/gtdynamics/universal_robot/Joint.cpp index 1a58ee317..25a9c5374 100644 --- a/gtdynamics/universal_robot/Joint.cpp +++ b/gtdynamics/universal_robot/Joint.cpp @@ -14,6 +14,7 @@ * @brief Abstract representation of a robot joint. */ +#include #include #include #include diff --git a/gtdynamics/universal_robot/Link.cpp b/gtdynamics/universal_robot/Link.cpp index 3ceb6a714..d424411b6 100644 --- a/gtdynamics/universal_robot/Link.cpp +++ b/gtdynamics/universal_robot/Link.cpp @@ -14,6 +14,8 @@ * @brief Abstract representation of a robot link. */ +#include +#include #include #include #include diff --git a/gtdynamics/utils/Initializer.cpp b/gtdynamics/utils/Initializer.cpp index 6b014816f..518cf700c 100644 --- a/gtdynamics/utils/Initializer.cpp +++ b/gtdynamics/utils/Initializer.cpp @@ -11,6 +11,7 @@ * @authors Alejandro Escontrela, Yetong Zhang, Varun Agrawal */ +#include #include #include #include diff --git a/gtdynamics/utils/Trajectory.cpp b/gtdynamics/utils/Trajectory.cpp index 4c86894d4..ddbba1a4c 100644 --- a/gtdynamics/utils/Trajectory.cpp +++ b/gtdynamics/utils/Trajectory.cpp @@ -12,6 +12,7 @@ * @author: Frank Dellaert, Gerry Chen, Frank Dellaert */ +#include #include #include #include @@ -27,7 +28,6 @@ using gtsam::NonlinearFactorGraph; using gtsam::Point3; using gtsam::SharedNoiseModel; using gtsam::Values; -using gtsam::Z_6x1; using std::map; using std::string; using std::to_string; @@ -119,12 +119,12 @@ void Trajectory::addBoundaryConditions( // Initial link pose, twists. graph->add(LinkObjectives(link->id(), 0) .pose(link->bMcom(), pose_model) - .twist(Z_6x1, twist_model)); + .twist(gtsam::Z_6x1, twist_model)); // Final link twists, accelerations. graph->add(LinkObjectives(link->id(), K) - .twist(Z_6x1, twist_model) - .twistAccel(Z_6x1, twist_acceleration_model)); + .twist(gtsam::Z_6x1, twist_model) + .twistAccel(gtsam::Z_6x1, twist_acceleration_model)); } // Add joint boundary conditions to FG. diff --git a/python/tests/test_forward_inv_kinematics.py b/python/tests/test_forward_inv_kinematics.py index cf8669306..57baff0cb 100644 --- a/python/tests/test_forward_inv_kinematics.py +++ b/python/tests/test_forward_inv_kinematics.py @@ -146,6 +146,44 @@ def test_forward_inv_kinematics(self): got = gtd.JointAngle(result, j.id(), k) self.assertAlmostEqual(got, self.angle_rad[j.name()], delta=1e-2) + def test_gantry_prior_sigma(self): + """Looser gantry prior sigmas let the gantry absorb more of the motion.""" + k = self.k + # Fix the gantry base so the gantry joints lie in the chain to the EE. + robot = gtd.CreateRobotFromFile( + str(Path(gtd.URDF_PATH) / "bar_lab.urdf")).fixLink("columns") + link6 = robot.link("robot1_link_6") + gantry_joints = ["bridge1_joint_EA_X", "robot1_joint_EA_Y", + "robot1_joint_EA_Z"] + + # Reachable goal from a config that uses the gantry (so it is redundant). + config = { + "bridge1_joint_EA_X": 2.0, "robot1_joint_EA_Y": 1.5, + "robot1_joint_EA_Z": 0.5, "robot1_joint_1": 0.3, + "robot1_joint_2": 0.4, "robot1_joint_3": -0.5, + "robot1_joint_4": 0.2, "robot1_joint_5": 0.6, "robot1_joint_6": -0.3, + } + q = gtsam.Values() + for j in robot.joints(): + gtd.InsertJointAngle(q, j.id(), k, config.get(j.name(), 0.0)) + fk = robot.forwardKinematics(q, k, "columns") + pose_goals = {k: gtd.PoseGoal(link6, gtsam.Pose3(), + gtd.Pose(fk, link6.id(), k))} + slice0 = gtd.Slice(k) + + # Total gantry travel from the zero prior mean for a given gantry sigma. + def gantry_travel(gantry_sigma): + params = gtd.KinematicsParameters() + for name in gantry_joints: + params.setJointPriorSigma(robot.joint(name).key(), gantry_sigma) + result = gtd.Kinematics(params).inverse( + slice0, robot, pose_goals, gtsam.Values(), True) + return sum(abs(gtd.JointAngle(result, robot.joint(n).id(), k)) + for n in gantry_joints) + + # Looser gantry priors -> the gantry moves more to reach the goal. + self.assertLess(gantry_travel(0.1), gantry_travel(10.0)) + diff --git a/tests/testCollocationFactor.cpp b/tests/testCollocationFactor.cpp index cb2ac80f3..537109c61 100644 --- a/tests/testCollocationFactor.cpp +++ b/tests/testCollocationFactor.cpp @@ -11,6 +11,7 @@ * @author Yetong Zhang */ +#include #include #include #include diff --git a/tests/testConvexIQPSolver.cpp b/tests/testConvexIQPSolver.cpp index b21b7d390..4483a77f1 100644 --- a/tests/testConvexIQPSolver.cpp +++ b/tests/testConvexIQPSolver.cpp @@ -11,6 +11,7 @@ * @author Yetong Zhang */ +#include #include #include #include @@ -32,12 +33,12 @@ TEST(SolveConvexIQP, example_2D) { GaussianFactorGraph graph; auto model = noiseModel::Unit::Create(1); - graph.add(JacobianFactor(x_key, I_1x1, Vector::Zero(1), model)); - graph.add(JacobianFactor(y_key, I_1x1, Vector1(1), model)); + graph.add(JacobianFactor(x_key, gtsam::I_1x1, Vector::Zero(1), model)); + graph.add(JacobianFactor(y_key, gtsam::I_1x1, Vector1(1), model)); LinearInequalityConstraints constraints; - auto factor1 = std::make_shared(x_key, I_1x1, y_key, -I_1x1, Vector::Zero(1), model); - auto factor2 = std::make_shared(x_key, I_1x1, y_key, I_1x1, Vector::Zero(1), model); + auto factor1 = std::make_shared(x_key, gtsam::I_1x1, y_key, -gtsam::I_1x1, Vector::Zero(1), model); + auto factor2 = std::make_shared(x_key, gtsam::I_1x1, y_key, gtsam::I_1x1, Vector::Zero(1), model); constraints.emplace_shared(factor1); constraints.emplace_shared(factor2); diff --git a/tests/testMinTorqueFactor.cpp b/tests/testMinTorqueFactor.cpp index 1a3e57cd2..d7b5a71bd 100644 --- a/tests/testMinTorqueFactor.cpp +++ b/tests/testMinTorqueFactor.cpp @@ -11,6 +11,7 @@ * @author Alejandro Escontrela */ +#include #include #include #include diff --git a/tests/testMultiJacobian.cpp b/tests/testMultiJacobian.cpp index 56a28f264..b3d0542e9 100644 --- a/tests/testMultiJacobian.cpp +++ b/tests/testMultiJacobian.cpp @@ -12,6 +12,7 @@ */ +#include #include #include #include @@ -55,7 +56,7 @@ TEST(MultiJacobian, Add_Mult) { jac12_sum += jac2; EXPECT(expected_sum.equals(jac12_sum)); - Matrix m = I_2x2 * 2; + Matrix m = gtsam::I_2x2 * 2; MultiJacobian expected_mult1; expected_mult1.insert({x1, (Matrix(2, 2) << 2, 4, 6, 8).finished()}); expected_mult1.insert({x2, (Matrix(2, 1) << 6, 4).finished()}); @@ -80,26 +81,26 @@ TEST(MultiJacobians, Mult) { Key x4 = 4; MultiJacobian jac1_x1, jac1_x2, jac1_x3, jac1_x4; - jac1_x1.insert({x1, I_2x2}); - jac1_x2.insert({x2, I_2x2}); - jac1_x3.insert({x3, I_2x2}); + jac1_x1.insert({x1, gtsam::I_2x2}); + jac1_x2.insert({x2, gtsam::I_2x2}); + jac1_x3.insert({x3, gtsam::I_2x2}); jac1_x4.insert({x1, (Matrix(1,2)<<1,2).finished()}); jac1_x4.insert({x2, (Matrix(1,2)<<2,1).finished()}); jac1_x4.insert({x3, (Matrix(1,2)<<1,-1).finished()}); MultiJacobians jacs1{{x1, jac1_x1}, {x2, jac1_x2}, {x3, jac1_x3}, {x4, jac1_x4}}; MultiJacobian jac2_x1, jac2_x2, jac2_x3; - jac2_x1.insert({x1, I_2x2}); - jac2_x2.insert({x2, I_2x2}); - jac2_x3.insert({x1, 2*I_2x2}); - jac2_x3.insert({x2, -1*I_2x2}); + jac2_x1.insert({x1, gtsam::I_2x2}); + jac2_x2.insert({x2, gtsam::I_2x2}); + jac2_x3.insert({x1, 2*gtsam::I_2x2}); + jac2_x3.insert({x2, -1*gtsam::I_2x2}); MultiJacobians jacs2{{x1, jac2_x1}, {x2, jac2_x2}, {x3, jac2_x3}}; MultiJacobian expected_jac_x1, expected_jac_x2, expected_jac_x3, expected_jac_x4; - expected_jac_x1.insert({x1, I_2x2}); - expected_jac_x2.insert({x2, I_2x2}); - expected_jac_x3.insert({x1, 2*I_2x2}); - expected_jac_x3.insert({x2, -1*I_2x2}); + expected_jac_x1.insert({x1, gtsam::I_2x2}); + expected_jac_x2.insert({x2, gtsam::I_2x2}); + expected_jac_x3.insert({x1, 2*gtsam::I_2x2}); + expected_jac_x3.insert({x2, -1*gtsam::I_2x2}); expected_jac_x4.insert({x1, (Matrix(1,2)<<1+2,2-2).finished()}); expected_jac_x4.insert({x2, (Matrix(1,2)<<2-1,1+1).finished()}); @@ -121,11 +122,11 @@ TEST(MultiJacobian, ComputeBayesNetJacobian) { GaussianFactorGraph graph; auto model1 = noiseModel::Isotropic::Sigma(1, 1.0); auto model2 = noiseModel::Isotropic::Sigma(2, 1.0); - graph.add(JacobianFactor(x1, I_1x1, x2, I_1x1, x3, -I_1x1, Vector1(0), model1)); + graph.add(JacobianFactor(x1, gtsam::I_1x1, x2, gtsam::I_1x1, x3, -gtsam::I_1x1, Vector1(0), model1)); Matrix21 H_3, H_4; H_3 << 1, 0; H_4 << 0, 1; - graph.add(JacobianFactor(x3, H_3, x4, H_4, x5, -I_2x2, Vector2(0, 0), model2)); + graph.add(JacobianFactor(x3, H_3, x4, H_4, x5, -gtsam::I_2x2, Vector2(0, 0), model2)); Ordering ordering; ordering.push_back(x5); @@ -143,8 +144,8 @@ TEST(MultiJacobian, ComputeBayesNetJacobian) { ComputeBayesNetJacobian(*bayes_net, basis_keys, var_dim, jacobians); MultiJacobian jacobian_x3, jacobian_x5; - jacobian_x3.addJacobian(x1, I_1x1); - jacobian_x3.addJacobian(x2, I_1x1); + jacobian_x3.addJacobian(x1, gtsam::I_1x1); + jacobian_x3.addJacobian(x2, gtsam::I_1x1); jacobian_x5.addJacobian(x1, H_3); jacobian_x5.addJacobian(x2, H_3); jacobian_x5.addJacobian(x4, H_4); diff --git a/tests/testObjectiveFactors.cpp b/tests/testObjectiveFactors.cpp index 6f8fd05bc..e68b2acf8 100644 --- a/tests/testObjectiveFactors.cpp +++ b/tests/testObjectiveFactors.cpp @@ -11,6 +11,7 @@ * @author Frank Dellaert */ +#include #include #include #include diff --git a/tests/testPreintegratedContactFactors.cpp b/tests/testPreintegratedContactFactors.cpp index ad6fb67d1..f837ac3dc 100644 --- a/tests/testPreintegratedContactFactors.cpp +++ b/tests/testPreintegratedContactFactors.cpp @@ -11,6 +11,7 @@ * @author Varun Agrawal */ +#include #include #include #include @@ -36,8 +37,8 @@ using gtsam::assert_equal; TEST(PreintegratedPointContactMeasurements, Constructor) { PreintegratedPointContactMeasurements(); PreintegratedPointContactMeasurements pcm( - Pose3(), Pose3(Rot3(), Vector3(0, 0, 1)), 0.01, I_3x3); - EXPECT(assert_equal(I_3x3 * 1e-4, pcm.preintMeasCov())); + Pose3(), Pose3(Rot3(), Vector3(0, 0, 1)), 0.01, gtsam::I_3x3); + EXPECT(assert_equal(gtsam::I_3x3 * 1e-4, pcm.preintMeasCov())); } /* ************************************************************************* */ @@ -45,17 +46,17 @@ TEST(PreintegratedPointContactMeasurements, Constructor) { TEST(PreintegratedPointContactMeasurements, IntegrateMeasurement) { double dt = 0.01; PreintegratedPointContactMeasurements pcm( - Pose3(), Pose3(Rot3(), Vector3(0, 0, 1)), dt, I_3x3); + Pose3(), Pose3(Rot3(), Vector3(0, 0, 1)), dt, gtsam::I_3x3); Rot3 deltaRik = Rot3::Ry(M_PI_4); Pose3 contact_k(Rot3(), Vector3(0, 0, 1)); // Regression pcm.integrateMeasurement(deltaRik, contact_k, dt); - EXPECT(assert_equal(I_3x3 * 2e-4, pcm.preintMeasCov())); + EXPECT(assert_equal(gtsam::I_3x3 * 2e-4, pcm.preintMeasCov())); // Regression pcm.integrateMeasurement(deltaRik, Pose3(Rot3(), Vector3(0, 0, 1.01)), dt); - EXPECT(assert_equal(I_3x3 * 3e-4, pcm.preintMeasCov())); + EXPECT(assert_equal(gtsam::I_3x3 * 3e-4, pcm.preintMeasCov())); } /* ************************************************************************* */ @@ -63,7 +64,7 @@ TEST(PreintegratedPointContactMeasurements, IntegrateMeasurement) { TEST(PreintegratedPointContactFactor, Constructor) { double dt = 0.01; PreintegratedPointContactMeasurements pcm( - Pose3(), Pose3(Rot3(), Vector3(0, 0, 1)), dt, I_3x3); + Pose3(), Pose3(Rot3(), Vector3(0, 0, 1)), dt, gtsam::I_3x3); size_t base_id = 0, contact_id = 1; PreintegratedPointContactFactor ppcf( PoseKey(base_id, 0), PoseKey(contact_id, 0), PoseKey(base_id, 1), @@ -83,7 +84,7 @@ TEST(PreintegratedPointContactFactor, Error) { wTb_j = Pose3(Rot3(), Point3(0.1, 0, 0)), wTc_j = Pose3(Rot3::Ry(M_PI_4), Point3(0, 0, 1)); - PreintegratedPointContactMeasurements pcm(wTb_i, wTc_i, 0.01, I_3x3); + PreintegratedPointContactMeasurements pcm(wTb_i, wTc_i, 0.01, gtsam::I_3x3); PreintegratedPointContactFactor factor( PoseKey(base_id, t0), PoseKey(contact_id, t0), PoseKey(base_id, t1), PoseKey(contact_id, t1), pcm); @@ -104,7 +105,7 @@ TEST(PreintegratedPointContactFactor, Jacobians) { wTb_j = Pose3(Rot3::Ry(M_PI_4), Point3(0.1, 0, 0)), wTc_j = Pose3(Rot3::Ry(M_PI_4), Point3(0, 0, 1)); - PreintegratedPointContactMeasurements pcm(wTb_i, wTc_i, 0.01, I_3x3); + PreintegratedPointContactMeasurements pcm(wTb_i, wTc_i, 0.01, gtsam::I_3x3); PreintegratedPointContactFactor factor( PoseKey(base_id, t0), PoseKey(contact_id, t0), PoseKey(base_id, t1), PoseKey(contact_id, t1), pcm); @@ -124,9 +125,9 @@ TEST(PreintegratedPointContactFactor, Jacobians) { TEST(PreintegratedRigidContactMeasurements, Constructor) { PreintegratedRigidContactMeasurements(); - PreintegratedRigidContactMeasurements pcm(I_3x3, I_3x3); + PreintegratedRigidContactMeasurements pcm(gtsam::I_3x3, gtsam::I_3x3); // Covariance is 0 initially - EXPECT(assert_equal(Z_6x6, pcm.preintMeasCov())); + EXPECT(assert_equal(gtsam::Z_6x6, pcm.preintMeasCov())); } /* ************************************************************************* */ @@ -134,11 +135,11 @@ TEST(PreintegratedRigidContactMeasurements, Constructor) { // Measurements object works as expected. TEST(PreintegratedRigidContactMeasurements, IntegrateMeasurementConstant) { double deltaT = 0.01; - PreintegratedRigidContactMeasurements pcm(I_3x3, I_3x3); + PreintegratedRigidContactMeasurements pcm(gtsam::I_3x3, gtsam::I_3x3); // Regression pcm.integrateMeasurement(deltaT); - EXPECT(assert_equal(I_6x6 * deltaT, pcm.preintMeasCov())); + EXPECT(assert_equal(gtsam::I_6x6 * deltaT, pcm.preintMeasCov())); } /* ************************************************************************* */ @@ -146,17 +147,17 @@ TEST(PreintegratedRigidContactMeasurements, IntegrateMeasurementConstant) { // Contact Measurements object works as expected. TEST(PreintegratedRigidContactMeasurements, IntegrateMeasurementVarying) { double dt = 0.01; - PreintegratedRigidContactMeasurements pcm(I_3x3, I_3x3); + PreintegratedRigidContactMeasurements pcm(gtsam::I_3x3, gtsam::I_3x3); Matrix6 expected; // Regression - pcm.integrateMeasurement(I_3x3 * 0.05, I_3x3 * 0.01, dt); - expected << I_3x3 * 0.05, Z_3x3, Z_3x3, I_3x3 * 0.01; + pcm.integrateMeasurement(gtsam::I_3x3 * 0.05, gtsam::I_3x3 * 0.01, dt); + expected << gtsam::I_3x3 * 0.05, gtsam::Z_3x3, gtsam::Z_3x3, gtsam::I_3x3 * 0.01; EXPECT(assert_equal(expected * dt * dt, pcm.preintMeasCov())); // Regression 2 - pcm.integrateMeasurement(I_3x3 * 0.06, I_3x3 * 0.02, dt); - expected << I_3x3 * (0.05 + 0.06), Z_3x3, Z_3x3, I_3x3 * (0.01 + 0.02); + pcm.integrateMeasurement(gtsam::I_3x3 * 0.06, gtsam::I_3x3 * 0.02, dt); + expected << gtsam::I_3x3 * (0.05 + 0.06), gtsam::Z_3x3, gtsam::Z_3x3, gtsam::I_3x3 * (0.01 + 0.02); EXPECT(assert_equal(expected * dt * dt, pcm.preintMeasCov())); } @@ -164,7 +165,7 @@ TEST(PreintegratedRigidContactMeasurements, IntegrateMeasurementVarying) { // Test constructor for Preintegrated Rigid Contact Factor. TEST(PreintegratedRigidContactFactor, Constructor) { size_t contact_id = 0; - PreintegratedRigidContactMeasurements pcm(I_3x3, I_3x3); + PreintegratedRigidContactMeasurements pcm(gtsam::I_3x3, gtsam::I_3x3); PreintegratedRigidContactFactor ppcf(PoseKey(contact_id, 0), PoseKey(contact_id, 1), pcm); } @@ -179,7 +180,7 @@ TEST(PreintegratedRigidContactFactor, Error) { Pose3 wTc_i = Pose3(Rot3(), Point3(0, 0, 1)), wTc_j = Pose3(Rot3::Ry(M_PI_4), Point3(0, 0, 1)); - PreintegratedRigidContactMeasurements pcm(I_3x3, I_3x3); + PreintegratedRigidContactMeasurements pcm(gtsam::I_3x3, gtsam::I_3x3); // Integrate with constant noise pcm.integrateMeasurement(0.1); @@ -198,7 +199,7 @@ TEST(PreintegratedRigidContactFactor, Jacobians) { size_t contact_id = 0; size_t t0 = 0, t1 = 1; - PreintegratedRigidContactMeasurements pcm(I_3x3, I_3x3); + PreintegratedRigidContactMeasurements pcm(gtsam::I_3x3, gtsam::I_3x3); // Integrate with constant noise pcm.integrateMeasurement(1); diff --git a/tests/testSpiderWalking.cpp b/tests/testSpiderWalking.cpp index 240322a8e..cd3d405c8 100644 --- a/tests/testSpiderWalking.cpp +++ b/tests/testSpiderWalking.cpp @@ -13,6 +13,7 @@ * @author: Frank Dellaert, Varun Agrawal, Stefanos Charalambous */ +#include #include #include #include diff --git a/tests/testTspaceBasis.cpp b/tests/testTspaceBasis.cpp index 170d99cdd..7fb9086f5 100644 --- a/tests/testTspaceBasis.cpp +++ b/tests/testTspaceBasis.cpp @@ -11,6 +11,7 @@ * @author Yetong Zhang */ +#include #include #include #include @@ -174,10 +175,10 @@ TEST(TspaceBasis, linear_system) { EXPECT(assert_equal(0.0, linear_graph->error(vector))); } - Matrix expected_H_x1 = I_1x1; - Matrix expected_H_x2 = I_1x1*0; - Matrix expected_H_x3 = I_1x1 * -1; - Matrix expected_H_x4 = I_1x1; + Matrix expected_H_x1 = gtsam::I_1x1; + Matrix expected_H_x2 = gtsam::I_1x1*0; + Matrix expected_H_x3 = gtsam::I_1x1 * -1; + Matrix expected_H_x4 = gtsam::I_1x1; EXPECT(assert_equal(expected_H_x1, new_basis_e->recoverJacobian(x1_key))); EXPECT(assert_equal(expected_H_x2, new_basis_e->recoverJacobian(x2_key))); EXPECT(assert_equal(expected_H_x3, new_basis_e->recoverJacobian(x3_key))); diff --git a/tests/testUtils.cpp b/tests/testUtils.cpp index e2cf0d26d..c3ddf0dd0 100644 --- a/tests/testUtils.cpp +++ b/tests/testUtils.cpp @@ -11,6 +11,7 @@ * @author Mandy Xie and Alejandro Escontrela */ +#include #include #include #include