diff --git a/examples/example_cart_pole_trajectory_optimization/main.cpp b/examples/example_cart_pole_trajectory_optimization/main.cpp index 3a723af2..7f4093e5 100644 --- a/examples/example_cart_pole_trajectory_optimization/main.cpp +++ b/examples/example_cart_pole_trajectory_optimization/main.cpp @@ -56,41 +56,41 @@ int main(int argc, char** argv) { std::cout << "Goal State: " << X_T.transpose() << std::endl << std::endl; // Create trajectory factor graph. - auto graph_builder = DynamicsGraph(); + auto graph_builder = DynamicsGraph(gravity); auto graph = graph_builder.trajectoryFG( - cp, t_steps, dt, DynamicsGraph::CollocationScheme::Trapezoidal, gravity); + cp, t_steps, dt, DynamicsGraph::CollocationScheme::Trapezoidal); // Set the pendulum joint to be unactuated. for (int t = 0; t <= t_steps; t++) - graph.addPrior(TorqueKey(j1_id, t), 0.0, Constrained::All(1)); + graph.addPrior(internal::TorqueKey(j1_id, t), 0.0, Constrained::All(1)); // Add initial conditions to trajectory factor graph. - graph.addPrior(JointAngleKey(j0_id, 0), X_i[0], dynamics_model); - graph.addPrior(JointVelKey(j0_id, 0), X_i[1], dynamics_model); - graph.addPrior(JointAngleKey(j1_id, 0), X_i[3], dynamics_model); - graph.addPrior(JointVelKey(j1_id, 0), X_i[4], dynamics_model); + graph.addPrior(internal::JointAngleKey(j0_id, 0), X_i[0], dynamics_model); + graph.addPrior(internal::JointVelKey(j0_id, 0), X_i[1], dynamics_model); + graph.addPrior(internal::JointAngleKey(j1_id, 0), X_i[3], dynamics_model); + graph.addPrior(internal::JointVelKey(j1_id, 0), X_i[4], dynamics_model); // Add terminal conditions to the factor graph. - graph.addPrior(JointVelKey(j0_id, t_steps), X_T[1], objectives_model); - graph.addPrior(JointAccelKey(j0_id, t_steps), X_T[2], objectives_model); - graph.addPrior(JointVelKey(j1_id, t_steps), X_T[4], objectives_model); - graph.addPrior(JointAccelKey(j1_id, t_steps), X_T[5], objectives_model); + graph.addPrior(internal::JointVelKey(j0_id, t_steps), X_T[1], objectives_model); + graph.addPrior(internal::JointAccelKey(j0_id, t_steps), X_T[2], objectives_model); + graph.addPrior(internal::JointVelKey(j1_id, t_steps), X_T[4], objectives_model); + graph.addPrior(internal::JointAccelKey(j1_id, t_steps), X_T[5], objectives_model); // Insert position objective (x, theta) factor at every timestep or only at // the terminal state. Adding the position objective at every timestep will // force the system to converge to the desired state quicker at the cost of // more impulsive control actions. bool apply_pos_objective_all_dt = false; - graph.addPrior(JointAngleKey(j0_id, t_steps), X_T[0], pos_objectives_model); - graph.addPrior(JointAngleKey(j1_id, t_steps), X_T[3], pos_objectives_model); + graph.addPrior(internal::JointAngleKey(j0_id, t_steps), X_T[0], pos_objectives_model); + graph.addPrior(internal::JointAngleKey(j1_id, t_steps), X_T[3], pos_objectives_model); if (apply_pos_objective_all_dt) { for (int t = 0; t < t_steps; t++) { - graph.addPrior(JointAngleKey(j0_id, t), X_T[0], pos_objectives_model); - graph.addPrior(JointAngleKey(j1_id, t), X_T[3], pos_objectives_model); + graph.addPrior(internal::JointAngleKey(j0_id, t), X_T[0], pos_objectives_model); + graph.addPrior(internal::JointAngleKey(j1_id, t), X_T[3], pos_objectives_model); } } for (int t = 0; t <= t_steps; t++) - graph.emplace_shared(TorqueKey(j0_id, t), control_model); + graph.emplace_shared(internal::TorqueKey(j0_id, t), control_model); // Initialize solution. auto init_vals = ZeroValuesTrajectory(cp, t_steps, 0, 0.0); @@ -107,10 +107,10 @@ int main(int argc, char** argv) { double t_elapsed = 0; for (int t = 0; t <= t_steps; t++, t_elapsed += dt) { std::vector keys = { - JointAngleKey(j0_id, t), JointVelKey(j0_id, t), - JointAccelKey(j0_id, t), TorqueKey(j0_id, t), - JointAngleKey(j1_id, t), JointVelKey(j1_id, t), - JointAccelKey(j1_id, t), TorqueKey(j1_id, t)}; + internal::JointAngleKey(j0_id, t), internal::JointVelKey(j0_id, t), + internal::JointAccelKey(j0_id, t), internal::TorqueKey(j0_id, t), + internal::JointAngleKey(j1_id, t), internal::JointVelKey(j1_id, t), + internal::JointAccelKey(j1_id, t), internal::TorqueKey(j1_id, t)}; std::vector vals = {std::to_string(t_elapsed)}; for (auto&& k : keys) vals.push_back(std::to_string(results.atDouble(k))); traj_file << boost::algorithm::join(vals, ",") << "\n"; diff --git a/examples/example_forward_dynamics/main.cpp b/examples/example_forward_dynamics/main.cpp index dc911ca2..7127a668 100644 --- a/examples/example_forward_dynamics/main.cpp +++ b/examples/example_forward_dynamics/main.cpp @@ -12,11 +12,11 @@ int main(int argc, char** argv) { auto simple_rr = CreateRobotFromFile("../simple_rr.sdf", "simple_rr_sdf"); simple_rr.print(); - auto graph_builder = DynamicsGraph(); gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, -9.8).finished(); + auto graph_builder = DynamicsGraph(gravity); auto kdfg = graph_builder.dynamicsFactorGraph(simple_rr, - 0, // timestep - gravity); + 0 // timestep + ); // Specify the forward dynamics priors and add them to the factor graph. gtsam::Values known_values; @@ -39,4 +39,4 @@ int main(int argc, char** argv) { graph_builder.printValues(results); return 0; -} \ No newline at end of file +} diff --git a/examples/example_inverted_pendulum_trajectory_optimization/main.cpp b/examples/example_inverted_pendulum_trajectory_optimization/main.cpp index cb59457b..82cc1a78 100644 --- a/examples/example_inverted_pendulum_trajectory_optimization/main.cpp +++ b/examples/example_inverted_pendulum_trajectory_optimization/main.cpp @@ -52,26 +52,25 @@ int main(int argc, char** argv) { double theta_T = M_PI, dtheta_T = 0, ddtheta_T = 0; // Create trajectory factor graph. - auto graph_builder = DynamicsGraph(); + auto graph_builder = DynamicsGraph(gravity, planar_axis); auto graph = graph_builder.trajectoryFG( - ip, t_steps, dt, DynamicsGraph::CollocationScheme::Trapezoidal, gravity, - planar_axis); + ip, t_steps, dt, DynamicsGraph::CollocationScheme::Trapezoidal); // Add initial conditions to trajectory factor graph. - graph.addPrior(JointAngleKey(j1_id, 0), theta_i, dynamics_model); - graph.addPrior(JointVelKey(j1_id, 0), dtheta_i, dynamics_model); + graph.addPrior(internal::JointAngleKey(j1_id, 0), theta_i, dynamics_model); + graph.addPrior(internal::JointVelKey(j1_id, 0), dtheta_i, dynamics_model); // Add state and min torque objectives to trajectory factor graph. - graph.addPrior(JointVelKey(j1_id, t_steps), dtheta_T, objectives_model); - graph.addPrior(JointAccelKey(j1_id, t_steps), dtheta_T, objectives_model); + graph.addPrior(internal::JointVelKey(j1_id, t_steps), dtheta_T, objectives_model); + graph.addPrior(internal::JointAccelKey(j1_id, t_steps), dtheta_T, objectives_model); bool apply_theta_objective_all_dt = false; - graph.addPrior(JointAngleKey(j1_id, t_steps), theta_T, objectives_model); + graph.addPrior(internal::JointAngleKey(j1_id, t_steps), theta_T, objectives_model); if (apply_theta_objective_all_dt) { for (int t = 0; t < t_steps; t++) - graph.addPrior(JointAngleKey(j1_id, t), theta_T, objectives_model); + graph.addPrior(internal::JointAngleKey(j1_id, t), theta_T, objectives_model); } for (int t = 0; t <= t_steps; t++) - graph.emplace_shared(TorqueKey(j1_id, t), control_model); + graph.emplace_shared(internal::TorqueKey(j1_id, t), control_model); // Initialize solution. auto init_vals = ZeroValuesTrajectory(ip, t_steps, 0, 0.0); @@ -88,8 +87,8 @@ int main(int argc, char** argv) { double t_elapsed = 0; for (int t = 0; t <= t_steps; t++, t_elapsed += dt) { std::vector keys = { - JointAngleKey(j1_id, t), JointVelKey(j1_id, t), JointAccelKey(j1_id, t), - TorqueKey(j1_id, t)}; + internal::JointAngleKey(j1_id, t), internal::JointVelKey(j1_id, t), internal::JointAccelKey(j1_id, t), + internal::TorqueKey(j1_id, t)}; std::vector vals = {std::to_string(t_elapsed)}; for (auto&& k : keys) vals.push_back(std::to_string(results.atDouble(k))); traj_file << boost::algorithm::join(vals, ",") << "\n"; diff --git a/examples/example_quadruped_mp/main.cpp b/examples/example_quadruped_mp/main.cpp index b25aece1..aed7724b 100644 --- a/examples/example_quadruped_mp/main.cpp +++ b/examples/example_quadruped_mp/main.cpp @@ -38,9 +38,9 @@ using gtsam::Pose3; using gtsam::Rot3; using gtsam::Vector3; -typedef std::vector CoeffVector; -typedef std::map> TargetFootholds; -typedef std::map TargetPoses; +using CoeffMatrix = gtsam::Matrix43; +using TargetFootholds = std::map>; +using TargetPoses = std::map; using namespace gtdynamics; @@ -55,60 +55,67 @@ using namespace gtdynamics; * @param x_1_p tangent at the ending knot * @param horizon timestep between the knot at the start and the knot at the end * - * @return 3*4 matrix of coefficients - * - * Since uE[0,1] in the parametrization of the hermite spline equation, for - * tE[0,horizon], u is set equal to t/horizon + * @return 4*3 matrix of coefficients. + * + * Since u ∈ [0,1] in the parametrization of the hermite spline equation, for + * t ∈ [0,horizon], u is set equal to `t/horizon`. * * Refer to this lecture for more info on the hermite parameterization * for cubic polynomials: * http://www.cs.cmu.edu/afs/cs/academic/class/15462-s10/www/lec-slides/lec06.pdf */ -CoeffVector compute_spline_coefficients(const Pose3 &wTb_i, const Pose3 &wTb_f, +CoeffMatrix compute_spline_coefficients(const Pose3 &wTb_i, const Pose3 &wTb_f, const Vector3 &x_0_p, const Vector3 &x_1_p, const double horizon) { - // Extract position at the starting and ending knot. Vector3 x_0 = wTb_i.translation(); Vector3 x_1 = wTb_f.translation(); - // Hermite parameterization: p(u)=U(u)*B*C where vector U(u) includes the + // Hermite parameterization: p(u)=U(u)*B*C where vector U(u) includes the // polynomial terms, and B and C are the basis matrix and control matrices // respectively. - // Vectors a_0 to a_3 constitute the matrix product A of the basis, B, and control, - // C, matrices leading to p(u)=U(u)*A where A=B*C. - Vector3 a_0 = x_0, a_1 = x_0_p; - // Since u=t/horizon, rearrange to integrate the horizon in the matrix product - // of B and C. - Vector3 a_2 = -std::pow(horizon, -2) * - (3 * (x_0 - x_1) + horizon * (2 * x_0_p + x_1_p)); - Vector3 a_3 = - std::pow(horizon, -3) * (2 * (x_0 - x_1) + horizon * (x_0_p + x_1_p)); - - std::vector coeffs; - coeffs.push_back(a_0); - coeffs.push_back(a_1); - coeffs.push_back(a_2); - coeffs.push_back(a_3); - - return coeffs; + gtsam::Matrix43 C; + C.row(0) = x_0; + C.row(1) = x_1; + C.row(2) = x_0_p; + C.row(3) = x_1_p; + + gtsam::Matrix4 B; + B << 2, -2, 1, 1, -3, 3, -2, -1, 0, 0, 1, 0, 1, 0, 0, 0; + gtsam::Matrix43 A = B * C; + + // Scale U by the horizon `t = u/horizon` so that we can directly use t. + A.row(0) /= (horizon * horizon * horizon); + A.row(1) /= (horizon * horizon); + A.row(2) /= horizon; + + return A; } /** - * Compute the robot base pose as defined by the hermite spline. - * TODO(frank): document better + * Compute the robot base pose as defined by the calculated trajectory. + * The calculated trajectory is a piecewise polynomial consisting of + * cubic hermite splines; the base pose will move along this trajectory. + * + * @param coeffs 4*3 matrix of cubic polynomial coefficients + * @param x_0_p tangent at the starting knot + * @param t time at which pose will be evaluated + * @param wTb_i initial pose corresponding to the knot at the start + * + * @return rotation and position */ -Pose3 compute_hermite_pose(const CoeffVector &coeffs, const Vector3 &x_0_p, +Pose3 compute_hermite_pose(const CoeffMatrix &coeffs, const Vector3 &x_0_p, const double t, const Pose3 &wTb_i) { // The position computed from the spline equation as a function of time, - // p(t)=U(t)*A where U(t)=[1,t,t^2,t^3]. - Point3 p(coeffs[0] + coeffs[1] * t + coeffs[2] * std::pow(t, 2) + - coeffs[3] * std::pow(t, 3)); + // p(t)=U(t)*A where U(t)=[t^3,t^2,t,1]. + gtsam::Matrix14 t_vec(std::pow(t, 3), std::pow(t, 2), t, 1); + Point3 p(t_vec * coeffs); // Differentiate position with respect to t for velocity. - Point3 dpdt_v3 = - Point3(coeffs[1] + 2 * coeffs[2] * t + 3 * coeffs[3] * std::pow(t, 2)); + gtsam::Matrix14 du(3 * t * t, 2 * t, 1, 0); + Point3 dpdt_v3 = Point3(du * coeffs); + // Unit vector for velocity. dpdt_v3 = dpdt_v3 / dpdt_v3.norm(); Point3 x_0_p_point(x_0_p); @@ -123,11 +130,10 @@ Pose3 compute_hermite_pose(const CoeffVector &coeffs, const Vector3 &x_0_p, } /** Compute the target footholds for each support phase. */ -TargetFootholds -compute_target_footholds(const CoeffVector &coeffs, const Vector3 &x_0_p, - const Pose3 &wTb_i, const double horizon, - const double t_support, - const std::map &bTfs) { +TargetFootholds compute_target_footholds( + const CoeffMatrix &coeffs, const Vector3 &x_0_p, const Pose3 &wTb_i, + const double horizon, const double t_support, + const std::map &bTfs) { TargetFootholds target_footholds; double t_swing = t_support / 4.0; // Time for each swing foot trajectory. @@ -153,7 +159,7 @@ TargetPoses compute_target_poses(const TargetFootholds &targ_footholds, const double horizon, const double t_support, const double t, const std::vector &swing_sequence, - const CoeffVector &coeffs, + const CoeffMatrix &coeffs, const Vector3 &x_0_p, const Pose3 &wTb_i) { TargetPoses t_poses; // Compute the body pose. @@ -221,7 +227,6 @@ struct CsvWriter { ~CsvWriter() { pose_file.close(); } void writeheader() { - pose_file.open("../traj.csv"); pose_file << "bodyx,bodyy,bodyz"; for (auto &&leg : swing_sequence) pose_file << "," << leg << "x"