From b14cd9856708d04807610050a838a5ae33eb558a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Feb 2026 11:32:02 -0500 Subject: [PATCH 01/11] Make example work --- examples/example_a1_walking/sim.py | 2 +- examples/example_quadruped_mp/main.cpp | 321 +++++++++++++------------ examples/example_quadruped_mp/sim.py | 8 +- 3 files changed, 169 insertions(+), 162 deletions(-) diff --git a/examples/example_a1_walking/sim.py b/examples/example_a1_walking/sim.py index 92335ffa..4e4d36ef 100644 --- a/examples/example_a1_walking/sim.py +++ b/examples/example_a1_walking/sim.py @@ -176,7 +176,7 @@ torques[:,1] = np.array(all_torques_hip) torques[:,2] = np.array(all_torques_upper) torques[:,3] = np.array(all_torques_lower) -np.savetxt('/home/dan/Desktop/Projects/GTDynamics/build/examples/example_a1_walking/torques_pyb.txt', torques) +np.savetxt('torques_pyb.txt', torques) pos, orn = p.getBasePositionAndOrientation(robot_id) print("Final Base\n\tPos: {}\n\tOrn: {}".format(pos, diff --git a/examples/example_quadruped_mp/main.cpp b/examples/example_quadruped_mp/main.cpp index 7dff4612..020d5039 100644 --- a/examples/example_quadruped_mp/main.cpp +++ b/examples/example_quadruped_mp/main.cpp @@ -11,7 +11,6 @@ * @author Alejandro Escontrela */ -#include #include #include #include @@ -65,156 +64,157 @@ using namespace gtdynamics; * for cubic polynomials: * http://www.cs.cmu.edu/afs/cs/academic/class/15462-s10/www/lec-slides/lec06.pdf */ -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 - // polynomial terms, and B and C are the basis matrix and control matrices - // respectively. - 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; -} +struct Spline { + Pose3 wTb_i; + CoeffMatrix A; // coefficients + + /** + * Construct spline data and precompute cubic Hermite coefficients. + */ + Spline(const Pose3 &wTb_i, const Pose3 &wTb_f, // + const Vector3 &x_0_p, // + const Vector3 &x_1_p, // + const double horizon) + : wTb_i(wTb_i) { + // 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 + // polynomial terms, and B and C are the basis matrix and control matrices + // respectively. + 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; + 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; + } -/** - * 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 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)=[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. - 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); - x_0_p_point = x_0_p_point / x_0_p_point.norm(); - - Point3 axis = x_0_p_point.cross(dpdt_v3); - double angle = std::acos(x_0_p_point.dot(dpdt_v3)); - // The rotation. - Rot3 R = wTb_i.rotation() * Rot3::AxisAngle(axis, angle); - - return Pose3(R, p); -} + /** + * 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 x_0_p tangent at the starting knot + * @param t time at which pose will be evaluated + * + * @return rotation and position + */ + Pose3 compute_hermite_pose(const Vector3 &x_0_p, const double t) const { + // The position computed from the spline equation as a function of time, + // 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 * A); + + // Differentiate position with respect to t for velocity. + gtsam::Matrix14 du(3 * t * t, 2 * t, 1, 0); + Point3 dpdt_v3 = Point3(du * A); + + // Unit vector for velocity. + dpdt_v3 = dpdt_v3 / dpdt_v3.norm(); + Point3 x_0_p_point(x_0_p); + x_0_p_point = x_0_p_point / x_0_p_point.norm(); + + Point3 axis = x_0_p_point.cross(dpdt_v3); + double angle = std::acos(x_0_p_point.dot(dpdt_v3)); + // The rotation. + Rot3 R = wTb_i.rotation() * Rot3::AxisAngle(axis, angle); + + return Pose3(R, p); + } -/** Compute the target footholds for each support phase. */ -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. - int n_support_phases = horizon / t_support; - - for (int i = 0; i <= n_support_phases; i++) { - Pose3 wTb = - compute_hermite_pose(coeffs, x_0_p, i * t_support / horizon, wTb_i); - std::map target_footholds_i; - for (auto &&bTf : bTfs) { - Pose3 wTf = wTb * bTf.second; - // TODO(frank): #179 make sure height is handled correctly. - Pose3 wTf_gh(wTf.rotation(), Point3(wTf.translation()[0], - wTf.translation()[1], GROUND_HEIGHT)); - target_footholds_i.emplace(bTf.first, wTf_gh); + /** Compute the target footholds for each support phase. */ + TargetFootholds compute_target_footholds( + const Vector3 &x_0_p, // + const double horizon, const double t_support, + const std::map &bTfs) const { + TargetFootholds target_footholds; + int n_support_phases = horizon / t_support; + + for (int i = 0; i <= n_support_phases; i++) { + Pose3 wTb = compute_hermite_pose(x_0_p, i * t_support / horizon); + std::map target_footholds_i; + for (auto &&bTf : bTfs) { + Pose3 wTf = wTb * bTf.second; + // TODO(frank): #179 make sure height is handled correctly. + Pose3 wTf_gh( + wTf.rotation(), + Point3(wTf.translation()[0], wTf.translation()[1], GROUND_HEIGHT)); + target_footholds_i.emplace(bTf.first, wTf_gh); + } + target_footholds.emplace(i, target_footholds_i); } - target_footholds.emplace(i, target_footholds_i); + return target_footholds; } - return target_footholds; -} -/** Get base pose and foot positions at any time t. */ -TargetPoses compute_target_poses(const TargetFootholds &targ_footholds, - const double horizon, const double t_support, - const double t, - const std::vector &swing_sequence, - const CoeffMatrix &coeffs, - const Vector3 &x_0_p, const Pose3 &wTb_i) { - TargetPoses t_poses; - // Compute the body pose. - Pose3 wTb = compute_hermite_pose(coeffs, x_0_p, t / horizon, wTb_i); - t_poses.emplace("body", wTb); - - const std::map &prev_targ_foothold = - targ_footholds.at(static_cast(std::floor(t / t_support))); - const std::map &next_targ_foothold = - targ_footholds.at(static_cast(std::ceil(t / t_support))); - - // Time spent in current support phase. - double t_in_support = std::fmod(t, t_support); - double t_swing = t_support / 4.0; // Duration of swing phase. - - int swing_leg_idx; - if (t_in_support <= t_swing) - swing_leg_idx = 0; - else if (t_in_support <= (2 * t_swing)) - swing_leg_idx = 1; - else if (t_in_support <= (3 * t_swing)) - swing_leg_idx = 2; - else - swing_leg_idx = 3; - - // Normalized swing duration. - double t_normed = (t_in_support - swing_leg_idx * t_swing) / t_swing; - - // Already completed swing phase in this support phase. - for (int i = 0; i < swing_leg_idx; i++) { - std::string leg_i = swing_sequence[i]; - t_poses.emplace(leg_i, next_targ_foothold.at(leg_i)); - } + /** Get base pose and foot positions at any time t. */ + TargetPoses compute_target_poses( + const TargetFootholds &targ_footholds, const double horizon, + const double t_support, const double t, + const std::vector &swing_sequence, + const Vector3 &x_0_p) const { + TargetPoses t_poses; + // Compute the body pose. + Pose3 wTb = compute_hermite_pose(x_0_p, t / horizon); + t_poses.emplace("body", wTb); + + const std::map &prev_targ_foothold = + targ_footholds.at(static_cast(std::floor(t / t_support))); + const std::map &next_targ_foothold = + targ_footholds.at(static_cast(std::ceil(t / t_support))); + + // Time spent in current support phase. + double t_in_support = std::fmod(t, t_support); + double t_swing = t_support / 4.0; // Duration of swing phase. + + int swing_leg_idx; + if (t_in_support <= t_swing) + swing_leg_idx = 0; + else if (t_in_support <= (2 * t_swing)) + swing_leg_idx = 1; + else if (t_in_support <= (3 * t_swing)) + swing_leg_idx = 2; + else + swing_leg_idx = 3; + + // Normalized swing duration. + double t_normed = (t_in_support - swing_leg_idx * t_swing) / t_swing; + + // Already completed swing phase in this support phase. + for (int i = 0; i < swing_leg_idx; i++) { + std::string leg_i = swing_sequence[i]; + t_poses.emplace(leg_i, next_targ_foothold.at(leg_i)); + } - // Currently swinging. - std::string swing_leg = swing_sequence[swing_leg_idx]; - auto prev_foot_pos = prev_targ_foothold.at(swing_leg).translation(); - auto next_foot_pos = next_targ_foothold.at(swing_leg).translation(); - Point3 curr_foot_pos = - prev_foot_pos + (next_foot_pos - prev_foot_pos) * t_normed; - double h = GROUND_HEIGHT + - 0.2 * std::pow(t_normed, 1.1) * std::pow(1 - t_normed, 0.7); - - t_poses.emplace(swing_leg, - Pose3(Rot3(), Point3(curr_foot_pos[0], curr_foot_pos[1], h))); - - // Yet to complete swing phase in this support phase. - for (int i = swing_leg_idx + 1; i < 4; i++) { - std::string leg_i = swing_sequence[i]; - t_poses.emplace(leg_i, prev_targ_foothold.at(leg_i)); + // Currently swinging. + std::string swing_leg = swing_sequence[swing_leg_idx]; + auto prev_foot_pos = prev_targ_foothold.at(swing_leg).translation(); + auto next_foot_pos = next_targ_foothold.at(swing_leg).translation(); + Point3 curr_foot_pos = + prev_foot_pos + (next_foot_pos - prev_foot_pos) * t_normed; + double h = GROUND_HEIGHT + + 0.2 * std::pow(t_normed, 1.1) * std::pow(1 - t_normed, 0.7); + + t_poses.emplace(swing_leg, Pose3(Rot3(), Point3(curr_foot_pos[0], + curr_foot_pos[1], h))); + + // Yet to complete swing phase in this support phase. + for (int i = swing_leg_idx + 1; i < 4; i++) { + std::string leg_i = swing_sequence[i]; + t_poses.emplace(leg_i, prev_targ_foothold.at(leg_i)); + } + return t_poses; } - return t_poses; -} +}; struct CsvWriter { std::ofstream pose_file; @@ -277,8 +277,8 @@ int main(int argc, char **argv) { Point3 x_0_p(1, 0, 0); Point3 x_0_p_traj(1, 0, 0.4); Point3 x_1_p_traj(1, 0, 0); - auto coeffs = - compute_spline_coefficients(wTb_i, wTb_f, x_0_p_traj, x_1_p_traj, 1); + + Spline spline(wTb_i, wTb_f, x_0_p_traj, x_1_p_traj, 1.0); // Time horizon. double horizon = 72; @@ -307,19 +307,18 @@ int main(int argc, char **argv) { // Calculate foothold at the end of each support phase. TargetFootholds targ_footholds = - compute_target_footholds(coeffs, x_0_p, wTb_i, horizon, t_support, bTfs); + spline.compute_target_footholds(x_0_p, horizon, t_support, bTfs); // Iteratively solve the inverse kinematics problem to obtain joint angles. - double dt = 1. / 240., curr_t = 0.0; + double dt = 1. / 24., curr_t = 0.0; int k = 0; // The time index. - auto dgb = DynamicsGraph(); // Initialize values. gtsam::Values values; for (auto &&link : robot.links()) InsertPose(&values, link->id(), link->bMcom()); for (auto &&joint : robot.joints()) - InsertJointAngle(&values, joint->id(), 0.0); + InsertJointAngle(&values, joint->id(), 1.0); // Write body,foot poses and joint angles to csv file. CsvWriter writer("traj.csv", swing_sequence, robot); @@ -327,7 +326,7 @@ int main(int argc, char **argv) { // Set parameters for optimizer gtsam::LevenbergMarquardtParams params; - params.setMaxIterations(50); + // params.setMaxIterations(50); params.setlambdaInitial(1e5); auto model3 = gtsam::noiseModel::Constrained::All(3); KinematicsParameters kinematics_params; @@ -337,12 +336,16 @@ int main(int argc, char **argv) { // params.setVerbosityLM("SUMMARY"); while (curr_t < horizon) { - const TargetPoses tposes = - compute_target_poses(targ_footholds, horizon, t_support, curr_t, - swing_sequence, coeffs, x_0_p, wTb_i); + const TargetPoses tposes = spline.compute_target_poses( + targ_footholds, horizon, t_support, curr_t, swing_sequence, x_0_p); + // for (const auto &pose_pair : tposes) { + // const std::string &name = pose_pair.first; + // const Pose3 &pose = pose_pair.second; + // pose.print(name + ": "); + // } // Create factor graph of kinematics constraints. - gtsam::NonlinearFactorGraph kfg = dgb.qFactors(robot, k); + gtsam::NonlinearFactorGraph kfg = kinematics.qFactors(Slice(k), robot); // Constrain the base pose using trajectory value. kfg.addPrior(PoseKey(robot.link("body")->id(), k), tposes.at("body"), @@ -358,12 +361,17 @@ int main(int argc, char **argv) { } kfg.add(kinematics.pointGoalObjectives(Slice(k), contact_goals)); + double error_before = kfg.error(values); + // gtsam::LevenbergMarquardtOptimizer optimizer(kfg, values, params); gtsam::GaussNewtonOptimizer optimizer(kfg, values); gtsam::Values results = optimizer.optimize(); + double error_after = kfg.error(results); + if ((k % 100) == 0) - cout << "iter: " << k << ", err: " << kfg.error(results) << endl; + cout << "iter: " << k << ", err_before: " << error_before + << ", err_after: " << error_after << endl; // Update the values for next iteration. values.clear(); @@ -378,6 +386,7 @@ int main(int argc, char **argv) { writer.writerow(tposes, results, k); curr_t = curr_t + dt; k = k + 1; + // break; } return 0; diff --git a/examples/example_quadruped_mp/sim.py b/examples/example_quadruped_mp/sim.py index 8f30d9fc..829b6060 100644 --- a/examples/example_quadruped_mp/sim.py +++ b/examples/example_quadruped_mp/sim.py @@ -3,8 +3,6 @@ Author: Alejandro Escontrela """ -import time - import matplotlib.pyplot as plt import pandas as pd import pybullet as p @@ -30,7 +28,7 @@ df = pd.read_csv('traj.csv') -input("Press ENTER to continue.") +# input("Press ENTER to continue.") pos_des = df.loc[0][['bodyx', 'bodyy', 'bodyz']].tolist() pos_des[2] = pos_des[2] + 0.2 @@ -68,8 +66,8 @@ all_pos_des.append(new_pos_des) p.stepSimulation() - time.sleep(1. / 240.) - t += 1. / 240. + # time.sleep(1. / 24.) + t += 1. / 24. pos, orn = p.getBasePositionAndOrientation(quad_id) print("Final Base\n\tPos: {}\n\tOrn: {}".format(pos, From 491c8727a517b83f6ea54dcc85934327f0f69c19 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Feb 2026 12:26:02 -0500 Subject: [PATCH 02/11] Wrap chains --- gtdynamics.i | 33 +++++++++++++++++++++++++++++ python/tests/test_dynamics_graph.py | 14 ++++++++++++ 2 files changed, 47 insertions(+) diff --git a/gtdynamics.i b/gtdynamics.i index f15571f1..14c431cd 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -636,6 +636,30 @@ class DynamicsGraph { const gtdynamics::OptimizerSetting &opt() const; }; +#include +class ChainDynamicsGraph : gtdynamics::DynamicsGraph { + ChainDynamicsGraph(const gtdynamics::Robot &robot, + const gtdynamics::OptimizerSetting &opt); + + ChainDynamicsGraph(const gtdynamics::Robot &robot, + const gtdynamics::OptimizerSetting &opt, + const gtsam::Vector3 &gravity); + + gtsam::NonlinearFactorGraph qFactors( + const gtdynamics::Robot &robot, const int t, + const std::optional &contact_points) const; + + gtsam::NonlinearFactorGraph dynamicsFactors( + const gtdynamics::Robot &robot, const int t, + const std::optional &contact_points, + const std::optional &mu) const; + + gtsam::NonlinearFactorGraph dynamicsFactorGraph( + const gtdynamics::Robot &robot, const int t, + const std::optional &contact_points, + const std::optional &mu) const; +}; + /********************** Objective Factors **********************/ #include class LinkObjectives : gtsam::NonlinearFactorGraph { @@ -703,6 +727,15 @@ class Initializer { const std::optional& contact_points); }; +#include +class ChainInitializer : gtdynamics::Initializer { + ChainInitializer(); + + gtsam::Values ZeroValues( + const gtdynamics::Robot& robot, const int t, double gaussian_noise, + const std::optional& contact_points) const; +}; + /********************** symbols **********************/ #include diff --git a/python/tests/test_dynamics_graph.py b/python/tests/test_dynamics_graph.py index 5322fba1..c08c27ce 100644 --- a/python/tests/test_dynamics_graph.py +++ b/python/tests/test_dynamics_graph.py @@ -67,6 +67,20 @@ def test_objective_factors(self): self.assertEqual(graph.size(), 10) self.assertEqual(graph.keys().size(), 9) + def test_chain_wrappers(self): + """Test wrapped chain-graph classes are usable from Python.""" + a1 = gtd.CreateRobotFromFile(gtd.URDF_PATH + "/a1/a1.urdf", "a1") + opt = gtd.OptimizerSetting() + gravity = np.array([0.0, 0.0, -9.8]) + + chain_graph = gtd.ChainDynamicsGraph(a1, opt, gravity) + graph = chain_graph.dynamicsFactorGraph(a1, 0, None, None) + self.assertGreater(graph.size(), 0) + + chain_initializer = gtd.ChainInitializer() + init_values = chain_initializer.ZeroValues(a1, 0, 0.0, None) + self.assertGreater(init_values.size(), 0) + if __name__ == "__main__": unittest.main() From 0a06aa7c4b8c361456e30adb86c4830c1116ed0c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Feb 2026 12:44:12 -0500 Subject: [PATCH 03/11] Generate stubs --- README.md | 10 ++++++++++ gtdynamics.i | 32 ++++++++++++++++++++++++++------ python/CMakeLists.txt | 29 +++++++++++++++++++++++------ python/README.md | 39 ++++++++++++++++++++++++++++++++++----- 4 files changed, 93 insertions(+), 17 deletions(-) diff --git a/README.md b/README.md index fd68f1c2..5218dd4f 100644 --- a/README.md +++ b/README.md @@ -240,6 +240,16 @@ To compile and install the GTDynamics python library: make && make python-install ``` + To generate stubs explicitly (useful for IDEs/type checkers), run: + + ```sh + make python-stubs + ``` + + On non-Windows platforms, `python-install` depends on `python-stubs`. + + For VS Code / Pylance setup (including `python.analysis.extraPaths`), see `python/README.md`. + 4. To run the Python tests, you can simply run: ```sh diff --git a/gtdynamics.i b/gtdynamics.i index 14c431cd..0f10e10e 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -639,11 +639,16 @@ class DynamicsGraph { #include class ChainDynamicsGraph : gtdynamics::DynamicsGraph { ChainDynamicsGraph(const gtdynamics::Robot &robot, - const gtdynamics::OptimizerSetting &opt); + const gtdynamics::OptimizerSetting &opt, + const gtsam::Vector3 &gravity); ChainDynamicsGraph(const gtdynamics::Robot &robot, const gtdynamics::OptimizerSetting &opt, - const gtsam::Vector3 &gravity); + const gtsam::Vector3 &gravity, + const std::optional &planar_axis); + + ChainDynamicsGraph(const gtdynamics::Robot &robot, + const gtdynamics::OptimizerSetting &opt); gtsam::NonlinearFactorGraph qFactors( const gtdynamics::Robot &robot, const int t, @@ -719,11 +724,21 @@ class Initializer { Initializer(); gtsam::Values ZeroValues( - const gtdynamics::Robot& robot, const int t, double gaussian_noise); + const gtdynamics::Robot& robot, const int t, + double gaussian_noise = 0.0); + + gtsam::Values ZeroValues( + const gtdynamics::Robot& robot, const int t, + double gaussian_noise = 0.0, + const std::optional& contact_points); gtsam::Values ZeroValuesTrajectory( - const gtdynamics::Robot& robot, const int num_steps, const int num_phases, - double gaussian_noise, + const gtdynamics::Robot& robot, const int num_steps, + const int num_phases = -1, double gaussian_noise = 0.0); + + gtsam::Values ZeroValuesTrajectory( + const gtdynamics::Robot& robot, const int num_steps, + const int num_phases = -1, double gaussian_noise = 0.0, const std::optional& contact_points); }; @@ -732,7 +747,12 @@ class ChainInitializer : gtdynamics::Initializer { ChainInitializer(); gtsam::Values ZeroValues( - const gtdynamics::Robot& robot, const int t, double gaussian_noise, + const gtdynamics::Robot& robot, const int t, + double gaussian_noise = 0.0) const; + + gtsam::Values ZeroValues( + const gtdynamics::Robot& robot, const int t, + double gaussian_noise = 0.0, const std::optional& contact_points) const; }; diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 7a9b8d77..45c9d4ad 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -46,18 +46,35 @@ set_target_properties( RELWITHDEBINFO_POSTFIX "" # Otherwise you will have a wrong name ) -add_custom_target( - python-install - COMMAND ${PYTHON_EXECUTABLE} -m pip install . - DEPENDS ${PROJECT_NAME}_py - WORKING_DIRECTORY ${GTD_PYTHON_BINARY_DIR}) - if(UNIX) set(GTD_PATH_SEP ":") else() set(GTD_PATH_SEP ";") endif() +set(GTD_PYTHON_INSTALL_EXTRA "") +set(GTD_PYTHON_STUB_MODULE "${PROJECT_NAME}.${PROJECT_NAME}") + +add_custom_target( + python-stubs + COMMAND + ${CMAKE_COMMAND} -E env + "PYTHONPATH=${GTD_PYTHON_BINARY_DIR}${GTD_PATH_SEP}$ENV{PYTHONPATH}" + ${PYTHON_EXECUTABLE} -m pybind11_stubgen -o . --ignore-all-errors + ${GTD_PYTHON_STUB_MODULE} + DEPENDS ${PROJECT_NAME}_py + WORKING_DIRECTORY ${GTD_PYTHON_BINARY_DIR}) + +if(NOT WIN32) + list(APPEND GTD_PYTHON_INSTALL_EXTRA python-stubs) +endif() + +add_custom_target( + python-install + COMMAND ${PYTHON_EXECUTABLE} -m pip install . + DEPENDS ${PROJECT_NAME}_py ${GTD_PYTHON_INSTALL_EXTRA} + WORKING_DIRECTORY ${GTD_PYTHON_BINARY_DIR}) + # Unit tests set(python_unit_test_suites) macro(PYTHON_UNIT_TEST_SUITE suiteName directory) diff --git a/python/README.md b/python/README.md index 45cfe4ed..2c97e695 100644 --- a/python/README.md +++ b/python/README.md @@ -11,7 +11,7 @@ This directory is where the Pybind11-generated GTDynamics package lives and wher ## Build prerequisites 1. **GTSAM** must be built with Python support (i.e., `-DGTSAM_BUILD_PYTHON=ON`) and installed to a prefix that GTDynamics can see via `GTSAM_DIR` or `CMAKE_PREFIX_PATH`. -2. **Python tooling**: the CI job installs `setuptools<70`, `wheel`, `numpy`, `pyparsing`, `pyyaml`, and `pybind11-stubgen` before configuring the project; matching this list locally avoids the same runtime issues. +2. **Python tooling**: the CI job installs `setuptools<70`, `wheel`, `numpy`, `pyparsing`, `pyyaml`, and `pybind11-stubgen` before configuring the project; matching this list locally avoids the same runtime issues. `pybind11-stubgen` is required for the `python-stubs` CMake target. 3. On macOS, the workflow creates and activates a virtual environment (`pythonX -m venv venv`) so that `pip install` and the tests run in the same interpreter that baked the bindings. ## Building and installing locally @@ -30,7 +30,36 @@ This directory is where the Pybind11-generated GTDynamics package lives and wher ```sh make python-install ``` - This runs `${PYTHON_EXECUTABLE} -m pip install .` in `build/python`, which produces a wheel in `pip`'s cache before installing it. + This runs `${PYTHON_EXECUTABLE} -m pip install .` in `build/python`, which produces a wheel in `pip`'s cache before installing it. On non-Windows platforms, `python-install` depends on `python-stubs`, so `.pyi` files are generated first. + +## Generating type stubs + +- Run `make python-stubs` to generate stubs with `pybind11-stubgen`. +- Stubs are generated in `build/python/gtdynamics/*.pyi`. +- The generated `gtdynamics.pyi` file is what tools like Pylance use for attribute completion/type checking on wrapped symbols. + +### VS Code / Pylance configuration + +If your runtime works but Pylance still reports unknown attributes (for example, `CreateRobotFromFile`), make sure VS Code analyzes the build package path. + +In workspace `.vscode/settings.json`: + +```json +{ + "python.defaultInterpreterPath": "/path/to/your/python", + "python.analysis.extraPaths": [ + "${workspaceFolder}/build/python" + ] +} +``` + +Then run: + +```sh +make python-stubs +``` + +and reload the VS Code window. ## Running Python tests @@ -40,9 +69,9 @@ This directory is where the Pybind11-generated GTDynamics package lives and wher ## Packaging tips -- `python/templates/setup.py.in` reads the CMake-generated `requirements.txt` and packages the shared library blobs (`.so` / `.pyd`) from `python/gtdynamics` so running `pip wheel .` in `build/python` yields a complete asset. -- Keep `python/requirements.txt` in sync with the requirements file copied to `build/python/requirements.txt` so that CI and a local `pip install` use the same dependency list. -- If you need to publish a wheel manually, the packaged wheel that `pip install .` writes to `~/.cache/pip` already encodes the GTDynamics version reported by `CMakeLists.txt`. +- `python/templates/pyproject.toml.in` drives packaging in `build/python`. +- `make python-install` runs `pip install .` from `build/python`, which installs the generated extension module and package files from `build/python/gtdynamics`. +- If you need to publish a wheel manually, the wheel produced by `pip` already encodes the GTDynamics version reported by CMake. ## Wheels ### CI wheel pipeline From 89c805bcd2cb18dd67478919e029cd1ceb08aa1a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Feb 2026 13:16:28 -0500 Subject: [PATCH 04/11] Add FootContactConstraintSpec --- README.md | 2 + .../chain_dynamics_motion_planning.ipynb | 476 ++++++++++++++++++ gtdynamics.i | 24 +- python/README.md | 11 + python/gtdynamics/preamble/gtdynamics.h | 4 + 5 files changed, 515 insertions(+), 2 deletions(-) create mode 100644 examples/example_a1_walking/chain_dynamics_motion_planning.ipynb diff --git a/README.md b/README.md index 5218dd4f..5f5d4a68 100644 --- a/README.md +++ b/README.md @@ -250,6 +250,8 @@ To compile and install the GTDynamics python library: For VS Code / Pylance setup (including `python.analysis.extraPaths`), see `python/README.md`. + Important: use a `gtsam` Python package built from the same install/prefix as the GTSAM library linked into GTDynamics. Mixing a local GTDynamics build with an unrelated pip/conda `gtsam` wheel can cause runtime aborts. + 4. To run the Python tests, you can simply run: ```sh diff --git a/examples/example_a1_walking/chain_dynamics_motion_planning.ipynb b/examples/example_a1_walking/chain_dynamics_motion_planning.ipynb new file mode 100644 index 00000000..a6b12b65 --- /dev/null +++ b/examples/example_a1_walking/chain_dynamics_motion_planning.ipynb @@ -0,0 +1,476 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "ddab226b", + "metadata": {}, + "source": [ + "# A1 Walk-Cycle Motion Planning with `ChainDynamicsGraph` (Python)\n", + "\n", + "This notebook mirrors the C++ `newGraph()` pipeline in:\n", + "\n", + "`examples/example_a1_walking/main.cpp`\n", + "\n", + "The goal is to build the same multi-phase quadruped trajectory optimization with chain dynamics only, then inspect and plot the optimized motion." + ] + }, + { + "cell_type": "markdown", + "id": "9335499c", + "metadata": {}, + "source": [ + "## 1) Environment and Imports\n", + "\n", + "Two details matter for this notebook:\n", + "\n", + "1. We must import the local GTDynamics Python wrapper from `build/python`.\n", + "2. `gtsam` must come from the same build/install family as GTDynamics. Mixing with an unrelated pip/conda `gtsam` wheel can cause runtime aborts when factor graphs are modified." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "c5ecb7b3", + "metadata": {}, + "outputs": [], + "source": [ + "from pathlib import Path\n", + "import sys\n", + "import numpy as np\n", + "import pandas as pd\n", + "import matplotlib.pyplot as plt\n", + "\n", + "\n", + "def find_repo_root(start: Path) -> Path:\n", + " for candidate in [start, *start.parents]:\n", + " if (candidate / 'CMakeLists.txt').exists() and (candidate / 'examples' / 'example_a1_walking').exists():\n", + " return candidate\n", + " raise RuntimeError('Could not locate GTDynamics repository root from current directory.')\n", + "\n", + "\n", + "repo_root = find_repo_root(Path.cwd().resolve())\n", + "gtd_build_python = repo_root / 'build' / 'python'\n", + "gtsam_build_python = repo_root.parent / 'gtsam' / 'build' / 'python'\n", + "\n", + "if gtsam_build_python.exists() and str(gtsam_build_python) not in sys.path:\n", + " sys.path.insert(0, str(gtsam_build_python))\n", + "if str(gtd_build_python) not in sys.path:\n", + " sys.path.insert(0, str(gtd_build_python))\n", + "\n", + "import gtsam\n", + "import gtdynamics as gtd\n", + "\n", + "print('repo_root :', repo_root)\n", + "print('gtsam module :', gtsam.__file__)\n", + "print('gtdynamics module:', gtd.__file__)\n", + "\n", + "if gtsam_build_python.exists() and str(gtsam_build_python) not in gtsam.__file__:\n", + " raise RuntimeError(\n", + " 'Loaded a non-local gtsam package. Put '\n", + " f\"{gtsam_build_python} first on PYTHONPATH before running this notebook.\"\n", + " )" + ] + }, + { + "cell_type": "markdown", + "id": "a1953262", + "metadata": {}, + "source": [ + "## 2) Helper Functions (C++ `newGraph()` equivalents)\n", + "\n", + "These helpers correspond to pieces in `main.cpp`:\n", + "\n", + "- `build_walk_trajectory(...)` mirrors `getTrajectory(...)`.\n", + "- `set_massless_except_trunk(...)` mirrors the mass/inertia simplification used in `newGraph()`.\n", + "- `add_subgraph(...)` is needed because Python exposes graph `add(...)` for single factors, while many GTDynamics objective helpers return a small `NonlinearFactorGraph`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "265b6743", + "metadata": {}, + "outputs": [], + "source": [ + "def add_subgraph(dst_graph: gtsam.NonlinearFactorGraph,\n", + " src_graph: gtsam.NonlinearFactorGraph) -> None:\n", + " # Append all factors from src_graph into dst_graph.\n", + " for i in range(src_graph.size()):\n", + " if src_graph.exists(i):\n", + " dst_graph.add(src_graph.at(i))\n", + "\n", + "\n", + "def set_massless_except_trunk(robot: gtd.Robot) -> None:\n", + " # Match C++ newGraph(): all links except trunk are zero mass/inertia.\n", + " for link in robot.links():\n", + " if 'trunk' not in link.name():\n", + " link.setMass(0.0)\n", + " link.setInertia(np.zeros((3, 3)))\n", + "\n", + "\n", + "def build_walk_trajectory(robot: gtd.Robot, repeat: int = 1) -> gtd.Trajectory:\n", + " # Match getTrajectory() from C++: RRFL -> stationary -> RLFR -> stationary.\n", + " rlfr = [robot.link('RL_lower'), robot.link('FR_lower')]\n", + " rrfl = [robot.link('RR_lower'), robot.link('FL_lower')]\n", + " all_feet = rlfr + rrfl\n", + "\n", + " contact_in_com = gtsam.Point3(0.0, 0.0, -0.07)\n", + " stationary = gtd.FootContactConstraintSpec(all_feet, contact_in_com)\n", + " rlfr_state = gtd.FootContactConstraintSpec(rlfr, contact_in_com)\n", + " rrfl_state = gtd.FootContactConstraintSpec(rrfl, contact_in_com)\n", + "\n", + " states = [rrfl_state, stationary, rlfr_state, stationary]\n", + " phase_lengths = [25, 5, 25, 5]\n", + "\n", + " walk_cycle = gtd.WalkCycle(states, phase_lengths)\n", + " return gtd.Trajectory(walk_cycle, repeat)" + ] + }, + { + "cell_type": "markdown", + "id": "392b2442", + "metadata": {}, + "source": [ + "## 3) Build the Chain-Dynamics Graph and Objectives\n", + "\n", + "This cell follows the structure of `newGraph()` in C++:\n", + "\n", + "1. Load A1, make non-trunk links massless.\n", + "2. Build `ChainDynamicsGraph`.\n", + "3. Build multi-phase dynamics graph from the walk trajectory.\n", + "4. Add contact-point objectives.\n", + "5. Add chain-specific boundary objectives.\n", + "6. Add base pose/twist objectives for all timesteps.\n", + "7. Constrain integration time (`dt = 1/20`).\n", + "8. Add joint angle priors (`lower`, `hip`, `upper`)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "f6a2d434", + "metadata": {}, + "outputs": [], + "source": [ + "robot = gtd.CreateRobotFromFile(gtd.URDF_PATH + '/a1/a1.urdf', 'a1')\n", + "set_massless_except_trunk(robot)\n", + "\n", + "sigma_dynamics = 1e-3\n", + "sigma_objectives = 1e-3\n", + "\n", + "dynamics_model_6 = gtsam.noiseModel.Isotropic.Sigma(6, sigma_dynamics)\n", + "dynamics_model_1 = gtsam.noiseModel.Isotropic.Sigma(1, sigma_dynamics)\n", + "objectives_model_6 = gtsam.noiseModel.Isotropic.Sigma(6, sigma_objectives)\n", + "objectives_model_1 = gtsam.noiseModel.Isotropic.Sigma(1, sigma_objectives)\n", + "\n", + "gravity = np.array([0.0, 0.0, -9.8])\n", + "mu = 1.0\n", + "\n", + "opt = gtd.OptimizerSetting()\n", + "graph_builder = gtd.ChainDynamicsGraph(robot, opt, gravity)\n", + "\n", + "trajectory = build_walk_trajectory(robot, repeat=1)\n", + "collocation = gtd.CollocationScheme.Euler\n", + "\n", + "graph = trajectory.multiPhaseFactorGraph(robot, graph_builder, collocation, mu)\n", + "\n", + "ground_height = 1.0\n", + "step = gtsam.Point3(0.25, 0.0, 0.0)\n", + "objectives = trajectory.contactPointObjectives(\n", + " robot,\n", + " gtsam.noiseModel.Isotropic.Sigma(3, 1e-6),\n", + " step,\n", + " ground_height,\n", + ")\n", + "\n", + "K = trajectory.getEndTimeStep(trajectory.numPhases() - 1)\n", + "\n", + "for link in robot.links():\n", + " i = link.id()\n", + " if i == 0:\n", + " add_subgraph(\n", + " objectives,\n", + " gtd.LinkObjectives(i, 0)\n", + " .pose(link.bMcom(), gtsam.noiseModel.Isotropic.Sigma(6, 1e-3))\n", + " .twist(np.zeros(6), gtsam.noiseModel.Isotropic.Sigma(6, 1e-3)),\n", + " )\n", + " if i in (3, 6, 9, 12):\n", + " add_subgraph(\n", + " objectives,\n", + " gtd.LinkObjectives(i, 0).pose(\n", + " link.bMcom(), gtsam.noiseModel.Isotropic.Sigma(6, 1e-3)\n", + " ),\n", + " )\n", + "\n", + "add_subgraph(objectives, gtd.JointsAtRestObjectives(robot, objectives_model_1, objectives_model_1, 0))\n", + "add_subgraph(objectives, gtd.JointsAtRestObjectives(robot, objectives_model_1, objectives_model_1, K))\n", + "\n", + "trunk = robot.link('trunk')\n", + "for k in range(K + 1):\n", + " add_subgraph(\n", + " objectives,\n", + " gtd.LinkObjectives(trunk.id(), k)\n", + " .pose(\n", + " gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.0, 0.0, 0.4)),\n", + " gtsam.noiseModel.Isotropic.Sigma(6, 1e-2),\n", + " )\n", + " .twist(np.zeros(6), gtsam.noiseModel.Isotropic.Sigma(6, 5e-2)),\n", + " )\n", + "\n", + "desired_dt = 1.0 / 20.0\n", + "trajectory.addIntegrationTimeFactors(objectives, desired_dt, 1e-30)\n", + "\n", + "prior_model_angles = gtsam.noiseModel.Isotropic.Sigma(1, 1e-2)\n", + "prior_model_hip = gtsam.noiseModel.Isotropic.Sigma(1, 1e-2)\n", + "\n", + "for joint in robot.joints():\n", + " name = joint.name()\n", + " jid = joint.id()\n", + " for k in range(K + 1):\n", + " if 'lower' in name:\n", + " add_subgraph(objectives, gtd.JointObjectives(jid, k).angle(-1.4, prior_model_angles))\n", + " if 'hip' in name:\n", + " add_subgraph(objectives, gtd.JointObjectives(jid, k).angle(0.0, prior_model_hip))\n", + " if 'upper' in name:\n", + " add_subgraph(objectives, gtd.JointObjectives(jid, k).angle(0.7, prior_model_angles))\n", + "\n", + "add_subgraph(graph, objectives)\n", + "\n", + "print('Num phases :', trajectory.numPhases())\n", + "print('Final timestep K :', K)\n", + "print('Graph factors :', graph.size())\n", + "print('Graph variable keys :', graph.keys().size())" + ] + }, + { + "cell_type": "markdown", + "id": "b9b56d98", + "metadata": {}, + "source": [ + "## 4) Initialize with `ChainInitializer` and Optimize\n", + "\n", + "This mirrors the C++ initialization and Levenberg-Marquardt setup." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "648f6d08", + "metadata": {}, + "outputs": [], + "source": [ + "gaussian_noise = 1e-30\n", + "initializer = gtd.ChainInitializer()\n", + "init_values = trajectory.multiPhaseInitialValues(robot, initializer, gaussian_noise, desired_dt)\n", + "\n", + "print('Initial values:', init_values.size())\n", + "\n", + "params = gtsam.LevenbergMarquardtParams()\n", + "params.setlambdaInitial(1e10)\n", + "params.setlambdaLowerBound(1e-7)\n", + "params.setlambdaUpperBound(1e10)\n", + "params.setAbsoluteErrorTol(1.0)\n", + "\n", + "optimizer = gtsam.LevenbergMarquardtOptimizer(graph, init_values, params)\n", + "result = optimizer.optimize()\n", + "\n", + "print('Final objective error:', graph.error(result))" + ] + }, + { + "cell_type": "markdown", + "id": "b0444d54", + "metadata": {}, + "source": [ + "## 5) Convert the optimized `Values` to a table\n", + "\n", + "We record joint angle/velocity/acceleration/torque over time. We also track trunk position to visualize forward progression." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "0ab62e4c", + "metadata": {}, + "outputs": [], + "source": [ + "def read_or_nan(values: gtsam.Values, key: int) -> float:\n", + " return values.atDouble(key) if values.exists(key) else np.nan\n", + "\n", + "rows = []\n", + "joints = list(robot.joints())\n", + "trunk_id = robot.link('trunk').id()\n", + "\n", + "for k in range(K + 1):\n", + " row = {'t': k * desired_dt}\n", + "\n", + " pose_k = gtd.Pose(result, trunk_id, k)\n", + " row['trunk_x'] = pose_k.translation()[0]\n", + " row['trunk_y'] = pose_k.translation()[1]\n", + " row['trunk_z'] = pose_k.translation()[2]\n", + "\n", + " for joint in joints:\n", + " j = joint.id()\n", + " name = joint.name()\n", + " row[name] = read_or_nan(result, gtd.JointAngleKey(j, k))\n", + " row[f'{name}.1'] = read_or_nan(result, gtd.JointVelKey(j, k))\n", + " row[f'{name}.2'] = read_or_nan(result, gtd.JointAccelKey(j, k))\n", + " row[f'{name}.3'] = read_or_nan(result, gtd.TorqueKey(j, k))\n", + "\n", + " rows.append(row)\n", + "\n", + "traj_df = pd.DataFrame(rows)\n", + "traj_df.head(3)" + ] + }, + { + "cell_type": "markdown", + "id": "cdd62425", + "metadata": {}, + "source": [ + "## 6) Save trajectory CSV\n", + "\n", + "We save both:\n", + "\n", + "- A pandas-friendly table with extra columns (for analysis/plotting).\n", + "- The native GTDynamics CSV format via `trajectory.writeToFile(...)`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "35e015c9", + "metadata": {}, + "outputs": [], + "source": [ + "out_dir = repo_root / 'build' / 'examples' / 'example_a1_walking'\n", + "out_dir.mkdir(parents=True, exist_ok=True)\n", + "\n", + "csv_table = out_dir / 'a1_traj_chain_dynamics_graph_python.csv'\n", + "traj_df.to_csv(csv_table, index=False)\n", + "print('Wrote table CSV:', csv_table)\n", + "\n", + "# ChainInitializer does not necessarily populate every key that writeToFile()\n", + "# expects (notably torque keys). Create an export copy and fill missing entries.\n", + "export_values = gtsam.Values(result)\n", + "\n", + "for joint in robot.joints():\n", + " j = joint.id()\n", + " for k in range(K + 1):\n", + " tk = gtd.TorqueKey(j, k)\n", + " if not export_values.exists(tk):\n", + " export_values.insert(tk, 0.0)\n", + "\n", + "for pidx in range(trajectory.numPhases()):\n", + " pk = gtd.PhaseKey(pidx)\n", + " if not export_values.exists(pk):\n", + " export_values.insert(pk, desired_dt)\n", + "\n", + "native_name = 'a1_traj_CDG_massless_python.csv'\n", + "trajectory.writeToFile(robot, native_name, export_values)\n", + "print('Wrote native GTDynamics CSV in current working directory:', native_name)\n" + ] + }, + { + "cell_type": "markdown", + "id": "7ff53c69", + "metadata": {}, + "source": [ + "## 7) Plot joint trajectories and contact-point heights\n", + "\n", + "The second figure shows foot contact-point heights (world Z) for all four feet, which helps check alternating stance/swing behavior." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "1d644dbd", + "metadata": {}, + "outputs": [], + "source": [ + "legs = ['FL', 'FR', 'RL', 'RR']\n", + "joint_order = ['hip', 'upper', 'lower']\n", + "colors = {'hip': 'tab:blue', 'upper': 'tab:orange', 'lower': 'tab:green'}\n", + "\n", + "\n", + "def plot_joint_group(suffix: str, title: str, y_label: str):\n", + " fig, axs = plt.subplots(4, 1, figsize=(12, 11), sharex=True)\n", + " for i, leg in enumerate(legs):\n", + " ax = axs[i]\n", + " for joint in joint_order:\n", + " col = f'{leg}_{joint}_joint{suffix}'\n", + " if col in traj_df.columns:\n", + " ax.plot(traj_df['t'], traj_df[col], label=joint, color=colors[joint], linewidth=1.4)\n", + " ax.grid(alpha=0.3)\n", + " ax.set_ylabel(f'{leg} {y_label}')\n", + " ax.legend(loc='upper right', ncol=3)\n", + " axs[-1].set_xlabel('time [s]')\n", + " fig.suptitle(title)\n", + " fig.tight_layout()\n", + " plt.show()\n", + "\n", + "\n", + "plot_joint_group('', 'Joint Angles (ChainDynamicsGraph walk cycle)', 'q')\n", + "plot_joint_group('.1', 'Joint Velocities (ChainDynamicsGraph walk cycle)', 'qdot')\n", + "\n", + "contact_in_com = gtsam.Point3(0.0, 0.0, -0.07)\n", + "foot_links = ['FL_lower', 'FR_lower', 'RL_lower', 'RR_lower']\n", + "\n", + "foot_z = {'t': traj_df['t'].to_numpy()}\n", + "for name in foot_links:\n", + " cp = gtd.PointOnLink(robot.link(name).shared(), contact_in_com)\n", + " foot_z[name] = np.array([cp.predict(result, k)[2] for k in range(K + 1)])\n", + "\n", + "fig, ax = plt.subplots(figsize=(12, 4))\n", + "for name in foot_links:\n", + " ax.plot(foot_z['t'], foot_z[name], label=name, linewidth=1.5)\n", + "ax.grid(alpha=0.3)\n", + "ax.set_xlabel('time [s]')\n", + "ax.set_ylabel('contact point z [m]')\n", + "ax.set_title('Foot contact-point heights in world frame')\n", + "ax.legend(ncol=4)\n", + "plt.tight_layout()\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "id": "8a059a8b", + "metadata": {}, + "source": [ + "## 8) Quick diagnostics" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "48b7e79f", + "metadata": {}, + "outputs": [], + "source": [ + "angle_cols = [c for c in traj_df.columns if c not in ['t', 'trunk_x', 'trunk_y', 'trunk_z'] and not c.endswith('.1') and not c.endswith('.2') and not c.endswith('.3')]\n", + "vel_cols = [c for c in traj_df.columns if c.endswith('.1')]\n", + "acc_cols = [c for c in traj_df.columns if c.endswith('.2')]\n", + "\n", + "print('Finite ratio (angles):', np.isfinite(traj_df[angle_cols].to_numpy()).mean())\n", + "print('Finite ratio (vels) :', np.isfinite(traj_df[vel_cols].to_numpy()).mean())\n", + "print('Finite ratio (accels):', np.isfinite(traj_df[acc_cols].to_numpy()).mean())\n", + "print('Trunk z mean/std :', traj_df['trunk_z'].mean(), traj_df['trunk_z'].std())\n", + "print('Trunk x start/end :', traj_df['trunk_x'].iloc[0], traj_df['trunk_x'].iloc[-1])" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "name": "python", + "version": "3.12" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/gtdynamics.i b/gtdynamics.i index 0f10e10e..572d9eac 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -152,8 +152,10 @@ class Link { size_t numJoints() const; string name() const; double mass() const; + void setMass(const double mass); const gtsam::Pose3 ¢erOfMass(); const gtsam::Matrix &inertia(); + void setInertia(const gtsam::Matrix3 &inertia); gtsam::Matrix6 inertiaMatrix(); void print(const std::string &s = "") const; @@ -281,7 +283,7 @@ class Robot { #include // Only SDF files require the model_name specified.. -gtdynamics::Robot CreateRobotFromFile(const string &urdf_file_path, +gtdynamics::Robot CreateRobotFromFile(const string &file_path, const string &model_name = "", bool preserve_fixed_joint = false); @@ -290,7 +292,7 @@ gtdynamics::Robot CreateRobotFromFile(const string &urdf_file_path, class PointOnLink { PointOnLink(); - PointOnLink(const gtdynamics::Link* link, const gtsam::Point3 &point); + PointOnLink(const gtdynamics::LinkSharedPtr& link, const gtsam::Point3 &point); gtdynamics::LinkSharedPtr link; gtsam::Point3 point; @@ -301,6 +303,24 @@ class PointOnLink { // PointOnLinks defined in specializations.h +#include +virtual class ConstraintSpec { + void print(const string &s) const; +}; + +#include +class FootContactConstraintSpec : gtdynamics::ConstraintSpec { + FootContactConstraintSpec(); + FootContactConstraintSpec(const std::vector &points_on_links); + FootContactConstraintSpec(const std::vector &links, + const gtsam::Point3 &contact_in_com); + const gtdynamics::PointOnLinks &contactPoints() const; + bool hasContact(const gtdynamics::LinkSharedPtr &link) const; + const gtsam::Point3 &contactPoint(const string &link_name) const; + void print(const string &s = "") const; + std::vector swingLinks() const; +}; + /********************** Optimizer **********************/ #include class OptimizationParameters { diff --git a/python/README.md b/python/README.md index 2c97e695..e271e3fc 100644 --- a/python/README.md +++ b/python/README.md @@ -32,6 +32,17 @@ This directory is where the Pybind11-generated GTDynamics package lives and wher ``` This runs `${PYTHON_EXECUTABLE} -m pip install .` in `build/python`, which produces a wheel in `pip`'s cache before installing it. On non-Windows platforms, `python-install` depends on `python-stubs`, so `.pyi` files are generated first. +## GTSAM Python compatibility (important) + +Use a `gtsam` Python package built from the same GTSAM install/prefix that GTDynamics links against. +Mixing a local GTDynamics build with an unrelated pip/conda `gtsam` wheel can cause hard runtime failures (for example, process aborts when adding factors to a graph). + +If you built GTSAM from source in a sibling repo, prepend it before importing: + +```sh +export PYTHONPATH=/path/to/gtsam/build/python:/path/to/GTDynamics/build/python:$PYTHONPATH +``` + ## Generating type stubs - Run `make python-stubs` to generate stubs with `pybind11-stubgen`. diff --git a/python/gtdynamics/preamble/gtdynamics.h b/python/gtdynamics/preamble/gtdynamics.h index e69de29b..79caefd0 100644 --- a/python/gtdynamics/preamble/gtdynamics.h +++ b/python/gtdynamics/preamble/gtdynamics.h @@ -0,0 +1,4 @@ +// Ensure these STL aliases are treated as opaque container types so the +// bind_vector/bind_map specializations behave predictably in Python. +PYBIND11_MAKE_OPAQUE(gtdynamics::PointOnLinks); +PYBIND11_MAKE_OPAQUE(gtdynamics::ContactPointGoals); From 49bf7fa21deb4fa84ab75d09eb37c0764947637e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Feb 2026 17:01:25 -0500 Subject: [PATCH 05/11] Some refactoring --- .gitignore | 5 + .../chain_dynamics_motion_planning.ipynb | 130 +++++++++--------- gtdynamics/dynamics/Chain.cpp | 20 +-- gtdynamics/dynamics/Chain.h | 19 ++- gtdynamics/dynamics/ChainDynamicsGraph.cpp | 37 ++--- webdemo/.gitignore | 49 +++++++ 6 files changed, 159 insertions(+), 101 deletions(-) create mode 100644 webdemo/.gitignore diff --git a/.gitignore b/.gitignore index d9ee594a..38a01352 100644 --- a/.gitignore +++ b/.gitignore @@ -122,3 +122,8 @@ traj.csv results/ _codeql_detected_source_root + +# Generated by scripts/bootstrap_mujoco_models.py +models/mujoco/unitree_a1 +models/mujoco/unitree_g1 +models/mujoco/boston_dynamics_spot diff --git a/examples/example_a1_walking/chain_dynamics_motion_planning.ipynb b/examples/example_a1_walking/chain_dynamics_motion_planning.ipynb index a6b12b65..3bd2cd3b 100644 --- a/examples/example_a1_walking/chain_dynamics_motion_planning.ipynb +++ b/examples/example_a1_walking/chain_dynamics_motion_planning.ipynb @@ -7,9 +7,7 @@ "source": [ "# A1 Walk-Cycle Motion Planning with `ChainDynamicsGraph` (Python)\n", "\n", - "This notebook mirrors the C++ `newGraph()` pipeline in:\n", - "\n", - "`examples/example_a1_walking/main.cpp`\n", + "This notebook mirrors the C++ `newGraph()` pipeline in [main.cpp](main.cpp).\n", "\n", "The goal is to build the same multi-phase quadruped trajectory optimization with chain dynamics only, then inspect and plot the optimized motion." ] @@ -81,7 +79,6 @@ "These helpers correspond to pieces in `main.cpp`:\n", "\n", "- `build_walk_trajectory(...)` mirrors `getTrajectory(...)`.\n", - "- `set_massless_except_trunk(...)` mirrors the mass/inertia simplification used in `newGraph()`.\n", "- `add_subgraph(...)` is needed because Python exposes graph `add(...)` for single factors, while many GTDynamics objective helpers return a small `NonlinearFactorGraph`." ] }, @@ -92,22 +89,6 @@ "metadata": {}, "outputs": [], "source": [ - "def add_subgraph(dst_graph: gtsam.NonlinearFactorGraph,\n", - " src_graph: gtsam.NonlinearFactorGraph) -> None:\n", - " # Append all factors from src_graph into dst_graph.\n", - " for i in range(src_graph.size()):\n", - " if src_graph.exists(i):\n", - " dst_graph.add(src_graph.at(i))\n", - "\n", - "\n", - "def set_massless_except_trunk(robot: gtd.Robot) -> None:\n", - " # Match C++ newGraph(): all links except trunk are zero mass/inertia.\n", - " for link in robot.links():\n", - " if 'trunk' not in link.name():\n", - " link.setMass(0.0)\n", - " link.setInertia(np.zeros((3, 3)))\n", - "\n", - "\n", "def build_walk_trajectory(robot: gtd.Robot, repeat: int = 1) -> gtd.Trajectory:\n", " # Match getTrajectory() from C++: RRFL -> stationary -> RLFR -> stationary.\n", " rlfr = [robot.link('RL_lower'), robot.link('FR_lower')]\n", @@ -135,7 +116,7 @@ "\n", "This cell follows the structure of `newGraph()` in C++:\n", "\n", - "1. Load A1, make non-trunk links massless.\n", + "1. Load A1.\n", "2. Build `ChainDynamicsGraph`.\n", "3. Build multi-phase dynamics graph from the walk trajectory.\n", "4. Add contact-point objectives.\n", @@ -152,16 +133,7 @@ "metadata": {}, "outputs": [], "source": [ - "robot = gtd.CreateRobotFromFile(gtd.URDF_PATH + '/a1/a1.urdf', 'a1')\n", - "set_massless_except_trunk(robot)\n", - "\n", - "sigma_dynamics = 1e-3\n", - "sigma_objectives = 1e-3\n", - "\n", - "dynamics_model_6 = gtsam.noiseModel.Isotropic.Sigma(6, sigma_dynamics)\n", - "dynamics_model_1 = gtsam.noiseModel.Isotropic.Sigma(1, sigma_dynamics)\n", - "objectives_model_6 = gtsam.noiseModel.Isotropic.Sigma(6, sigma_objectives)\n", - "objectives_model_1 = gtsam.noiseModel.Isotropic.Sigma(1, sigma_objectives)\n", + "robot = gtd.CreateRobotFromFile(gtd.URDF_PATH + \"/a1/a1.urdf\", \"a1\")\n", "\n", "gravity = np.array([0.0, 0.0, -9.8])\n", "mu = 1.0\n", @@ -173,6 +145,26 @@ "collocation = gtd.CollocationScheme.Euler\n", "\n", "graph = trajectory.multiPhaseFactorGraph(robot, graph_builder, collocation, mu)\n", + "print('Graph has', graph.size(), 'factors and', graph.keys().size(), 'variables.')" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "60f38e64", + "metadata": {}, + "outputs": [], + "source": [ + "graph.print(\"Graph:\\n\", gtd.GTDKeyFormatter)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "5270bfa7", + "metadata": {}, + "outputs": [], + "source": [ "\n", "ground_height = 1.0\n", "step = gtsam.Point3(0.25, 0.0, 0.0)\n", @@ -188,33 +180,31 @@ "for link in robot.links():\n", " i = link.id()\n", " if i == 0:\n", - " add_subgraph(\n", - " objectives,\n", + " objectives.push_back(\n", " gtd.LinkObjectives(i, 0)\n", - " .pose(link.bMcom(), gtsam.noiseModel.Isotropic.Sigma(6, 1e-3))\n", - " .twist(np.zeros(6), gtsam.noiseModel.Isotropic.Sigma(6, 1e-3)),\n", + " .pose(link.bMcom(), gtsam.noiseModel.Isotropic.Sigma(6, 1e-3))\n", + " .twist(np.zeros(6), gtsam.noiseModel.Isotropic.Sigma(6, 1e-3)),\n", " )\n", " if i in (3, 6, 9, 12):\n", - " add_subgraph(\n", - " objectives,\n", + " objectives.push_back(\n", " gtd.LinkObjectives(i, 0).pose(\n", " link.bMcom(), gtsam.noiseModel.Isotropic.Sigma(6, 1e-3)\n", " ),\n", " )\n", "\n", - "add_subgraph(objectives, gtd.JointsAtRestObjectives(robot, objectives_model_1, objectives_model_1, 0))\n", - "add_subgraph(objectives, gtd.JointsAtRestObjectives(robot, objectives_model_1, objectives_model_1, K))\n", + "rest_model = gtsam.noiseModel.Isotropic.Sigma(1, 1e-3)\n", + "objectives.push_back(gtd.JointsAtRestObjectives(robot, rest_model, rest_model, 0))\n", + "objectives.push_back(gtd.JointsAtRestObjectives(robot, rest_model, rest_model, K))\n", "\n", - "trunk = robot.link('trunk')\n", + "trunk = robot.link(\"trunk\")\n", "for k in range(K + 1):\n", - " add_subgraph(\n", - " objectives,\n", + " objectives.push_back(\n", " gtd.LinkObjectives(trunk.id(), k)\n", - " .pose(\n", - " gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.0, 0.0, 0.4)),\n", - " gtsam.noiseModel.Isotropic.Sigma(6, 1e-2),\n", - " )\n", - " .twist(np.zeros(6), gtsam.noiseModel.Isotropic.Sigma(6, 5e-2)),\n", + " .pose(\n", + " gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.0, 0.0, 0.4)),\n", + " gtsam.noiseModel.Isotropic.Sigma(6, 1e-2),\n", + " )\n", + " .twist(np.zeros(6), gtsam.noiseModel.Isotropic.Sigma(6, 5e-2)),\n", " )\n", "\n", "desired_dt = 1.0 / 20.0\n", @@ -227,19 +217,25 @@ " name = joint.name()\n", " jid = joint.id()\n", " for k in range(K + 1):\n", - " if 'lower' in name:\n", - " add_subgraph(objectives, gtd.JointObjectives(jid, k).angle(-1.4, prior_model_angles))\n", - " if 'hip' in name:\n", - " add_subgraph(objectives, gtd.JointObjectives(jid, k).angle(0.0, prior_model_hip))\n", - " if 'upper' in name:\n", - " add_subgraph(objectives, gtd.JointObjectives(jid, k).angle(0.7, prior_model_angles))\n", - "\n", - "add_subgraph(graph, objectives)\n", - "\n", - "print('Num phases :', trajectory.numPhases())\n", - "print('Final timestep K :', K)\n", - "print('Graph factors :', graph.size())\n", - "print('Graph variable keys :', graph.keys().size())" + " if \"lower\" in name:\n", + " objectives.push_back(\n", + " gtd.JointObjectives(jid, k).angle(-1.4, prior_model_angles)\n", + " )\n", + " if \"hip\" in name:\n", + " objectives.push_back(\n", + " gtd.JointObjectives(jid, k).angle(0.0, prior_model_hip)\n", + " )\n", + " if \"upper\" in name:\n", + " objectives.push_back(\n", + " gtd.JointObjectives(jid, k).angle(0.7, prior_model_angles)\n", + " )\n", + "\n", + "graph.push_back(objectives)\n", + "\n", + "print(\"Num phases :\", trajectory.numPhases())\n", + "print(\"Final timestep K :\", K)\n", + "print(\"Graph factors :\", graph.size())\n", + "print(\"Graph variable keys :\", graph.keys().size())" ] }, { @@ -366,7 +362,7 @@ " if not export_values.exists(pk):\n", " export_values.insert(pk, desired_dt)\n", "\n", - "native_name = 'a1_traj_CDG_massless_python.csv'\n", + "native_name = 'a1_traj_CDG_python.csv'\n", "trajectory.writeToFile(robot, native_name, export_values)\n", "print('Wrote native GTDynamics CSV in current working directory:', native_name)\n" ] @@ -462,13 +458,21 @@ ], "metadata": { "kernelspec": { - "display_name": "Python 3", + "display_name": "py312", "language": "python", "name": "python3" }, "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", "name": "python", - "version": "3.12" + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" } }, "nbformat": 4, diff --git a/gtdynamics/dynamics/Chain.cpp b/gtdynamics/dynamics/Chain.cpp index 4baf55e5..77d6243a 100644 --- a/gtdynamics/dynamics/Chain.cpp +++ b/gtdynamics/dynamics/Chain.cpp @@ -13,7 +13,6 @@ #include - namespace gtdynamics { Chain operator*(const Chain &chainA, const Chain &chainB) { @@ -237,19 +236,20 @@ gtsam::Vector6 Chain::AdjointWrenchEquality3( Pose3 T_theta = poe(angles, {}, H_angles ? &J_theta : nullptr); // Get end-effector wrench by Adjoint. This is true for a massless leg. - gtsam::Vector6 transformed_wrench = - T_theta.AdjointTranspose(wrench_body,H_angles ? &H_T : nullptr , H_wrench_body); + gtsam::Vector6 transformed_wrench = T_theta.AdjointTranspose( + wrench_body, H_angles ? &H_T : nullptr, H_wrench_body); if (H_angles) { - *H_angles = H_T * J_theta; + *H_angles = H_T * J_theta; } return transformed_wrench; } -gtsam::Vector6_ Chain::Poe3Factor(const std::vector &joints, - const gtsam::Key wTb_key, - const gtsam::Key wTe_key, size_t k) const { +gtsam::Vector6_ Chain::Poe3Expression(const std::vector &joints, + const gtsam::Key wTb_key, + const gtsam::Key wTe_key, + size_t k) const { // Get Expression for poses gtsam::Pose3_ wTb(wTb_key); gtsam::Pose3_ wTe(wTe_key); @@ -266,12 +266,12 @@ gtsam::Vector6_ Chain::Poe3Factor(const std::vector &joints, std::placeholders::_2), angles); - // compose + // compose gtsam::Pose3_ wTe_hat = wTb * end_effector_pose; // get error expression gtsam::Vector6_ error_expression = gtsam::logmap(wTe_hat, wTe); - + return error_expression; } @@ -283,7 +283,7 @@ gtsam::Pose3 Chain::PoeEquality3(const gtsam::Vector3 &angles, Pose3 T_theta = poe(angles, {}, H_angles ? &J_theta : nullptr); if (H_angles) { - *H_angles =J_theta; + *H_angles = J_theta; } return T_theta; diff --git a/gtdynamics/dynamics/Chain.h b/gtdynamics/dynamics/Chain.h index dc580b05..47a603c5 100644 --- a/gtdynamics/dynamics/Chain.h +++ b/gtdynamics/dynamics/Chain.h @@ -176,9 +176,9 @@ class Chain { * @param k .................... Time slice. * @return ..................... GTSAM expression of the chain constraint. */ - gtsam::Vector6_ Poe3Factor(const std::vector &joints, - const gtsam::Key wTb_key, const gtsam::Key wTe_key, - size_t k) const; + gtsam::Vector6_ Poe3Expression(const std::vector &joints, + const gtsam::Key wTb_key, + const gtsam::Key wTe_key, size_t k) const; /** * This function calculates the end-effector pose using POE and chain @@ -188,19 +188,18 @@ class Chain { * BODY TO END-EFFECTOR. * @return ................... gtsam expression of the end-effector wrench */ - gtsam::Pose3 PoeEquality3( - const gtsam::Vector3 &angles, - gtsam::OptionalJacobian<6, 3> H_angles = {}) const; + gtsam::Pose3 PoeEquality3(const gtsam::Vector3 &angles, + gtsam::OptionalJacobian<6, 3> H_angles = {}) const; }; // Chain class // Helper function to create expression with a vector, used in // ChainConstraint3. inline gtsam::Vector3 MakeVector3(const double &value0, const double &value1, - const double &value2, - gtsam::OptionalJacobian<3, 1> J0 = {}, - gtsam::OptionalJacobian<3, 1> J1 = {}, - gtsam::OptionalJacobian<3, 1> J2 = {}) { + const double &value2, + gtsam::OptionalJacobian<3, 1> J0 = {}, + gtsam::OptionalJacobian<3, 1> J1 = {}, + gtsam::OptionalJacobian<3, 1> J2 = {}) { gtsam::Vector3 q; q << value0, value1, value2; if (J0) { diff --git a/gtdynamics/dynamics/ChainDynamicsGraph.cpp b/gtdynamics/dynamics/ChainDynamicsGraph.cpp index 063566c2..5cecb451 100644 --- a/gtdynamics/dynamics/ChainDynamicsGraph.cpp +++ b/gtdynamics/dynamics/ChainDynamicsGraph.cpp @@ -12,6 +12,7 @@ */ #include "gtdynamics/dynamics/ChainDynamicsGraph.h" + #include #include @@ -117,36 +118,35 @@ NonlinearFactorGraph ChainDynamicsGraph::dynamicsFactors( // Create expression for wrench constraint on trunk Vector6_ trunk_wrench_constraint = gravity_wrench; - // Set tolerances - // Get tolerance Vector6 wrench_tolerance = opt().f_cost_model->sigmas(); Vector3 contact_tolerance = opt().cm_cost_model->sigmas(); std::vector wrench_keys; - for (int i = 0; i < 4; ++i) { + constexpr int root = 0; // TODO(Frank): Hard-coded for A1. + const Vector6 wrench_zero = gtsam::Z_6x1; + + for (int leg = 0; leg < 4; ++leg) { bool foot_in_contact = false; // Get key for wrench at hip joint with id 0 - const Key wrench_key_3i_T = WrenchKey(0, 3 * i, t); - - // create expression for the wrench and initialize to zero - Vector6_ wrench_3i_T(Vector6::Zero()); + const int joint_id = 3 * leg; // TODO(Frank): Hard-coded for A1. + const Key wrench_key = WrenchKey(root, joint_id, t); // Set wrench expression on trunk by leg, according to contact for (auto &&cp : *contact_points) { - if (cp.link->id() == 3 * (i + 1)) { - wrench_3i_T = Vector6_(wrench_key_3i_T); + if (cp.link->id() == 3 * (leg + 1)) { foot_in_contact = true; } } // add wrench to trunk constraint - wrench_keys.push_back(wrench_key_3i_T); + wrench_keys.push_back(wrench_key); // Get expression for end effector wrench using chain - Vector6_ wrench_end_effector = composed_chains_[i].AdjointWrenchConstraint3( - chain_joints_[i], wrench_key_3i_T, t); + Vector6_ wrench_end_effector = + composed_chains_[leg].AdjointWrenchConstraint3(chain_joints_[leg], + wrench_key, t); // Create contact constraint Point3 contact_in_com(0.0, 0.0, -0.07); @@ -158,8 +158,7 @@ NonlinearFactorGraph ChainDynamicsGraph::dynamicsFactors( if (foot_in_contact) { graph.add(contact_expression.penaltyFactor(1.0)); } else { - Vector6 wrench_zero = gtsam::Z_6x1; - graph.addPrior(wrench_key_3i_T, wrench_zero, opt().f_cost_model); + graph.addPrior(wrench_key, wrench_zero, opt().f_cost_model); } } @@ -186,7 +185,7 @@ gtsam::NonlinearFactorGraph ChainDynamicsGraph::qFactors( const Key end_effector_key = PoseKey(3 * (i + 1), t); // Get expression for end effector pose - gtsam::Vector6_ chain_expression = composed_chains_[i].Poe3Factor( + gtsam::Vector6_ chain_expression = composed_chains_[i].Poe3Expression( chain_joints_[i], base_key, end_effector_key, t); gtsam::ExpressionEqualityConstraint chain_constraint( @@ -198,8 +197,8 @@ gtsam::NonlinearFactorGraph ChainDynamicsGraph::qFactors( // Add contact factors. if (contact_points) { const Kinematics kinematics(opt()); - graph.add( - kinematics.contactHeightObjectives(Slice(t), *contact_points, gravity())); + graph.add(kinematics.contactHeightObjectives(Slice(t), *contact_points, + gravity())); } return graph; @@ -211,7 +210,9 @@ gtsam::NonlinearFactorGraph ChainDynamicsGraph::dynamicsFactorGraph( const std::optional &mu) const { NonlinearFactorGraph graph; graph.add(qFactors(robot, t, contact_points)); - // TODO(Varun): Why are these commented out? + // NOTE(Frank): In chain factors, there is no need to model the intermediate + // velocities/accelerations of the joints, as they do not influence the + // solution, see Dan's thesis. // graph.add(vFactors(robot, t, contact_points)); // graph.add(aFactors(robot, t, contact_points)); graph.add(dynamicsFactors(robot, t, contact_points, mu)); diff --git a/webdemo/.gitignore b/webdemo/.gitignore new file mode 100644 index 00000000..5162a7e2 --- /dev/null +++ b/webdemo/.gitignore @@ -0,0 +1,49 @@ +package-lock.json + +# Dependencies +/node_modules + +# IDE - VSCode +.vscode/* +!.vscode/extensions.json +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json + +# IDE - JetBrains +.idea +*.iml +*.ipr +*.iws + +# Local env files +.env +.env.local +.env.*.local + +# Log files +npm-debug.log* +yarn-debug.log* +yarn-error.log* +pnpm-debug.log* + +# Editor directories and files +.DS_Store +Thumbs.db +*.suo +*.ntvs* +*.njsproj +*.sln +*.sw? + +# Build files +/build + +# Cache +.cache +.temp +dist + +public/examples/scenes/unitree_g1 +public/examples/scenes/unitree_a1 +public/examples/scenes/boston_dynamics_spot From 79c5e97479a18fab41528886e7b56ae0dfc19403 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Feb 2026 23:25:36 -0500 Subject: [PATCH 06/11] Switch to specifying ground height in world --- gtdynamics.i | 21 ++++++++++------ gtdynamics/dynamics/ChainDynamicsGraph.cpp | 11 +++++---- gtdynamics/dynamics/ChainDynamicsGraph.h | 6 +++-- gtdynamics/dynamics/DynamicsGraph.cpp | 26 ++++++++++++-------- gtdynamics/dynamics/DynamicsGraph.h | 15 ++++++++--- gtdynamics/kinematics/ContactHeightFactor.h | 2 +- gtdynamics/kinematics/Kinematics.h | 8 ++++-- gtdynamics/kinematics/KinematicsInterval.cpp | 4 +-- gtdynamics/kinematics/KinematicsPhase.cpp | 4 +-- gtdynamics/kinematics/KinematicsSlice.cpp | 10 +++++--- gtdynamics/utils/Trajectory.cpp | 15 +++++++---- gtdynamics/utils/Trajectory.h | 12 ++++++--- gtdynamics/utils/WalkCycle.cpp | 6 +++-- gtdynamics/utils/WalkCycle.h | 6 ++--- tests/testTrajectory.cpp | 2 +- tests/testWalkCycle.cpp | 13 +++++----- 16 files changed, 101 insertions(+), 60 deletions(-) diff --git a/gtdynamics.i b/gtdynamics.i index 572d9eac..b4c91859 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -502,7 +502,8 @@ class DynamicsGraph { gtsam::NonlinearFactorGraph qFactors( const gtdynamics::Robot &robot, const int k, - const std::optional &contact_points) const; + const std::optional &contact_points, + double ground_plane_height = 0.0) const; /* return v-level nonlinear factor graph (twist related factors) */ gtsam::NonlinearFactorGraph vFactors( @@ -523,7 +524,8 @@ class DynamicsGraph { gtsam::NonlinearFactorGraph dynamicsFactorGraph( const gtdynamics::Robot &robot, const int k, const std::optional &contact_points, - const std::optional &mu) const; + const std::optional &mu, + double ground_plane_height = 0.0) const; gtsam::NonlinearFactorGraph inverseDynamicsPriors( const gtdynamics::Robot &robot, const int k, @@ -544,7 +546,8 @@ class DynamicsGraph { const gtdynamics::Robot &robot, const int num_steps, const double dt, const gtdynamics::CollocationScheme collocation, const std::optional &contact_points, - const std::optional &mu) const; + const std::optional &mu, + double ground_plane_height = 0.0) const; gtsam::NonlinearFactorGraph multiPhaseTrajectoryFG( const gtdynamics::Robot &robot, @@ -672,7 +675,8 @@ class ChainDynamicsGraph : gtdynamics::DynamicsGraph { gtsam::NonlinearFactorGraph qFactors( const gtdynamics::Robot &robot, const int t, - const std::optional &contact_points) const; + const std::optional &contact_points, + double ground_plane_height = 0.0) const; gtsam::NonlinearFactorGraph dynamicsFactors( const gtdynamics::Robot &robot, const int t, @@ -682,7 +686,8 @@ class ChainDynamicsGraph : gtdynamics::DynamicsGraph { gtsam::NonlinearFactorGraph dynamicsFactorGraph( const gtdynamics::Robot &robot, const int t, const std::optional &contact_points, - const std::optional &mu) const; + const std::optional &mu, + double ground_plane_height = 0.0) const; }; /********************** Objective Factors **********************/ @@ -957,12 +962,14 @@ class Trajectory { std::vector getTransitionGraphs(const gtdynamics::Robot& robot, const gtdynamics::DynamicsGraph &graph_builder, - double mu) const; + double mu, + double ground_plane_height = 0.0) const; gtsam::NonlinearFactorGraph multiPhaseFactorGraph(const gtdynamics::Robot& robot, const gtdynamics::DynamicsGraph &graph_builder, const gtdynamics::CollocationScheme collocation, - double mu) const; + double mu, + double ground_plane_height = 0.0) const; std::vector transitionPhaseInitialValues(const gtdynamics::Robot& robot, const gtdynamics::Initializer &initializer, double gaussian_noise) const; diff --git a/gtdynamics/dynamics/ChainDynamicsGraph.cpp b/gtdynamics/dynamics/ChainDynamicsGraph.cpp index 5cecb451..a40bc70f 100644 --- a/gtdynamics/dynamics/ChainDynamicsGraph.cpp +++ b/gtdynamics/dynamics/ChainDynamicsGraph.cpp @@ -171,7 +171,8 @@ NonlinearFactorGraph ChainDynamicsGraph::dynamicsFactors( gtsam::NonlinearFactorGraph ChainDynamicsGraph::qFactors( const Robot &robot, const int t, - const std::optional &contact_points) const { + const std::optional &contact_points, + double ground_plane_height) const { NonlinearFactorGraph graph; // Get Pose key for base link @@ -197,8 +198,8 @@ gtsam::NonlinearFactorGraph ChainDynamicsGraph::qFactors( // Add contact factors. if (contact_points) { const Kinematics kinematics(opt()); - graph.add(kinematics.contactHeightObjectives(Slice(t), *contact_points, - gravity())); + graph.add(kinematics.contactHeightObjectives( + Slice(t), *contact_points, gravity(), ground_plane_height)); } return graph; @@ -207,9 +208,9 @@ gtsam::NonlinearFactorGraph ChainDynamicsGraph::qFactors( gtsam::NonlinearFactorGraph ChainDynamicsGraph::dynamicsFactorGraph( const Robot &robot, const int t, const std::optional &contact_points, - const std::optional &mu) const { + const std::optional &mu, double ground_plane_height) const { NonlinearFactorGraph graph; - graph.add(qFactors(robot, t, contact_points)); + graph.add(qFactors(robot, t, contact_points, ground_plane_height)); // NOTE(Frank): In chain factors, there is no need to model the intermediate // velocities/accelerations of the joints, as they do not influence the // solution, see Dan's thesis. diff --git a/gtdynamics/dynamics/ChainDynamicsGraph.h b/gtdynamics/dynamics/ChainDynamicsGraph.h index 3813549c..24bd3631 100644 --- a/gtdynamics/dynamics/ChainDynamicsGraph.h +++ b/gtdynamics/dynamics/ChainDynamicsGraph.h @@ -60,7 +60,8 @@ class ChainDynamicsGraph : public DynamicsGraph { /// Return q-level nonlinear factor graph (pose related factors) NonlinearFactorGraph qFactors( const Robot &robot, const int t, - const std::optional &contact_points = {}) const override; + const std::optional &contact_points = {}, + double ground_plane_height = 0.0) const override; /** * Return nonlinear factor graph of all dynamics factors. @@ -84,7 +85,8 @@ class ChainDynamicsGraph : public DynamicsGraph { NonlinearFactorGraph dynamicsFactorGraph( const Robot &robot, const int t, const std::optional &contact_points = {}, - const std::optional &mu = {}) const override; + const std::optional &mu = {}, + double ground_plane_height = 0.0) const override; // Get a vector of legs, each leg is a vector of its joints static std::vector> getChainJoints( diff --git a/gtdynamics/dynamics/DynamicsGraph.cpp b/gtdynamics/dynamics/DynamicsGraph.cpp index d75b504e..19fb5d33 100644 --- a/gtdynamics/dynamics/DynamicsGraph.cpp +++ b/gtdynamics/dynamics/DynamicsGraph.cpp @@ -197,9 +197,11 @@ Values DynamicsGraph::linearSolveID(const Robot &robot, const int k, gtsam::NonlinearFactorGraph DynamicsGraph::qFactors( const Robot &robot, const int k, - const std::optional &contact_points) const { + const std::optional &contact_points, + double ground_plane_height) const { const Slice slice(k); - return kinematics_.qFactors(slice, robot, contact_points, gravity_); + return kinematics_.qFactors(slice, robot, contact_points, gravity_, + ground_plane_height); } gtsam::NonlinearFactorGraph DynamicsGraph::vFactors( @@ -232,9 +234,9 @@ gtsam::NonlinearFactorGraph DynamicsGraph::dynamicsFactors( gtsam::NonlinearFactorGraph DynamicsGraph::dynamicsFactorGraph( const Robot &robot, const int k, const std::optional &contact_points, - const std::optional &mu) const { + const std::optional &mu, double ground_plane_height) const { NonlinearFactorGraph graph; - graph.add(qFactors(robot, k, contact_points)); + graph.add(qFactors(robot, k, contact_points, ground_plane_height)); graph.add(vFactors(robot, k, contact_points)); graph.add(aFactors(robot, k, contact_points)); graph.add(dynamicsFactors(robot, k, contact_points, mu)); @@ -245,10 +247,11 @@ gtsam::NonlinearFactorGraph DynamicsGraph::trajectoryFG( const Robot &robot, const int num_steps, const double dt, const CollocationScheme collocation, const std::optional &contact_points, - const std::optional &mu) const { + const std::optional &mu, double ground_plane_height) const { NonlinearFactorGraph graph; for (int k = 0; k <= num_steps; k++) { - graph.add(dynamicsFactorGraph(robot, k, contact_points, mu)); + graph.add( + dynamicsFactorGraph(robot, k, contact_points, mu, ground_plane_height)); if (k < num_steps) { graph.add(collocationFactors(robot, k, dt, collocation)); } @@ -261,7 +264,7 @@ gtsam::NonlinearFactorGraph DynamicsGraph::multiPhaseTrajectoryFG( const std::vector &transition_graphs, const CollocationScheme collocation, const std::optional> &phase_contact_points, - const std::optional &mu) const { + const std::optional &mu, double ground_plane_height) const { NonlinearFactorGraph graph; int num_phases = phase_steps.size(); @@ -273,18 +276,21 @@ gtsam::NonlinearFactorGraph DynamicsGraph::multiPhaseTrajectoryFG( }; // First slice, k==0 - graph.add(dynamicsFactorGraph(robot, 0, contact_points(0), mu)); + graph.add( + dynamicsFactorGraph(robot, 0, contact_points(0), mu, ground_plane_height)); int k = 0; for (int p = 0; p < num_phases; p++) { // in-phase // add dynamics for each step for (int step = 0; step < phase_steps[p] - 1; step++) { - graph.add(dynamicsFactorGraph(robot, ++k, contact_points(p), mu)); + graph.add(dynamicsFactorGraph(robot, ++k, contact_points(p), mu, + ground_plane_height)); } if (p == num_phases - 1) { // Last slice, k==K-1 - graph.add(dynamicsFactorGraph(robot, ++k, contact_points(p), mu)); + graph.add(dynamicsFactorGraph(robot, ++k, contact_points(p), mu, + ground_plane_height)); } else { // transition graph.add(transition_graphs[p]); diff --git a/gtdynamics/dynamics/DynamicsGraph.h b/gtdynamics/dynamics/DynamicsGraph.h index 7d3d6f7d..9cfb58cd 100644 --- a/gtdynamics/dynamics/DynamicsGraph.h +++ b/gtdynamics/dynamics/DynamicsGraph.h @@ -130,7 +130,8 @@ class DynamicsGraph { /// Return q-level nonlinear factor graph (pose related factors) virtual gtsam::NonlinearFactorGraph qFactors( const Robot &robot, const int k, - const std::optional &contact_points = {}) const; + const std::optional &contact_points = {}, + double ground_plane_height = 0.0) const; /// Return v-level nonlinear factor graph (twist related factors) gtsam::NonlinearFactorGraph vFactors( @@ -154,11 +155,13 @@ class DynamicsGraph { * @param k time step * @param contact_points optional vector of contact points. * @param mu optional coefficient of static friction. + * @param ground_plane_height contact ground-plane height in world frame. */ virtual gtsam::NonlinearFactorGraph dynamicsFactorGraph( const Robot &robot, const int k, const std::optional &contact_points = {}, - const std::optional &mu = {}) const; + const std::optional &mu = {}, + double ground_plane_height = 0.0) const; /** * Return prior factors of torque, angle, velocity @@ -194,12 +197,14 @@ class DynamicsGraph { * @param num_steps total time steps * @param dt duration of each time step * @param collocation the collocation scheme + * @param ground_plane_height contact ground-plane height in world frame. */ gtsam::NonlinearFactorGraph trajectoryFG( const Robot &robot, const int num_steps, const double dt, const CollocationScheme collocation = Trapezoidal, const std::optional &contact_points = {}, - const std::optional &mu = {}) const; + const std::optional &mu = {}, + double ground_plane_height = 0.0) const; /** * Return nonlinear factor graph of the entire trajectory for multi-phase @@ -209,13 +214,15 @@ class DynamicsGraph { * @param collocation the collocation scheme * @param phase_contact_points contact points at each phase * @param mu optional coefficient of static friction + * @param ground_plane_height contact ground-plane height in world frame. */ gtsam::NonlinearFactorGraph multiPhaseTrajectoryFG( const Robot &robot, const std::vector &phase_steps, const std::vector &transition_graphs, const CollocationScheme collocation = Trapezoidal, const std::optional> &phase_contact_points = {}, - const std::optional &mu = {}) const; + const std::optional &mu = {}, + double ground_plane_height = 0.0) const; static std::shared_ptr> collocationFactorDouble(const gtsam::Key x0_key, const gtsam::Key x1_key, diff --git a/gtdynamics/kinematics/ContactHeightFactor.h b/gtdynamics/kinematics/ContactHeightFactor.h index bd8a8591..dc42c966 100644 --- a/gtdynamics/kinematics/ContactHeightFactor.h +++ b/gtdynamics/kinematics/ContactHeightFactor.h @@ -7,7 +7,7 @@ /** * @file ContactHeightFactor.h - * @brief Factor to enforce zero height at the contact point. + * @brief Factor to enforce ground-plane height at the contact point. * @author: Alejandro Escontrela, Varun Agrawal */ diff --git a/gtdynamics/kinematics/Kinematics.h b/gtdynamics/kinematics/Kinematics.h index 157b922d..eb2eb0a7 100644 --- a/gtdynamics/kinematics/Kinematics.h +++ b/gtdynamics/kinematics/Kinematics.h @@ -135,13 +135,15 @@ class Kinematics : public Optimizer { * @fn Create q-level kinematics factors. * @param contact_points Optional contact points on links. * @param gravity Gravity vector used to define up direction. + * @param ground_plane_height Contact ground-plane height in world frame. * @returns graph with q-level factors. */ template gtsam::NonlinearFactorGraph qFactors( const CONTEXT& context, const Robot& robot, const std::optional& contact_points = {}, - const gtsam::Vector3& gravity = gtsam::Vector3(0, 0, -9.8)) const; + const gtsam::Vector3& gravity = gtsam::Vector3(0, 0, -9.8), + double ground_plane_height = 0.0) const; /** * @fn Create v-level kinematics factors. @@ -182,12 +184,14 @@ class Kinematics : public Optimizer { * @fn Create contact-height objectives. * @param contact_points Contact points on links. * @param gravity Gravity vector used to define up direction. + * @param ground_plane_height Contact ground-plane height in world frame. * @returns graph with contact-height factors. */ template gtsam::NonlinearFactorGraph contactHeightObjectives( const CONTEXT& context, const PointOnLinks& contact_points, - const gtsam::Vector3& gravity) const; + const gtsam::Vector3& gravity, + double ground_plane_height = 0.0) const; /** * @fn Create fixed-link pose prior objectives. diff --git a/gtdynamics/kinematics/KinematicsInterval.cpp b/gtdynamics/kinematics/KinematicsInterval.cpp index 1de6ffd6..195e14e7 100644 --- a/gtdynamics/kinematics/KinematicsInterval.cpp +++ b/gtdynamics/kinematics/KinematicsInterval.cpp @@ -36,9 +36,9 @@ template <> NonlinearFactorGraph Kinematics::qFactors( const Interval& interval, const Robot& robot, const std::optional& contact_points, - const gtsam::Vector3& gravity) const { + const gtsam::Vector3& gravity, double ground_plane_height) const { return collectFactors(interval, [&](const Slice& slice) { - return qFactors(slice, robot, contact_points, gravity); + return qFactors(slice, robot, contact_points, gravity, ground_plane_height); }); } diff --git a/gtdynamics/kinematics/KinematicsPhase.cpp b/gtdynamics/kinematics/KinematicsPhase.cpp index ee3f4829..4e4d51ea 100644 --- a/gtdynamics/kinematics/KinematicsPhase.cpp +++ b/gtdynamics/kinematics/KinematicsPhase.cpp @@ -36,9 +36,9 @@ template <> NonlinearFactorGraph Kinematics::qFactors( const Phase& phase, const Robot& robot, const std::optional& contact_points, - const gtsam::Vector3& gravity) const { + const gtsam::Vector3& gravity, double ground_plane_height) const { const Interval& interval = static_cast(phase); - return qFactors(interval, robot, contact_points, gravity); + return qFactors(interval, robot, contact_points, gravity, ground_plane_height); } template <> diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index 069cfe69..4fe5803a 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -196,12 +196,13 @@ NonlinearFactorGraph Kinematics::pointGoalObjectives( template <> NonlinearFactorGraph Kinematics::contactHeightObjectives( const Slice& slice, const PointOnLinks& contact_points, - const gtsam::Vector3& gravity) const { + const gtsam::Vector3& gravity, double ground_plane_height) const { NonlinearFactorGraph graph; for (const PointOnLink& cp : contact_points) { graph.emplace_shared( - PoseKey(cp.link->id(), slice.k), p_.cp_cost_model, cp.point, gravity); + PoseKey(cp.link->id(), slice.k), p_.cp_cost_model, cp.point, gravity, + ground_plane_height); } return graph; @@ -211,12 +212,13 @@ template <> NonlinearFactorGraph Kinematics::qFactors( const Slice& slice, const Robot& robot, const std::optional& contact_points, - const gtsam::Vector3& gravity) const { + const gtsam::Vector3& gravity, double ground_plane_height) const { NonlinearFactorGraph graph; graph.add(fixedLinkObjectives(slice, robot)); graph.add(this->graph(slice, robot)); if (contact_points) { - graph.add(contactHeightObjectives(slice, *contact_points, gravity)); + graph.add(contactHeightObjectives(slice, *contact_points, gravity, + ground_plane_height)); } return graph; } diff --git a/gtdynamics/utils/Trajectory.cpp b/gtdynamics/utils/Trajectory.cpp index 8226e80e..4c86894d 100644 --- a/gtdynamics/utils/Trajectory.cpp +++ b/gtdynamics/utils/Trajectory.cpp @@ -36,25 +36,30 @@ using std::vector; namespace gtdynamics { vector Trajectory::getTransitionGraphs( - const Robot &robot, const DynamicsGraph &graph_builder, double mu) const { + const Robot &robot, const DynamicsGraph &graph_builder, double mu, + double ground_plane_height) const { vector transition_graphs; const vector final_timesteps = finalTimeSteps(); const vector trans_cps = transitionContactPoints(); for (int p = 1; p < numPhases(); p++) { transition_graphs.push_back(graph_builder.dynamicsFactorGraph( - robot, final_timesteps[p - 1], trans_cps[p - 1], mu)); + robot, final_timesteps[p - 1], trans_cps[p - 1], mu, + ground_plane_height)); } return transition_graphs; } NonlinearFactorGraph Trajectory::multiPhaseFactorGraph( const Robot &robot, const DynamicsGraph &graph_builder, - const CollocationScheme collocation, double mu) const { + const CollocationScheme collocation, double mu, + double ground_plane_height) const { // Graphs for transition between phases + their initial values. - auto transition_graphs = getTransitionGraphs(robot, graph_builder, mu); + auto transition_graphs = + getTransitionGraphs(robot, graph_builder, mu, ground_plane_height); return graph_builder.multiPhaseTrajectoryFG(robot, phaseDurations(), transition_graphs, collocation, - phaseContactPoints(), mu); + phaseContactPoints(), mu, + ground_plane_height); } vector Trajectory::transitionPhaseInitialValues( diff --git a/gtdynamics/utils/Trajectory.h b/gtdynamics/utils/Trajectory.h index c8eb4831..2a421bc1 100644 --- a/gtdynamics/utils/Trajectory.h +++ b/gtdynamics/utils/Trajectory.h @@ -103,10 +103,12 @@ class Trajectory { * @param[in] robot Robot specification from URDF/SDF. * @param[in] graph_builder Dynamics Graph * @param[in] mu Coefficient of static friction + * @param[in] ground_plane_height Contact ground-plane height in world frame. * @return Vector of Transition Graphs */ std::vector getTransitionGraphs( - const Robot &robot, const DynamicsGraph &graph_builder, double mu) const; + const Robot &robot, const DynamicsGraph &graph_builder, double mu, + double ground_plane_height = 0.0) const; /** * @fn Builds multi-phase factor graph. @@ -114,11 +116,13 @@ class Trajectory { * @param[in] graph_builder GraphBuilder instance. * @param[in] collocation Which collocation scheme to use. * @param[in] mu Coefficient of static friction. + * @param[in] ground_plane_height Contact ground-plane height in world frame. * @return Multi-phase factor graph */ gtsam::NonlinearFactorGraph multiPhaseFactorGraph( const Robot &robot, const DynamicsGraph &graph_builder, - const CollocationScheme collocation, double mu) const; + const CollocationScheme collocation, double mu, + double ground_plane_height = 0.0) const; /** * @fn Returns Initial values for transition graphs. @@ -209,7 +213,7 @@ class Trajectory { * @param[in] robot Robot specification from URDF/SDF. * @param[in] cost_model Noise model * @param[in] step The 3D vector the foot moves in a step. - * @param[in] ground_height z-coordinate of ground in URDF/SDF rest config. + * @param[in] ground_height z-coordinate of ground plane in world frame. * @return All objective factors as a NonlinearFactorGraph */ gtsam::NonlinearFactorGraph contactPointObjectives( @@ -230,7 +234,7 @@ class Trajectory { /** * @fn Create objective factors for slice 0 and slice K. * - * Links at time step 0 are constrainted to their wTcom poses and + * Links at time step 0 are constrained to their wTcom poses and * zero twist, and zero twist and twist acceleration at last time step. * Joint angles velocities and accelerations are set to zero for all joints at * start *and* end. diff --git a/gtdynamics/utils/WalkCycle.cpp b/gtdynamics/utils/WalkCycle.cpp index d2464e7a..2f9ffe34 100644 --- a/gtdynamics/utils/WalkCycle.cpp +++ b/gtdynamics/utils/WalkCycle.cpp @@ -61,7 +61,6 @@ size_t WalkCycle::numTimeSteps() const { ContactPointGoals WalkCycle::initContactPointGoal(const Robot &robot, double ground_height) const { ContactPointGoals cp_goals; - const Point3 adjust(0, 0, -ground_height); // Go over all phases, and all contact points for (auto &&phase : phases_) { @@ -73,7 +72,10 @@ ContactPointGoals WalkCycle::initContactPointGoal(const Robot &robot, // If no goal set yet, add it here if (cp_goals.count(link_name) == 0) { LinkSharedPtr link = robot.link(link_name); - const Point3 foot_w = link->bMcom() * cp.point + adjust; + // Preserve nominal x/y and set contact z to the requested world + // ground height. + const Point3 foot_rest_w = link->bMcom() * cp.point; + const Point3 foot_w(foot_rest_w.x(), foot_rest_w.y(), ground_height); cp_goals.emplace(link_name, foot_w); } } diff --git a/gtdynamics/utils/WalkCycle.h b/gtdynamics/utils/WalkCycle.h index 5aaa3f5b..8cae40bc 100644 --- a/gtdynamics/utils/WalkCycle.h +++ b/gtdynamics/utils/WalkCycle.h @@ -134,10 +134,10 @@ class WalkCycle { /// Returns the number of time steps, summing over all phases. size_t numTimeSteps() const; - /** + /** * @fn Returns the initial contact point goal for every contact link. * @param[in] robot Robot specification from URDF/SDF. - * @param[in] ground_height z-coordinate of ground in URDF/SDF rest config. + * @param[in] ground_height z-coordinate of ground plane in world frame. * @return Map from link name to goal points. */ ContactPointGoals initContactPointGoal(const Robot &robot, @@ -148,7 +148,7 @@ class WalkCycle { * @param[in] robot Robot specification from URDF/SDF. * @param[in] cost_model Noise model * @param[in] step The 3D vector the foot moves in a step. - * @param[in] ground_height z-coordinate of ground in URDF/SDF rest config. + * @param[in] ground_height z-coordinate of ground plane in world frame. * @return All objective factors as a NonlinearFactorGraph */ gtsam::NonlinearFactorGraph contactPointObjectives( diff --git a/tests/testTrajectory.cpp b/tests/testTrajectory.cpp index 0a363fc8..f3b254b7 100644 --- a/tests/testTrajectory.cpp +++ b/tests/testTrajectory.cpp @@ -67,7 +67,7 @@ TEST(Trajectory, error) { auto cp_goals = walk_cycle.initContactPointGoal(robot, 0); EXPECT_LONGS_EQUAL(5, cp_goals.size()); // regression - EXPECT(gtsam::assert_equal(gtsam::Point3(-0.926417, 1.19512, 0.000151302), + EXPECT(gtsam::assert_equal(gtsam::Point3(-0.926417, 1.19512, 0.0), cp_goals["tarsus_2_L2"], 1e-5)); double gaussian_noise = 1e-5; diff --git a/tests/testWalkCycle.cpp b/tests/testWalkCycle.cpp index 5a447751..4c093063 100644 --- a/tests/testWalkCycle.cpp +++ b/tests/testWalkCycle.cpp @@ -92,14 +92,15 @@ TEST(WalkCycle, objectives) { EXPECT_LONGS_EQUAL(swing_links0.size(), 2); EXPECT_LONGS_EQUAL(swing_links1.size(), 2); - // Expected contact goal points. - Point3 goal_LH(-0.371306, 0.1575, 0); // LH - Point3 goal_LF(0.278694, 0.1575, 0); // LF - Point3 goal_RF(0.278694, -0.1575, 0); // RF - Point3 goal_RH(-0.371306, -0.1575, 0); // RH + // Expected contact goal points use world-frame ground height. + constexpr double ground_height = -0.191839; + Point3 goal_LH(-0.371306, 0.1575, ground_height); // LH + Point3 goal_LF(0.278694, 0.1575, ground_height); // LF + Point3 goal_RF(0.278694, -0.1575, ground_height); // RF + Point3 goal_RH(-0.371306, -0.1575, ground_height); // RH // Check initalization of contact goals. - auto cp_goals = walk_cycle.initContactPointGoal(robot, -0.191839); + auto cp_goals = walk_cycle.initContactPointGoal(robot, ground_height); EXPECT_LONGS_EQUAL(4, cp_goals.size()); EXPECT(gtsam::assert_equal(goal_LH, cp_goals["lower1"], 1e-6)); EXPECT(gtsam::assert_equal(goal_LF, cp_goals["lower0"], 1e-6)); From 93a958a26fc0456c5906aa84c79abb14417470da Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Feb 2026 23:25:56 -0500 Subject: [PATCH 07/11] Make test deliberately not 0 --- .../dynamics/tests/testDynamicsGraph.cpp | 59 ++++++++++++++----- .../tests/testContactHeightFactor.cpp | 23 ++++---- tests/testConstraintManifold.cpp | 4 +- tests/testTrajectory.cpp | 13 ++-- tests/testWalkCycle.cpp | 4 +- 5 files changed, 68 insertions(+), 35 deletions(-) diff --git a/gtdynamics/dynamics/tests/testDynamicsGraph.cpp b/gtdynamics/dynamics/tests/testDynamicsGraph.cpp index ba6f6296..fa90c39a 100644 --- a/gtdynamics/dynamics/tests/testDynamicsGraph.cpp +++ b/gtdynamics/dynamics/tests/testDynamicsGraph.cpp @@ -45,6 +45,16 @@ using gtsam::Values; using gtsam::Vector; using gtsam::Vector6; using std::vector; +static constexpr double kGroundHeight = 4.2; + +static gtsam::Pose3 poseWithContactHeight(const gtsam::Pose3& wTl, + const gtsam::Point3& comPc, + double ground_height) { + const double current_height = (wTl * comPc).z(); + const double delta_z = ground_height - current_height; + const gtsam::Point3 t = wTl.translation(); + return gtsam::Pose3(wTl.rotation(), gtsam::Point3(t.x(), t.y(), t.z() + delta_z)); +} // ============================ VALUES-STYLE ================================ @@ -102,7 +112,8 @@ TEST(dynamicsFactorGraph_FD, simple_urdf_eq_mass) { size_t t = 777; DynamicsGraph graph_builder(simple_urdf_eq_mass::gravity, simple_urdf_eq_mass::planar_axis); - auto graph = graph_builder.dynamicsFactorGraph(robot, t); + auto graph = graph_builder.dynamicsFactorGraph(robot, t, {}, {}, + kGroundHeight); // Create values with rest kinematics and unit torques Values known_values = zero_values(robot, t); @@ -157,7 +168,8 @@ TEST(dynamicsFactorGraph_FD, four_bar_linkage_pure) { graph_builder.opt().bv_cost_model); } - auto graph = graph_builder.dynamicsFactorGraph(robot, 0); + auto graph = graph_builder.dynamicsFactorGraph(robot, 0, {}, {}, + kGroundHeight); graph.add(prior_factors); Initializer initializer; @@ -172,7 +184,7 @@ TEST(dynamicsFactorGraph_FD, four_bar_linkage_pure) { // test the condition when we fix link "l1" robot = robot.fixLink("l1"); - graph = graph_builder.dynamicsFactorGraph(robot, 0); + graph = graph_builder.dynamicsFactorGraph(robot, 0, {}, {}, kGroundHeight); graph.add(prior_factors); gtsam::GaussNewtonOptimizer optimizer2(graph, init_values); @@ -200,7 +212,8 @@ TEST(dynamicsFactorGraph_FD, jumping_robot) { // build the dynamics factor graph DynamicsGraph graph_builder(jumping_robot::gravity, jumping_robot::planar_axis); - auto graph = graph_builder.dynamicsFactorGraph(robot, 0); + auto graph = graph_builder.dynamicsFactorGraph(robot, 0, {}, {}, + kGroundHeight); graph.add(graph_builder.forwardDynamicsPriors(robot, 0, known_values)); // test jumping robot FD @@ -334,7 +347,8 @@ TEST(dynamicsTrajectoryFG, simple_urdf_eq_mass) { // test Euler auto euler_graph = graph_builder.trajectoryFG(robot, num_steps, dt, - CollocationScheme::Euler); + CollocationScheme::Euler, {}, + {}, kGroundHeight); euler_graph.add( graph_builder.trajectoryFDPriors(robot, num_steps, known_values)); @@ -350,7 +364,8 @@ TEST(dynamicsTrajectoryFG, simple_urdf_eq_mass) { // test trapezoidal auto trapezoidal_graph = graph_builder.trajectoryFG( - robot, num_steps, dt, CollocationScheme::Trapezoidal); + robot, num_steps, dt, CollocationScheme::Trapezoidal, {}, {}, + kGroundHeight); trapezoidal_graph.add( graph_builder.trajectoryFDPriors(robot, num_steps, known_values)); @@ -366,7 +381,8 @@ TEST(dynamicsTrajectoryFG, simple_urdf_eq_mass) { // test the scenario with dt as a variable vector phase_steps{1, 1}; - auto transition_graph = graph_builder.dynamicsFactorGraph(robot, 1); + auto transition_graph = + graph_builder.dynamicsFactorGraph(robot, 1, {}, {}, kGroundHeight); vector transition_graphs{transition_graph}; double dt0 = 1; double dt1 = 2; @@ -380,7 +396,8 @@ TEST(dynamicsTrajectoryFG, simple_urdf_eq_mass) { // multi-phase Euler NonlinearFactorGraph mp_euler_graph = graph_builder.multiPhaseTrajectoryFG( - robot, phase_steps, transition_graphs, CollocationScheme::Euler); + robot, phase_steps, transition_graphs, CollocationScheme::Euler, {}, {}, + kGroundHeight); mp_euler_graph.add(mp_prior_graph); gtsam::GaussNewtonOptimizer optimizer_mpe(mp_euler_graph, init_values); Values mp_euler_result = optimizer_mpe.optimize(); @@ -400,7 +417,8 @@ TEST(dynamicsTrajectoryFG, simple_urdf_eq_mass) { // multi-phase Trapezoidal auto mp_trapezoidal_graph = graph_builder.multiPhaseTrajectoryFG( - robot, phase_steps, transition_graphs, CollocationScheme::Trapezoidal); + robot, phase_steps, transition_graphs, CollocationScheme::Trapezoidal, + {}, {}, kGroundHeight); mp_trapezoidal_graph.add(mp_prior_graph); gtsam::GaussNewtonOptimizer optimizer_mpt(mp_trapezoidal_graph, mp_euler_result); @@ -428,12 +446,14 @@ TEST(dynamicsFactorGraph_Contacts, dynamics_graph_simple_rr) { // Add some contact points. PointOnLinks contact_points; LinkSharedPtr l0 = robot.link("link_0"); - contact_points.emplace_back(l0, gtsam::Point3(0, 0, -0.1)); + const gtsam::Point3 contact_in_com(0, 0, -0.1); + contact_points.emplace_back(l0, contact_in_com); // Build the dynamics FG. gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, -9.8).finished(); DynamicsGraph graph_builder(gravity); - auto graph = graph_builder.dynamicsFactorGraph(robot, 0, contact_points, 1.0); + auto graph = graph_builder.dynamicsFactorGraph(robot, 0, contact_points, 1.0, + kGroundHeight); Values known_values = zero_values(robot, 0, true); // Compute inverse dynamics prior factors. @@ -441,7 +461,9 @@ TEST(dynamicsFactorGraph_Contacts, dynamics_graph_simple_rr) { graph_builder.inverseDynamicsPriors(robot, 0, known_values); // Specify pose and twist priors for one leg. - prior_factors.addPrior(PoseKey(l0->id(), 0), l0->bMcom(), + prior_factors.addPrior(PoseKey(l0->id(), 0), + poseWithContactHeight(l0->bMcom(), contact_in_com, + kGroundHeight), gtsam::noiseModel::Constrained::All(6)); prior_factors.addPrior(TwistKey(l0->id(), 0), gtsam::Z_6x1, gtsam::noiseModel::Constrained::All(6)); @@ -488,7 +510,8 @@ TEST(dynamicsFactorGraph_Contacts, dynamics_graph_biped) { // Build the dynamics FG. gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, -9.81).finished(); DynamicsGraph graph_builder(gravity); - auto graph = graph_builder.dynamicsFactorGraph(biped, 0, contact_points, 1.0); + auto graph = graph_builder.dynamicsFactorGraph(biped, 0, contact_points, 1.0, + kGroundHeight); // Compute inverse dynamics prior factors. // Inverse dynamics priors. We care about the torques. @@ -560,12 +583,14 @@ TEST(dynamicsFactorGraph_Contacts, dynamics_graph_simple_rrr) { // Add some contact points. PointOnLinks contact_points; LinkSharedPtr l0 = robot.link("link_0"); - contact_points.emplace_back(l0, gtsam::Point3(0, 0, -0.1)); + const gtsam::Point3 contact_in_com(0, 0, -0.1); + contact_points.emplace_back(l0, contact_in_com); // Build the dynamics FG. gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, -9.8).finished(); DynamicsGraph graph_builder(gravity); - auto graph = graph_builder.dynamicsFactorGraph(robot, 0, contact_points, 1.0); + auto graph = graph_builder.dynamicsFactorGraph(robot, 0, contact_points, 1.0, + kGroundHeight); // Compute inverse dynamics prior factors. gtsam::Values known_values = zero_values(robot, 0, true); @@ -573,7 +598,9 @@ TEST(dynamicsFactorGraph_Contacts, dynamics_graph_simple_rrr) { graph_builder.inverseDynamicsPriors(robot, 0, known_values); // Specify pose and twist priors for one leg. - prior_factors.addPrior(PoseKey(l0->id(), 0), l0->bMcom(), + prior_factors.addPrior(PoseKey(l0->id(), 0), + poseWithContactHeight(l0->bMcom(), contact_in_com, + kGroundHeight), gtsam::noiseModel::Constrained::All(6)); prior_factors.addPrior(TwistKey(l0->id(), 0), gtsam::Z_6x1, gtsam::noiseModel::Constrained::All(6)); diff --git a/gtdynamics/kinematics/tests/testContactHeightFactor.cpp b/gtdynamics/kinematics/tests/testContactHeightFactor.cpp index 71c43c12..857776d2 100644 --- a/gtdynamics/kinematics/tests/testContactHeightFactor.cpp +++ b/gtdynamics/kinematics/tests/testContactHeightFactor.cpp @@ -29,6 +29,7 @@ using namespace gtdynamics; using gtsam::assert_equal; using gtsam::Vector3, gtsam::Vector1, gtsam::Rot3, gtsam::Pose3, gtsam::Point3; +static constexpr double kGroundHeight = 4.2; /** * Test the unwhitenedError method with various link twists. @@ -42,21 +43,21 @@ TEST(ContactHeightFactor, Error) { // Transform from the robot com to the link end. Point3 comPc(0, 0, 1); ContactHeightFactor factor(pose_key, cost_model, comPc, - gtsam::Vector3(0, 0, -9.8), 0); + gtsam::Vector3(0, 0, -9.8), kGroundHeight); // Leg oriented upwards with contact away from the ground. gtsam::Values values1; - values1.insert(pose_key, Pose3(Rot3(), Point3(0., 0., 2.))); + values1.insert(pose_key, Pose3(Rot3(), Point3(0., 0., 6.2))); EXPECT(assert_equal(factor.unwhitenedError(values1), Vector1(3))); // Leg oriented down with contact 1m away from the ground. gtsam::Values values2; - values2.insert(pose_key, Pose3(Rot3::Rx(M_PI), Point3(0., 0., 2.))); + values2.insert(pose_key, Pose3(Rot3::Rx(M_PI), Point3(0., 0., 6.2))); EXPECT(assert_equal(factor.unwhitenedError(values2), Vector1(1))); // Contact touching the ground. gtsam::Values values3; - values3.insert(pose_key, Pose3(Rot3::Rx(M_PI), Point3(0., 0., 1.))); + values3.insert(pose_key, Pose3(Rot3::Rx(M_PI), Point3(0., 0., 5.2))); EXPECT(assert_equal(factor.unwhitenedError(values3), Vector1(0))); // Check that Jacobian computation is correct by comparison to finite @@ -93,23 +94,23 @@ TEST(ContactHeightFactor, ErrorWithHeight) { // Transform from the contact frame to the link com. Point3 comPc(0, 0, 1); - // Create a factor that establishes a ground plane at z = -1.0. + // Create a factor that establishes a ground plane at z = 4.2. ContactHeightFactor factor(pose_key, cost_model, comPc, - gtsam::Vector3(0, 0, -9.8), -1.0); + gtsam::Vector3(0, 0, -9.8), kGroundHeight); // Leg oriented upwards with contact away from the ground. gtsam::Values values1; - values1.insert(pose_key, Pose3(Rot3(), Point3(0., 0., 2.))); + values1.insert(pose_key, Pose3(Rot3(), Point3(0., 0., 7.2))); EXPECT(assert_equal(factor.unwhitenedError(values1), gtsam::Vector1(4))); // Leg oriented down with contact 1m away from the ground. gtsam::Values values2; - values2.insert(pose_key, Pose3(Rot3::Rx(M_PI), Point3(0., 0., 2.))); + values2.insert(pose_key, Pose3(Rot3::Rx(M_PI), Point3(0., 0., 7.2))); EXPECT(assert_equal(factor.unwhitenedError(values2), gtsam::Vector1(2))); - // Contact touching the ground. + // Contact 1m above the ground. gtsam::Values values3; - values3.insert(pose_key, Pose3(Rot3::Rx(M_PI), Point3(0., 0., 1.))); + values3.insert(pose_key, Pose3(Rot3::Rx(M_PI), Point3(0., 0., 6.2))); EXPECT(assert_equal(factor.unwhitenedError(values3), gtsam::Vector1(1))); // Check that Jacobian computation is correct by comparison to finite @@ -147,7 +148,7 @@ TEST(ContactHeightFactor, Optimization) { // Transform from the contact frame to the link com. Point3 comPc(0, 0, 1); ContactHeightFactor factor(pose_key, cost_model, comPc, - gtsam::Vector3(0, 0, -9.8)); + gtsam::Vector3(0, 0, -9.8), kGroundHeight); // Initial link pose. Pose3 link_pose_init = diff --git a/tests/testConstraintManifold.cpp b/tests/testConstraintManifold.cpp index b6af6533..364fab81 100644 --- a/tests/testConstraintManifold.cpp +++ b/tests/testConstraintManifold.cpp @@ -418,12 +418,14 @@ TEST(ConstraintManifold_retract, cart_pole_dynamics) { .fixLink("l0"); int j0_id = robot.joint("j0")->id(), j1_id = robot.joint("j1")->id(); const gtsam::Vector3 gravity(0, 0, -10); + constexpr double ground_plane_height = 4.2; OptimizerSetting opt; auto graph_builder = DynamicsGraph(opt, gravity); // constraints graph NonlinearFactorGraph constraints_graph; - constraints_graph.add(graph_builder.dynamicsFactorGraph(robot, 0)); + constraints_graph.add( + graph_builder.dynamicsFactorGraph(robot, 0, {}, {}, ground_plane_height)); // initial values Initializer initializer; diff --git a/tests/testTrajectory.cpp b/tests/testTrajectory.cpp index f3b254b7..ef65b5ca 100644 --- a/tests/testTrajectory.cpp +++ b/tests/testTrajectory.cpp @@ -29,6 +29,7 @@ auto kModel1 = gtsam::noiseModel::Unit::Create(1); auto kModel6 = gtsam::noiseModel::Unit::Create(6); using namespace gtdynamics; +static constexpr double kGroundHeight = 4.2; TEST(Trajectory, error) { using namespace walk_cycle_example; @@ -64,10 +65,10 @@ TEST(Trajectory, error) { EXPECT_LONGS_EQUAL(6, trajectory.getStartTimeStep(2)); EXPECT_LONGS_EQUAL(7, trajectory.getEndTimeStep(2)); - auto cp_goals = walk_cycle.initContactPointGoal(robot, 0); + auto cp_goals = walk_cycle.initContactPointGoal(robot, kGroundHeight); EXPECT_LONGS_EQUAL(5, cp_goals.size()); // regression - EXPECT(gtsam::assert_equal(gtsam::Point3(-0.926417, 1.19512, 0.0), + EXPECT(gtsam::assert_equal(gtsam::Point3(-0.926417, 1.19512, kGroundHeight), cp_goals["tarsus_2_L2"], 1e-5)); double gaussian_noise = 1e-5; @@ -84,14 +85,16 @@ TEST(Trajectory, error) { // Check transition graphs vector transition_graphs = - trajectory.getTransitionGraphs(robot, graph_builder, mu); + trajectory.getTransitionGraphs(robot, graph_builder, mu, + kGroundHeight); EXPECT_LONGS_EQUAL(repeat * 2 - 1, transition_graphs.size()); // regression test EXPECT_LONGS_EQUAL(203, transition_graphs[0].size()); // Test multi-phase factor graph. auto graph = trajectory.multiPhaseFactorGraph(robot, graph_builder, - CollocationScheme::Euler, mu); + CollocationScheme::Euler, mu, + kGroundHeight); // regression test EXPECT_LONGS_EQUAL(4298, graph.size()); EXPECT_LONGS_EQUAL(4712, graph.keys().size()); @@ -102,7 +105,7 @@ TEST(Trajectory, error) { // Test objectives for contact links. const Point3 step(0, 0.4, 0); auto contact_link_objectives = trajectory.contactPointObjectives( - robot, noiseModel::Isotropic::Sigma(3, 1e-7), step, 0); + robot, noiseModel::Isotropic::Sigma(3, 1e-7), step, kGroundHeight); // steps = 2+3 per walk cycle, 5 legs involved const size_t expected = repeat * ((2 + 3) * 5); EXPECT_LONGS_EQUAL(expected, contact_link_objectives.size()); diff --git a/tests/testWalkCycle.cpp b/tests/testWalkCycle.cpp index 4c093063..82fff0da 100644 --- a/tests/testWalkCycle.cpp +++ b/tests/testWalkCycle.cpp @@ -92,8 +92,8 @@ TEST(WalkCycle, objectives) { EXPECT_LONGS_EQUAL(swing_links0.size(), 2); EXPECT_LONGS_EQUAL(swing_links1.size(), 2); - // Expected contact goal points use world-frame ground height. - constexpr double ground_height = -0.191839; + // Use a non-zero ground height to exercise explicit wiring. + constexpr double ground_height = 4.2; Point3 goal_LH(-0.371306, 0.1575, ground_height); // LH Point3 goal_LF(0.278694, 0.1575, ground_height); // LF Point3 goal_RF(0.278694, -0.1575, ground_height); // RF From 6e116ad43dc4551cba29e6a3e33771bee8b217db Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Feb 2026 23:26:17 -0500 Subject: [PATCH 08/11] Motion planning now works --- .../chain_dynamics_motion_planning.ipynb | 230 ++++++++++-------- 1 file changed, 135 insertions(+), 95 deletions(-) diff --git a/examples/example_a1_walking/chain_dynamics_motion_planning.ipynb b/examples/example_a1_walking/chain_dynamics_motion_planning.ipynb index 3bd2cd3b..014da3f1 100644 --- a/examples/example_a1_walking/chain_dynamics_motion_planning.ipynb +++ b/examples/example_a1_walking/chain_dynamics_motion_planning.ipynb @@ -32,41 +32,16 @@ "metadata": {}, "outputs": [], "source": [ - "from pathlib import Path\n", - "import sys\n", "import numpy as np\n", "import pandas as pd\n", "import matplotlib.pyplot as plt\n", "\n", - "\n", - "def find_repo_root(start: Path) -> Path:\n", - " for candidate in [start, *start.parents]:\n", - " if (candidate / 'CMakeLists.txt').exists() and (candidate / 'examples' / 'example_a1_walking').exists():\n", - " return candidate\n", - " raise RuntimeError('Could not locate GTDynamics repository root from current directory.')\n", - "\n", - "\n", - "repo_root = find_repo_root(Path.cwd().resolve())\n", - "gtd_build_python = repo_root / 'build' / 'python'\n", - "gtsam_build_python = repo_root.parent / 'gtsam' / 'build' / 'python'\n", - "\n", - "if gtsam_build_python.exists() and str(gtsam_build_python) not in sys.path:\n", - " sys.path.insert(0, str(gtsam_build_python))\n", - "if str(gtd_build_python) not in sys.path:\n", - " sys.path.insert(0, str(gtd_build_python))\n", - "\n", "import gtsam\n", "import gtdynamics as gtd\n", + "import roboplot # type: ignore\n", "\n", - "print('repo_root :', repo_root)\n", - "print('gtsam module :', gtsam.__file__)\n", - "print('gtdynamics module:', gtd.__file__)\n", - "\n", - "if gtsam_build_python.exists() and str(gtsam_build_python) not in gtsam.__file__:\n", - " raise RuntimeError(\n", - " 'Loaded a non-local gtsam package. Put '\n", - " f\"{gtsam_build_python} first on PYTHONPATH before running this notebook.\"\n", - " )" + "CONTACT_IN_COM = gtsam.Point3(0.0, 0.0, -0.11)\n", + "GROUND_HEIGHT = 1.6" ] }, { @@ -91,17 +66,17 @@ "source": [ "def build_walk_trajectory(robot: gtd.Robot, repeat: int = 1) -> gtd.Trajectory:\n", " # Match getTrajectory() from C++: RRFL -> stationary -> RLFR -> stationary.\n", - " rlfr = [robot.link('RL_lower'), robot.link('FR_lower')]\n", - " rrfl = [robot.link('RR_lower'), robot.link('FL_lower')]\n", + " rlfr = [robot.link(\"RL_lower\"), robot.link(\"FR_lower\")]\n", + " rrfl = [robot.link(\"RR_lower\"), robot.link(\"FL_lower\")]\n", " all_feet = rlfr + rrfl\n", "\n", - " contact_in_com = gtsam.Point3(0.0, 0.0, -0.07)\n", - " stationary = gtd.FootContactConstraintSpec(all_feet, contact_in_com)\n", - " rlfr_state = gtd.FootContactConstraintSpec(rlfr, contact_in_com)\n", - " rrfl_state = gtd.FootContactConstraintSpec(rrfl, contact_in_com)\n", + " stationary = gtd.FootContactConstraintSpec(all_feet, CONTACT_IN_COM)\n", + " rlfr_state = gtd.FootContactConstraintSpec(rlfr, CONTACT_IN_COM)\n", + " rrfl_state = gtd.FootContactConstraintSpec(rrfl, CONTACT_IN_COM)\n", "\n", - " states = [rrfl_state, stationary, rlfr_state, stationary]\n", - " phase_lengths = [25, 5, 25, 5]\n", + " N = 5\n", + " states = [rrfl_state, stationary, rlfr_state, stationary] * N\n", + " phase_lengths = [25, 5, 25, 5] * N\n", "\n", " walk_cycle = gtd.WalkCycle(states, phase_lengths)\n", " return gtd.Trajectory(walk_cycle, repeat)" @@ -134,6 +109,16 @@ "outputs": [], "source": [ "robot = gtd.CreateRobotFromFile(gtd.URDF_PATH + \"/a1/a1.urdf\", \"a1\")\n", + "# robot.print()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "4d012b64", + "metadata": {}, + "outputs": [], + "source": [ "\n", "gravity = np.array([0.0, 0.0, -9.8])\n", "mu = 1.0\n", @@ -144,18 +129,9 @@ "trajectory = build_walk_trajectory(robot, repeat=1)\n", "collocation = gtd.CollocationScheme.Euler\n", "\n", - "graph = trajectory.multiPhaseFactorGraph(robot, graph_builder, collocation, mu)\n", - "print('Graph has', graph.size(), 'factors and', graph.keys().size(), 'variables.')" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "60f38e64", - "metadata": {}, - "outputs": [], - "source": [ - "graph.print(\"Graph:\\n\", gtd.GTDKeyFormatter)" + "graph = trajectory.multiPhaseFactorGraph(robot, graph_builder, collocation, mu, GROUND_HEIGHT)\n", + "print('Graph has', graph.size(), 'factors and', graph.keys().size(), 'variables.')\n", + "# graph.print(\"Graph:\\n\", gtd.GTDKeyFormatter)" ] }, { @@ -165,47 +141,42 @@ "metadata": {}, "outputs": [], "source": [ - "\n", - "ground_height = 1.0\n", "step = gtsam.Point3(0.25, 0.0, 0.0)\n", "objectives = trajectory.contactPointObjectives(\n", - " robot,\n", - " gtsam.noiseModel.Isotropic.Sigma(3, 1e-6),\n", - " step,\n", - " ground_height,\n", + " robot, gtsam.noiseModel.Isotropic.Sigma(3, 1e-6), step, GROUND_HEIGHT\n", ")\n", "\n", "K = trajectory.getEndTimeStep(trajectory.numPhases() - 1)\n", "\n", - "for link in robot.links():\n", - " i = link.id()\n", - " if i == 0:\n", - " objectives.push_back(\n", - " gtd.LinkObjectives(i, 0)\n", - " .pose(link.bMcom(), gtsam.noiseModel.Isotropic.Sigma(6, 1e-3))\n", - " .twist(np.zeros(6), gtsam.noiseModel.Isotropic.Sigma(6, 1e-3)),\n", - " )\n", - " if i in (3, 6, 9, 12):\n", - " objectives.push_back(\n", - " gtd.LinkObjectives(i, 0).pose(\n", - " link.bMcom(), gtsam.noiseModel.Isotropic.Sigma(6, 1e-3)\n", - " ),\n", - " )\n", + "# for link in robot.links():\n", + "# i = link.id()\n", + "# if i == 0:\n", + "# objectives.push_back(\n", + "# gtd.LinkObjectives(i, 0)\n", + "# .pose(link.bMcom(), gtsam.noiseModel.Isotropic.Sigma(6, 1e-3))\n", + "# .twist(np.zeros(6), gtsam.noiseModel.Isotropic.Sigma(6, 1e-3)),\n", + "# )\n", + "# if i in (3, 6, 9, 12):\n", + "# objectives.push_back(\n", + "# gtd.LinkObjectives(i, 0).pose(\n", + "# link.bMcom(), gtsam.noiseModel.Isotropic.Sigma(6, 1e-3)\n", + "# ),\n", + "# )\n", "\n", "rest_model = gtsam.noiseModel.Isotropic.Sigma(1, 1e-3)\n", "objectives.push_back(gtd.JointsAtRestObjectives(robot, rest_model, rest_model, 0))\n", "objectives.push_back(gtd.JointsAtRestObjectives(robot, rest_model, rest_model, K))\n", "\n", - "trunk = robot.link(\"trunk\")\n", - "for k in range(K + 1):\n", - " objectives.push_back(\n", - " gtd.LinkObjectives(trunk.id(), k)\n", - " .pose(\n", - " gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.0, 0.0, 0.4)),\n", - " gtsam.noiseModel.Isotropic.Sigma(6, 1e-2),\n", - " )\n", - " .twist(np.zeros(6), gtsam.noiseModel.Isotropic.Sigma(6, 5e-2)),\n", - " )\n", + "# trunk = robot.link(\"trunk\")\n", + "# for k in range(K + 1):\n", + "# objectives.push_back(\n", + "# gtd.LinkObjectives(trunk.id(), k)\n", + "# .pose(\n", + "# gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.0, 0.0, GROUND_HEIGHT + 0.4)),\n", + "# gtsam.noiseModel.Isotropic.Sigma(6, 1e-3),\n", + "# )\n", + "# .twist(np.zeros(6), gtsam.noiseModel.Isotropic.Sigma(6, 5e-2)),\n", + "# )\n", "\n", "desired_dt = 1.0 / 20.0\n", "trajectory.addIntegrationTimeFactors(objectives, desired_dt, 1e-30)\n", @@ -260,6 +231,7 @@ "init_values = trajectory.multiPhaseInitialValues(robot, initializer, gaussian_noise, desired_dt)\n", "\n", "print('Initial values:', init_values.size())\n", + "print('Final objective error:', graph.error(init_values))\n", "\n", "params = gtsam.LevenbergMarquardtParams()\n", "params.setlambdaInitial(1e10)\n", @@ -309,9 +281,9 @@ " j = joint.id()\n", " name = joint.name()\n", " row[name] = read_or_nan(result, gtd.JointAngleKey(j, k))\n", - " row[f'{name}.1'] = read_or_nan(result, gtd.JointVelKey(j, k))\n", - " row[f'{name}.2'] = read_or_nan(result, gtd.JointAccelKey(j, k))\n", - " row[f'{name}.3'] = read_or_nan(result, gtd.TorqueKey(j, k))\n", + " # row[f'{name}.1'] = read_or_nan(result, gtd.JointVelKey(j, k))\n", + " # row[f'{name}.2'] = read_or_nan(result, gtd.JointAccelKey(j, k))\n", + " # row[f'{name}.3'] = read_or_nan(result, gtd.TorqueKey(j, k))\n", "\n", " rows.append(row)\n", "\n", @@ -339,10 +311,11 @@ "metadata": {}, "outputs": [], "source": [ - "out_dir = repo_root / 'build' / 'examples' / 'example_a1_walking'\n", + "from pathlib import Path\n", + "out_dir = Path(gtd.DATA_PATH) / 'example_a1_walking'\n", "out_dir.mkdir(parents=True, exist_ok=True)\n", "\n", - "csv_table = out_dir / 'a1_traj_chain_dynamics_graph_python.csv'\n", + "csv_table = out_dir / 'traj_df.csv'\n", "traj_df.to_csv(csv_table, index=False)\n", "print('Wrote table CSV:', csv_table)\n", "\n", @@ -362,9 +335,9 @@ " if not export_values.exists(pk):\n", " export_values.insert(pk, desired_dt)\n", "\n", - "native_name = 'a1_traj_CDG_python.csv'\n", - "trajectory.writeToFile(robot, native_name, export_values)\n", - "print('Wrote native GTDynamics CSV in current working directory:', native_name)\n" + "native_name = out_dir / 'a1_traj_CDG.csv'\n", + "trajectory.writeToFile(robot, str(native_name), export_values)\n", + "print('Wrote native GTDynamics CSV to:', native_name)\n" ] }, { @@ -372,11 +345,43 @@ "id": "7ff53c69", "metadata": {}, "source": [ - "## 7) Plot joint trajectories and contact-point heights\n", + "## 7) Plot base pose, joint trajectories, and contact-point heights\n", + "\n", + "We use `roboplot` to visualize the trunk/base pose trajectory (position + orientation frames) over time.\n", "\n", "The second figure shows foot contact-point heights (world Z) for all four feet, which helps check alternating stance/swing behavior." ] }, + { + "cell_type": "code", + "execution_count": null, + "id": "6e0f8d47", + "metadata": {}, + "outputs": [], + "source": [ + "trunk_id = robot.link('trunk').id()\n", + "base_poses = gtsam.Values()\n", + "for k in range(K + 1):\n", + " base_poses.insert(k, gtd.Pose(result, trunk_id, k))\n", + "roboplot.plot_trajectory(base_poses, fignum=1, scale=0.08, show=True);" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "a9429bc0", + "metadata": {}, + "outputs": [], + "source": [ + "fig, ax = plt.subplots(nrows=3,figsize=(12, 4))\n", + "times = traj_df['t'].to_numpy()\n", + "ax[0].plot(times, traj_df['trunk_x'], label='Base', linewidth=1.5)\n", + "ax[1].plot(times, traj_df['trunk_y'], label='Base', linewidth=1.5)\n", + "ax[2].plot(times, traj_df['trunk_z'], label='Base', linewidth=1.5)\n", + "plt.tight_layout()\n", + "plt.show()" + ] + }, { "cell_type": "code", "execution_count": null, @@ -396,7 +401,7 @@ " for joint in joint_order:\n", " col = f'{leg}_{joint}_joint{suffix}'\n", " if col in traj_df.columns:\n", - " ax.plot(traj_df['t'], traj_df[col], label=joint, color=colors[joint], linewidth=1.4)\n", + " ax.plot(times, traj_df[col], label=joint, color=colors[joint], linewidth=1.4)\n", " ax.grid(alpha=0.3)\n", " ax.set_ylabel(f'{leg} {y_label}')\n", " ax.legend(loc='upper right', ncol=3)\n", @@ -407,23 +412,58 @@ "\n", "\n", "plot_joint_group('', 'Joint Angles (ChainDynamicsGraph walk cycle)', 'q')\n", - "plot_joint_group('.1', 'Joint Velocities (ChainDynamicsGraph walk cycle)', 'qdot')\n", + "# plot_joint_group('.3', 'Joint Torques (ChainDynamicsGraph walk cycle)', 'tau')\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "d2638128", + "metadata": {}, + "outputs": [], + "source": [ + "foot_links = ['RL_lower', 'RR_lower']\n", "\n", - "contact_in_com = gtsam.Point3(0.0, 0.0, -0.07)\n", - "foot_links = ['FL_lower', 'FR_lower', 'RL_lower', 'RR_lower']\n", + "foot_z = {}\n", + "for name in foot_links:\n", + " cp = gtd.PointOnLink(robot.link(name).shared(), CONTACT_IN_COM)\n", + " foot_z[name] = np.array([cp.predict(result, k)[2] for k in range(K + 1)])\n", + "\n", + "fig, ax = plt.subplots(figsize=(12, 4))\n", + "ax.plot(times, np.array(traj_df['trunk_z']), label='Base', linewidth=1.5)\n", + "for name in foot_links:\n", + " ax.plot(times, foot_z[name], label=name, linewidth=1.5)\n", + "ax.grid(alpha=0.3)\n", + "ax.set_xlabel('time [s]')\n", + "ax.set_ylabel('contact point z [m]')\n", + "ax.set_title('REAR feet contact-point heights in world frame')\n", + "ax.legend(ncol=4)\n", + "plt.tight_layout()\n", + "plt.show()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "6896ee55", + "metadata": {}, + "outputs": [], + "source": [ + "foot_links = ['FL_lower', 'FR_lower']\n", "\n", - "foot_z = {'t': traj_df['t'].to_numpy()}\n", + "foot_z = {}\n", "for name in foot_links:\n", - " cp = gtd.PointOnLink(robot.link(name).shared(), contact_in_com)\n", + " cp = gtd.PointOnLink(robot.link(name).shared(), CONTACT_IN_COM)\n", " foot_z[name] = np.array([cp.predict(result, k)[2] for k in range(K + 1)])\n", "\n", "fig, ax = plt.subplots(figsize=(12, 4))\n", + "ax.plot(times, np.array(traj_df['trunk_z']), label='Base', linewidth=1.5)\n", "for name in foot_links:\n", - " ax.plot(foot_z['t'], foot_z[name], label=name, linewidth=1.5)\n", + " ax.plot(times, foot_z[name], label=name, linewidth=1.5)\n", "ax.grid(alpha=0.3)\n", "ax.set_xlabel('time [s]')\n", "ax.set_ylabel('contact point z [m]')\n", - "ax.set_title('Foot contact-point heights in world frame')\n", + "ax.set_title('FRONT feet contact-point heights in world frame')\n", "ax.legend(ncol=4)\n", "plt.tight_layout()\n", "plt.show()" From 81153b74c991a5f04ae986c89d4c4537f80a44c8 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Mon, 16 Feb 2026 14:37:58 +0000 Subject: [PATCH 09/11] Initial plan From fabb3472ce32f6f83779eb43f822503ddc52bb73 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Mon, 16 Feb 2026 15:23:16 +0000 Subject: [PATCH 10/11] Keep only python stubs infrastructure changes Co-authored-by: dellaert <10515273+dellaert@users.noreply.github.com> --- .gitignore | 5 - .../chain_dynamics_motion_planning.ipynb | 520 ------------------ examples/example_a1_walking/sim.py | 2 +- examples/example_quadruped_mp/main.cpp | 321 ++++++----- examples/example_quadruped_mp/sim.py | 8 +- gtdynamics.i | 100 +--- gtdynamics/dynamics/Chain.cpp | 20 +- gtdynamics/dynamics/Chain.h | 19 +- gtdynamics/dynamics/ChainDynamicsGraph.cpp | 44 +- gtdynamics/dynamics/ChainDynamicsGraph.h | 6 +- gtdynamics/dynamics/DynamicsGraph.cpp | 26 +- gtdynamics/dynamics/DynamicsGraph.h | 15 +- .../dynamics/tests/testDynamicsGraph.cpp | 59 +- gtdynamics/kinematics/ContactHeightFactor.h | 2 +- gtdynamics/kinematics/Kinematics.h | 8 +- gtdynamics/kinematics/KinematicsInterval.cpp | 4 +- gtdynamics/kinematics/KinematicsPhase.cpp | 4 +- gtdynamics/kinematics/KinematicsSlice.cpp | 10 +- .../tests/testContactHeightFactor.cpp | 23 +- gtdynamics/utils/Trajectory.cpp | 15 +- gtdynamics/utils/Trajectory.h | 12 +- gtdynamics/utils/WalkCycle.cpp | 6 +- gtdynamics/utils/WalkCycle.h | 6 +- python/gtdynamics/preamble/gtdynamics.h | 4 - python/tests/test_dynamics_graph.py | 14 - tests/testConstraintManifold.cpp | 4 +- tests/testTrajectory.cpp | 13 +- tests/testWalkCycle.cpp | 13 +- webdemo/.gitignore | 49 -- 29 files changed, 293 insertions(+), 1039 deletions(-) delete mode 100644 examples/example_a1_walking/chain_dynamics_motion_planning.ipynb delete mode 100644 webdemo/.gitignore diff --git a/.gitignore b/.gitignore index 38a01352..d9ee594a 100644 --- a/.gitignore +++ b/.gitignore @@ -122,8 +122,3 @@ traj.csv results/ _codeql_detected_source_root - -# Generated by scripts/bootstrap_mujoco_models.py -models/mujoco/unitree_a1 -models/mujoco/unitree_g1 -models/mujoco/boston_dynamics_spot diff --git a/examples/example_a1_walking/chain_dynamics_motion_planning.ipynb b/examples/example_a1_walking/chain_dynamics_motion_planning.ipynb deleted file mode 100644 index 014da3f1..00000000 --- a/examples/example_a1_walking/chain_dynamics_motion_planning.ipynb +++ /dev/null @@ -1,520 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "ddab226b", - "metadata": {}, - "source": [ - "# A1 Walk-Cycle Motion Planning with `ChainDynamicsGraph` (Python)\n", - "\n", - "This notebook mirrors the C++ `newGraph()` pipeline in [main.cpp](main.cpp).\n", - "\n", - "The goal is to build the same multi-phase quadruped trajectory optimization with chain dynamics only, then inspect and plot the optimized motion." - ] - }, - { - "cell_type": "markdown", - "id": "9335499c", - "metadata": {}, - "source": [ - "## 1) Environment and Imports\n", - "\n", - "Two details matter for this notebook:\n", - "\n", - "1. We must import the local GTDynamics Python wrapper from `build/python`.\n", - "2. `gtsam` must come from the same build/install family as GTDynamics. Mixing with an unrelated pip/conda `gtsam` wheel can cause runtime aborts when factor graphs are modified." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "c5ecb7b3", - "metadata": {}, - "outputs": [], - "source": [ - "import numpy as np\n", - "import pandas as pd\n", - "import matplotlib.pyplot as plt\n", - "\n", - "import gtsam\n", - "import gtdynamics as gtd\n", - "import roboplot # type: ignore\n", - "\n", - "CONTACT_IN_COM = gtsam.Point3(0.0, 0.0, -0.11)\n", - "GROUND_HEIGHT = 1.6" - ] - }, - { - "cell_type": "markdown", - "id": "a1953262", - "metadata": {}, - "source": [ - "## 2) Helper Functions (C++ `newGraph()` equivalents)\n", - "\n", - "These helpers correspond to pieces in `main.cpp`:\n", - "\n", - "- `build_walk_trajectory(...)` mirrors `getTrajectory(...)`.\n", - "- `add_subgraph(...)` is needed because Python exposes graph `add(...)` for single factors, while many GTDynamics objective helpers return a small `NonlinearFactorGraph`." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "265b6743", - "metadata": {}, - "outputs": [], - "source": [ - "def build_walk_trajectory(robot: gtd.Robot, repeat: int = 1) -> gtd.Trajectory:\n", - " # Match getTrajectory() from C++: RRFL -> stationary -> RLFR -> stationary.\n", - " rlfr = [robot.link(\"RL_lower\"), robot.link(\"FR_lower\")]\n", - " rrfl = [robot.link(\"RR_lower\"), robot.link(\"FL_lower\")]\n", - " all_feet = rlfr + rrfl\n", - "\n", - " stationary = gtd.FootContactConstraintSpec(all_feet, CONTACT_IN_COM)\n", - " rlfr_state = gtd.FootContactConstraintSpec(rlfr, CONTACT_IN_COM)\n", - " rrfl_state = gtd.FootContactConstraintSpec(rrfl, CONTACT_IN_COM)\n", - "\n", - " N = 5\n", - " states = [rrfl_state, stationary, rlfr_state, stationary] * N\n", - " phase_lengths = [25, 5, 25, 5] * N\n", - "\n", - " walk_cycle = gtd.WalkCycle(states, phase_lengths)\n", - " return gtd.Trajectory(walk_cycle, repeat)" - ] - }, - { - "cell_type": "markdown", - "id": "392b2442", - "metadata": {}, - "source": [ - "## 3) Build the Chain-Dynamics Graph and Objectives\n", - "\n", - "This cell follows the structure of `newGraph()` in C++:\n", - "\n", - "1. Load A1.\n", - "2. Build `ChainDynamicsGraph`.\n", - "3. Build multi-phase dynamics graph from the walk trajectory.\n", - "4. Add contact-point objectives.\n", - "5. Add chain-specific boundary objectives.\n", - "6. Add base pose/twist objectives for all timesteps.\n", - "7. Constrain integration time (`dt = 1/20`).\n", - "8. Add joint angle priors (`lower`, `hip`, `upper`)." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "f6a2d434", - "metadata": {}, - "outputs": [], - "source": [ - "robot = gtd.CreateRobotFromFile(gtd.URDF_PATH + \"/a1/a1.urdf\", \"a1\")\n", - "# robot.print()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "4d012b64", - "metadata": {}, - "outputs": [], - "source": [ - "\n", - "gravity = np.array([0.0, 0.0, -9.8])\n", - "mu = 1.0\n", - "\n", - "opt = gtd.OptimizerSetting()\n", - "graph_builder = gtd.ChainDynamicsGraph(robot, opt, gravity)\n", - "\n", - "trajectory = build_walk_trajectory(robot, repeat=1)\n", - "collocation = gtd.CollocationScheme.Euler\n", - "\n", - "graph = trajectory.multiPhaseFactorGraph(robot, graph_builder, collocation, mu, GROUND_HEIGHT)\n", - "print('Graph has', graph.size(), 'factors and', graph.keys().size(), 'variables.')\n", - "# graph.print(\"Graph:\\n\", gtd.GTDKeyFormatter)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "5270bfa7", - "metadata": {}, - "outputs": [], - "source": [ - "step = gtsam.Point3(0.25, 0.0, 0.0)\n", - "objectives = trajectory.contactPointObjectives(\n", - " robot, gtsam.noiseModel.Isotropic.Sigma(3, 1e-6), step, GROUND_HEIGHT\n", - ")\n", - "\n", - "K = trajectory.getEndTimeStep(trajectory.numPhases() - 1)\n", - "\n", - "# for link in robot.links():\n", - "# i = link.id()\n", - "# if i == 0:\n", - "# objectives.push_back(\n", - "# gtd.LinkObjectives(i, 0)\n", - "# .pose(link.bMcom(), gtsam.noiseModel.Isotropic.Sigma(6, 1e-3))\n", - "# .twist(np.zeros(6), gtsam.noiseModel.Isotropic.Sigma(6, 1e-3)),\n", - "# )\n", - "# if i in (3, 6, 9, 12):\n", - "# objectives.push_back(\n", - "# gtd.LinkObjectives(i, 0).pose(\n", - "# link.bMcom(), gtsam.noiseModel.Isotropic.Sigma(6, 1e-3)\n", - "# ),\n", - "# )\n", - "\n", - "rest_model = gtsam.noiseModel.Isotropic.Sigma(1, 1e-3)\n", - "objectives.push_back(gtd.JointsAtRestObjectives(robot, rest_model, rest_model, 0))\n", - "objectives.push_back(gtd.JointsAtRestObjectives(robot, rest_model, rest_model, K))\n", - "\n", - "# trunk = robot.link(\"trunk\")\n", - "# for k in range(K + 1):\n", - "# objectives.push_back(\n", - "# gtd.LinkObjectives(trunk.id(), k)\n", - "# .pose(\n", - "# gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.0, 0.0, GROUND_HEIGHT + 0.4)),\n", - "# gtsam.noiseModel.Isotropic.Sigma(6, 1e-3),\n", - "# )\n", - "# .twist(np.zeros(6), gtsam.noiseModel.Isotropic.Sigma(6, 5e-2)),\n", - "# )\n", - "\n", - "desired_dt = 1.0 / 20.0\n", - "trajectory.addIntegrationTimeFactors(objectives, desired_dt, 1e-30)\n", - "\n", - "prior_model_angles = gtsam.noiseModel.Isotropic.Sigma(1, 1e-2)\n", - "prior_model_hip = gtsam.noiseModel.Isotropic.Sigma(1, 1e-2)\n", - "\n", - "for joint in robot.joints():\n", - " name = joint.name()\n", - " jid = joint.id()\n", - " for k in range(K + 1):\n", - " if \"lower\" in name:\n", - " objectives.push_back(\n", - " gtd.JointObjectives(jid, k).angle(-1.4, prior_model_angles)\n", - " )\n", - " if \"hip\" in name:\n", - " objectives.push_back(\n", - " gtd.JointObjectives(jid, k).angle(0.0, prior_model_hip)\n", - " )\n", - " if \"upper\" in name:\n", - " objectives.push_back(\n", - " gtd.JointObjectives(jid, k).angle(0.7, prior_model_angles)\n", - " )\n", - "\n", - "graph.push_back(objectives)\n", - "\n", - "print(\"Num phases :\", trajectory.numPhases())\n", - "print(\"Final timestep K :\", K)\n", - "print(\"Graph factors :\", graph.size())\n", - "print(\"Graph variable keys :\", graph.keys().size())" - ] - }, - { - "cell_type": "markdown", - "id": "b9b56d98", - "metadata": {}, - "source": [ - "## 4) Initialize with `ChainInitializer` and Optimize\n", - "\n", - "This mirrors the C++ initialization and Levenberg-Marquardt setup." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "648f6d08", - "metadata": {}, - "outputs": [], - "source": [ - "gaussian_noise = 1e-30\n", - "initializer = gtd.ChainInitializer()\n", - "init_values = trajectory.multiPhaseInitialValues(robot, initializer, gaussian_noise, desired_dt)\n", - "\n", - "print('Initial values:', init_values.size())\n", - "print('Final objective error:', graph.error(init_values))\n", - "\n", - "params = gtsam.LevenbergMarquardtParams()\n", - "params.setlambdaInitial(1e10)\n", - "params.setlambdaLowerBound(1e-7)\n", - "params.setlambdaUpperBound(1e10)\n", - "params.setAbsoluteErrorTol(1.0)\n", - "\n", - "optimizer = gtsam.LevenbergMarquardtOptimizer(graph, init_values, params)\n", - "result = optimizer.optimize()\n", - "\n", - "print('Final objective error:', graph.error(result))" - ] - }, - { - "cell_type": "markdown", - "id": "b0444d54", - "metadata": {}, - "source": [ - "## 5) Convert the optimized `Values` to a table\n", - "\n", - "We record joint angle/velocity/acceleration/torque over time. We also track trunk position to visualize forward progression." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "0ab62e4c", - "metadata": {}, - "outputs": [], - "source": [ - "def read_or_nan(values: gtsam.Values, key: int) -> float:\n", - " return values.atDouble(key) if values.exists(key) else np.nan\n", - "\n", - "rows = []\n", - "joints = list(robot.joints())\n", - "trunk_id = robot.link('trunk').id()\n", - "\n", - "for k in range(K + 1):\n", - " row = {'t': k * desired_dt}\n", - "\n", - " pose_k = gtd.Pose(result, trunk_id, k)\n", - " row['trunk_x'] = pose_k.translation()[0]\n", - " row['trunk_y'] = pose_k.translation()[1]\n", - " row['trunk_z'] = pose_k.translation()[2]\n", - "\n", - " for joint in joints:\n", - " j = joint.id()\n", - " name = joint.name()\n", - " row[name] = read_or_nan(result, gtd.JointAngleKey(j, k))\n", - " # row[f'{name}.1'] = read_or_nan(result, gtd.JointVelKey(j, k))\n", - " # row[f'{name}.2'] = read_or_nan(result, gtd.JointAccelKey(j, k))\n", - " # row[f'{name}.3'] = read_or_nan(result, gtd.TorqueKey(j, k))\n", - "\n", - " rows.append(row)\n", - "\n", - "traj_df = pd.DataFrame(rows)\n", - "traj_df.head(3)" - ] - }, - { - "cell_type": "markdown", - "id": "cdd62425", - "metadata": {}, - "source": [ - "## 6) Save trajectory CSV\n", - "\n", - "We save both:\n", - "\n", - "- A pandas-friendly table with extra columns (for analysis/plotting).\n", - "- The native GTDynamics CSV format via `trajectory.writeToFile(...)`." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "35e015c9", - "metadata": {}, - "outputs": [], - "source": [ - "from pathlib import Path\n", - "out_dir = Path(gtd.DATA_PATH) / 'example_a1_walking'\n", - "out_dir.mkdir(parents=True, exist_ok=True)\n", - "\n", - "csv_table = out_dir / 'traj_df.csv'\n", - "traj_df.to_csv(csv_table, index=False)\n", - "print('Wrote table CSV:', csv_table)\n", - "\n", - "# ChainInitializer does not necessarily populate every key that writeToFile()\n", - "# expects (notably torque keys). Create an export copy and fill missing entries.\n", - "export_values = gtsam.Values(result)\n", - "\n", - "for joint in robot.joints():\n", - " j = joint.id()\n", - " for k in range(K + 1):\n", - " tk = gtd.TorqueKey(j, k)\n", - " if not export_values.exists(tk):\n", - " export_values.insert(tk, 0.0)\n", - "\n", - "for pidx in range(trajectory.numPhases()):\n", - " pk = gtd.PhaseKey(pidx)\n", - " if not export_values.exists(pk):\n", - " export_values.insert(pk, desired_dt)\n", - "\n", - "native_name = out_dir / 'a1_traj_CDG.csv'\n", - "trajectory.writeToFile(robot, str(native_name), export_values)\n", - "print('Wrote native GTDynamics CSV to:', native_name)\n" - ] - }, - { - "cell_type": "markdown", - "id": "7ff53c69", - "metadata": {}, - "source": [ - "## 7) Plot base pose, joint trajectories, and contact-point heights\n", - "\n", - "We use `roboplot` to visualize the trunk/base pose trajectory (position + orientation frames) over time.\n", - "\n", - "The second figure shows foot contact-point heights (world Z) for all four feet, which helps check alternating stance/swing behavior." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "6e0f8d47", - "metadata": {}, - "outputs": [], - "source": [ - "trunk_id = robot.link('trunk').id()\n", - "base_poses = gtsam.Values()\n", - "for k in range(K + 1):\n", - " base_poses.insert(k, gtd.Pose(result, trunk_id, k))\n", - "roboplot.plot_trajectory(base_poses, fignum=1, scale=0.08, show=True);" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "a9429bc0", - "metadata": {}, - "outputs": [], - "source": [ - "fig, ax = plt.subplots(nrows=3,figsize=(12, 4))\n", - "times = traj_df['t'].to_numpy()\n", - "ax[0].plot(times, traj_df['trunk_x'], label='Base', linewidth=1.5)\n", - "ax[1].plot(times, traj_df['trunk_y'], label='Base', linewidth=1.5)\n", - "ax[2].plot(times, traj_df['trunk_z'], label='Base', linewidth=1.5)\n", - "plt.tight_layout()\n", - "plt.show()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "1d644dbd", - "metadata": {}, - "outputs": [], - "source": [ - "legs = ['FL', 'FR', 'RL', 'RR']\n", - "joint_order = ['hip', 'upper', 'lower']\n", - "colors = {'hip': 'tab:blue', 'upper': 'tab:orange', 'lower': 'tab:green'}\n", - "\n", - "\n", - "def plot_joint_group(suffix: str, title: str, y_label: str):\n", - " fig, axs = plt.subplots(4, 1, figsize=(12, 11), sharex=True)\n", - " for i, leg in enumerate(legs):\n", - " ax = axs[i]\n", - " for joint in joint_order:\n", - " col = f'{leg}_{joint}_joint{suffix}'\n", - " if col in traj_df.columns:\n", - " ax.plot(times, traj_df[col], label=joint, color=colors[joint], linewidth=1.4)\n", - " ax.grid(alpha=0.3)\n", - " ax.set_ylabel(f'{leg} {y_label}')\n", - " ax.legend(loc='upper right', ncol=3)\n", - " axs[-1].set_xlabel('time [s]')\n", - " fig.suptitle(title)\n", - " fig.tight_layout()\n", - " plt.show()\n", - "\n", - "\n", - "plot_joint_group('', 'Joint Angles (ChainDynamicsGraph walk cycle)', 'q')\n", - "# plot_joint_group('.3', 'Joint Torques (ChainDynamicsGraph walk cycle)', 'tau')\n" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "d2638128", - "metadata": {}, - "outputs": [], - "source": [ - "foot_links = ['RL_lower', 'RR_lower']\n", - "\n", - "foot_z = {}\n", - "for name in foot_links:\n", - " cp = gtd.PointOnLink(robot.link(name).shared(), CONTACT_IN_COM)\n", - " foot_z[name] = np.array([cp.predict(result, k)[2] for k in range(K + 1)])\n", - "\n", - "fig, ax = plt.subplots(figsize=(12, 4))\n", - "ax.plot(times, np.array(traj_df['trunk_z']), label='Base', linewidth=1.5)\n", - "for name in foot_links:\n", - " ax.plot(times, foot_z[name], label=name, linewidth=1.5)\n", - "ax.grid(alpha=0.3)\n", - "ax.set_xlabel('time [s]')\n", - "ax.set_ylabel('contact point z [m]')\n", - "ax.set_title('REAR feet contact-point heights in world frame')\n", - "ax.legend(ncol=4)\n", - "plt.tight_layout()\n", - "plt.show()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "6896ee55", - "metadata": {}, - "outputs": [], - "source": [ - "foot_links = ['FL_lower', 'FR_lower']\n", - "\n", - "foot_z = {}\n", - "for name in foot_links:\n", - " cp = gtd.PointOnLink(robot.link(name).shared(), CONTACT_IN_COM)\n", - " foot_z[name] = np.array([cp.predict(result, k)[2] for k in range(K + 1)])\n", - "\n", - "fig, ax = plt.subplots(figsize=(12, 4))\n", - "ax.plot(times, np.array(traj_df['trunk_z']), label='Base', linewidth=1.5)\n", - "for name in foot_links:\n", - " ax.plot(times, foot_z[name], label=name, linewidth=1.5)\n", - "ax.grid(alpha=0.3)\n", - "ax.set_xlabel('time [s]')\n", - "ax.set_ylabel('contact point z [m]')\n", - "ax.set_title('FRONT feet contact-point heights in world frame')\n", - "ax.legend(ncol=4)\n", - "plt.tight_layout()\n", - "plt.show()" - ] - }, - { - "cell_type": "markdown", - "id": "8a059a8b", - "metadata": {}, - "source": [ - "## 8) Quick diagnostics" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "48b7e79f", - "metadata": {}, - "outputs": [], - "source": [ - "angle_cols = [c for c in traj_df.columns if c not in ['t', 'trunk_x', 'trunk_y', 'trunk_z'] and not c.endswith('.1') and not c.endswith('.2') and not c.endswith('.3')]\n", - "vel_cols = [c for c in traj_df.columns if c.endswith('.1')]\n", - "acc_cols = [c for c in traj_df.columns if c.endswith('.2')]\n", - "\n", - "print('Finite ratio (angles):', np.isfinite(traj_df[angle_cols].to_numpy()).mean())\n", - "print('Finite ratio (vels) :', np.isfinite(traj_df[vel_cols].to_numpy()).mean())\n", - "print('Finite ratio (accels):', np.isfinite(traj_df[acc_cols].to_numpy()).mean())\n", - "print('Trunk z mean/std :', traj_df['trunk_z'].mean(), traj_df['trunk_z'].std())\n", - "print('Trunk x start/end :', traj_df['trunk_x'].iloc[0], traj_df['trunk_x'].iloc[-1])" - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "py312", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.12.6" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/examples/example_a1_walking/sim.py b/examples/example_a1_walking/sim.py index 4e4d36ef..92335ffa 100644 --- a/examples/example_a1_walking/sim.py +++ b/examples/example_a1_walking/sim.py @@ -176,7 +176,7 @@ torques[:,1] = np.array(all_torques_hip) torques[:,2] = np.array(all_torques_upper) torques[:,3] = np.array(all_torques_lower) -np.savetxt('torques_pyb.txt', torques) +np.savetxt('/home/dan/Desktop/Projects/GTDynamics/build/examples/example_a1_walking/torques_pyb.txt', torques) pos, orn = p.getBasePositionAndOrientation(robot_id) print("Final Base\n\tPos: {}\n\tOrn: {}".format(pos, diff --git a/examples/example_quadruped_mp/main.cpp b/examples/example_quadruped_mp/main.cpp index 020d5039..7dff4612 100644 --- a/examples/example_quadruped_mp/main.cpp +++ b/examples/example_quadruped_mp/main.cpp @@ -11,6 +11,7 @@ * @author Alejandro Escontrela */ +#include #include #include #include @@ -64,157 +65,156 @@ using namespace gtdynamics; * for cubic polynomials: * http://www.cs.cmu.edu/afs/cs/academic/class/15462-s10/www/lec-slides/lec06.pdf */ -struct Spline { - Pose3 wTb_i; - CoeffMatrix A; // coefficients - - /** - * Construct spline data and precompute cubic Hermite coefficients. - */ - Spline(const Pose3 &wTb_i, const Pose3 &wTb_f, // - const Vector3 &x_0_p, // - const Vector3 &x_1_p, // - const double horizon) - : wTb_i(wTb_i) { - // 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 - // polynomial terms, and B and C are the basis matrix and control matrices - // respectively. - 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; - 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; - } +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 + // polynomial terms, and B and C are the basis matrix and control matrices + // respectively. + 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 calculated trajectory. - * The calculated trajectory is a piecewise polynomial consisting of - * cubic hermite splines; the base pose will move along this trajectory. - * - * @param x_0_p tangent at the starting knot - * @param t time at which pose will be evaluated - * - * @return rotation and position - */ - Pose3 compute_hermite_pose(const Vector3 &x_0_p, const double t) const { - // The position computed from the spline equation as a function of time, - // 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 * A); - - // Differentiate position with respect to t for velocity. - gtsam::Matrix14 du(3 * t * t, 2 * t, 1, 0); - Point3 dpdt_v3 = Point3(du * A); - - // Unit vector for velocity. - dpdt_v3 = dpdt_v3 / dpdt_v3.norm(); - Point3 x_0_p_point(x_0_p); - x_0_p_point = x_0_p_point / x_0_p_point.norm(); - - Point3 axis = x_0_p_point.cross(dpdt_v3); - double angle = std::acos(x_0_p_point.dot(dpdt_v3)); - // The rotation. - Rot3 R = wTb_i.rotation() * Rot3::AxisAngle(axis, angle); - - return Pose3(R, p); - } +/** + * 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 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)=[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. + 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); + x_0_p_point = x_0_p_point / x_0_p_point.norm(); + + Point3 axis = x_0_p_point.cross(dpdt_v3); + double angle = std::acos(x_0_p_point.dot(dpdt_v3)); + // The rotation. + Rot3 R = wTb_i.rotation() * Rot3::AxisAngle(axis, angle); + + return Pose3(R, p); +} - /** Compute the target footholds for each support phase. */ - TargetFootholds compute_target_footholds( - const Vector3 &x_0_p, // - const double horizon, const double t_support, - const std::map &bTfs) const { - TargetFootholds target_footholds; - int n_support_phases = horizon / t_support; - - for (int i = 0; i <= n_support_phases; i++) { - Pose3 wTb = compute_hermite_pose(x_0_p, i * t_support / horizon); - std::map target_footholds_i; - for (auto &&bTf : bTfs) { - Pose3 wTf = wTb * bTf.second; - // TODO(frank): #179 make sure height is handled correctly. - Pose3 wTf_gh( - wTf.rotation(), - Point3(wTf.translation()[0], wTf.translation()[1], GROUND_HEIGHT)); - target_footholds_i.emplace(bTf.first, wTf_gh); - } - target_footholds.emplace(i, target_footholds_i); +/** Compute the target footholds for each support phase. */ +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. + int n_support_phases = horizon / t_support; + + for (int i = 0; i <= n_support_phases; i++) { + Pose3 wTb = + compute_hermite_pose(coeffs, x_0_p, i * t_support / horizon, wTb_i); + std::map target_footholds_i; + for (auto &&bTf : bTfs) { + Pose3 wTf = wTb * bTf.second; + // TODO(frank): #179 make sure height is handled correctly. + Pose3 wTf_gh(wTf.rotation(), Point3(wTf.translation()[0], + wTf.translation()[1], GROUND_HEIGHT)); + target_footholds_i.emplace(bTf.first, wTf_gh); } - return target_footholds; + target_footholds.emplace(i, target_footholds_i); } + return target_footholds; +} - /** Get base pose and foot positions at any time t. */ - TargetPoses compute_target_poses( - const TargetFootholds &targ_footholds, const double horizon, - const double t_support, const double t, - const std::vector &swing_sequence, - const Vector3 &x_0_p) const { - TargetPoses t_poses; - // Compute the body pose. - Pose3 wTb = compute_hermite_pose(x_0_p, t / horizon); - t_poses.emplace("body", wTb); - - const std::map &prev_targ_foothold = - targ_footholds.at(static_cast(std::floor(t / t_support))); - const std::map &next_targ_foothold = - targ_footholds.at(static_cast(std::ceil(t / t_support))); - - // Time spent in current support phase. - double t_in_support = std::fmod(t, t_support); - double t_swing = t_support / 4.0; // Duration of swing phase. - - int swing_leg_idx; - if (t_in_support <= t_swing) - swing_leg_idx = 0; - else if (t_in_support <= (2 * t_swing)) - swing_leg_idx = 1; - else if (t_in_support <= (3 * t_swing)) - swing_leg_idx = 2; - else - swing_leg_idx = 3; - - // Normalized swing duration. - double t_normed = (t_in_support - swing_leg_idx * t_swing) / t_swing; - - // Already completed swing phase in this support phase. - for (int i = 0; i < swing_leg_idx; i++) { - std::string leg_i = swing_sequence[i]; - t_poses.emplace(leg_i, next_targ_foothold.at(leg_i)); - } +/** Get base pose and foot positions at any time t. */ +TargetPoses compute_target_poses(const TargetFootholds &targ_footholds, + const double horizon, const double t_support, + const double t, + const std::vector &swing_sequence, + const CoeffMatrix &coeffs, + const Vector3 &x_0_p, const Pose3 &wTb_i) { + TargetPoses t_poses; + // Compute the body pose. + Pose3 wTb = compute_hermite_pose(coeffs, x_0_p, t / horizon, wTb_i); + t_poses.emplace("body", wTb); + + const std::map &prev_targ_foothold = + targ_footholds.at(static_cast(std::floor(t / t_support))); + const std::map &next_targ_foothold = + targ_footholds.at(static_cast(std::ceil(t / t_support))); + + // Time spent in current support phase. + double t_in_support = std::fmod(t, t_support); + double t_swing = t_support / 4.0; // Duration of swing phase. + + int swing_leg_idx; + if (t_in_support <= t_swing) + swing_leg_idx = 0; + else if (t_in_support <= (2 * t_swing)) + swing_leg_idx = 1; + else if (t_in_support <= (3 * t_swing)) + swing_leg_idx = 2; + else + swing_leg_idx = 3; + + // Normalized swing duration. + double t_normed = (t_in_support - swing_leg_idx * t_swing) / t_swing; + + // Already completed swing phase in this support phase. + for (int i = 0; i < swing_leg_idx; i++) { + std::string leg_i = swing_sequence[i]; + t_poses.emplace(leg_i, next_targ_foothold.at(leg_i)); + } - // Currently swinging. - std::string swing_leg = swing_sequence[swing_leg_idx]; - auto prev_foot_pos = prev_targ_foothold.at(swing_leg).translation(); - auto next_foot_pos = next_targ_foothold.at(swing_leg).translation(); - Point3 curr_foot_pos = - prev_foot_pos + (next_foot_pos - prev_foot_pos) * t_normed; - double h = GROUND_HEIGHT + - 0.2 * std::pow(t_normed, 1.1) * std::pow(1 - t_normed, 0.7); - - t_poses.emplace(swing_leg, Pose3(Rot3(), Point3(curr_foot_pos[0], - curr_foot_pos[1], h))); - - // Yet to complete swing phase in this support phase. - for (int i = swing_leg_idx + 1; i < 4; i++) { - std::string leg_i = swing_sequence[i]; - t_poses.emplace(leg_i, prev_targ_foothold.at(leg_i)); - } - return t_poses; + // Currently swinging. + std::string swing_leg = swing_sequence[swing_leg_idx]; + auto prev_foot_pos = prev_targ_foothold.at(swing_leg).translation(); + auto next_foot_pos = next_targ_foothold.at(swing_leg).translation(); + Point3 curr_foot_pos = + prev_foot_pos + (next_foot_pos - prev_foot_pos) * t_normed; + double h = GROUND_HEIGHT + + 0.2 * std::pow(t_normed, 1.1) * std::pow(1 - t_normed, 0.7); + + t_poses.emplace(swing_leg, + Pose3(Rot3(), Point3(curr_foot_pos[0], curr_foot_pos[1], h))); + + // Yet to complete swing phase in this support phase. + for (int i = swing_leg_idx + 1; i < 4; i++) { + std::string leg_i = swing_sequence[i]; + t_poses.emplace(leg_i, prev_targ_foothold.at(leg_i)); } -}; + return t_poses; +} struct CsvWriter { std::ofstream pose_file; @@ -277,8 +277,8 @@ int main(int argc, char **argv) { Point3 x_0_p(1, 0, 0); Point3 x_0_p_traj(1, 0, 0.4); Point3 x_1_p_traj(1, 0, 0); - - Spline spline(wTb_i, wTb_f, x_0_p_traj, x_1_p_traj, 1.0); + auto coeffs = + compute_spline_coefficients(wTb_i, wTb_f, x_0_p_traj, x_1_p_traj, 1); // Time horizon. double horizon = 72; @@ -307,18 +307,19 @@ int main(int argc, char **argv) { // Calculate foothold at the end of each support phase. TargetFootholds targ_footholds = - spline.compute_target_footholds(x_0_p, horizon, t_support, bTfs); + compute_target_footholds(coeffs, x_0_p, wTb_i, horizon, t_support, bTfs); // Iteratively solve the inverse kinematics problem to obtain joint angles. - double dt = 1. / 24., curr_t = 0.0; + double dt = 1. / 240., curr_t = 0.0; int k = 0; // The time index. + auto dgb = DynamicsGraph(); // Initialize values. gtsam::Values values; for (auto &&link : robot.links()) InsertPose(&values, link->id(), link->bMcom()); for (auto &&joint : robot.joints()) - InsertJointAngle(&values, joint->id(), 1.0); + InsertJointAngle(&values, joint->id(), 0.0); // Write body,foot poses and joint angles to csv file. CsvWriter writer("traj.csv", swing_sequence, robot); @@ -326,7 +327,7 @@ int main(int argc, char **argv) { // Set parameters for optimizer gtsam::LevenbergMarquardtParams params; - // params.setMaxIterations(50); + params.setMaxIterations(50); params.setlambdaInitial(1e5); auto model3 = gtsam::noiseModel::Constrained::All(3); KinematicsParameters kinematics_params; @@ -336,16 +337,12 @@ int main(int argc, char **argv) { // params.setVerbosityLM("SUMMARY"); while (curr_t < horizon) { - const TargetPoses tposes = spline.compute_target_poses( - targ_footholds, horizon, t_support, curr_t, swing_sequence, x_0_p); - // for (const auto &pose_pair : tposes) { - // const std::string &name = pose_pair.first; - // const Pose3 &pose = pose_pair.second; - // pose.print(name + ": "); - // } + const TargetPoses tposes = + compute_target_poses(targ_footholds, horizon, t_support, curr_t, + swing_sequence, coeffs, x_0_p, wTb_i); // Create factor graph of kinematics constraints. - gtsam::NonlinearFactorGraph kfg = kinematics.qFactors(Slice(k), robot); + gtsam::NonlinearFactorGraph kfg = dgb.qFactors(robot, k); // Constrain the base pose using trajectory value. kfg.addPrior(PoseKey(robot.link("body")->id(), k), tposes.at("body"), @@ -361,17 +358,12 @@ int main(int argc, char **argv) { } kfg.add(kinematics.pointGoalObjectives(Slice(k), contact_goals)); - double error_before = kfg.error(values); - // gtsam::LevenbergMarquardtOptimizer optimizer(kfg, values, params); gtsam::GaussNewtonOptimizer optimizer(kfg, values); gtsam::Values results = optimizer.optimize(); - double error_after = kfg.error(results); - if ((k % 100) == 0) - cout << "iter: " << k << ", err_before: " << error_before - << ", err_after: " << error_after << endl; + cout << "iter: " << k << ", err: " << kfg.error(results) << endl; // Update the values for next iteration. values.clear(); @@ -386,7 +378,6 @@ int main(int argc, char **argv) { writer.writerow(tposes, results, k); curr_t = curr_t + dt; k = k + 1; - // break; } return 0; diff --git a/examples/example_quadruped_mp/sim.py b/examples/example_quadruped_mp/sim.py index 829b6060..8f30d9fc 100644 --- a/examples/example_quadruped_mp/sim.py +++ b/examples/example_quadruped_mp/sim.py @@ -3,6 +3,8 @@ Author: Alejandro Escontrela """ +import time + import matplotlib.pyplot as plt import pandas as pd import pybullet as p @@ -28,7 +30,7 @@ df = pd.read_csv('traj.csv') -# input("Press ENTER to continue.") +input("Press ENTER to continue.") pos_des = df.loc[0][['bodyx', 'bodyy', 'bodyz']].tolist() pos_des[2] = pos_des[2] + 0.2 @@ -66,8 +68,8 @@ all_pos_des.append(new_pos_des) p.stepSimulation() - # time.sleep(1. / 24.) - t += 1. / 24. + time.sleep(1. / 240.) + t += 1. / 240. pos, orn = p.getBasePositionAndOrientation(quad_id) print("Final Base\n\tPos: {}\n\tOrn: {}".format(pos, diff --git a/gtdynamics.i b/gtdynamics.i index b4c91859..f15571f1 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -152,10 +152,8 @@ class Link { size_t numJoints() const; string name() const; double mass() const; - void setMass(const double mass); const gtsam::Pose3 ¢erOfMass(); const gtsam::Matrix &inertia(); - void setInertia(const gtsam::Matrix3 &inertia); gtsam::Matrix6 inertiaMatrix(); void print(const std::string &s = "") const; @@ -283,7 +281,7 @@ class Robot { #include // Only SDF files require the model_name specified.. -gtdynamics::Robot CreateRobotFromFile(const string &file_path, +gtdynamics::Robot CreateRobotFromFile(const string &urdf_file_path, const string &model_name = "", bool preserve_fixed_joint = false); @@ -292,7 +290,7 @@ gtdynamics::Robot CreateRobotFromFile(const string &file_path, class PointOnLink { PointOnLink(); - PointOnLink(const gtdynamics::LinkSharedPtr& link, const gtsam::Point3 &point); + PointOnLink(const gtdynamics::Link* link, const gtsam::Point3 &point); gtdynamics::LinkSharedPtr link; gtsam::Point3 point; @@ -303,24 +301,6 @@ class PointOnLink { // PointOnLinks defined in specializations.h -#include -virtual class ConstraintSpec { - void print(const string &s) const; -}; - -#include -class FootContactConstraintSpec : gtdynamics::ConstraintSpec { - FootContactConstraintSpec(); - FootContactConstraintSpec(const std::vector &points_on_links); - FootContactConstraintSpec(const std::vector &links, - const gtsam::Point3 &contact_in_com); - const gtdynamics::PointOnLinks &contactPoints() const; - bool hasContact(const gtdynamics::LinkSharedPtr &link) const; - const gtsam::Point3 &contactPoint(const string &link_name) const; - void print(const string &s = "") const; - std::vector swingLinks() const; -}; - /********************** Optimizer **********************/ #include class OptimizationParameters { @@ -502,8 +482,7 @@ class DynamicsGraph { gtsam::NonlinearFactorGraph qFactors( const gtdynamics::Robot &robot, const int k, - const std::optional &contact_points, - double ground_plane_height = 0.0) const; + const std::optional &contact_points) const; /* return v-level nonlinear factor graph (twist related factors) */ gtsam::NonlinearFactorGraph vFactors( @@ -524,8 +503,7 @@ class DynamicsGraph { gtsam::NonlinearFactorGraph dynamicsFactorGraph( const gtdynamics::Robot &robot, const int k, const std::optional &contact_points, - const std::optional &mu, - double ground_plane_height = 0.0) const; + const std::optional &mu) const; gtsam::NonlinearFactorGraph inverseDynamicsPriors( const gtdynamics::Robot &robot, const int k, @@ -546,8 +524,7 @@ class DynamicsGraph { const gtdynamics::Robot &robot, const int num_steps, const double dt, const gtdynamics::CollocationScheme collocation, const std::optional &contact_points, - const std::optional &mu, - double ground_plane_height = 0.0) const; + const std::optional &mu) const; gtsam::NonlinearFactorGraph multiPhaseTrajectoryFG( const gtdynamics::Robot &robot, @@ -659,37 +636,6 @@ class DynamicsGraph { const gtdynamics::OptimizerSetting &opt() const; }; -#include -class ChainDynamicsGraph : gtdynamics::DynamicsGraph { - ChainDynamicsGraph(const gtdynamics::Robot &robot, - const gtdynamics::OptimizerSetting &opt, - const gtsam::Vector3 &gravity); - - ChainDynamicsGraph(const gtdynamics::Robot &robot, - const gtdynamics::OptimizerSetting &opt, - const gtsam::Vector3 &gravity, - const std::optional &planar_axis); - - ChainDynamicsGraph(const gtdynamics::Robot &robot, - const gtdynamics::OptimizerSetting &opt); - - gtsam::NonlinearFactorGraph qFactors( - const gtdynamics::Robot &robot, const int t, - const std::optional &contact_points, - double ground_plane_height = 0.0) const; - - gtsam::NonlinearFactorGraph dynamicsFactors( - const gtdynamics::Robot &robot, const int t, - const std::optional &contact_points, - const std::optional &mu) const; - - gtsam::NonlinearFactorGraph dynamicsFactorGraph( - const gtdynamics::Robot &robot, const int t, - const std::optional &contact_points, - const std::optional &mu, - double ground_plane_height = 0.0) const; -}; - /********************** Objective Factors **********************/ #include class LinkObjectives : gtsam::NonlinearFactorGraph { @@ -749,38 +695,14 @@ class Initializer { Initializer(); gtsam::Values ZeroValues( - const gtdynamics::Robot& robot, const int t, - double gaussian_noise = 0.0); - - gtsam::Values ZeroValues( - const gtdynamics::Robot& robot, const int t, - double gaussian_noise = 0.0, - const std::optional& contact_points); + const gtdynamics::Robot& robot, const int t, double gaussian_noise); gtsam::Values ZeroValuesTrajectory( - const gtdynamics::Robot& robot, const int num_steps, - const int num_phases = -1, double gaussian_noise = 0.0); - - gtsam::Values ZeroValuesTrajectory( - const gtdynamics::Robot& robot, const int num_steps, - const int num_phases = -1, double gaussian_noise = 0.0, + const gtdynamics::Robot& robot, const int num_steps, const int num_phases, + double gaussian_noise, const std::optional& contact_points); }; -#include -class ChainInitializer : gtdynamics::Initializer { - ChainInitializer(); - - gtsam::Values ZeroValues( - const gtdynamics::Robot& robot, const int t, - double gaussian_noise = 0.0) const; - - gtsam::Values ZeroValues( - const gtdynamics::Robot& robot, const int t, - double gaussian_noise = 0.0, - const std::optional& contact_points) const; -}; - /********************** symbols **********************/ #include @@ -962,14 +884,12 @@ class Trajectory { std::vector getTransitionGraphs(const gtdynamics::Robot& robot, const gtdynamics::DynamicsGraph &graph_builder, - double mu, - double ground_plane_height = 0.0) const; + double mu) const; gtsam::NonlinearFactorGraph multiPhaseFactorGraph(const gtdynamics::Robot& robot, const gtdynamics::DynamicsGraph &graph_builder, const gtdynamics::CollocationScheme collocation, - double mu, - double ground_plane_height = 0.0) const; + double mu) const; std::vector transitionPhaseInitialValues(const gtdynamics::Robot& robot, const gtdynamics::Initializer &initializer, double gaussian_noise) const; diff --git a/gtdynamics/dynamics/Chain.cpp b/gtdynamics/dynamics/Chain.cpp index 77d6243a..4baf55e5 100644 --- a/gtdynamics/dynamics/Chain.cpp +++ b/gtdynamics/dynamics/Chain.cpp @@ -13,6 +13,7 @@ #include + namespace gtdynamics { Chain operator*(const Chain &chainA, const Chain &chainB) { @@ -236,20 +237,19 @@ gtsam::Vector6 Chain::AdjointWrenchEquality3( Pose3 T_theta = poe(angles, {}, H_angles ? &J_theta : nullptr); // Get end-effector wrench by Adjoint. This is true for a massless leg. - gtsam::Vector6 transformed_wrench = T_theta.AdjointTranspose( - wrench_body, H_angles ? &H_T : nullptr, H_wrench_body); + gtsam::Vector6 transformed_wrench = + T_theta.AdjointTranspose(wrench_body,H_angles ? &H_T : nullptr , H_wrench_body); if (H_angles) { - *H_angles = H_T * J_theta; + *H_angles = H_T * J_theta; } return transformed_wrench; } -gtsam::Vector6_ Chain::Poe3Expression(const std::vector &joints, - const gtsam::Key wTb_key, - const gtsam::Key wTe_key, - size_t k) const { +gtsam::Vector6_ Chain::Poe3Factor(const std::vector &joints, + const gtsam::Key wTb_key, + const gtsam::Key wTe_key, size_t k) const { // Get Expression for poses gtsam::Pose3_ wTb(wTb_key); gtsam::Pose3_ wTe(wTe_key); @@ -266,12 +266,12 @@ gtsam::Vector6_ Chain::Poe3Expression(const std::vector &joints, std::placeholders::_2), angles); - // compose + // compose gtsam::Pose3_ wTe_hat = wTb * end_effector_pose; // get error expression gtsam::Vector6_ error_expression = gtsam::logmap(wTe_hat, wTe); - + return error_expression; } @@ -283,7 +283,7 @@ gtsam::Pose3 Chain::PoeEquality3(const gtsam::Vector3 &angles, Pose3 T_theta = poe(angles, {}, H_angles ? &J_theta : nullptr); if (H_angles) { - *H_angles = J_theta; + *H_angles =J_theta; } return T_theta; diff --git a/gtdynamics/dynamics/Chain.h b/gtdynamics/dynamics/Chain.h index 47a603c5..dc580b05 100644 --- a/gtdynamics/dynamics/Chain.h +++ b/gtdynamics/dynamics/Chain.h @@ -176,9 +176,9 @@ class Chain { * @param k .................... Time slice. * @return ..................... GTSAM expression of the chain constraint. */ - gtsam::Vector6_ Poe3Expression(const std::vector &joints, - const gtsam::Key wTb_key, - const gtsam::Key wTe_key, size_t k) const; + gtsam::Vector6_ Poe3Factor(const std::vector &joints, + const gtsam::Key wTb_key, const gtsam::Key wTe_key, + size_t k) const; /** * This function calculates the end-effector pose using POE and chain @@ -188,18 +188,19 @@ class Chain { * BODY TO END-EFFECTOR. * @return ................... gtsam expression of the end-effector wrench */ - gtsam::Pose3 PoeEquality3(const gtsam::Vector3 &angles, - gtsam::OptionalJacobian<6, 3> H_angles = {}) const; + gtsam::Pose3 PoeEquality3( + const gtsam::Vector3 &angles, + gtsam::OptionalJacobian<6, 3> H_angles = {}) const; }; // Chain class // Helper function to create expression with a vector, used in // ChainConstraint3. inline gtsam::Vector3 MakeVector3(const double &value0, const double &value1, - const double &value2, - gtsam::OptionalJacobian<3, 1> J0 = {}, - gtsam::OptionalJacobian<3, 1> J1 = {}, - gtsam::OptionalJacobian<3, 1> J2 = {}) { + const double &value2, + gtsam::OptionalJacobian<3, 1> J0 = {}, + gtsam::OptionalJacobian<3, 1> J1 = {}, + gtsam::OptionalJacobian<3, 1> J2 = {}) { gtsam::Vector3 q; q << value0, value1, value2; if (J0) { diff --git a/gtdynamics/dynamics/ChainDynamicsGraph.cpp b/gtdynamics/dynamics/ChainDynamicsGraph.cpp index a40bc70f..063566c2 100644 --- a/gtdynamics/dynamics/ChainDynamicsGraph.cpp +++ b/gtdynamics/dynamics/ChainDynamicsGraph.cpp @@ -12,7 +12,6 @@ */ #include "gtdynamics/dynamics/ChainDynamicsGraph.h" - #include #include @@ -118,35 +117,36 @@ NonlinearFactorGraph ChainDynamicsGraph::dynamicsFactors( // Create expression for wrench constraint on trunk Vector6_ trunk_wrench_constraint = gravity_wrench; + // Set tolerances + // Get tolerance Vector6 wrench_tolerance = opt().f_cost_model->sigmas(); Vector3 contact_tolerance = opt().cm_cost_model->sigmas(); std::vector wrench_keys; - constexpr int root = 0; // TODO(Frank): Hard-coded for A1. - const Vector6 wrench_zero = gtsam::Z_6x1; - - for (int leg = 0; leg < 4; ++leg) { + for (int i = 0; i < 4; ++i) { bool foot_in_contact = false; // Get key for wrench at hip joint with id 0 - const int joint_id = 3 * leg; // TODO(Frank): Hard-coded for A1. - const Key wrench_key = WrenchKey(root, joint_id, t); + const Key wrench_key_3i_T = WrenchKey(0, 3 * i, t); + + // create expression for the wrench and initialize to zero + Vector6_ wrench_3i_T(Vector6::Zero()); // Set wrench expression on trunk by leg, according to contact for (auto &&cp : *contact_points) { - if (cp.link->id() == 3 * (leg + 1)) { + if (cp.link->id() == 3 * (i + 1)) { + wrench_3i_T = Vector6_(wrench_key_3i_T); foot_in_contact = true; } } // add wrench to trunk constraint - wrench_keys.push_back(wrench_key); + wrench_keys.push_back(wrench_key_3i_T); // Get expression for end effector wrench using chain - Vector6_ wrench_end_effector = - composed_chains_[leg].AdjointWrenchConstraint3(chain_joints_[leg], - wrench_key, t); + Vector6_ wrench_end_effector = composed_chains_[i].AdjointWrenchConstraint3( + chain_joints_[i], wrench_key_3i_T, t); // Create contact constraint Point3 contact_in_com(0.0, 0.0, -0.07); @@ -158,7 +158,8 @@ NonlinearFactorGraph ChainDynamicsGraph::dynamicsFactors( if (foot_in_contact) { graph.add(contact_expression.penaltyFactor(1.0)); } else { - graph.addPrior(wrench_key, wrench_zero, opt().f_cost_model); + Vector6 wrench_zero = gtsam::Z_6x1; + graph.addPrior(wrench_key_3i_T, wrench_zero, opt().f_cost_model); } } @@ -171,8 +172,7 @@ NonlinearFactorGraph ChainDynamicsGraph::dynamicsFactors( gtsam::NonlinearFactorGraph ChainDynamicsGraph::qFactors( const Robot &robot, const int t, - const std::optional &contact_points, - double ground_plane_height) const { + const std::optional &contact_points) const { NonlinearFactorGraph graph; // Get Pose key for base link @@ -186,7 +186,7 @@ gtsam::NonlinearFactorGraph ChainDynamicsGraph::qFactors( const Key end_effector_key = PoseKey(3 * (i + 1), t); // Get expression for end effector pose - gtsam::Vector6_ chain_expression = composed_chains_[i].Poe3Expression( + gtsam::Vector6_ chain_expression = composed_chains_[i].Poe3Factor( chain_joints_[i], base_key, end_effector_key, t); gtsam::ExpressionEqualityConstraint chain_constraint( @@ -198,8 +198,8 @@ gtsam::NonlinearFactorGraph ChainDynamicsGraph::qFactors( // Add contact factors. if (contact_points) { const Kinematics kinematics(opt()); - graph.add(kinematics.contactHeightObjectives( - Slice(t), *contact_points, gravity(), ground_plane_height)); + graph.add( + kinematics.contactHeightObjectives(Slice(t), *contact_points, gravity())); } return graph; @@ -208,12 +208,10 @@ gtsam::NonlinearFactorGraph ChainDynamicsGraph::qFactors( gtsam::NonlinearFactorGraph ChainDynamicsGraph::dynamicsFactorGraph( const Robot &robot, const int t, const std::optional &contact_points, - const std::optional &mu, double ground_plane_height) const { + const std::optional &mu) const { NonlinearFactorGraph graph; - graph.add(qFactors(robot, t, contact_points, ground_plane_height)); - // NOTE(Frank): In chain factors, there is no need to model the intermediate - // velocities/accelerations of the joints, as they do not influence the - // solution, see Dan's thesis. + graph.add(qFactors(robot, t, contact_points)); + // TODO(Varun): Why are these commented out? // graph.add(vFactors(robot, t, contact_points)); // graph.add(aFactors(robot, t, contact_points)); graph.add(dynamicsFactors(robot, t, contact_points, mu)); diff --git a/gtdynamics/dynamics/ChainDynamicsGraph.h b/gtdynamics/dynamics/ChainDynamicsGraph.h index 24bd3631..3813549c 100644 --- a/gtdynamics/dynamics/ChainDynamicsGraph.h +++ b/gtdynamics/dynamics/ChainDynamicsGraph.h @@ -60,8 +60,7 @@ class ChainDynamicsGraph : public DynamicsGraph { /// Return q-level nonlinear factor graph (pose related factors) NonlinearFactorGraph qFactors( const Robot &robot, const int t, - const std::optional &contact_points = {}, - double ground_plane_height = 0.0) const override; + const std::optional &contact_points = {}) const override; /** * Return nonlinear factor graph of all dynamics factors. @@ -85,8 +84,7 @@ class ChainDynamicsGraph : public DynamicsGraph { NonlinearFactorGraph dynamicsFactorGraph( const Robot &robot, const int t, const std::optional &contact_points = {}, - const std::optional &mu = {}, - double ground_plane_height = 0.0) const override; + const std::optional &mu = {}) const override; // Get a vector of legs, each leg is a vector of its joints static std::vector> getChainJoints( diff --git a/gtdynamics/dynamics/DynamicsGraph.cpp b/gtdynamics/dynamics/DynamicsGraph.cpp index 19fb5d33..d75b504e 100644 --- a/gtdynamics/dynamics/DynamicsGraph.cpp +++ b/gtdynamics/dynamics/DynamicsGraph.cpp @@ -197,11 +197,9 @@ Values DynamicsGraph::linearSolveID(const Robot &robot, const int k, gtsam::NonlinearFactorGraph DynamicsGraph::qFactors( const Robot &robot, const int k, - const std::optional &contact_points, - double ground_plane_height) const { + const std::optional &contact_points) const { const Slice slice(k); - return kinematics_.qFactors(slice, robot, contact_points, gravity_, - ground_plane_height); + return kinematics_.qFactors(slice, robot, contact_points, gravity_); } gtsam::NonlinearFactorGraph DynamicsGraph::vFactors( @@ -234,9 +232,9 @@ gtsam::NonlinearFactorGraph DynamicsGraph::dynamicsFactors( gtsam::NonlinearFactorGraph DynamicsGraph::dynamicsFactorGraph( const Robot &robot, const int k, const std::optional &contact_points, - const std::optional &mu, double ground_plane_height) const { + const std::optional &mu) const { NonlinearFactorGraph graph; - graph.add(qFactors(robot, k, contact_points, ground_plane_height)); + graph.add(qFactors(robot, k, contact_points)); graph.add(vFactors(robot, k, contact_points)); graph.add(aFactors(robot, k, contact_points)); graph.add(dynamicsFactors(robot, k, contact_points, mu)); @@ -247,11 +245,10 @@ gtsam::NonlinearFactorGraph DynamicsGraph::trajectoryFG( const Robot &robot, const int num_steps, const double dt, const CollocationScheme collocation, const std::optional &contact_points, - const std::optional &mu, double ground_plane_height) const { + const std::optional &mu) const { NonlinearFactorGraph graph; for (int k = 0; k <= num_steps; k++) { - graph.add( - dynamicsFactorGraph(robot, k, contact_points, mu, ground_plane_height)); + graph.add(dynamicsFactorGraph(robot, k, contact_points, mu)); if (k < num_steps) { graph.add(collocationFactors(robot, k, dt, collocation)); } @@ -264,7 +261,7 @@ gtsam::NonlinearFactorGraph DynamicsGraph::multiPhaseTrajectoryFG( const std::vector &transition_graphs, const CollocationScheme collocation, const std::optional> &phase_contact_points, - const std::optional &mu, double ground_plane_height) const { + const std::optional &mu) const { NonlinearFactorGraph graph; int num_phases = phase_steps.size(); @@ -276,21 +273,18 @@ gtsam::NonlinearFactorGraph DynamicsGraph::multiPhaseTrajectoryFG( }; // First slice, k==0 - graph.add( - dynamicsFactorGraph(robot, 0, contact_points(0), mu, ground_plane_height)); + graph.add(dynamicsFactorGraph(robot, 0, contact_points(0), mu)); int k = 0; for (int p = 0; p < num_phases; p++) { // in-phase // add dynamics for each step for (int step = 0; step < phase_steps[p] - 1; step++) { - graph.add(dynamicsFactorGraph(robot, ++k, contact_points(p), mu, - ground_plane_height)); + graph.add(dynamicsFactorGraph(robot, ++k, contact_points(p), mu)); } if (p == num_phases - 1) { // Last slice, k==K-1 - graph.add(dynamicsFactorGraph(robot, ++k, contact_points(p), mu, - ground_plane_height)); + graph.add(dynamicsFactorGraph(robot, ++k, contact_points(p), mu)); } else { // transition graph.add(transition_graphs[p]); diff --git a/gtdynamics/dynamics/DynamicsGraph.h b/gtdynamics/dynamics/DynamicsGraph.h index 9cfb58cd..7d3d6f7d 100644 --- a/gtdynamics/dynamics/DynamicsGraph.h +++ b/gtdynamics/dynamics/DynamicsGraph.h @@ -130,8 +130,7 @@ class DynamicsGraph { /// Return q-level nonlinear factor graph (pose related factors) virtual gtsam::NonlinearFactorGraph qFactors( const Robot &robot, const int k, - const std::optional &contact_points = {}, - double ground_plane_height = 0.0) const; + const std::optional &contact_points = {}) const; /// Return v-level nonlinear factor graph (twist related factors) gtsam::NonlinearFactorGraph vFactors( @@ -155,13 +154,11 @@ class DynamicsGraph { * @param k time step * @param contact_points optional vector of contact points. * @param mu optional coefficient of static friction. - * @param ground_plane_height contact ground-plane height in world frame. */ virtual gtsam::NonlinearFactorGraph dynamicsFactorGraph( const Robot &robot, const int k, const std::optional &contact_points = {}, - const std::optional &mu = {}, - double ground_plane_height = 0.0) const; + const std::optional &mu = {}) const; /** * Return prior factors of torque, angle, velocity @@ -197,14 +194,12 @@ class DynamicsGraph { * @param num_steps total time steps * @param dt duration of each time step * @param collocation the collocation scheme - * @param ground_plane_height contact ground-plane height in world frame. */ gtsam::NonlinearFactorGraph trajectoryFG( const Robot &robot, const int num_steps, const double dt, const CollocationScheme collocation = Trapezoidal, const std::optional &contact_points = {}, - const std::optional &mu = {}, - double ground_plane_height = 0.0) const; + const std::optional &mu = {}) const; /** * Return nonlinear factor graph of the entire trajectory for multi-phase @@ -214,15 +209,13 @@ class DynamicsGraph { * @param collocation the collocation scheme * @param phase_contact_points contact points at each phase * @param mu optional coefficient of static friction - * @param ground_plane_height contact ground-plane height in world frame. */ gtsam::NonlinearFactorGraph multiPhaseTrajectoryFG( const Robot &robot, const std::vector &phase_steps, const std::vector &transition_graphs, const CollocationScheme collocation = Trapezoidal, const std::optional> &phase_contact_points = {}, - const std::optional &mu = {}, - double ground_plane_height = 0.0) const; + const std::optional &mu = {}) const; static std::shared_ptr> collocationFactorDouble(const gtsam::Key x0_key, const gtsam::Key x1_key, diff --git a/gtdynamics/dynamics/tests/testDynamicsGraph.cpp b/gtdynamics/dynamics/tests/testDynamicsGraph.cpp index fa90c39a..ba6f6296 100644 --- a/gtdynamics/dynamics/tests/testDynamicsGraph.cpp +++ b/gtdynamics/dynamics/tests/testDynamicsGraph.cpp @@ -45,16 +45,6 @@ using gtsam::Values; using gtsam::Vector; using gtsam::Vector6; using std::vector; -static constexpr double kGroundHeight = 4.2; - -static gtsam::Pose3 poseWithContactHeight(const gtsam::Pose3& wTl, - const gtsam::Point3& comPc, - double ground_height) { - const double current_height = (wTl * comPc).z(); - const double delta_z = ground_height - current_height; - const gtsam::Point3 t = wTl.translation(); - return gtsam::Pose3(wTl.rotation(), gtsam::Point3(t.x(), t.y(), t.z() + delta_z)); -} // ============================ VALUES-STYLE ================================ @@ -112,8 +102,7 @@ TEST(dynamicsFactorGraph_FD, simple_urdf_eq_mass) { size_t t = 777; DynamicsGraph graph_builder(simple_urdf_eq_mass::gravity, simple_urdf_eq_mass::planar_axis); - auto graph = graph_builder.dynamicsFactorGraph(robot, t, {}, {}, - kGroundHeight); + auto graph = graph_builder.dynamicsFactorGraph(robot, t); // Create values with rest kinematics and unit torques Values known_values = zero_values(robot, t); @@ -168,8 +157,7 @@ TEST(dynamicsFactorGraph_FD, four_bar_linkage_pure) { graph_builder.opt().bv_cost_model); } - auto graph = graph_builder.dynamicsFactorGraph(robot, 0, {}, {}, - kGroundHeight); + auto graph = graph_builder.dynamicsFactorGraph(robot, 0); graph.add(prior_factors); Initializer initializer; @@ -184,7 +172,7 @@ TEST(dynamicsFactorGraph_FD, four_bar_linkage_pure) { // test the condition when we fix link "l1" robot = robot.fixLink("l1"); - graph = graph_builder.dynamicsFactorGraph(robot, 0, {}, {}, kGroundHeight); + graph = graph_builder.dynamicsFactorGraph(robot, 0); graph.add(prior_factors); gtsam::GaussNewtonOptimizer optimizer2(graph, init_values); @@ -212,8 +200,7 @@ TEST(dynamicsFactorGraph_FD, jumping_robot) { // build the dynamics factor graph DynamicsGraph graph_builder(jumping_robot::gravity, jumping_robot::planar_axis); - auto graph = graph_builder.dynamicsFactorGraph(robot, 0, {}, {}, - kGroundHeight); + auto graph = graph_builder.dynamicsFactorGraph(robot, 0); graph.add(graph_builder.forwardDynamicsPriors(robot, 0, known_values)); // test jumping robot FD @@ -347,8 +334,7 @@ TEST(dynamicsTrajectoryFG, simple_urdf_eq_mass) { // test Euler auto euler_graph = graph_builder.trajectoryFG(robot, num_steps, dt, - CollocationScheme::Euler, {}, - {}, kGroundHeight); + CollocationScheme::Euler); euler_graph.add( graph_builder.trajectoryFDPriors(robot, num_steps, known_values)); @@ -364,8 +350,7 @@ TEST(dynamicsTrajectoryFG, simple_urdf_eq_mass) { // test trapezoidal auto trapezoidal_graph = graph_builder.trajectoryFG( - robot, num_steps, dt, CollocationScheme::Trapezoidal, {}, {}, - kGroundHeight); + robot, num_steps, dt, CollocationScheme::Trapezoidal); trapezoidal_graph.add( graph_builder.trajectoryFDPriors(robot, num_steps, known_values)); @@ -381,8 +366,7 @@ TEST(dynamicsTrajectoryFG, simple_urdf_eq_mass) { // test the scenario with dt as a variable vector phase_steps{1, 1}; - auto transition_graph = - graph_builder.dynamicsFactorGraph(robot, 1, {}, {}, kGroundHeight); + auto transition_graph = graph_builder.dynamicsFactorGraph(robot, 1); vector transition_graphs{transition_graph}; double dt0 = 1; double dt1 = 2; @@ -396,8 +380,7 @@ TEST(dynamicsTrajectoryFG, simple_urdf_eq_mass) { // multi-phase Euler NonlinearFactorGraph mp_euler_graph = graph_builder.multiPhaseTrajectoryFG( - robot, phase_steps, transition_graphs, CollocationScheme::Euler, {}, {}, - kGroundHeight); + robot, phase_steps, transition_graphs, CollocationScheme::Euler); mp_euler_graph.add(mp_prior_graph); gtsam::GaussNewtonOptimizer optimizer_mpe(mp_euler_graph, init_values); Values mp_euler_result = optimizer_mpe.optimize(); @@ -417,8 +400,7 @@ TEST(dynamicsTrajectoryFG, simple_urdf_eq_mass) { // multi-phase Trapezoidal auto mp_trapezoidal_graph = graph_builder.multiPhaseTrajectoryFG( - robot, phase_steps, transition_graphs, CollocationScheme::Trapezoidal, - {}, {}, kGroundHeight); + robot, phase_steps, transition_graphs, CollocationScheme::Trapezoidal); mp_trapezoidal_graph.add(mp_prior_graph); gtsam::GaussNewtonOptimizer optimizer_mpt(mp_trapezoidal_graph, mp_euler_result); @@ -446,14 +428,12 @@ TEST(dynamicsFactorGraph_Contacts, dynamics_graph_simple_rr) { // Add some contact points. PointOnLinks contact_points; LinkSharedPtr l0 = robot.link("link_0"); - const gtsam::Point3 contact_in_com(0, 0, -0.1); - contact_points.emplace_back(l0, contact_in_com); + contact_points.emplace_back(l0, gtsam::Point3(0, 0, -0.1)); // Build the dynamics FG. gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, -9.8).finished(); DynamicsGraph graph_builder(gravity); - auto graph = graph_builder.dynamicsFactorGraph(robot, 0, contact_points, 1.0, - kGroundHeight); + auto graph = graph_builder.dynamicsFactorGraph(robot, 0, contact_points, 1.0); Values known_values = zero_values(robot, 0, true); // Compute inverse dynamics prior factors. @@ -461,9 +441,7 @@ TEST(dynamicsFactorGraph_Contacts, dynamics_graph_simple_rr) { graph_builder.inverseDynamicsPriors(robot, 0, known_values); // Specify pose and twist priors for one leg. - prior_factors.addPrior(PoseKey(l0->id(), 0), - poseWithContactHeight(l0->bMcom(), contact_in_com, - kGroundHeight), + prior_factors.addPrior(PoseKey(l0->id(), 0), l0->bMcom(), gtsam::noiseModel::Constrained::All(6)); prior_factors.addPrior(TwistKey(l0->id(), 0), gtsam::Z_6x1, gtsam::noiseModel::Constrained::All(6)); @@ -510,8 +488,7 @@ TEST(dynamicsFactorGraph_Contacts, dynamics_graph_biped) { // Build the dynamics FG. gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, -9.81).finished(); DynamicsGraph graph_builder(gravity); - auto graph = graph_builder.dynamicsFactorGraph(biped, 0, contact_points, 1.0, - kGroundHeight); + auto graph = graph_builder.dynamicsFactorGraph(biped, 0, contact_points, 1.0); // Compute inverse dynamics prior factors. // Inverse dynamics priors. We care about the torques. @@ -583,14 +560,12 @@ TEST(dynamicsFactorGraph_Contacts, dynamics_graph_simple_rrr) { // Add some contact points. PointOnLinks contact_points; LinkSharedPtr l0 = robot.link("link_0"); - const gtsam::Point3 contact_in_com(0, 0, -0.1); - contact_points.emplace_back(l0, contact_in_com); + contact_points.emplace_back(l0, gtsam::Point3(0, 0, -0.1)); // Build the dynamics FG. gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, -9.8).finished(); DynamicsGraph graph_builder(gravity); - auto graph = graph_builder.dynamicsFactorGraph(robot, 0, contact_points, 1.0, - kGroundHeight); + auto graph = graph_builder.dynamicsFactorGraph(robot, 0, contact_points, 1.0); // Compute inverse dynamics prior factors. gtsam::Values known_values = zero_values(robot, 0, true); @@ -598,9 +573,7 @@ TEST(dynamicsFactorGraph_Contacts, dynamics_graph_simple_rrr) { graph_builder.inverseDynamicsPriors(robot, 0, known_values); // Specify pose and twist priors for one leg. - prior_factors.addPrior(PoseKey(l0->id(), 0), - poseWithContactHeight(l0->bMcom(), contact_in_com, - kGroundHeight), + prior_factors.addPrior(PoseKey(l0->id(), 0), l0->bMcom(), gtsam::noiseModel::Constrained::All(6)); prior_factors.addPrior(TwistKey(l0->id(), 0), gtsam::Z_6x1, gtsam::noiseModel::Constrained::All(6)); diff --git a/gtdynamics/kinematics/ContactHeightFactor.h b/gtdynamics/kinematics/ContactHeightFactor.h index dc42c966..bd8a8591 100644 --- a/gtdynamics/kinematics/ContactHeightFactor.h +++ b/gtdynamics/kinematics/ContactHeightFactor.h @@ -7,7 +7,7 @@ /** * @file ContactHeightFactor.h - * @brief Factor to enforce ground-plane height at the contact point. + * @brief Factor to enforce zero height at the contact point. * @author: Alejandro Escontrela, Varun Agrawal */ diff --git a/gtdynamics/kinematics/Kinematics.h b/gtdynamics/kinematics/Kinematics.h index eb2eb0a7..157b922d 100644 --- a/gtdynamics/kinematics/Kinematics.h +++ b/gtdynamics/kinematics/Kinematics.h @@ -135,15 +135,13 @@ class Kinematics : public Optimizer { * @fn Create q-level kinematics factors. * @param contact_points Optional contact points on links. * @param gravity Gravity vector used to define up direction. - * @param ground_plane_height Contact ground-plane height in world frame. * @returns graph with q-level factors. */ template gtsam::NonlinearFactorGraph qFactors( const CONTEXT& context, const Robot& robot, const std::optional& contact_points = {}, - const gtsam::Vector3& gravity = gtsam::Vector3(0, 0, -9.8), - double ground_plane_height = 0.0) const; + const gtsam::Vector3& gravity = gtsam::Vector3(0, 0, -9.8)) const; /** * @fn Create v-level kinematics factors. @@ -184,14 +182,12 @@ class Kinematics : public Optimizer { * @fn Create contact-height objectives. * @param contact_points Contact points on links. * @param gravity Gravity vector used to define up direction. - * @param ground_plane_height Contact ground-plane height in world frame. * @returns graph with contact-height factors. */ template gtsam::NonlinearFactorGraph contactHeightObjectives( const CONTEXT& context, const PointOnLinks& contact_points, - const gtsam::Vector3& gravity, - double ground_plane_height = 0.0) const; + const gtsam::Vector3& gravity) const; /** * @fn Create fixed-link pose prior objectives. diff --git a/gtdynamics/kinematics/KinematicsInterval.cpp b/gtdynamics/kinematics/KinematicsInterval.cpp index 195e14e7..1de6ffd6 100644 --- a/gtdynamics/kinematics/KinematicsInterval.cpp +++ b/gtdynamics/kinematics/KinematicsInterval.cpp @@ -36,9 +36,9 @@ template <> NonlinearFactorGraph Kinematics::qFactors( const Interval& interval, const Robot& robot, const std::optional& contact_points, - const gtsam::Vector3& gravity, double ground_plane_height) const { + const gtsam::Vector3& gravity) const { return collectFactors(interval, [&](const Slice& slice) { - return qFactors(slice, robot, contact_points, gravity, ground_plane_height); + return qFactors(slice, robot, contact_points, gravity); }); } diff --git a/gtdynamics/kinematics/KinematicsPhase.cpp b/gtdynamics/kinematics/KinematicsPhase.cpp index 4e4d51ea..ee3f4829 100644 --- a/gtdynamics/kinematics/KinematicsPhase.cpp +++ b/gtdynamics/kinematics/KinematicsPhase.cpp @@ -36,9 +36,9 @@ template <> NonlinearFactorGraph Kinematics::qFactors( const Phase& phase, const Robot& robot, const std::optional& contact_points, - const gtsam::Vector3& gravity, double ground_plane_height) const { + const gtsam::Vector3& gravity) const { const Interval& interval = static_cast(phase); - return qFactors(interval, robot, contact_points, gravity, ground_plane_height); + return qFactors(interval, robot, contact_points, gravity); } template <> diff --git a/gtdynamics/kinematics/KinematicsSlice.cpp b/gtdynamics/kinematics/KinematicsSlice.cpp index 4fe5803a..069cfe69 100644 --- a/gtdynamics/kinematics/KinematicsSlice.cpp +++ b/gtdynamics/kinematics/KinematicsSlice.cpp @@ -196,13 +196,12 @@ NonlinearFactorGraph Kinematics::pointGoalObjectives( template <> NonlinearFactorGraph Kinematics::contactHeightObjectives( const Slice& slice, const PointOnLinks& contact_points, - const gtsam::Vector3& gravity, double ground_plane_height) const { + const gtsam::Vector3& gravity) const { NonlinearFactorGraph graph; for (const PointOnLink& cp : contact_points) { graph.emplace_shared( - PoseKey(cp.link->id(), slice.k), p_.cp_cost_model, cp.point, gravity, - ground_plane_height); + PoseKey(cp.link->id(), slice.k), p_.cp_cost_model, cp.point, gravity); } return graph; @@ -212,13 +211,12 @@ template <> NonlinearFactorGraph Kinematics::qFactors( const Slice& slice, const Robot& robot, const std::optional& contact_points, - const gtsam::Vector3& gravity, double ground_plane_height) const { + const gtsam::Vector3& gravity) const { NonlinearFactorGraph graph; graph.add(fixedLinkObjectives(slice, robot)); graph.add(this->graph(slice, robot)); if (contact_points) { - graph.add(contactHeightObjectives(slice, *contact_points, gravity, - ground_plane_height)); + graph.add(contactHeightObjectives(slice, *contact_points, gravity)); } return graph; } diff --git a/gtdynamics/kinematics/tests/testContactHeightFactor.cpp b/gtdynamics/kinematics/tests/testContactHeightFactor.cpp index 857776d2..71c43c12 100644 --- a/gtdynamics/kinematics/tests/testContactHeightFactor.cpp +++ b/gtdynamics/kinematics/tests/testContactHeightFactor.cpp @@ -29,7 +29,6 @@ using namespace gtdynamics; using gtsam::assert_equal; using gtsam::Vector3, gtsam::Vector1, gtsam::Rot3, gtsam::Pose3, gtsam::Point3; -static constexpr double kGroundHeight = 4.2; /** * Test the unwhitenedError method with various link twists. @@ -43,21 +42,21 @@ TEST(ContactHeightFactor, Error) { // Transform from the robot com to the link end. Point3 comPc(0, 0, 1); ContactHeightFactor factor(pose_key, cost_model, comPc, - gtsam::Vector3(0, 0, -9.8), kGroundHeight); + gtsam::Vector3(0, 0, -9.8), 0); // Leg oriented upwards with contact away from the ground. gtsam::Values values1; - values1.insert(pose_key, Pose3(Rot3(), Point3(0., 0., 6.2))); + values1.insert(pose_key, Pose3(Rot3(), Point3(0., 0., 2.))); EXPECT(assert_equal(factor.unwhitenedError(values1), Vector1(3))); // Leg oriented down with contact 1m away from the ground. gtsam::Values values2; - values2.insert(pose_key, Pose3(Rot3::Rx(M_PI), Point3(0., 0., 6.2))); + values2.insert(pose_key, Pose3(Rot3::Rx(M_PI), Point3(0., 0., 2.))); EXPECT(assert_equal(factor.unwhitenedError(values2), Vector1(1))); // Contact touching the ground. gtsam::Values values3; - values3.insert(pose_key, Pose3(Rot3::Rx(M_PI), Point3(0., 0., 5.2))); + values3.insert(pose_key, Pose3(Rot3::Rx(M_PI), Point3(0., 0., 1.))); EXPECT(assert_equal(factor.unwhitenedError(values3), Vector1(0))); // Check that Jacobian computation is correct by comparison to finite @@ -94,23 +93,23 @@ TEST(ContactHeightFactor, ErrorWithHeight) { // Transform from the contact frame to the link com. Point3 comPc(0, 0, 1); - // Create a factor that establishes a ground plane at z = 4.2. + // Create a factor that establishes a ground plane at z = -1.0. ContactHeightFactor factor(pose_key, cost_model, comPc, - gtsam::Vector3(0, 0, -9.8), kGroundHeight); + gtsam::Vector3(0, 0, -9.8), -1.0); // Leg oriented upwards with contact away from the ground. gtsam::Values values1; - values1.insert(pose_key, Pose3(Rot3(), Point3(0., 0., 7.2))); + values1.insert(pose_key, Pose3(Rot3(), Point3(0., 0., 2.))); EXPECT(assert_equal(factor.unwhitenedError(values1), gtsam::Vector1(4))); // Leg oriented down with contact 1m away from the ground. gtsam::Values values2; - values2.insert(pose_key, Pose3(Rot3::Rx(M_PI), Point3(0., 0., 7.2))); + values2.insert(pose_key, Pose3(Rot3::Rx(M_PI), Point3(0., 0., 2.))); EXPECT(assert_equal(factor.unwhitenedError(values2), gtsam::Vector1(2))); - // Contact 1m above the ground. + // Contact touching the ground. gtsam::Values values3; - values3.insert(pose_key, Pose3(Rot3::Rx(M_PI), Point3(0., 0., 6.2))); + values3.insert(pose_key, Pose3(Rot3::Rx(M_PI), Point3(0., 0., 1.))); EXPECT(assert_equal(factor.unwhitenedError(values3), gtsam::Vector1(1))); // Check that Jacobian computation is correct by comparison to finite @@ -148,7 +147,7 @@ TEST(ContactHeightFactor, Optimization) { // Transform from the contact frame to the link com. Point3 comPc(0, 0, 1); ContactHeightFactor factor(pose_key, cost_model, comPc, - gtsam::Vector3(0, 0, -9.8), kGroundHeight); + gtsam::Vector3(0, 0, -9.8)); // Initial link pose. Pose3 link_pose_init = diff --git a/gtdynamics/utils/Trajectory.cpp b/gtdynamics/utils/Trajectory.cpp index 4c86894d..8226e80e 100644 --- a/gtdynamics/utils/Trajectory.cpp +++ b/gtdynamics/utils/Trajectory.cpp @@ -36,30 +36,25 @@ using std::vector; namespace gtdynamics { vector Trajectory::getTransitionGraphs( - const Robot &robot, const DynamicsGraph &graph_builder, double mu, - double ground_plane_height) const { + const Robot &robot, const DynamicsGraph &graph_builder, double mu) const { vector transition_graphs; const vector final_timesteps = finalTimeSteps(); const vector trans_cps = transitionContactPoints(); for (int p = 1; p < numPhases(); p++) { transition_graphs.push_back(graph_builder.dynamicsFactorGraph( - robot, final_timesteps[p - 1], trans_cps[p - 1], mu, - ground_plane_height)); + robot, final_timesteps[p - 1], trans_cps[p - 1], mu)); } return transition_graphs; } NonlinearFactorGraph Trajectory::multiPhaseFactorGraph( const Robot &robot, const DynamicsGraph &graph_builder, - const CollocationScheme collocation, double mu, - double ground_plane_height) const { + const CollocationScheme collocation, double mu) const { // Graphs for transition between phases + their initial values. - auto transition_graphs = - getTransitionGraphs(robot, graph_builder, mu, ground_plane_height); + auto transition_graphs = getTransitionGraphs(robot, graph_builder, mu); return graph_builder.multiPhaseTrajectoryFG(robot, phaseDurations(), transition_graphs, collocation, - phaseContactPoints(), mu, - ground_plane_height); + phaseContactPoints(), mu); } vector Trajectory::transitionPhaseInitialValues( diff --git a/gtdynamics/utils/Trajectory.h b/gtdynamics/utils/Trajectory.h index 2a421bc1..c8eb4831 100644 --- a/gtdynamics/utils/Trajectory.h +++ b/gtdynamics/utils/Trajectory.h @@ -103,12 +103,10 @@ class Trajectory { * @param[in] robot Robot specification from URDF/SDF. * @param[in] graph_builder Dynamics Graph * @param[in] mu Coefficient of static friction - * @param[in] ground_plane_height Contact ground-plane height in world frame. * @return Vector of Transition Graphs */ std::vector getTransitionGraphs( - const Robot &robot, const DynamicsGraph &graph_builder, double mu, - double ground_plane_height = 0.0) const; + const Robot &robot, const DynamicsGraph &graph_builder, double mu) const; /** * @fn Builds multi-phase factor graph. @@ -116,13 +114,11 @@ class Trajectory { * @param[in] graph_builder GraphBuilder instance. * @param[in] collocation Which collocation scheme to use. * @param[in] mu Coefficient of static friction. - * @param[in] ground_plane_height Contact ground-plane height in world frame. * @return Multi-phase factor graph */ gtsam::NonlinearFactorGraph multiPhaseFactorGraph( const Robot &robot, const DynamicsGraph &graph_builder, - const CollocationScheme collocation, double mu, - double ground_plane_height = 0.0) const; + const CollocationScheme collocation, double mu) const; /** * @fn Returns Initial values for transition graphs. @@ -213,7 +209,7 @@ class Trajectory { * @param[in] robot Robot specification from URDF/SDF. * @param[in] cost_model Noise model * @param[in] step The 3D vector the foot moves in a step. - * @param[in] ground_height z-coordinate of ground plane in world frame. + * @param[in] ground_height z-coordinate of ground in URDF/SDF rest config. * @return All objective factors as a NonlinearFactorGraph */ gtsam::NonlinearFactorGraph contactPointObjectives( @@ -234,7 +230,7 @@ class Trajectory { /** * @fn Create objective factors for slice 0 and slice K. * - * Links at time step 0 are constrained to their wTcom poses and + * Links at time step 0 are constrainted to their wTcom poses and * zero twist, and zero twist and twist acceleration at last time step. * Joint angles velocities and accelerations are set to zero for all joints at * start *and* end. diff --git a/gtdynamics/utils/WalkCycle.cpp b/gtdynamics/utils/WalkCycle.cpp index 2f9ffe34..d2464e7a 100644 --- a/gtdynamics/utils/WalkCycle.cpp +++ b/gtdynamics/utils/WalkCycle.cpp @@ -61,6 +61,7 @@ size_t WalkCycle::numTimeSteps() const { ContactPointGoals WalkCycle::initContactPointGoal(const Robot &robot, double ground_height) const { ContactPointGoals cp_goals; + const Point3 adjust(0, 0, -ground_height); // Go over all phases, and all contact points for (auto &&phase : phases_) { @@ -72,10 +73,7 @@ ContactPointGoals WalkCycle::initContactPointGoal(const Robot &robot, // If no goal set yet, add it here if (cp_goals.count(link_name) == 0) { LinkSharedPtr link = robot.link(link_name); - // Preserve nominal x/y and set contact z to the requested world - // ground height. - const Point3 foot_rest_w = link->bMcom() * cp.point; - const Point3 foot_w(foot_rest_w.x(), foot_rest_w.y(), ground_height); + const Point3 foot_w = link->bMcom() * cp.point + adjust; cp_goals.emplace(link_name, foot_w); } } diff --git a/gtdynamics/utils/WalkCycle.h b/gtdynamics/utils/WalkCycle.h index 8cae40bc..5aaa3f5b 100644 --- a/gtdynamics/utils/WalkCycle.h +++ b/gtdynamics/utils/WalkCycle.h @@ -134,10 +134,10 @@ class WalkCycle { /// Returns the number of time steps, summing over all phases. size_t numTimeSteps() const; - /** + /** * @fn Returns the initial contact point goal for every contact link. * @param[in] robot Robot specification from URDF/SDF. - * @param[in] ground_height z-coordinate of ground plane in world frame. + * @param[in] ground_height z-coordinate of ground in URDF/SDF rest config. * @return Map from link name to goal points. */ ContactPointGoals initContactPointGoal(const Robot &robot, @@ -148,7 +148,7 @@ class WalkCycle { * @param[in] robot Robot specification from URDF/SDF. * @param[in] cost_model Noise model * @param[in] step The 3D vector the foot moves in a step. - * @param[in] ground_height z-coordinate of ground plane in world frame. + * @param[in] ground_height z-coordinate of ground in URDF/SDF rest config. * @return All objective factors as a NonlinearFactorGraph */ gtsam::NonlinearFactorGraph contactPointObjectives( diff --git a/python/gtdynamics/preamble/gtdynamics.h b/python/gtdynamics/preamble/gtdynamics.h index 79caefd0..e69de29b 100644 --- a/python/gtdynamics/preamble/gtdynamics.h +++ b/python/gtdynamics/preamble/gtdynamics.h @@ -1,4 +0,0 @@ -// Ensure these STL aliases are treated as opaque container types so the -// bind_vector/bind_map specializations behave predictably in Python. -PYBIND11_MAKE_OPAQUE(gtdynamics::PointOnLinks); -PYBIND11_MAKE_OPAQUE(gtdynamics::ContactPointGoals); diff --git a/python/tests/test_dynamics_graph.py b/python/tests/test_dynamics_graph.py index c08c27ce..5322fba1 100644 --- a/python/tests/test_dynamics_graph.py +++ b/python/tests/test_dynamics_graph.py @@ -67,20 +67,6 @@ def test_objective_factors(self): self.assertEqual(graph.size(), 10) self.assertEqual(graph.keys().size(), 9) - def test_chain_wrappers(self): - """Test wrapped chain-graph classes are usable from Python.""" - a1 = gtd.CreateRobotFromFile(gtd.URDF_PATH + "/a1/a1.urdf", "a1") - opt = gtd.OptimizerSetting() - gravity = np.array([0.0, 0.0, -9.8]) - - chain_graph = gtd.ChainDynamicsGraph(a1, opt, gravity) - graph = chain_graph.dynamicsFactorGraph(a1, 0, None, None) - self.assertGreater(graph.size(), 0) - - chain_initializer = gtd.ChainInitializer() - init_values = chain_initializer.ZeroValues(a1, 0, 0.0, None) - self.assertGreater(init_values.size(), 0) - if __name__ == "__main__": unittest.main() diff --git a/tests/testConstraintManifold.cpp b/tests/testConstraintManifold.cpp index 364fab81..b6af6533 100644 --- a/tests/testConstraintManifold.cpp +++ b/tests/testConstraintManifold.cpp @@ -418,14 +418,12 @@ TEST(ConstraintManifold_retract, cart_pole_dynamics) { .fixLink("l0"); int j0_id = robot.joint("j0")->id(), j1_id = robot.joint("j1")->id(); const gtsam::Vector3 gravity(0, 0, -10); - constexpr double ground_plane_height = 4.2; OptimizerSetting opt; auto graph_builder = DynamicsGraph(opt, gravity); // constraints graph NonlinearFactorGraph constraints_graph; - constraints_graph.add( - graph_builder.dynamicsFactorGraph(robot, 0, {}, {}, ground_plane_height)); + constraints_graph.add(graph_builder.dynamicsFactorGraph(robot, 0)); // initial values Initializer initializer; diff --git a/tests/testTrajectory.cpp b/tests/testTrajectory.cpp index ef65b5ca..0a363fc8 100644 --- a/tests/testTrajectory.cpp +++ b/tests/testTrajectory.cpp @@ -29,7 +29,6 @@ auto kModel1 = gtsam::noiseModel::Unit::Create(1); auto kModel6 = gtsam::noiseModel::Unit::Create(6); using namespace gtdynamics; -static constexpr double kGroundHeight = 4.2; TEST(Trajectory, error) { using namespace walk_cycle_example; @@ -65,10 +64,10 @@ TEST(Trajectory, error) { EXPECT_LONGS_EQUAL(6, trajectory.getStartTimeStep(2)); EXPECT_LONGS_EQUAL(7, trajectory.getEndTimeStep(2)); - auto cp_goals = walk_cycle.initContactPointGoal(robot, kGroundHeight); + auto cp_goals = walk_cycle.initContactPointGoal(robot, 0); EXPECT_LONGS_EQUAL(5, cp_goals.size()); // regression - EXPECT(gtsam::assert_equal(gtsam::Point3(-0.926417, 1.19512, kGroundHeight), + EXPECT(gtsam::assert_equal(gtsam::Point3(-0.926417, 1.19512, 0.000151302), cp_goals["tarsus_2_L2"], 1e-5)); double gaussian_noise = 1e-5; @@ -85,16 +84,14 @@ TEST(Trajectory, error) { // Check transition graphs vector transition_graphs = - trajectory.getTransitionGraphs(robot, graph_builder, mu, - kGroundHeight); + trajectory.getTransitionGraphs(robot, graph_builder, mu); EXPECT_LONGS_EQUAL(repeat * 2 - 1, transition_graphs.size()); // regression test EXPECT_LONGS_EQUAL(203, transition_graphs[0].size()); // Test multi-phase factor graph. auto graph = trajectory.multiPhaseFactorGraph(robot, graph_builder, - CollocationScheme::Euler, mu, - kGroundHeight); + CollocationScheme::Euler, mu); // regression test EXPECT_LONGS_EQUAL(4298, graph.size()); EXPECT_LONGS_EQUAL(4712, graph.keys().size()); @@ -105,7 +102,7 @@ TEST(Trajectory, error) { // Test objectives for contact links. const Point3 step(0, 0.4, 0); auto contact_link_objectives = trajectory.contactPointObjectives( - robot, noiseModel::Isotropic::Sigma(3, 1e-7), step, kGroundHeight); + robot, noiseModel::Isotropic::Sigma(3, 1e-7), step, 0); // steps = 2+3 per walk cycle, 5 legs involved const size_t expected = repeat * ((2 + 3) * 5); EXPECT_LONGS_EQUAL(expected, contact_link_objectives.size()); diff --git a/tests/testWalkCycle.cpp b/tests/testWalkCycle.cpp index 82fff0da..5a447751 100644 --- a/tests/testWalkCycle.cpp +++ b/tests/testWalkCycle.cpp @@ -92,15 +92,14 @@ TEST(WalkCycle, objectives) { EXPECT_LONGS_EQUAL(swing_links0.size(), 2); EXPECT_LONGS_EQUAL(swing_links1.size(), 2); - // Use a non-zero ground height to exercise explicit wiring. - constexpr double ground_height = 4.2; - Point3 goal_LH(-0.371306, 0.1575, ground_height); // LH - Point3 goal_LF(0.278694, 0.1575, ground_height); // LF - Point3 goal_RF(0.278694, -0.1575, ground_height); // RF - Point3 goal_RH(-0.371306, -0.1575, ground_height); // RH + // Expected contact goal points. + Point3 goal_LH(-0.371306, 0.1575, 0); // LH + Point3 goal_LF(0.278694, 0.1575, 0); // LF + Point3 goal_RF(0.278694, -0.1575, 0); // RF + Point3 goal_RH(-0.371306, -0.1575, 0); // RH // Check initalization of contact goals. - auto cp_goals = walk_cycle.initContactPointGoal(robot, ground_height); + auto cp_goals = walk_cycle.initContactPointGoal(robot, -0.191839); EXPECT_LONGS_EQUAL(4, cp_goals.size()); EXPECT(gtsam::assert_equal(goal_LH, cp_goals["lower1"], 1e-6)); EXPECT(gtsam::assert_equal(goal_LF, cp_goals["lower0"], 1e-6)); diff --git a/webdemo/.gitignore b/webdemo/.gitignore deleted file mode 100644 index 5162a7e2..00000000 --- a/webdemo/.gitignore +++ /dev/null @@ -1,49 +0,0 @@ -package-lock.json - -# Dependencies -/node_modules - -# IDE - VSCode -.vscode/* -!.vscode/extensions.json -!.vscode/settings.json -!.vscode/tasks.json -!.vscode/launch.json - -# IDE - JetBrains -.idea -*.iml -*.ipr -*.iws - -# Local env files -.env -.env.local -.env.*.local - -# Log files -npm-debug.log* -yarn-debug.log* -yarn-error.log* -pnpm-debug.log* - -# Editor directories and files -.DS_Store -Thumbs.db -*.suo -*.ntvs* -*.njsproj -*.sln -*.sw? - -# Build files -/build - -# Cache -.cache -.temp -dist - -public/examples/scenes/unitree_g1 -public/examples/scenes/unitree_a1 -public/examples/scenes/boston_dynamics_spot From d2ee2467dbc72dd5da0899925d5ef0dd04ad39ba Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Mon, 16 Feb 2026 18:01:41 +0000 Subject: [PATCH 11/11] Fix Ubuntu CI: Add library path for python-stubs target Co-authored-by: dellaert <10515273+dellaert@users.noreply.github.com> --- python/CMakeLists.txt | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 45c9d4ad..44345bb8 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -55,11 +55,23 @@ endif() set(GTD_PYTHON_INSTALL_EXTRA "") set(GTD_PYTHON_STUB_MODULE "${PROJECT_NAME}.${PROJECT_NAME}") +# Determine library path for stub generation +# On Linux/Unix, we need LD_LIBRARY_PATH; on macOS, DYLD_LIBRARY_PATH +get_target_property(GTSAM_LIBRARY_LOCATION gtsam LOCATION) +get_filename_component(GTSAM_LIBRARY_DIR "${GTSAM_LIBRARY_LOCATION}" DIRECTORY) + +if(APPLE) + set(GTD_STUB_LIB_PATH_VAR "DYLD_LIBRARY_PATH") +else() + set(GTD_STUB_LIB_PATH_VAR "LD_LIBRARY_PATH") +endif() + add_custom_target( python-stubs COMMAND ${CMAKE_COMMAND} -E env "PYTHONPATH=${GTD_PYTHON_BINARY_DIR}${GTD_PATH_SEP}$ENV{PYTHONPATH}" + "${GTD_STUB_LIB_PATH_VAR}=${GTSAM_LIBRARY_DIR}${GTD_PATH_SEP}${CMAKE_LIBRARY_OUTPUT_DIRECTORY}${GTD_PATH_SEP}$ENV{${GTD_STUB_LIB_PATH_VAR}}" ${PYTHON_EXECUTABLE} -m pybind11_stubgen -o . --ignore-all-errors ${GTD_PYTHON_STUB_MODULE} DEPENDS ${PROJECT_NAME}_py