Conversation
There was a problem hiding this comment.
Pull request overview
This PR restores changes from #421 that were accidentally deleted, fixing kinodynamic planning functionality for the A1 quadruped robot. The primary fix involves properly handling ground plane height throughout the dynamics and kinematics stack.
Changes:
- Added explicit
ground_plane_heightparameter to dynamics and kinematics methods to properly handle contact constraints - Updated test cases to use non-zero ground heights to verify the fix
- Added Python bindings for chain dynamics classes and improved the SWIG interface
Reviewed changes
Copilot reviewed 28 out of 29 changed files in this pull request and generated 4 comments.
Show a summary per file
| File | Description |
|---|---|
gtdynamics/utils/WalkCycle.cpp |
Fixed ground height calculation to preserve nominal x/y coordinates while setting z to requested world height |
gtdynamics/dynamics/DynamicsGraph.h/cpp |
Added ground_plane_height parameter to dynamics factor graph methods |
gtdynamics/dynamics/ChainDynamicsGraph.h/cpp |
Propagated ground plane height through chain dynamics, renamed Poe3Factor to Poe3Expression, cleaned up variable naming |
gtdynamics/kinematics/Kinematics.h |
Added ground_plane_height parameter to kinematics methods |
gtdynamics/kinematics/ContactHeightFactor.h |
Updated documentation to clarify ground-plane height enforcement |
tests/test*.cpp |
Updated tests to use non-zero ground height constant for validation |
gtdynamics.i |
Added Python bindings for chain dynamics classes and constraint specs |
python/gtdynamics/preamble/gtdynamics.h |
Added pybind11 opaque type declarations |
examples/example_a1_walking/chain_dynamics_motion_planning.ipynb |
New Python notebook demonstrating chain dynamics motion planning |
examples/example_quadruped_mp/main.cpp |
Refactored spline computation into Spline struct for better encapsulation |
| 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) |
There was a problem hiding this comment.
The hardcoded file path was replaced with a relative path, but there's no guarantee this file will be writable in the current working directory. Consider using a dedicated output directory or making the path configurable.
| InsertPose(&values, link->id(), link->bMcom()); | ||
| for (auto &&joint : robot.joints()) | ||
| InsertJointAngle(&values, joint->id(), 0.0); | ||
| InsertJointAngle(&values, joint->id(), 1.0); |
There was a problem hiding this comment.
The initial joint angle was changed from 0.0 to 1.0 without explanation. This magic number should be documented or made a named constant to clarify why this specific value is used for initialization.
| // Set parameters for optimizer | ||
| gtsam::LevenbergMarquardtParams params; | ||
| params.setMaxIterations(50); | ||
| // params.setMaxIterations(50); |
There was a problem hiding this comment.
This commented-out code should either be removed or explained with a comment indicating why it's disabled and under what conditions it should be re-enabled.
| // params.setMaxIterations(50); |
| writer.writerow(tposes, results, k); | ||
| curr_t = curr_t + dt; | ||
| k = k + 1; | ||
| // break; |
There was a problem hiding this comment.
This commented-out break statement appears to be debug code and should be removed before merging to production.
| // break; |
1 participant
Changes from #421 that were accidentally deleted by Copilot.
Behavior change
Smaller things
New motion planning notebook
examples/example_a1_walking/chain_dynamics_motion_planning.ipynbNice swing phases obtained entirely by kino-dynamic MP:
and trunk xyz:
