Rename global planner #280

jcnorby · 2022-05-24T16:19:52Z

From Fast Global Motion Planner to Global Body Planner for Legged Robots (GBP-L).

…obots (gbpl)

global_body_planner/README.md

* Add images of topic structure * Update promo figure * Add files via upload * Update README.md * Update README.md * Add files via upload * Add files via upload * Update promo figure * update gbp and utils documentation * fixed image formatting * Rename Fast Global Motion Planner to Global Body Planner for Legged Robots (gbpl) * Update GBP readme with new name * Update README.md * Update README.md * Update README.md Co-authored-by: Alex Stutt <[email protected]> 076f0a6

* Switch SpiritKinematics to QuadKD, switch inverse dynamics function to QuadKD * Fix swing torque bug * Unify computation of swing and stance torques * Bugfix * Fix torque mapping to index correctly past body coordinates * Put identified leg inertial parameters into spirit.sdf and spirit.urdf files. * Update GRFs in RViz Interface * Update RViz settings * Add GRF Array to inverse dynamics output * Remove branch restrictions from CircleCI config * Update tests for leg controller and inverse dynamics * Make leg parameter definitions uniform in QuadKD by loading leg lengths from RBDL-loaded urdf in QuadKD constructor * update NMPC, adaptive body height and constraints using terrain information * add new topic: /state/ground_truth_body_frame that independent on mocap and use imu twist * Fix most bugs in SLIP GRF parameterization, returns plans * Add rough terrain, adaptive body height and orientation, fix foot velocity pseudo inverse * Remove z filtering so SLIP profiles are unaltered * Fix step_20cm terrain shift * Begin gbp tweaks for efficiency, fix rotate GRF * Update closed form solution for linear dynamics in NMPC * Attempt to handle slopes by predicting z_max * Switch build system to catkin_tools, switch spirit* to quad* * Fix timeout in catkin_tools install * Reduce jobs in circle ci to avoid compiling issues * Initial commit * Fix spirit namespace in parsing scripts * fix sdls matrix inverse by comparing with the max eigen value, change nmpc controller test to a trotting at the same point * Add restart flag to mblink converter * Increase update rate of mblink converter to 500Hz * Update model.config * Update README.md * Update processLog.m * Update LICENSE * Update LICENSE.txt * Update LICENSE * Update global_body_planner.yaml * Update planning_utils.h * Update CMakeLists.txt Removed from line 31: include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../external/QuadSDK/SDK/thirdparty) * Change a few more namespacing instances, remove .travis.yml * Delete parseSpiritBag.m * Finish initial LegControllerTemplate integration * Add joint controller (needs cleaning) * Switch back to grf controller * Switch leg controller to leg controller interface, implement leg controller template as abstract class * Fix leg controller test bug * add panic variable, better warm start * fix inverse dynamics, fix gait round up problem * fix inverse dynamics, add hip clearance for footstep planner, revert plan index and current position for twist planner * set the control and estimation rate to 500Hz so as to match the real robot * Switch FastTerrainMap queries from O(n^2) to O(1) * Add getNearestValidFoothold function * Add traversability mask layer to terrain map * Switch to flying trot * Fix bugs in merge (missed some namespaces) * Switch foot markers to spheres * Fix timestamp bug from merge * fix swing foot ID * fix ID again * Begin cleanup of global body planner spin * Add GlobalBodyPlan class and clean up global_body_planner.cpp interface * Remove matlab gen msg * About to begin switch to Eigen for state * cleaned up the mpc stuff and removed dependencies * removed additional extra files going to write unitest for computeFootPos next * Added spirit_rotors.sdf with rotor inertias and launch flag to switch spirit.sdf/spirit_rotors.sdf. Also spirit.sdf upper link masses now match spirit.urdf (from inertial identification). * Correct bugs in gbp actions, add tests, update sim rate, add features to fast terrain map * removed global footstep planner as well as qpoases and osqp * First commit after gbp overhaul with valid leap plans, update some sim update rates, increase slope penalty on footholds * Fix some bugs in the merge * Reduce traversability threshold * clean up pull request * fix ipopt building error * delete qpOASES * specify coinbrew version * Fix out of range bug in FTM, connect timing bug in GBP, update sim timesteps for worlds * Reasonable GBP performance * Fix bug in NMPC test, rename time_ahead to first_element_duration * online update swing foot trajectory * fix merge issues * init nominal prev foot postion, fix interp when z duration = 0, update pass foothold more frequently * limit acceleration of swing foot * Clean up GBP formatting * Remove NMPC test requirement to solve problem given CIs limitations * better swing foot acceleration limit, fix acceleration interplation at the end, smaller feedback on velocity, consider toe radius * fix init of grf position * PR updates (mostly refactoring for cleanliness and const protection) * Rename GlobalBodyPlan functions * Unify stance and swing foot planning (#199) * Initial commit moving to computeFootPlan, not compiled yet * computeFootPlan works, beginning refactoring * Fix impact acceleration bug, switch to MultiFootState for past footholds, clean LFP * Fix timestamp for foot message, clean Co-authored-by: Yanhao Yang <[email protected]> Co-authored-by: Yanhao Yang <[email protected]> * intermediate commit * Finihsed adding tests * intermediate commit before rebase * added pipeline fix * Addressed pr comments * Improve simulation and nmpc (#202) * Improve simulation friction, contact, and physics * Sync new nmpc updates * fix typo in warmstart initialization * Update ipopt options, update discretization * Fix another typo, and hopefully fix the coinbrew building error * update the rotor sdf Co-authored-by: Joe Norby <[email protected]> * Reformat to Google Style for C++ and Add CI Linting (#205) * Reformat all cpp, h, hpp files to google style, implement linting script and add to tests (one error inserted for CI testing) * Make scripts folder for cleaner root, fix test formatting error * Fix CircleCI script path * Format updates to pass linting checks * Update logging and planning launch files, remove global footstep planner (#209) * Update live plot to show ground truth grf, uniform colors and labels (#210) * Add teleop_twist_joy and configure to robot driver control modes (#211) * Add teleop_twist_joy and configure to robot driver control modes * Remove teleop_twist_joy tests of enable button, add cmd_vel to visualization * Remove EKF_tests_2021_12_3 * add grf plan initialization * update initialization * Add stand mode for local planner, remove twist body planner (#217) * Add stand mode for local planner, remove twist body planner * Fix weird formatting changes * Change stand threshold variables to ROS params, add cleaner statement of conditions * Update threshold for end of global plan to ROS param * Added teleop-twist-joy to setup_deps since joystick doesn't respond without this install. * Remove stray lines Co-authored-by: Justin Yim <[email protected]> * Improve swing foot plan (#214) * fix bugs, fix foothold heuristics, better swing foot acc limitation * remove online swing foot update, revert pd gains * Foothold nominal position minimize the maximum distance from hip plan MPC will not modify body and grf plan if fail to solve * fix centrifugal tracking term * Clean * Clean * Remove unused files and functions * Keep the original style * Fix some comments and warning info * Pass test * Resolve review requests * code format * Revert stand pose * Add getNextLiftoffIndex Use contact schedule for end_of_stance Move computeSwingApex to cpp * Fix test All the parameters should match the server... Co-authored-by: Joe Norby <[email protected]> * Clean unused or duplicate params in local planner and nmpc controller (#220) * Add stand mode for local planner, remove twist body planner * Fix weird formatting changes * Change stand threshold variables to ROS params, add cleaner statement of conditions * Update threshold for end of global plan to ROS param * Added teleop-twist-joy to setup_deps since joystick doesn't respond without this install. * Remove local_body_planner params (they are in nmpc yaml) and horizon length/timestep from nmpc (they are in local planner) * Remove stray lines * Update params in CMPC test and remove adaptive complexity test (will come in later PR) * Update local planner test with new param Co-authored-by: Justin Yim <[email protected]> * multiple robot spawning working * Add dash profile for rqt and roslaunch support * intermediate commit * Fix end of stance bug (#224) * Fix end of stance bug * Add comment clarifying logic * Add tcpnodelay to logging, sim control, and sim contact state publishing (#225) * Add tcpnodelay to controller plugin, contact state publisher, and logging scripts, change local plan timestamp to publish time not state time * Use auto for ros::Time declaration * Add CMU text to spirit.urdf and spirit_meshes.urdf * Realign xml text * Add tcpnodelay hint to sim command topic, may actually affect performance (should improve) * Add state traces to RVizInterface (#230) * multi robot sim working * cleaned up launch file * intermediate commit * cleaned up launch files * modified launch file * minor changes * intermediate commit * multi robot teleop working * Adjust local planner to support arbitrary horizons and match length of body and foot plans (#221) * Add stand mode for local planner, remove twist body planner * Fix weird formatting changes * Change stand threshold variables to ROS params, add cleaner statement of conditions * Update threshold for end of global plan to ROS param * Added teleop-twist-joy to setup_deps since joystick doesn't respond without this install. * Remove local_body_planner params (they are in nmpc yaml) and horizon length/timestep from nmpc (they are in local planner) * Remove stray lines * Update params in CMPC test and remove adaptive complexity test (will come in later PR) * Update local planner test with new param * Modify horizon lengths in local planner, nmpc, and tests * Update nmpc controller to support arbitrary horizons, change RVizInterface to show one frame per gait period * Add comment clarifying indexing difference * Fix body height bug in LFP * Update NMPC temporal weights to saturate at a given parameter (was unbounded before), reduce surface normal filter radius Co-authored-by: Justin Yim <[email protected]> * addressed pr comments * Add joint data view (#238) * Add accurate joint limits and linear motor model to gazebo (#236) * Add accurate joint limits and linear motor model to gazebo * Add soft joint limit for knees * fixed formatting errors * fixed formatting * formatting * address pr comments * Add gap worlds and handle missing map data (#237) * Add gaps and pit worlds, add x and y layers to maps, add traversability mask option * Update rest of repo to use z_inpainted, mark areas with differences between z and z_inpainted as untraversable * Add buffer of untraversability around areas with no information, expand foothold search radius from 0.2 to 0.25 * Improve contact stability in simulation (#239) * Improve contact stability: Format contact state publisher Modify contact kp and kd setting to improve contact stability Modify simulation solver setting to improve accuracy * Synchronize changes to the new world files * Fix urdf * Fix indexing bug in gbp replanning (#240) * Update terrain to match mocap space, add some extra worlds (#241) * Remove slope from traversability, add parkour world * Make terrains fit mocap space, add some other terrains, remove pit (buggy) * Update ode solver settings for new worlds * Add abstract hardware interface and refactor Robot Driver (#242) * Add abstract hardware interface class and spirit interface, refactor robot_driver to include robot name and folders for controllers and interfaces, remove open loop robot controller and leg override * Remove deprecated launch files * Correct some comments * Add include for hardware interfaces class in robot driver * Add user_rx_data to RobotDriver, Update is_hw to is_hardware * Make user data var names more specific * Add joint state publisher to robot driver, undo incorrect imu acc mapping * Switch /mcu/state/* to /state/* to differentiate from GR topics, remove robot_proxy and ground_truth_publisher Co-authored-by: RML group <[email protected]> * addressed comments * Add utility function to load multiple sets of casadi code * Respect linting checks * roslaunch robot in arguments * able to stand up * a1 is working * change robot_ to robot_name_, reformat * Refactor quad logger, include reference state and GRFs (#248) * Update quad_logger scripts to handle trajectory data, state GRFs, better fig exporting, and clean and add comments * Update quad logger legend locations and figure limits * Update trajectory publisher to correctly publish traj state, remove deprecated support for publishing the entire trajectory * Add trajectory_publisher to planning.launch * Add IPOPT install fix for CI * Quad logger bugfix for empty data structs * Updated Ghost license from Avik and updated quad-software/LICENSE to CMU & RML. (#254) Co-authored-by: Justin Yim <[email protected]> * Add Doxygen-updating Github workflow (#255) * Remove matplotlibcpp support * Update repo name in Doxygen * Add doxygen workflow * Add devel to Doxygen updated branches * Remove matplotlibcpp comments from gbp * Fix complementary filter in Robot Driver (#253) * Update quad_logger scripts to handle trajectory data, state GRFs, better fig exporting, and clean and add comments * Update quad logger legend locations and figure limits * Update trajectory publisher to correctly publish traj state, remove deprecated support for publishing the entire trajectory * Add trajectory_publisher to planning.launch * Add IPOPT install fix for CI * Quad logger bugfix for empty data structs * Fix bug in velocity comp filter * Second-order filters * Bug fix Co-authored-by: Robomechanics Lab <[email protected]> Co-authored-by: Yanhao Yang <[email protected]> * Finalize complementary filter bug fixes (#258) * Finalize * Move the acceleration flip to the spirit interface. * Add framework for adaptive complexity, GBP updates, and many miscellaneous tweaks (#256) * Add motor model * Start adding state lifting * First draft of centroidal and full state validity checking * Start passing constraint violations out of validity checking functions * First commit with working validity checking * Add hard coded local plan * Leaping with local planner, still have multiple finite elements during the flight phase, add closed form linear dynamics * Initial commit for adaptive complexity * Updating nmpc member variables and params * Streamline debug print statements, switch to more flexible validity checking * Implement maximum height at liftoff * update constraint bounds * Start adding utils test * Add adaptive complexity functions * Update contact sequence and state weights for leap * Continue updates * Nearly finished with compute_nnz_jac_g * Add phase dependent reachability back in * Finish updating nnz functions, still buggy * Move gen code to new folder, fix compute nnz bug, eval_f, bounds * Implement eval_g * Finish main virtual functions * Reformat quad_nlp.cpp and switch to VectorXd instead of MatrixXd where relevant * Remove accidental cold start in update_complexity_schedule * Add indexing vectors to prep for merge with indexing functions * Add support for world frame foot pos in local planner * First pass at merging with nmpc indexing * fix bugs, fix foothold heuristics, better swing foot acc limitation * Continue pre-merge with nmpc_indexing * Remove first_step check form test * Update eval functions and unit test * More bugfixes, passes nnz checks for jac and hess * Fix nmpc_controller test to set complexity schedule length to N+1 * Fix bugs in simple model and switch to world frame foot positions (complex still fails) * remove online swing foot update, revert pd gains * Re-add GBP planning utils updates from merge * Fix some bugs in the merge (formatting, quad kd tests, planning.launch) * Foothold nominal position minimize the maximum distance from hip plan MPC will not modify body and grf plan if fail to solve * Update logging and planning launch files, remove global footstep planner * fix centrifugal tracking term * Fix vector slicing bug - complex sometimes solves, mostly does not * Clean * Add copy constructor for quadNLP * update sdf to increase mu to 1 * Continue adding to shift initial guess * Finish adding warm start for null vars, bug in setup somewhere * constraint indexing fixed, use correct parameters (may still be structure bug) * Add flexibility for initial state * Update constraint funcs, fix small complexity bugs * Fix kinematics bug, add constraints back, increase ma57_pre_alloc * Readd contact schedule and state update to test for to match shift initial guess * Clean * Remove unused files and functions * Keep the original style * Fix some comments and warning info * Add stand mode for local planner, remove twist body planner * Pass test * Resolve review requests * code format * Update warm start to leave new complex decision variables unchanged from last solve * Revert stand pose * Begin simulation testing * Fix weird formatting changes * Fix merge bug * Change to update structure and update initial guess so we can handle changing complexity (still bugs) * Disable warm start if problem structure got more complex * Begin unifying kinematic constraint evaluation between local planner and nlp * Change stand threshold variables to ROS params, add cleaner statement of conditions * Update threshold for end of global plan to ROS param * Added teleop-twist-joy to setup_deps since joystick doesn't respond without this install. * Remove local_body_planner params (they are in nmpc yaml) and horizon length/timestep from nmpc (they are in local planner) * Remove stray lines * Update params in CMPC test and remove adaptive complexity test (will come in later PR) * Update gen functions to match QuadKD * Add getNextLiftoffIndex Use contact schedule for end_of_stance Move computeSwingApex to cpp * Fix test All the parameters should match the server... * Implement evalLiftedTrajectoryConstraints * Update local planner test with new param * Modify horizon lengths in local planner, nmpc, and tests * Update to use equal horizon for foot and body plan * Begin debugging solve fails, increase sim friction * Turn of require_init for better solves * Turn off hip threshold, turn on slack vars, reduce normals vector filter, increase GBP clearance to plan over step * Fix end of stance bug, add a few parameter changes for more feasible leaps * Begin adding panic var for constraints * Finish first pass of constraint relaxations * Add separate panic weigts for constraints * Added parameters for hard and soft bounds, about to edit quadNLP internals to use * Mid transition to relaxed constraints * First pass at soft constraint penalty with hard constraint, runs but not tested for performance * Bugs mostly solved, takes a while to solve though * Nightly commit * Remove takeoff state weight, enforce back leg reachability, add tcpnodelay to controller plugin * More debugging, always enforce hard joint limits * Start adding adaptive case * Refactor GBP terrain height functions, add grid_map support * Add traversability heuristic to gbp * Increase filter size for surface normals (may reverse this), add minimum foot clearance during swing * Add state traces to RVizInterface * First successful leap - front legs lead rear, reduce friction * Start softening all constraints * Finish implementing relaxation of all complex constraints but leave only knee relaxed, solves pretty well but fails fk * Tweak weights, much better performance. Verified constraint jacobian * Add cartesian space PD feedback for swing legs * Remove unnecessary private vars * Reduce jacobian inv damping, align world more accurately, use only cartesian joint PD, tracking looking much better * Add kinematic cost to valid footholds penalizing deviations from nominal and previous * Add accurate joint limits and linear motor model to gazebo * Add gaps and pit worlds, add x and y layers to maps, add traversability mask option * Update rest of repo to use z_inpainted, mark areas with differences between z and z_inpainted as untraversable * Add soft joint limit for knees * Change starting location in quad_gazebo * Add buffer of untraversability around areas with no information, expand foothold search radius from 0.2 to 0.25 * Add lognorm velocity sampling to GBP, restrict replanning to connect phases, tweak vel parameters * Remove slope from traversability, add parkour world * Reduce cartesian feedback gains, add stiff pd for knee hard stop, relax buffer in GBP leap * Make terrains fit mocap space, add some other terrains, remove pit (buggy) * Update ode solver settings for new worlds * Fix indexing bug in gbp replanning * Squashed commit of the following: commit 14a26dfd3d0541733438a170e3c2d41e61d9dca7 Author: jcnorby <[email protected]> Date: Fri Apr 15 08:46:40 2022 -0400 Squashed commit of the following: commit 88276122a535e4e1047b1cbedeab0bba3c304b8e Author: Joe Norby <[email protected]> Date: Thu Apr 14 19:25:07 2022 -0400 Add abstract hardware interface and refactor Robot Driver (#242) * Add abstract hardware interface class and spirit interface, refactor robot_driver to include robot name and folders for controllers and interfaces, remove open loop robot controller and leg override * Remove deprecated launch files * Correct some comments * Add include for hardware interfaces class in robot driver * Add user_rx_data to RobotDriver, Update is_hw to is_hardware * Make user data var names more specific * Add joint state publisher to robot driver, undo incorrect imu acc mapping * Switch /mcu/state/* to /state/* to differentiate from GR topics, remove robot_proxy and ground_truth_publisher Co-authored-by: RML group <[email protected]> commit f96ab7550ac96c63fb110b19f6e063061f82e83f Author: jcnorby <[email protected]> Date: Fri Apr 15 08:41:39 2022 -0400 Squashed commit of the following: commit e3b179ae3f65dbd996dcd25e33332caa5bb4e27c Author: jcnorby <[email protected]> Date: Thu Apr 14 17:00:44 2022 -0400 Fix bug in setting current state and in evaluating constraints, add terrain constraint, update constraint name map commit fdb2b2a0dcffea09a792558a8e6717e1083d0f09 Author: Joe Norby <[email protected]> Date: Wed Apr 13 22:41:50 2022 -0400 Simplify warm start, correct application of foot constraitnts, add terrain variable commit 172247e7b579d78e8053e80e9f706c1ef5554bde Author: jcnorby <[email protected]> Date: Wed Apr 13 18:01:02 2022 -0400 Switch to ZOH foot input, revert to N-1 control vars and remove u1 from finite elements, update conditions for foot dynamics and constraints commit 277720bee311225cabe7efd6e4fe2b74ced75faf Author: jcnorby <[email protected]> Date: Tue Apr 12 18:02:56 2022 -0400 Fix bug which turned foot EOM off, apply new foot traj online (still violates constraints) commit de5990352dc15515012c7e7ab7196430e9feabca Author: jcnorby <[email protected]> Date: Mon Apr 11 12:42:28 2022 -0400 Turn on foot EOM constraints during swing commit a3fd2e7c0f6f88efe13302771c99f53e641dd652 Author: Joe Norby <[email protected]> Date: Thu Apr 7 23:02:31 2022 -0400 Clean up print statements commit 451c43e7089ea4e1ef7360c5ec2c6181565bf196 Merge: 0e94b60e d2b2e18a Author: Joe Norby <[email protected]> Date: Thu Apr 7 21:54:57 2022 -0400 Merge branch 'melodic_devel_adaptive_complexity_add_foot_state_correct' into melodic_devel_adaptive_complexity_add_foot_state commit d2b2e18a93394efcf7a38088caec9d67e05a1433 Author: Joe Norby <[email protected]> Date: Thu Apr 7 21:53:47 2022 -0400 Add terminal input to cost, recovers previous solve time commit 393337c0a16b33b78eaf9de626ad0bdecc2c878c Author: Joe Norby <[email protected]> Date: Thu Apr 7 20:53:36 2022 -0400 Correct foot state warm start and cost commit 0e94b60e21ddbef22415d5caa537f04f4c1430e6 Author: jcnorby <[email protected]> Date: Thu Apr 7 15:51:22 2022 -0400 Start updating cost to include control at last iteration commit 6b58687abf38115515f3cad4d09d53fab43f3474 Merge: a1a19e9a 14f0624c Author: jcnorby <[email protected]> Date: Thu Apr 7 13:37:29 2022 -0400 Merge branch 'melodic_devel_adaptive_complexity' into melodic_devel_adaptive_complexity_add_foot_state commit a1a19e9af9c73675553bec1f0fb6785e036a415c Author: Joe Norby <[email protected]> Date: Wed Apr 6 23:10:14 2022 -0400 Fix tail mode bug, a few more indexing issues commit 6df542a7976d1a6398e1814b1913674f701fc765 Author: Joe Norby <[email protected]> Date: Tue Apr 5 21:21:10 2022 -0400 Initial commit wit foot state vars, midway through porting commit 52a26803419a0d97f9bc965f9d69e5fb4890e6df Merge: 14f0624c dd517bb5 Author: jcnorby <[email protected]> Date: Mon Apr 11 12:43:43 2022 -0400 Merge branch 'devel' into melodic_devel_adaptive_complexity commit dd517bb5c54166340e6b6d42893d690fe7a7d0d4 Author: Joe Norby <[email protected]> Date: Sat Apr 9 11:55:02 2022 -0400 Update terrain to match mocap space, add some extra worlds (#241) * Remove slope from traversability, add parkour world * Make terrains fit mocap space, add some other terrains, remove pit (buggy) * Update ode solver settings for new worlds commit 4be6347d5717532a7ebb10eeefd0605a5462d931 Author: Joe Norby <[email protected]> Date: Thu Apr 7 19:27:06 2022 -0400 Fix indexing bug in gbp replanning (#240) commit 14f0624cae1b31524271dda6e282b49690d94c13 Merge: b3963391 e4c7a964 Author: jcnorby <[email protected]> Date: Thu Apr 7 13:21:54 2022 -0400 Merge branch 'melodic_devel_mocap_terrain' into melodic_devel_adaptive_complexity commit b39633911f8135d4eb93b635312788e7035deec0 Merge: 5ca1d105 399836d2 Author: jcnorby <[email protected]> Date: Thu Apr 7 13:15:23 2022 -0400 Merge branch 'devel' into melodic_devel_adaptive_complexity commit 5ca1d10534770a30d53a610383ba1cc15ed9cd72 Author: Joe Norby <[email protected]> Date: Thu Mar 31 17:55:15 2022 -0400 Add velocity constraint back in, relax knee constraint, tighten soft joint limits, soften cartesian PD commit 5a9487d164462903c61721a95890a46718bbd485 Merge: a8d357cc a509af75 Author: Joe Norby <[email protected]> Date: Wed Mar 30 18:23:25 2022 -0400 Merge branch 'melodic_devel_adaptive_complexity' of github.com:robomechanics/quad-software into melodic_devel_adaptive_complexity commit a509af75ccca8b028bfc028a7cb667dcd9e24acf Author: jcnorby <[email protected]> Date: Mon Mar 28 13:11:05 2022 -0400 Readd hip height constraint on foot trajectory, speed up sim commit a711fda92e0767c4a2bbf71f5dad8fa65bd1a89a Merge: 262d4a16 69841722 Author: jcnorby <[email protected]> Date: Mon Mar 28 12:39:39 2022 -0400 Merge branch 'melodic_devel_add_joint_data_to_liveplot' into melodic_devel_adaptive_complexity commit 698417222b2edaf461b4a9d1482dde96abb427d5 Author: jcnorby <[email protected]> Date: Mon Mar 28 12:11:37 2022 -0400 Add joint data view commit 262d4a16708f4694d477c81205f0b37ca06d5453 Author: jcnorby <[email protected]> Date: Mon Mar 28 11:56:01 2022 -0400 Lower max GRFs in NMPC and remove some print statements commit 6d3b39195817a12c13a2688b93e4d0b205028e3d Merge: f23165a4 15bef6bb Author: jcnorby <[email protected]> Date: Mon Mar 28 10:41:07 2022 -0400 Merge branch 'melodic_devel_correct_sim_kin_dyn_limits' into melodic_devel_adaptive_complexity commit a8d357cc777bcc343af97f1d430de8daa8d23551 Merge: 977e9812 50749b59 Author: Joe Norby <[email protected]> Date: Sun Mar 27 21:04:37 2022 -0400 Merge branch 'melodic_devel_mapping_set_untraversable_regions' into melodic_devel_adaptive_complexity * Merged devel, squashed commit of the following: commit 88276122a535e4e1047b1cbedeab0bba3c304b8e Author: Joe Norby <[email protected]> Date: Thu Apr 14 19:25:07 2022 -0400 Add abstract hardware interface and refactor Robot Driver (#242) * Add abstract hardware interface class and spirit interface, refactor robot_driver to include robot name and folders for controllers and interfaces, remove open loop robot controller and leg override * Remove deprecated launch files * Correct some comments * Add include for hardware interfaces class in robot driver * Add user_rx_data to RobotDriver, Update is_hw to is_hardware * Make user data var names more specific * Add joint state publisher to robot driver, undo incorrect imu acc mapping * Switch /mcu/state/* to /state/* to differentiate from GR topics, remove robot_proxy and ground_truth_publisher Co-authored-by: RML group <[email protected]> commit dd517bb5c54166340e6b6d42893d690fe7a7d0d4 Author: Joe Norby <[email protected]> Date: Sat Apr 9 11:55:02 2022 -0400 Update terrain to match mocap space, add some extra worlds (#241) * Remove slope from traversability, add parkour world * Make terrains fit mocap space, add some other terrains, remove pit (buggy) * Update ode solver settings for new worlds commit 4be6347d5717532a7ebb10eeefd0605a5462d931 Author: Joe Norby <[email protected]> Date: Thu Apr 7 19:27:06 2022 -0400 Fix indexing bug in gbp replanning (#240) * Add additional terrains and update starting position * Expand traversability filter * Initial commit benchmarking timing performance before removing slack variables * Update settings to match current adaptive complexity branch * Add parameters to enable leaping and AC * Unify terrain and traversability settings for leaping and AC * Disable linear mm, add test fixture to gbp and begin porting tests * Update gbp to pass tests, tweak some refinement parameters * Beginning modifications to re-enable leaping: readd Jinv damping, remove cartesian feedback * Soften knee hard stop and add slight front liftoff period in contact schedule * Update quad_logger scripts to handle trajectory data, state GRFs, better fig exporting, and clean and add comments * Update quad logger legend locations and figure limits * Update trajectory publisher to correctly publish traj state, remove deprecated support for publishing the entire trajectory * Update QuadKD to enforce knee-above-hip, use accurate joint limits, and select knee position to minimize error * Update nmpc gains, u_max, solve times to old values, use forward euler dynamics, disable hard complex bounds, disable foot dynamics, remove front foot liftoff period * Update robot driver to publish current traj state, quad utils to permit this * Nightly commit - re-add foot traj modification, complex elements * Increase friction, change current_plan_index from 0 to -1 * Update complimentarity filter and add debug statements * Fix bug in velocity comp filter * Add nmpc and lp parameter changes for 1.5m/s trotting in hw * Add foot height constraint framework, data not yet passed. Revert casadi funcs to 4/20 commit but with terrain added * Add terrain height and slope into nlp, solves but fails on steps * Mid-debugging overconstraining of foot positions * Refactor nmpc param loading, constructor, quadnlp constructor, test that OG casadi fcns work * Add vectors for dimension of cost terms and include in cost calculations. Not tested with complex yet * Add new casadi funcs which match devel's (except using closed form integration for linear acc rather than Euler) * Remove feet_in_simple param, begin correcting foot constraints * Set LP update rate to 300Hz to match mean solve time, tweak warm start * Remove unnecessary changes from devel * Clean up NMPC tests and primary working functions * Pass linting checks * Clean up comments and PR review refactors * Make parameter loading a member of the PlannerConfig struct, integrate in test fixture and ros interface * Add todo comment * Reduce catkin tests parallel jobs * Move node handle constructor to text fixture body (attempt to fix CircleCI bug), remove some print statements * More cleaning for PR * Reduce nmpc max solve time * Parameter updates for successful hardware deployment * Final merge updates - fix global plan state publishing, make traversability names more clear, enforce legbases stay over terrain in GBP * Update names to quad-sdk, update README with construction notice Co-authored-by: Yanhao Yang <[email protected]> Co-authored-by: Justin Yim <[email protected]> Co-authored-by: RML group <[email protected]> Co-authored-by: Robomechanics Lab <[email protected]> * Fix small bug in checking isInMap (switch to using grid_map) (#260) * Update to c++14 (#262) * removing unneeded files * removing commented out .sdf sections. * remove unused simulator plugins * Permit variable horizon lengths in Local Planner, add NLP diagnostics, add working mixed complexity solving. (#263) * Add motor model * Start adding state lifting * First draft of centroidal and full state validity checking * Start passing constraint violations out of validity checking functions * First commit with working validity checking * Add hard coded local plan * Leaping with local planner, still have multiple finite elements during the flight phase, add closed form linear dynamics * Initial commit for adaptive complexity * Updating nmpc member variables and params * Streamline debug print statements, switch to more flexible validity checking * Implement maximum height at liftoff * update constraint bounds * Start adding utils test * Add adaptive complexity functions * Update contact sequence and state weights for leap * Continue updates * Nearly finished with compute_nnz_jac_g * Add phase dependent reachability back in * Finish updating nnz functions, still buggy * Move gen code to new folder, fix compute nnz bug, eval_f, bounds * Implement eval_g * Finish main virtual functions * Reformat quad_nlp.cpp and switch to VectorXd instead of MatrixXd where relevant * Remove accidental cold start in update_complexity_schedule * Add indexing vectors to prep for merge with indexing functions * Add support for world frame foot pos in local planner * First pass at merging with nmpc indexing * fix bugs, fix foothold heuristics, better swing foot acc limitation * Continue pre-merge with nmpc_indexing * Remove first_step check form test * Update eval functions and unit test * More bugfixes, passes nnz checks for jac and hess * Fix nmpc_controller test to set complexity schedule length to N+1 * Fix bugs in simple model and switch to world frame foot positions (complex still fails) * remove online swing foot update, revert pd gains * Re-add GBP planning utils updates from merge * Fix some bugs in the merge (formatting, quad kd tests, planning.launch) * Foothold nominal position minimize the maximum distance from hip plan MPC will not modify body and grf plan if fail to solve * Update logging and planning launch files, remove global footstep planner * fix centrifugal tracking term * Fix vector slicing bug - complex sometimes solves, mostly does not * Clean * Add copy constructor for quadNLP * update sdf to increase mu to 1 * Continue adding to shift initial guess * Finish adding warm start for null vars, bug in setup somewhere * constraint indexing fixed, use correct parameters (may still be structure bug) * Add flexibility for initial state * Update constraint funcs, fix small complexity bugs * Fix kinematics bug, add constraints back, increase ma57_pre_alloc * Readd contact schedule and state update to test for to match shift initial guess * Clean * Remove unused files and functions * Keep the original style * Fix some comments and warning info * Add stand mode for local planner, remove twist body planner * Pass test * Resolve review requests * code format * Update warm start to leave new complex decision variables unchanged from last solve * Revert stand pose * Begin simulation testing * Fix weird formatting changes * Fix merge bug * Change to update structure and update initial guess so we can handle changing complexity (still bugs) * Disable warm start if problem structure got more complex * Begin unifying kinematic constraint evaluation between local planner and nlp * Change stand threshold variables to ROS params, add cleaner statement of conditions * Update threshold for end of global plan to ROS param * Added teleop-twist-joy to setup_deps since joystick doesn't respond without this install. * Remove local_body_planner params (they are in nmpc yaml) and horizon length/timestep from nmpc (they are in local planner) * Remove stray lines * Update params in CMPC test and remove adaptive complexity test (will come in later PR) * Update gen functions to match QuadKD * Add getNextLiftoffIndex Use contact schedule for end_of_stance Move computeSwingApex to cpp * Fix test All the parameters should match the server... * Implement evalLiftedTrajectoryConstraints * Update local planner test with new param * Modify horizon lengths in local planner, nmpc, and tests * Update to use equal horizon for foot and body plan * Begin debugging solve fails, increase sim friction * Turn of require_init for better solves * Turn off hip threshold, turn on slack vars, reduce normals vector filter, increase GBP clearance to plan over step * Fix end of stance bug, add a few parameter changes for more feasible leaps * Begin adding panic var for constraints * Finish first pass of constraint relaxations * Add separate panic weigts for constraints * Added parameters for hard and soft bounds, about to edit quadNLP internals to use * Mid transition to relaxed constraints * First pass at soft constraint penalty with hard constraint, runs but not tested for performance * Bugs mostly solved, takes a while to solve though * Nightly commit * Remove takeoff state weight, enforce back leg reachability, add tcpnodelay to controller plugin * More debugging, always enforce hard joint limits * Start adding adaptive case * Refactor GBP terrain height functions, add grid_map support * Add traversability heuristic to gbp * Increase filter size for surface normals (may reverse this), add minimum foot clearance during swing * Add state traces to RVizInterface * First successful leap - front legs lead rear, reduce friction * Start softening all constraints * Finish implementing relaxation of all complex constraints but leave only knee relaxed, solves pretty well but fails fk * Tweak weights, much better performance. Verified constraint jacobian * Add cartesian space PD feedback for swing legs * Remove unnecessary private vars * Reduce jacobian inv damping, align world more accurately, use only cartesian joint PD, tracking looking much better * Add kinematic cost to valid footholds penalizing deviations from nominal and previous * Add accurate joint limits and linear motor model to gazebo * Add gaps and pit worlds, add x and y layers to maps, add traversability mask option * Update rest of repo to use z_inpainted, mark areas with differences between z and z_inpainted as untraversable * Add soft joint limit for knees * Change starting location in quad_gazebo * Add buffer of untraversability around areas with no information, expand foothold search radius from 0.2 to 0.25 * Add lognorm velocity sampling to GBP, restrict replanning to connect phases, tweak vel parameters * Remove slope from traversability, add parkour world * Reduce cartesian feedback gains, add stiff pd for knee hard stop, relax buffer in GBP leap * Make terrains fit mocap space, add some other terrains, remove pit (buggy) * Update ode solver settings for new worlds * Fix indexing bug in gbp replanning * Squashed commit of the following: commit 14a26dfd3d0541733438a170e3c2d41e61d9dca7 Author: jcnorby <[email protected]> Date: Fri Apr 15 08:46:40 2022 -0400 Squashed commit of the following: commit 88276122a535e4e1047b1cbedeab0bba3c304b8e Author: Joe Norby <[email protected]> Date: Thu Apr 14 19:25:07 2022 -0400 Add abstract hardware interface and refactor Robot Driver (#242) * Add abstract hardware interface class and spirit interface, refactor robot_driver to include robot name and folders for controllers and interfaces, remove open loop robot controller and leg override * Remove deprecated launch files * Correct some comments * Add include for hardware interfaces class in robot driver * Add user_rx_data to RobotDriver, Update is_hw to is_hardware * Make user data var names more specific * Add joint state publisher to robot driver, undo incorrect imu acc mapping * Switch /mcu/state/* to /state/* to differentiate from GR topics, remove robot_proxy and ground_truth_publisher Co-authored-by: RML group <[email protected]> commit f96ab7550ac96c63fb110b19f6e063061f82e83f Author: jcnorby <[email protected]> Date: Fri Apr 15 08:41:39 2022 -0400 Squashed commit of the following: commit e3b179ae3f65dbd996dcd25e33332caa5bb4e27c Author: jcnorby <[email protected]> Date: Thu Apr 14 17:00:44 2022 -0400 Fix bug in setting current state and in evaluating constraints, add terrain constraint, update constraint name map commit fdb2b2a0dcffea09a792558a8e6717e1083d0f09 Author: Joe Norby <[email protected]> Date: Wed Apr 13 22:41:50 2022 -0400 Simplify warm start, correct application of foot constraitnts, add terrain variable commit 172247e7b579d78e8053e80e9f706c1ef5554bde Author: jcnorby <[email protected]> Date: Wed Apr 13 18:01:02 2022 -0400 Switch to ZOH foot input, revert to N-1 control vars and remove u1 from finite elements, update conditions for foot dynamics and constraints commit 277720bee311225cabe7efd6e4fe2b74ced75faf Author: jcnorby <[email protected]> Date: Tue Apr 12 18:02:56 2022 -0400 Fix bug which turned foot EOM off, apply new foot traj online (still violates constraints) commit de5990352dc15515012c7e7ab7196430e9feabca Author: jcnorby <[email protected]> Date: Mon Apr 11 12:42:28 2022 -0400 Turn on foot EOM constraints during swing commit a3fd2e7c0f6f88efe13302771c99f53e641dd652 Author: Joe Norby <[email protected]> Date: Thu Apr 7 23:02:31 2022 -0400 Clean up print statements commit 451c43e7089ea4e1ef7360c5ec2c6181565bf196 Merge: 0e94b60e d2b2e18a Author: Joe Norby <[email protected]> Date: Thu Apr 7 21:54:57 2022 -0400 Merge branch 'melodic_devel_adaptive_complexity_add_foot_state_correct' into melodic_devel_adaptive_complexity_add_foot_state commit d2b2e18a93394efcf7a38088caec9d67e05a1433 Author: Joe Norby <[email protected]> Date: Thu Apr 7 21:53:47 2022 -0400 Add terminal input to cost, recovers previous solve time commit 393337c0a16b33b78eaf9de626ad0bdecc2c878c Author: Joe Norby <[email protected]> Date: Thu Apr 7 20:53:36 2022 -0400 Correct foot state warm start and cost commit 0e94b60e21ddbef22415d5caa537f04f4c1430e6 Author: jcnorby <[email protected]> Date: Thu Apr 7 15:51:22 2022 -0400 Start updating cost to include control at last iteration commit 6b58687abf38115515f3cad4d09d53fab43f3474 Merge: a1a19e9a 14f0624c Author: jcnorby <[email protected]> Date: Thu Apr 7 13:37:29 2022 -0400 Merge branch 'melodic_devel_adaptive_complexity' into melodic_devel_adaptive_complexity_add_foot_state commit a1a19e9af9c73675553bec1f0fb6785e036a415c Author: Joe Norby <[email protected]> Date: Wed Apr 6 23:10:14 2022 -0400 Fix tail mode bug, a few more indexing issues commit 6df542a7976d1a6398e1814b1913674f701fc765 Author: Joe Norby <[email protected]> Date: Tue Apr 5 21:21:10 2022 -0400 Initial commit wit foot state vars, midway through porting commit 52a26803419a0d97f9bc965f9d69e5fb4890e6df Merge: 14f0624c dd517bb5 Author: jcnorby <[email protected]> Date: Mon Apr 11 12:43:43 2022 -0400 Merge branch 'devel' into melodic_devel_adaptive_complexity commit dd517bb5c54166340e6b6d42893d690fe7a7d0d4 Author: Joe Norby <[email protected]> Date: Sat Apr 9 11:55:02 2022 -0400 Update terrain to match mocap space, add some extra worlds (#241) * Remove slope from traversability, add parkour world * Make terrains fit mocap space, add some other terrains, remove pit (buggy) * Update ode solver settings for new worlds commit 4be6347d5717532a7ebb10eeefd0605a5462d931 Author: Joe Norby <[email protected]> Date: Thu Apr 7 19:27:06 2022 -0400 Fix indexing bug in gbp replanning (#240) commit 14f0624cae1b31524271dda6e282b49690d94c13 Merge: b3963391 e4c7a964 Author: jcnorby <[email protected]> Date: Thu Apr 7 13:21:54 2022 -0400 Merge branch 'melodic_devel_mocap_terrain' into melodic_devel_adaptive_complexity commit b39633911f8135d4eb93b635312788e7035deec0 Merge: 5ca1d105 399836d2 Author: jcnorby <[email protected]> Date: Thu Apr 7 13:15:23 2022 -0400 Merge branch 'devel' into melodic_devel_adaptive_complexity commit 5ca1d10534770a30d53a610383ba1cc15ed9cd72 Author: Joe Norby <[email protected]> Date: Thu Mar 31 17:55:15 2022 -0400 Add velocity constraint back in, relax knee constraint, tighten soft joint limits, soften cartesian PD commit 5a9487d164462903c61721a95890a46718bbd485 Merge: a8d357cc a509af75 Author: Joe Norby <[email protected]> Date: Wed Mar 30 18:23:25 2022 -0400 Merge branch 'melodic_devel_adaptive_complexity' of github.com:robomechanics/quad-software into melodic_devel_adaptive_complexity commit a509af75ccca8b028bfc028a7cb667dcd9e24acf Author: jcnorby <[email protected]> Date: Mon Mar 28 13:11:05 2022 -0400 Readd hip height constraint on foot trajectory, speed up sim commit a711fda92e0767c4a2bbf71f5dad8fa65bd1a89a Merge: 262d4a16 69841722 Author: jcnorby <[email protected]> Date: Mon Mar 28 12:39:39 2022 -0400 Merge branch 'melodic_devel_add_joint_data_to_liveplot' into melodic_devel_adaptive_complexity commit 698417222b2edaf461b4a9d1482dde96abb427d5 Author: jcnorby <[email protected]> Date: Mon Mar 28 12:11:37 2022 -0400 Add joint data view commit 262d4a16708f4694d477c81205f0b37ca06d5453 Author: jcnorby <[email protected]> Date: Mon Mar 28 11:56:01 2022 -0400 Lower max GRFs in NMPC and remove some print statements commit 6d3b39195817a12c13a2688b93e4d0b205028e3d Merge: f23165a4 15bef6bb Author: jcnorby <[email protected]> Date: Mon Mar 28 10:41:07 2022 -0400 Merge branch 'melodic_devel_correct_sim_kin_dyn_limits' into melodic_devel_adaptive_complexity commit a8d357cc777bcc343af97f1d430de8daa8d23551 Merge: 977e9812 50749b59 Author: Joe Norby <[email protected]> Date: Sun Mar 27 21:04:37 2022 -0400 Merge branch 'melodic_devel_mapping_set_untraversable_regions' into melodic_devel_adaptive_complexity * Merged devel, squashed commit of the following: commit 88276122a535e4e1047b1cbedeab0bba3c304b8e Author: Joe Norby <[email protected]> Date: Thu Apr 14 19:25:07 2022 -0400 Add abstract hardware interface and refactor Robot Driver (#242) * Add abstract hardware interface class and spirit interface, refactor robot_driver to include robot name and folders for controllers and interfaces, remove open loop robot controller and leg override * Remove deprecated launch files * Correct some comments * Add include for hardware interfaces class in robot driver * Add user_rx_data to RobotDriver, Update is_hw to is_hardware * Make user data var names more specific * Add joint state publisher to robot driver, undo incorrect imu acc mapping * Switch /mcu/state/* to /state/* to differentiate from GR topics, remove robot_proxy and ground_truth_publisher Co-authored-by: RML group <[email protected]> commit dd517bb5c54166340e6b6d42893d690fe7a7d0d4 Author: Joe Norby <[email protected]> Date: Sat Apr 9 11:55:02 2022 -0400 Update terrain to match mocap space, add some extra worlds (#241) * Remove slope from traversability, add parkour world * Make terrains fit mocap space, add some other terrains, remove pit (buggy) * Update ode solver settings for new worlds commit 4be6347d5717532a7ebb10eeefd0605a5462d931 Author: Joe Norby <[email protected]> Date: Thu Apr 7 19:27:06 2022 -0400 Fix indexing bug in gbp replanning (#240) * Add additional terrains and update starting position * Expand traversability filter * Initial commit benchmarking timing performance before removing slack variables * Update settings to match current adaptive complexity branch * Add parameters to enable leaping and AC * Unify terrain and traversability settings for leaping and AC * Disable linear mm, add test fixture to gbp and begin porting tests * Update gbp to pass tests, tweak some refinement parameters * Beginning modifications to re-enable leaping: readd Jinv damping, remove cartesian feedback * Soften knee hard stop and add slight front liftoff period in contact schedule * Update quad_logger scripts to handle trajectory data, state GRFs, better fig exporting, and clean and add comments * Update quad logger legend locations and figure limits * Update trajectory publisher to correctly publish traj state, remove deprecated support for publishing the entire trajectory * Update QuadKD to enforce knee-above-hip, use accurate joint limits, and select knee position to minimize error * Update nmpc gains, u_max, solve times to old values, use forward euler dynamics, disable hard complex bounds, disable foot dynamics, remove front foot liftoff period * Update robot driver to publish current traj state, quad utils to permit this * Nightly commit - re-add foot traj modification, complex elements * Increase friction, change current_plan_index from 0 to -1 * Update complimentarity filter and add debug statements * Fix bug in velocity comp filter * Add nmpc and lp parameter changes for 1.5m/s trotting in hw * Add foot height constraint framework, data not yet passed. Revert casadi funcs to 4/20 commit but with terrain added * Add terrain height and slope into nlp, solves but fails on steps * Mid-debugging overconstraining of foot positions * Refactor nmpc param loading, constructor, quadnlp constructor, test that OG casadi fcns work * Add vectors for dimension of cost terms and include in cost calculations. Not tested with complex yet * Add new casadi funcs which match devel's (except using closed form integration for linear acc rather than Euler) * Remove feet_in_simple param, begin correcting foot constraints * Set LP update rate to 300Hz to match mean solve time, tweak warm start * Remove unnecessary changes from devel * Clean up NMPC tests and primary working functions * Pass linting checks * Complex elements work but foot still overconstrained if allowed to move * Clean up comments and PR review refactors * Make parameter loading a member of the PlannerConfig struct, integrate in test fixture and ros interface * Add todo comment * Squashed commit of the following: commit af732b46f705250058b3fae1ad38987588499859 Author: Joe Norby <[email protected]> Date: Tue May 3 19:48:36 2022 -0400 Add todo comment commit 4fca314d6988789199778653e36585f4efbee3f7 Author: Joe Norby <[email protected]> Date: Tue May 3 19:45:35 2022 -0400 Make parameter loading a member of the PlannerConfig struct, integrate in test fixture and ros interface * Reduce catkin tests parallel jobs * Move node handle constructor to text fixture body (attempt to fix CircleCI bug), remove some print statements * Add logging and plotting support for NLP data * Foot state constraints are cleaner and not infeasible but still not solving, likely a cost bug * Switch to piecewise FOH foot control, fixes overconstraint issue! * Begin debugging terrain height constraint, need to try debug mode * Update logging.launch to take bag_name as an arg * Relax foot height constraint and make clearance a function of foot vel, solves better but not perfect * Repair nmpc traj lifting and evaluating constraints * Support variable horizon planning * Readd stabilization of foot plan (which is important), relax mm constraint, move updateHorizonLength to its own function * Increase foot tracking weights to incentivize changing body pose, fix bug in joint vel constraint bounds * Add step_25cm world * Add low pass filter on new foothold positions * Add batch sim script * Add launchfile to issue a standing command * Better align step_20cm grid map with world * Change settings to match devel * Change nmpc print and AC settings to standard values * Readd NMPCController test * Clean up comments in nmpc_controller.yaml * Remove unnecessary LPF on foot position which caused failed tests * Respect linting checks * Bugfixes for variable horizon support (don't change foot plan length) * Readd index update of body and grf plan in local planner Co-authored-by: Yanhao Yang <[email protected]> Co-authored-by: Justin Yim <[email protected]> Co-authored-by: RML group <[email protected]> Co-authored-by: Robomechanics Lab <[email protected]> * solve namespace issue, but still has problems * Resolve multi-robot tf tree and rviz namespaces * Respect linting checks * Resolve test issuses * merge with devel, resolve conflicts, catkin test succeeds, lint respected * Commit to test CI * Have multi robot plan just call planning.launch, fix logging scripts to support a namespace arg * solve single and multiple robots launch issue * rename quad_plan.launch * started refactor branch to work on quad_utils and quad_msgs * added a readme to describe the quad util package * added quad_msgs, quad_utils readme, and delete BodyFroceEstimate.msg * remove RobotStateTrajectory, modified quad_msgs/README * delete unused msgs in CMakeList * delete trajectory_publisher and related functions * delete body_force_estimator and contact_detection packages * delete nmpc dependency on quad_msgs/RobotStateTrajectory.h * launch file cleanup * delete local_planner dependency on quad_msgs/RobotStateTrajectory.h * exclude contact/body force.yaml in load_params.launch file * solve replay issue, reorganize the rviz profile * Updated quad_simulator readme and removed unused launch files. * readme tweaks. * Changed math_utils fxns to pass inputs by const * Fix lint * Local Planner Refactor (#266) * clean local planner * Fix test * Fix test again * README + small cleanups * Format * Add info Co-authored-by: Alex Stutt <[email protected]> * Global Body Planner Refactor (#264) * Update README.md * Update parameter formatting in GBP, clean some unused ones * Remove temp file * Update README.md * Remove deprecated functions from planning_utils * Streamlining gbp refactors, adding vertical velocity params * Fix small bugs in param loading and valid state checking * Update connect stance time so that max vel = planning_config.h_nom * Update leaping traversability checks to yield better takeoff locations * Update goal state to only require x/y position * Update README.md * Remove a few more unnecessary parameters and update traversability to permit planning in rough terrain * Update README.md * add documentation to planning_utils * temp changes * Update tests to be more robust * Respect linting checks * intermediate commit * finish planning_uutils documentation, add enum * respect lint * Tweak comment to trigger CI * respect lint * Update README.md Co-authored-by: jrenaf <[email protected]> * Update main Readme (#272) * Update README.md * Update README.md * Add front page figure * Update README.md * Update README.md * Update README.md * Update README.md * divide param launch * reorganize param * Fix local planner timestep bug and readd trajectory publisher (#274) * Readd trajectory publisher * Fix global plan timestamp setting in local planner * Readd gazebo plugins * Add correct plugins back in and begin implementing global param reading * Update visualization and logging to fully support multi-robot operation * Update plotjuggler to add robot_1 namespace * Split topic yamls into global and robot topics * Update remaining parameter setting and checking, tweak some leaping parameters for good measure * Add arg to load robot params in circleci script * Update README.md * Remove ekf estimator package (now in robot driver) and deprecated launch files * Update documentation (#278) * Add images of topic structure * Update promo figure * Add files via upload * Update README.md * Update README.md * Add files via upload * Add files via upload * Update promo figure * update gbp and utils documentation * fixed image formatting * fixed links in documentation * readme formatting Co-authored-by: Alex Stutt <[email protected]> * Rename global planner (#280) * Add images of topic structure * Update promo figure * Add files via upload * Update README.md * Update README.md * Add files via upload * Add files via upload * Update promo figure * update gbp and utils documentation * fixed image formatting * Rename Fast Global Motion Planner to Global Body Planner for Legged Robots (gbpl) * Update GBP readme with new name * Update README.md * Update README.md * Update README.md Co-authored-by: Alex Stutt <[email protected]> * cleaned out all osqp stuff (#281) * Additional changes to match devel Co-authored-by: Justin Yim <[email protected]> Co-authored-by: Yanhao Yang <[email protected]> Co-authored-by: Robomechanics Lab <[email protected]> Co-authored-by: justinyim <[email protected]> Co-authored-by: ardalantj <[email protected]> Co-authored-by: Yanhao Yang <[email protected]> Co-authored-by: Bill Yu <[email protected]> Co-authored-by: jrenaf <[email protected]> Co-authored-by: Jiming Ren <[email protected]> Co-authored-by: RML group <[email protected]> Co-authored-by: qishuny <[email protected]> Co-authored-by: Alex Stutt <[email protected]>

* Switch SpiritKinematics to QuadKD, switch inverse dynamics function to QuadKD * Fix swing torque bug * Unify computation of swing and stance torques * Bugfix * Fix torque mapping to index correctly past body coordinates * Put identified leg inertial parameters into spirit.sdf and spirit.urdf files. * Update GRFs in RViz Interface * Update RViz settings * Add GRF Array to inverse dynamics output * Remove branch restrictions from CircleCI config * Update tests for leg controller and inverse dynamics * Make leg parameter definitions uniform in QuadKD by loading leg lengths from RBDL-loaded urdf in QuadKD constructor * update NMPC, adaptive body height and constraints using terrain information * add new topic: /state/ground_truth_body_frame that independent on mocap and use imu twist * Fix most bugs in SLIP GRF parameterization, returns plans * Add rough terrain, adaptive body height and orientation, fix foot velocity pseudo inverse * Remove z filtering so SLIP profiles are unaltered * Fix step_20cm terrain shift * Begin gbp tweaks for efficiency, fix rotate GRF * Update closed form solution for linear dynamics in NMPC * Attempt to handle slopes by predicting z_max * Switch build system to catkin_tools, switch spirit* to quad* * Fix timeout in catkin_tools install * Reduce jobs in circle ci to avoid compiling issues * Initial commit * Fix spirit namespace in parsing scripts * fix sdls matrix inverse by comparing with the max eigen value, change nmpc controller test to a trotting at the same point * Add restart flag to mblink converter * Increase update rate of mblink converter to 500Hz * Update model.config * Update README.md * Update processLog.m * Update LICENSE * Update LICENSE.txt * Update LICENSE * Update global_body_planner.yaml * Update planning_utils.h * Update CMakeLists.txt Removed from line 31: include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../external/QuadSDK/SDK/thirdparty) * Change a few more namespacing instances, remove .travis.yml * Delete parseSpiritBag.m * Finish initial LegControllerTemplate integration * Add joint controller (needs cleaning) * Switch back to grf controller * Switch leg controller to leg controller interface, implement leg controller template as abstract class * Fix leg controller test bug * add panic variable, better warm start * fix inverse dynamics, fix gait round up problem * fix inverse dynamics, add hip clearance for footstep planner, revert plan index and current position for twist planner * set the control and estimation rate to 500Hz so as to match the real robot * Switch FastTerrainMap queries from O(n^2) to O(1) * Add getNearestValidFoothold function * Add traversability mask layer to terrain map * Switch to flying trot * Fix bugs in merge (missed some namespaces) * Switch foot markers to spheres * Fix timestamp bug from merge * fix swing foot ID * fix ID again * Begin cleanup of global body planner spin * Add GlobalBodyPlan class and clean up global_body_planner.cpp interface * Remove matlab gen msg * About to begin switch to Eigen for state * cleaned up the mpc stuff and removed dependencies * removed additional extra files going to write unitest for computeFootPos next * Added spirit_rotors.sdf with rotor inertias and launch flag to switch spirit.sdf/spirit_rotors.sdf. Also spirit.sdf upper link masses now match spirit.urdf (from inertial identification). * Correct bugs in gbp actions, add tests, update sim rate, add features to fast terrain map * removed global footstep planner as well as qpoases and osqp * First commit after gbp overhaul with valid leap plans, update some sim update rates, increase slope penalty on footholds * Fix some bugs in the merge * Reduce traversability threshold * clean up pull request * fix ipopt building error * delete qpOASES * specify coinbrew version * Fix out of range bug in FTM, connect timing bug in GBP, update sim timesteps for worlds * Reasonable GBP performance * Fix bug in NMPC test, rename time_ahead to first_element_duration * online update swing foot trajectory * fix merge issues * init nominal prev foot postion, fix interp when z duration = 0, update pass foothold more frequently * limit acceleration of swing foot * Clean up GBP formatting * Remove NMPC test requirement to solve problem given CIs limitations * better swing foot acceleration limit, fix acceleration interplation at the end, smaller feedback on velocity, consider toe radius * fix init of grf position * PR updates (mostly refactoring for cleanliness and const protection) * Rename GlobalBodyPlan functions * Unify stance and swing foot planning (#199) * Initial commit moving to computeFootPlan, not compiled yet * computeFootPlan works, beginning refactoring * Fix impact acceleration bug, switch to MultiFootState for past footholds, clean LFP * Fix timestamp for foot message, clean Co-authored-by: Yanhao Yang <[email protected]> Co-authored-by: Yanhao Yang <[email protected]> * intermediate commit * Finihsed adding tests * intermediate commit before rebase * added pipeline fix * Addressed pr comments * Improve simulation and nmpc (#202) * Improve simulation friction, contact, and physics * Sync new nmpc updates * fix typo in warmstart initialization * Update ipopt options, update discretization * Fix another typo, and hopefully fix the coinbrew building error * update the rotor sdf Co-authored-by: Joe Norby <[email protected]> * Reformat to Google Style for C++ and Add CI Linting (#205) * Reformat all cpp, h, hpp files to google style, implement linting script and add to tests (one error inserted for CI testing) * Make scripts folder for cleaner root, fix test formatting error * Fix CircleCI script path * Format updates to pass linting checks * Update logging and planning launch files, remove global footstep planner (#209) * Update live plot to show ground truth grf, uniform colors and labels (#210) * Add teleop_twist_joy and configure to robot driver control modes (#211) * Add teleop_twist_joy and configure to robot driver control modes * Remove teleop_twist_joy tests of enable button, add cmd_vel to visualization * Remove EKF_tests_2021_12_3 * add grf plan initialization * update initialization * Add stand mode for local planner, remove twist body planner (#217) * Add stand mode for local planner, remove twist body planner * Fix weird formatting changes * Change stand threshold variables to ROS params, add cleaner statement of conditions * Update threshold for end of global plan to ROS param * Added teleop-twist-joy to setup_deps since joystick doesn't respond without this install. * Remove stray lines Co-authored-by: Justin Yim <[email protected]> * Improve swing foot plan (#214) * fix bugs, fix foothold heuristics, better swing foot acc limitation * remove online swing foot update, revert pd gains * Foothold nominal position minimize the maximum distance from hip plan MPC will not modify body and grf plan if fail to solve * fix centrifugal tracking term * Clean * Clean * Remove unused files and functions * Keep the original style * Fix some comments and warning info * Pass test * Resolve review requests * code format * Revert stand pose * Add getNextLiftoffIndex Use contact schedule for end_of_stance Move computeSwingApex to cpp * Fix test All the parameters should match the server... Co-authored-by: Joe Norby <[email protected]> * Clean unused or duplicate params in local planner and nmpc controller (#220) * Add stand mode for local planner, remove twist body planner * Fix weird formatting changes * Change stand threshold variables to ROS params, add cleaner statement of conditions * Update threshold for end of global plan to ROS param * Added teleop-twist-joy to setup_deps since joystick doesn't respond without this install. * Remove local_body_planner params (they are in nmpc yaml) and horizon length/timestep from nmpc (they are in local planner) * Remove stray lines * Update params in CMPC test and remove adaptive complexity test (will come in later PR) * Update local planner test with new param Co-authored-by: Justin Yim <[email protected]> * multiple robot spawning working * Add dash profile for rqt and roslaunch support * intermediate commit * Fix end of stance bug (#224) * Fix end of stance bug * Add comment clarifying logic * Add tcpnodelay to logging, sim control, and sim contact state publishing (#225) * Add tcpnodelay to controller plugin, contact state publisher, and logging scripts, change local plan timestamp to publish time not state time * Use auto for ros::Time declaration * Add CMU text to spirit.urdf and spirit_meshes.urdf * Realign xml text * Add tcpnodelay hint to sim command topic, may actually affect performance (should improve) * Add state traces to RVizInterface (#230) * multi robot sim working * cleaned up launch file * intermediate commit * cleaned up launch files * modified launch file * minor changes * intermediate commit * multi robot teleop working * Adjust local planner to support arbitrary horizons and match length of body and foot plans (#221) * Add stand mode for local planner, remove twist body planner * Fix weird formatting changes * Change stand threshold variables to ROS params, add cleaner statement of conditions * Update threshold for end of global plan to ROS param * Added teleop-twist-joy to setup_deps since joystick doesn't respond without this install. * Remove local_body_planner params (they are in nmpc yaml) and horizon length/timestep from nmpc (they are in local planner) * Remove stray lines * Update params in CMPC test and remove adaptive complexity test (will come in later PR) * Update local planner test with new param * Modify horizon lengths in local planner, nmpc, and tests * Update nmpc controller to support arbitrary horizons, change RVizInterface to show one frame per gait period * Add comment clarifying indexing difference * Fix body height bug in LFP * Update NMPC temporal weights to saturate at a given parameter (was unbounded before), reduce surface normal filter radius Co-authored-by: Justin Yim <[email protected]> * addressed pr comments * Add joint data view (#238) * Add accurate joint limits and linear motor model to gazebo (#236) * Add accurate joint limits and linear motor model to gazebo * Add soft joint limit for knees * fixed formatting errors * fixed formatting * formatting * address pr comments * Add gap worlds and handle missing map data (#237) * Add gaps and pit worlds, add x and y layers to maps, add traversability mask option * Update rest of repo to use z_inpainted, mark areas with differences between z and z_inpainted as untraversable * Add buffer of untraversability around areas with no information, expand foothold search radius from 0.2 to 0.25 * Improve contact stability in simulation (#239) * Improve contact stability: Format contact state publisher Modify contact kp and kd setting to improve contact stability Modify simulation solver setting to improve accuracy * Synchronize changes to the new world files * Fix urdf * Fix indexing bug in gbp replanning (#240) * Update terrain to match mocap space, add some extra worlds (#241) * Remove slope from traversability, add parkour world * Make terrains fit mocap space, add some other terrains, remove pit (buggy) * Update ode solver settings for new worlds * Add abstract hardware interface and refactor Robot Driver (#242) * Add abstract hardware interface class and spirit interface, refactor robot_driver to include robot name and folders for controllers and interfaces, remove open loop robot controller and leg override * Remove deprecated launch files * Correct some comments * Add include for hardware interfaces class in robot driver * Add user_rx_data to RobotDriver, Update is_hw to is_hardware * Make user data var names more specific * Add joint state publisher to robot driver, undo incorrect imu acc mapping * Switch /mcu/state/* to /state/* to differentiate from GR topics, remove robot_proxy and ground_truth_publisher Co-authored-by: RML group <[email protected]> * addressed comments * Add utility function to load multiple sets of casadi code * Respect linting checks * roslaunch robot in arguments * able to stand up * a1 is working * change robot_ to robot_name_, reformat * Refactor quad logger, include reference state and GRFs (#248) * Update quad_logger scripts to handle trajectory data, state GRFs, better fig exporting, and clean and add comments * Update quad logger legend locations and figure limits * Update trajectory publisher to correctly publish traj state, remove deprecated support for publishing the entire trajectory * Add trajectory_publisher to planning.launch * Add IPOPT install fix for CI * Quad logger bugfix for empty data structs * Updated Ghost license from Avik and updated quad-software/LICENSE to CMU & RML. (#254) Co-authored-by: Justin Yim <[email protected]> * Add Doxygen-updating Github workflow (#255) * Remove matplotlibcpp support * Update repo name in Doxygen * Add doxygen workflow * Add devel to Doxygen updated branches * Remove matplotlibcpp comments from gbp * Fix complementary filter in Robot Driver (#253) * Update quad_logger scripts to handle trajectory data, state GRFs, better fig exporting, and clean and add comments * Update quad logger legend locations and figure limits * Update trajectory publisher to correctly publish traj state, remove deprecated support for publishing the entire trajectory * Add trajectory_publisher to planning.launch * Add IPOPT install fix for CI * Quad logger bugfix for empty data structs * Fix bug in velocity comp filter * Second-order filters * Bug fix Co-authored-by: Robomechanics Lab <[email protected]> Co-authored-by: Yanhao Yang <[email protected]> * Finalize complementary filter bug fixes (#258) * Finalize * Move the acceleration flip to the spirit interface. * Add framework for adaptive complexity, GBP updates, and many miscellaneous tweaks (#256) * Add motor model * Start adding state lifting * First draft of centroidal and full state validity checking * Start passing constraint violations out of validity checking functions * First commit with working validity checking * Add hard coded local plan * Leaping with local planner, still have multiple finite elements during the flight phase, add closed form linear dynamics * Initial commit for adaptive complexity * Updating nmpc member variables and params * Streamline debug print statements, switch to more flexible validity checking * Implement maximum height at liftoff * update constraint bounds * Start adding utils test * Add adaptive complexity functions * Update contact sequence and state weights for leap * Continue updates * Nearly finished with compute_nnz_jac_g * Add phase dependent reachability back in * Finish updating nnz functions, still buggy * Move gen code to new folder, fix compute nnz bug, eval_f, bounds * Implement eval_g * Finish main virtual functions * Reformat quad_nlp.cpp and switch to VectorXd instead of MatrixXd where relevant * Remove accidental cold start in update_complexity_schedule * Add indexing vectors to prep for merge with indexing functions * Add support for world frame foot pos in local planner * First pass at merging with nmpc indexing * fix bugs, fix foothold heuristics, better swing foot acc limitation * Continue pre-merge with nmpc_indexing * Remove first_step check form test * Update eval functions and unit test * More bugfixes, passes nnz checks for jac and hess * Fix nmpc_controller test to set complexity schedule length to N+1 * Fix bugs in simple model and switch to world frame foot positions (complex still fails) * remove online swing foot update, revert pd gains * Re-add GBP planning utils updates from merge * Fix some bugs in the merge (formatting, quad kd tests, planning.launch) * Foothold nominal position minimize the maximum distance from hip plan MPC will not modify body and grf plan if fail to solve * Update logging and planning launch files, remove global footstep planner * fix centrifugal tracking term * Fix vector slicing bug - complex sometimes solves, mostly does not * Clean * Add copy constructor for quadNLP * update sdf to increase mu to 1 * Continue adding to shift initial guess * Finish adding warm start for null vars, bug in setup somewhere * constraint indexing fixed, use correct parameters (may still be structure bug) * Add flexibility for initial state * Update constraint funcs, fix small complexity bugs * Fix kinematics bug, add constraints back, increase ma57_pre_alloc * Readd contact schedule and state update to test for to match shift initial guess * Clean * Remove unused files and functions * Keep the original style * Fix some comments and warning info * Add stand mode for local planner, remove twist body planner * Pass test * Resolve review requests * code format * Update warm start to leave new complex decision variables unchanged from last solve * Revert stand pose * Begin simulation testing * Fix weird formatting changes * Fix merge bug * Change to update structure and update initial guess so we can handle changing complexity (still bugs) * Disable warm start if problem structure got more complex * Begin unifying kinematic constraint evaluation between local planner and nlp * Change stand threshold variables to ROS params, add cleaner statement of conditions * Update threshold for end of global plan to ROS param * Added teleop-twist-joy to setup_deps since joystick doesn't respond without this install. * Remove local_body_planner params (they are in nmpc yaml) and horizon length/timestep from nmpc (they are in local planner) * Remove stray lines * Update params in CMPC test and remove adaptive complexity test (will come in later PR) * Update gen functions to match QuadKD * Add getNextLiftoffIndex Use contact schedule for end_of_stance Move computeSwingApex to cpp * Fix test All the parameters should match the server... * Implement evalLiftedTrajectoryConstraints * Update local planner test with new param * Modify horizon lengths in local planner, nmpc, and tests * Update to use equal horizon for foot and body plan * Begin debugging solve fails, increase sim friction * Turn of require_init for better solves * Turn off hip threshold, turn on slack vars, reduce normals vector filter, increase GBP clearance to plan over step * Fix end of stance bug, add a few parameter changes for more feasible leaps * Begin adding panic var for constraints * Finish first pass of constraint relaxations * Add separate panic weigts for constraints * Added parameters for hard and soft bounds, about to edit quadNLP internals to use * Mid transition to relaxed constraints * First pass at soft constraint penalty with hard constraint, runs but not tested for performance * Bugs mostly solved, takes a while to solve though * Nightly commit * Remove takeoff state weight, enforce back leg reachability, add tcpnodelay to controller plugin * More debugging, always enforce hard joint limits * Start adding adaptive case * Refactor GBP terrain height functions, add grid_map support * Add traversability heuristic to gbp * Increase filter size for surface normals (may reverse this), add minimum foot clearance during swing * Add state traces to RVizInterface * First successful leap - front legs lead rear, reduce friction * Start softening all constraints * Finish implementing relaxation of all complex constraints but leave only knee relaxed, solves pretty well but fails fk * Tweak weights, much better performance. Verified constraint jacobian * Add cartesian space PD feedback for swing legs * Remove unnecessary private vars * Reduce jacobian inv damping, align world more accurately, use only cartesian joint PD, tracking looking much better * Add kinematic cost to valid footholds penalizing deviations from nominal and previous * Add accurate joint limits and linear motor model to gazebo * Add gaps and pit worlds, add x and y layers to maps, add traversability mask option * Update rest of repo to use z_inpainted, mark areas with differences between z and z_inpainted as untraversable * Add soft joint limit for knees * Change starting location in quad_gazebo * Add buffer of untraversability around areas with no information, expand foothold search radius from 0.2 to 0.25 * Add lognorm velocity sampling to GBP, restrict replanning to connect phases, tweak vel parameters * Remove slope from traversability, add parkour world * Reduce cartesian feedback gains, add stiff pd for knee hard stop, relax buffer in GBP leap * Make terrains fit mocap space, add some other terrains, remove pit (buggy) * Update ode solver settings for new worlds * Fix indexing bug in gbp replanning * Squashed commit of the following: commit 14a26dfd3d0541733438a170e3c2d41e61d9dca7 Author: jcnorby <[email protected]> Date: Fri Apr 15 08:46:40 2022 -0400 Squashed commit of the following: commit 88276122a535e4e1047b1cbedeab0bba3c304b8e Author: Joe Norby <[email protected]> Date: Thu Apr 14 19:25:07 2022 -0400 Add abstract hardware interface and refactor Robot Driver (#242) * Add abstract hardware interface class and spirit interface, refactor robot_driver to include robot name and folders for controllers and interfaces, remove open loop robot controller and leg override * Remove deprecated launch files * Correct some comments * Add include for hardware interfaces class in robot driver * Add user_rx_data to RobotDriver, Update is_hw to is_hardware * Make user data var names more specific * Add joint state publisher to robot driver, undo incorrect imu acc mapping * Switch /mcu/state/* to /state/* to differentiate from GR topics, remove robot_proxy and ground_truth_publisher Co-authored-by: RML group <[email protected]> commit f96ab7550ac96c63fb110b19f6e063061f82e83f Author: jcnorby <[email protected]> Date: Fri Apr 15 08:41:39 2022 -0400 Squashed commit of the following: commit e3b179ae3f65dbd996dcd25e33332caa5bb4e27c Author: jcnorby <[email protected]> Date: Thu Apr 14 17:00:44 2022 -0400 Fix bug in setting current state and in evaluating constraints, add terrain constraint, update constraint name map commit fdb2b2a0dcffea09a792558a8e6717e1083d0f09 Author: Joe Norby <[email protected]> Date: Wed Apr 13 22:41:50 2022 -0400 Simplify warm start, correct application of foot constraitnts, add terrain variable commit 172247e7b579d78e8053e80e9f706c1ef5554bde Author: jcnorby <[email protected]> Date: Wed Apr 13 18:01:02 2022 -0400 Switch to ZOH foot input, revert to N-1 control vars and remove u1 from finite elements, update conditions for foot dynamics and constraints commit 277720bee311225cabe7efd6e4fe2b74ced75faf Author: jcnorby <[email protected]> Date: Tue Apr 12 18:02:56 2022 -0400 Fix bug which turned foot EOM off, apply new foot traj online (still violates constraints) commit de5990352dc15515012c7e7ab7196430e9feabca Author: jcnorby <[email protected]> Date: Mon Apr 11 12:42:28 2022 -0400 Turn on foot EOM constraints during swing commit a3fd2e7c0f6f88efe13302771c99f53e641dd652 Author: Joe Norby <[email protected]> Date: Thu Apr 7 23:02:31 2022 -0400 Clean up print statements commit 451c43e7089ea4e1ef7360c5ec2c6181565bf196 Merge: 0e94b60e d2b2e18a Author: Joe Norby <[email protected]> Date: Thu Apr 7 21:54:57 2022 -0400 Merge branch 'melodic_devel_adaptive_complexity_add_foot_state_correct' into melodic_devel_adaptive_complexity_add_foot_state commit d2b2e18a93394efcf7a38088caec9d67e05a1433 Author: Joe Norby <[email protected]> Date: Thu Apr 7 21:53:47 2022 -0400 Add terminal input to cost, recovers previous solve time commit 393337c0a16b33b78eaf9de626ad0bdecc2c878c Author: Joe Norby <[email protected]> Date: Thu Apr 7 20:53:36 2022 -0400 Correct foot state warm start and cost commit 0e94b60e21ddbef22415d5caa537f04f4c1430e6 Author: jcnorby <[email protected]> Date: Thu Apr 7 15:51:22 2022 -0400 Start updating cost to include control at last iteration commit 6b58687abf38115515f3cad4d09d53fab43f3474 Merge: a1a19e9a 14f0624c Author: jcnorby <[email protected]> Date: Thu Apr 7 13:37:29 2022 -0400 Merge branch 'melodic_devel_adaptive_complexity' into melodic_devel_adaptive_complexity_add_foot_state commit a1a19e9af9c73675553bec1f0fb6785e036a415c Author: Joe Norby <[email protected]> Date: Wed Apr 6 23:10:14 2022 -0400 Fix tail mode bug, a few more indexing issues commit 6df542a7976d1a6398e1814b1913674f701fc765 Author: Joe Norby <[email protected]> Date: Tue Apr 5 21:21:10 2022 -0400 Initial commit wit foot state vars, midway through porting commit 52a26803419a0d97f9bc965f9d69e5fb4890e6df Merge: 14f0624c dd517bb5 Author: jcnorby <[email protected]> Date: Mon Apr 11 12:43:43 2022 -0400 Merge branch 'devel' into melodic_devel_adaptive_complexity commit dd517bb5c54166340e6b6d42893d690fe7a7d0d4 Author: Joe Norby <[email protected]> Date: Sat Apr 9 11:55:02 2022 -0400 Update terrain to match mocap space, add some extra worlds (#241) * Remove slope from traversability, add parkour world * Make terrains fit mocap space, add some other terrains, remove pit (buggy) * Update ode solver settings for new worlds commit 4be6347d5717532a7ebb10eeefd0605a5462d931 Author: Joe Norby <[email protected]> Date: Thu Apr 7 19:27:06 2022 -0400 Fix indexing bug in gbp replanning (#240) commit 14f0624cae1b31524271dda6e282b49690d94c13 Merge: b3963391 e4c7a964 Author: jcnorby <[email protected]> Date: Thu Apr 7 13:21:54 2022 -0400 Merge branch 'melodic_devel_mocap_terrain' into melodic_devel_adaptive_complexity commit b39633911f8135d4eb93b635312788e7035deec0 Merge: 5ca1d105 399836d2 Author: jcnorby <[email protected]> Date: Thu Apr 7 13:15:23 2022 -0400 Merge branch 'devel' into melodic_devel_adaptive_complexity commit 5ca1d10534770a30d53a610383ba1cc15ed9cd72 Author: Joe Norby <[email protected]> Date: Thu Mar 31 17:55:15 2022 -0400 Add velocity constraint back in, relax knee constraint, tighten soft joint limits, soften cartesian PD commit 5a9487d164462903c61721a95890a46718bbd485 Merge: a8d357cc a509af75 Author: Joe Norby <[email protected]> Date: Wed Mar 30 18:23:25 2022 -0400 Merge branch 'melodic_devel_adaptive_complexity' of github.com:robomechanics/quad-software into melodic_devel_adaptive_complexity commit a509af75ccca8b028bfc028a7cb667dcd9e24acf Author: jcnorby <[email protected]> Date: Mon Mar 28 13:11:05 2022 -0400 Readd hip height constraint on foot trajectory, speed up sim commit a711fda92e0767c4a2bbf71f5dad8fa65bd1a89a Merge: 262d4a16 69841722 Author: jcnorby <[email protected]> Date: Mon Mar 28 12:39:39 2022 -0400 Merge branch 'melodic_devel_add_joint_data_to_liveplot' into melodic_devel_adaptive_complexity commit 698417222b2edaf461b4a9d1482dde96abb427d5 Author: jcnorby <[email protected]> Date: Mon Mar 28 12:11:37 2022 -0400 Add joint data view commit 262d4a16708f4694d477c81205f0b37ca06d5453 Author: jcnorby <[email protected]> Date: Mon Mar 28 11:56:01 2022 -0400 Lower max GRFs in NMPC and remove some print statements commit 6d3b39195817a12c13a2688b93e4d0b205028e3d Merge: f23165a4 15bef6bb Author: jcnorby <[email protected]> Date: Mon Mar 28 10:41:07 2022 -0400 Merge branch 'melodic_devel_correct_sim_kin_dyn_limits' into melodic_devel_adaptive_complexity commit a8d357cc777bcc343af97f1d430de8daa8d23551 Merge: 977e9812 50749b59 Author: Joe Norby <[email protected]> Date: Sun Mar 27 21:04:37 2022 -0400 Merge branch 'melodic_devel_mapping_set_untraversable_regions' into melodic_devel_adaptive_complexity * Merged devel, squashed commit of the following: commit 88276122a535e4e1047b1cbedeab0bba3c304b8e Author: Joe Norby <[email protected]> Date: Thu Apr 14 19:25:07 2022 -0400 Add abstract hardware interface and refactor Robot Driver (#242) * Add abstract hardware interface class and spirit interface, refactor robot_driver to include robot name and folders for controllers and interfaces, remove open loop robot controller and leg override * Remove deprecated launch files * Correct some comments * Add include for hardware interfaces class in robot driver * Add user_rx_data to RobotDriver, Update is_hw to is_hardware * Make user data var names more specific * Add joint state publisher to robot driver, undo incorrect imu acc mapping * Switch /mcu/state/* to /state/* to differentiate from GR topics, remove robot_proxy and ground_truth_publisher Co-authored-by: RML group <[email protected]> commit dd517bb5c54166340e6b6d42893d690fe7a7d0d4 Author: Joe Norby <[email protected]> Date: Sat Apr 9 11:55:02 2022 -0400 Update terrain to match mocap space, add some extra worlds (#241) * Remove slope from traversability, add parkour world * Make terrains fit mocap space, add some other terrains, remove pit (buggy) * Update ode solver settings for new worlds commit 4be6347d5717532a7ebb10eeefd0605a5462d931 Author: Joe Norby <[email protected]> Date: Thu Apr 7 19:27:06 2022 -0400 Fix indexing bug in gbp replanning (#240) * Add additional terrains and update starting position * Expand traversability filter * Initial commit benchmarking timing performance before removing slack variables * Update settings to match current adaptive complexity branch * Add parameters to enable leaping and AC * Unify terrain and traversability settings for leaping and AC * Disable linear mm, add test fixture to gbp and begin porting tests * Update gbp to pass tests, tweak some refinement parameters * Beginning modifications to re-enable leaping: readd Jinv damping, remove cartesian feedback * Soften knee hard stop and add slight front liftoff period in contact schedule * Update quad_logger scripts to handle trajectory data, state GRFs, better fig exporting, and clean and add comments * Update quad logger legend locations and figure limits * Update trajectory publisher to correctly publish traj state, remove deprecated support for publishing the entire trajectory * Update QuadKD to enforce knee-above-hip, use accurate joint limits, and select knee position to minimize error * Update nmpc gains, u_max, solve times to old values, use forward euler dynamics, disable hard complex bounds, disable foot dynamics, remove front foot liftoff period * Update robot driver to publish current traj state, quad utils to permit this * Nightly commit - re-add foot traj modification, complex elements * Increase friction, change current_plan_index from 0 to -1 * Update complimentarity filter and add debug statements * Fix bug in velocity comp filter * Add nmpc and lp parameter changes for 1.5m/s trotting in hw * Add foot height constraint framework, data not yet passed. Revert casadi funcs to 4/20 commit but with terrain added * Add terrain height and slope into nlp, solves but fails on steps * Mid-debugging overconstraining of foot positions * Refactor nmpc param loading, constructor, quadnlp constructor, test that OG casadi fcns work * Add vectors for dimension of cost terms and include in cost calculations. Not tested with complex yet * Add new casadi funcs which match devel's (except using closed form integration for linear acc rather than Euler) * Remove feet_in_simple param, begin correcting foot constraints * Set LP update rate to 300Hz to match mean solve time, tweak warm start * Remove unnecessary changes from devel * Clean up NMPC tests and primary working functions * Pass linting checks * Clean up comments and PR review refactors * Make parameter loading a member of the PlannerConfig struct, integrate in test fixture and ros interface * Add todo comment * Reduce catkin tests parallel jobs * Move node handle constructor to text fixture body (attempt to fix CircleCI bug), remove some print statements * More cleaning for PR * Reduce nmpc max solve time * Parameter updates for successful hardware deployment * Final merge updates - fix global plan state publishing, make traversability names more clear, enforce legbases stay over terrain in GBP * Update names to quad-sdk, update README with construction notice Co-authored-by: Yanhao Yang <[email protected]> Co-authored-by: Justin Yim <[email protected]> Co-authored-by: RML group <[email protected]> Co-authored-by: Robomechanics Lab <[email protected]> * Fix small bug in checking isInMap (switch to using grid_map) (#260) * Update to c++14 (#262) * removing unneeded files * removing commented out .sdf sections. * remove unused simulator plugins * Permit variable horizon lengths in Local Planner, add NLP diagnostics, add working mixed complexity solving. (#263) * Add motor model * Start adding state lifting * First draft of centroidal and full state validity checking * Start passing constraint violations out of validity checking functions * First commit with working validity checking * Add hard coded local plan * Leaping with local planner, still have multiple finite elements during the flight phase, add closed form linear dynamics * Initial commit for adaptive complexity * Updating nmpc member variables and params * Streamline debug print statements, switch to more flexible validity checking * Implement maximum height at liftoff * update constraint bounds * Start adding utils test * Add adaptive complexity functions * Update contact sequence and state weights for leap * Continue updates * Nearly finished with compute_nnz_jac_g * Add phase dependent reachability back in * Finish updating nnz functions, still buggy * Move gen code to new folder, fix compute nnz bug, eval_f, bounds * Implement eval_g * Finish main virtual functions * Reformat quad_nlp.cpp and switch to VectorXd instead of MatrixXd where relevant * Remove accidental cold start in update_complexity_schedule * Add indexing vectors to prep for merge with indexing functions * Add support for world frame foot pos in local planner * First pass at merging with nmpc indexing * fix bugs, fix foothold heuristics, better swing foot acc limitation * Continue pre-merge with nmpc_indexing * Remove first_step check form test * Update eval functions and unit test * More bugfixes, passes nnz checks for jac and hess * Fix nmpc_controller test to set complexity schedule length to N+1 * Fix bugs in simple model and switch to world frame foot positions (complex still fails) * remove online swing foot update, revert pd gains * Re-add GBP planning utils updates from merge * Fix some bugs in the merge (formatting, quad kd tests, planning.launch) * Foothold nominal position minimize the maximum distance from hip plan MPC will not modify body and grf plan if fail to solve * Update logging and planning launch files, remove global footstep planner * fix centrifugal tracking term * Fix vector slicing bug - complex sometimes solves, mostly does not * Clean * Add copy constructor for quadNLP * update sdf to increase mu to 1 * Continue adding to shift initial guess * Finish adding warm start for null vars, bug in setup somewhere * constraint indexing fixed, use correct parameters (may still be structure bug) * Add flexibility for initial state * Update constraint funcs, fix small complexity bugs * Fix kinematics bug, add constraints back, increase ma57_pre_alloc * Readd contact schedule and state update to test for to match shift initial guess * Clean * Remove unused files and functions * Keep the original style * Fix some comments and warning info * Add stand mode for local planner, remove twist body planner * Pass test * Resolve review requests * code format * Update warm start to leave new complex decision variables unchanged from last solve * Revert stand pose * Begin simulation testing * Fix weird formatting changes * Fix merge bug * Change to update structure and update initial guess so we can handle changing complexity (still bugs) * Disable warm start if problem structure got more complex * Begin unifying kinematic constraint evaluation between local planner and nlp * Change stand threshold variables to ROS params, add cleaner statement of conditions * Update threshold for end of global plan to ROS param * Added teleop-twist-joy to setup_deps since joystick doesn't respond without this install. * Remove local_body_planner params (they are in nmpc yaml) and horizon length/timestep from nmpc (they are in local planner) * Remove stray lines * Update params in CMPC test and remove adaptive complexity test (will come in later PR) * Update gen functions to match QuadKD * Add getNextLiftoffIndex Use contact schedule for end_of_stance Move computeSwingApex to cpp * Fix test All the parameters should match the server... * Implement evalLiftedTrajectoryConstraints * Update local planner test with new param * Modify horizon lengths in local planner, nmpc, and tests * Update to use equal horizon for foot and body plan * Begin debugging solve fails, increase sim friction * Turn of require_init for better solves * Turn off hip threshold, turn on slack vars, reduce normals vector filter, increase GBP clearance to plan over step * Fix end of stance bug, add a few parameter changes for more feasible leaps * Begin adding panic var for constraints * Finish first pass of constraint relaxations * Add separate panic weigts for constraints * Added parameters for hard and soft bounds, about to edit quadNLP internals to use * Mid transition to relaxed constraints * First pass at soft constraint penalty with hard constraint, runs but not tested for performance * Bugs mostly solved, takes a while to solve though * Nightly commit * Remove takeoff state weight, enforce back leg reachability, add tcpnodelay to controller plugin * More debugging, always enforce hard joint limits * Start adding adaptive case * Refactor GBP terrain height functions, add grid_map support * Add traversability heuristic to gbp * Increase filter size for surface normals (may reverse this), add minimum foot clearance during swing * Add state traces to RVizInterface * First successful leap - front legs lead rear, reduce friction * Start softening all constraints * Finish implementing relaxation of all complex constraints but leave only knee relaxed, solves pretty well but fails fk * Tweak weights, much better performance. Verified constraint jacobian * Add cartesian space PD feedback for swing legs * Remove unnecessary private vars * Reduce jacobian inv damping, align world more accurately, use only cartesian joint PD, tracking looking much better * Add kinematic cost to valid footholds penalizing deviations from nominal and previous * Add accurate joint limits and linear motor model to gazebo * Add gaps and pit worlds, add x and y layers to maps, add traversability mask option * Update rest of repo to use z_inpainted, mark areas with differences between z and z_inpainted as untraversable * Add soft joint limit for knees * Change starting location in quad_gazebo * Add buffer of untraversability around areas with no information, expand foothold search radius from 0.2 to 0.25 * Add lognorm velocity sampling to GBP, restrict replanning to connect phases, tweak vel parameters * Remove slope from traversability, add parkour world * Reduce cartesian feedback gains, add stiff pd for knee hard stop, relax buffer in GBP leap * Make terrains fit mocap space, add some other terrains, remove pit (buggy) * Update ode solver settings for new worlds * Fix indexing bug in gbp replanning * Squashed commit of the following: commit 14a26dfd3d0541733438a170e3c2d41e61d9dca7 Author: jcnorby <[email protected]> Date: Fri Apr 15 08:46:40 2022 -0400 Squashed commit of the following: commit 88276122a535e4e1047b1cbedeab0bba3c304b8e Author: Joe Norby <[email protected]> Date: Thu Apr 14 19:25:07 2022 -0400 Add abstract hardware interface and refactor Robot Driver (#242) * Add abstract hardware interface class and spirit interface, refactor robot_driver to include robot name and folders for controllers and interfaces, remove open loop robot controller and leg override * Remove deprecated launch files * Correct some comments * Add include for hardware interfaces class in robot driver * Add user_rx_data to RobotDriver, Update is_hw to is_hardware * Make user data var names more specific * Add joint state publisher to robot driver, undo incorrect imu acc mapping * Switch /mcu/state/* to /state/* to differentiate from GR topics, remove robot_proxy and ground_truth_publisher Co-authored-by: RML group <[email protected]> commit f96ab7550ac96c63fb110b19f6e063061f82e83f Author: jcnorby <[email protected]> Date: Fri Apr 15 08:41:39 2022 -0400 Squashed commit of the following: commit e3b179ae3f65dbd996dcd25e33332caa5bb4e27c Author: jcnorby <[email protected]> Date: Thu Apr 14 17:00:44 2022 -0400 Fix bug in setting current state and in evaluating constraints, add terrain constraint, update constraint name map commit fdb2b2a0dcffea09a792558a8e6717e1083d0f09 Author: Joe Norby <[email protected]> Date: Wed Apr 13 22:41:50 2022 -0400 Simplify warm start, correct application of foot constraitnts, add terrain variable commit 172247e7b579d78e8053e80e9f706c1ef5554bde Author: jcnorby <[email protected]> Date: Wed Apr 13 18:01:02 2022 -0400 Switch to ZOH foot input, revert to N-1 control vars and remove u1 from finite elements, update conditions for foot dynamics and constraints commit 277720bee311225cabe7efd6e4fe2b74ced75faf Author: jcnorby <[email protected]> Date: Tue Apr 12 18:02:56 2022 -0400 Fix bug which turned foot EOM off, apply new foot traj online (still violates constraints) commit de5990352dc15515012c7e7ab7196430e9feabca Author: jcnorby <[email protected]> Date: Mon Apr 11 12:42:28 2022 -0400 Turn on foot EOM constraints during swing commit a3fd2e7c0f6f88efe13302771c99f53e641dd652 Author: Joe Norby <[email protected]> Date: Thu Apr 7 23:02:31 2022 -0400 Clean up print statements commit 451c43e7089ea4e1ef7360c5ec2c6181565bf196 Merge: 0e94b60e d2b2e18a Author: Joe Norby <[email protected]> Date: Thu Apr 7 21:54:57 2022 -0400 Merge branch 'melodic_devel_adaptive_complexity_add_foot_state_correct' into melodic_devel_adaptive_complexity_add_foot_state commit d2b2e18a93394efcf7a38088caec9d67e05a1433 Author: Joe Norby <[email protected]> Date: Thu Apr 7 21:53:47 2022 -0400 Add terminal input to cost, recovers previous solve time commit 393337c0a16b33b78eaf9de626ad0bdecc2c878c Author: Joe Norby <[email protected]> Date: Thu Apr 7 20:53:36 2022 -0400 Correct foot state warm start and cost commit 0e94b60e21ddbef22415d5caa537f04f4c1430e6 Author: jcnorby <[email protected]> Date: Thu Apr 7 15:51:22 2022 -0400 Start updating cost to include control at last iteration commit 6b58687abf38115515f3cad4d09d53fab43f3474 Merge: a1a19e9a 14f0624c Author: jcnorby <[email protected]> Date: Thu Apr 7 13:37:29 2022 -0400 Merge branch 'melodic_devel_adaptive_complexity' into melodic_devel_adaptive_complexity_add_foot_state commit a1a19e9af9c73675553bec1f0fb6785e036a415c Author: Joe Norby <[email protected]> Date: Wed Apr 6 23:10:14 2022 -0400 Fix tail mode bug, a few more indexing issues commit 6df542a7976d1a6398e1814b1913674f701fc765 Author: Joe Norby <[email protected]> Date: Tue Apr 5 21:21:10 2022 -0400 Initial commit wit foot state vars, midway through porting commit 52a26803419a0d97f9bc965f9d69e5fb4890e6df Merge: 14f0624c dd517bb5 Author: jcnorby <[email protected]> Date: Mon Apr 11 12:43:43 2022 -0400 Merge branch 'devel' into melodic_devel_adaptive_complexity commit dd517bb5c54166340e6b6d42893d690fe7a7d0d4 Author: Joe Norby <[email protected]> Date: Sat Apr 9 11:55:02 2022 -0400 Update terrain to match mocap space, add some extra worlds (#241) * Remove slope from traversability, add parkour world * Make terrains fit mocap space, add some other terrains, remove pit (buggy) * Update ode solver settings for new worlds commit 4be6347d5717532a7ebb10eeefd0605a5462d931 Author: Joe Norby <[email protected]> Date: Thu Apr 7 19:27:06 2022 -0400 Fix indexing bug in gbp replanning (#240) commit 14f0624cae1b31524271dda6e282b49690d94c13 Merge: b3963391 e4c7a964 Author: jcnorby <[email protected]> Date: Thu Apr 7 13:21:54 2022 -0400 Merge branch 'melodic_devel_mocap_terrain' into melodic_devel_adaptive_complexity commit b39633911f8135d4eb93b635312788e7035deec0 Merge: 5ca1d105 399836d2 Author: jcnorby <[email protected]> Date: Thu Apr 7 13:15:23 2022 -0400 Merge branch 'devel' into melodic_devel_adaptive_complexity commit 5ca1d10534770a30d53a610383ba1cc15ed9cd72 Author: Joe Norby <[email protected]> Date: Thu Mar 31 17:55:15 2022 -0400 Add velocity constraint back in, relax knee constraint, tighten soft joint limits, soften cartesian PD commit 5a9487d164462903c61721a95890a46718bbd485 Merge: a8d357cc a509af75 Author: Joe Norby <[email protected]> Date: Wed Mar 30 18:23:25 2022 -0400 Merge branch 'melodic_devel_adaptive_complexity' of github.com:robomechanics/quad-software into melodic_devel_adaptive_complexity commit a509af75ccca8b028bfc028a7cb667dcd9e24acf Author: jcnorby <[email protected]> Date: Mon Mar 28 13:11:05 2022 -0400 Readd hip height constraint on foot trajectory, speed up sim commit a711fda92e0767c4a2bbf71f5dad8fa65bd1a89a Merge: 262d4a16 69841722 Author: jcnorby <[email protected]> Date: Mon Mar 28 12:39:39 2022 -0400 Merge branch 'melodic_devel_add_joint_data_to_liveplot' into melodic_devel_adaptive_complexity commit 698417222b2edaf461b4a9d1482dde96abb427d5 Author: jcnorby <[email protected]> Date: Mon Mar 28 12:11:37 2022 -0400 Add joint data view commit 262d4a16708f4694d477c81205f0b37ca06d5453 Author: jcnorby <[email protected]> Date: Mon Mar 28 11:56:01 2022 -0400 Lower max GRFs in NMPC and remove some print statements commit 6d3b39195817a12c13a2688b93e4d0b205028e3d Merge: f23165a4 15bef6bb Author: jcnorby <[email protected]> Date: Mon Mar 28 10:41:07 2022 -0400 Merge branch 'melodic_devel_correct_sim_kin_dyn_limits' into melodic_devel_adaptive_complexity commit a8d357cc777bcc343af97f1d430de8daa8d23551 Merge: 977e9812 50749b59 Author: Joe Norby <[email protected]> Date: Sun Mar 27 21:04:37 2022 -0400 Merge branch 'melodic_devel_mapping_set_untraversable_regions' into melodic_devel_adaptive_complexity * Merged devel, squashed commit of the following: commit 88276122a535e4e1047b1cbedeab0bba3c304b8e Author: Joe Norby <[email protected]> Date: Thu Apr 14 19:25:07 2022 -0400 Add abstract hardware interface and refactor Robot Driver (#242) * Add abstract hardware interface class and spirit interface, refactor robot_driver to include robot name and folders for controllers and interfaces, remove open loop robot controller and leg override * Remove deprecated launch files * Correct some comments * Add include for hardware interfaces class in robot driver * Add user_rx_data to RobotDriver, Update is_hw to is_hardware * Make user data var names more specific * Add joint state publisher to robot driver, undo incorrect imu acc mapping * Switch /mcu/state/* to /state/* to differentiate from GR topics, remove robot_proxy and ground_truth_publisher Co-authored-by: RML group <[email protected]> commit dd517bb5c54166340e6b6d42893d690fe7a7d0d4 Author: Joe Norby <[email protected]> Date: Sat Apr 9 11:55:02 2022 -0400 Update terrain to match mocap space, add some extra worlds (#241) * Remove slope from traversability, add parkour world * Make terrains fit mocap space, add some other terrains, remove pit (buggy) * Update ode solver settings for new worlds commit 4be6347d5717532a7ebb10eeefd0605a5462d931 Author: Joe Norby <[email protected]> Date: Thu Apr 7 19:27:06 2022 -0400 Fix indexing bug in gbp replanning (#240) * Add additional terrains and update starting position * Expand traversability filter * Initial commit benchmarking timing performance before removing slack variables * Update settings to match current adaptive complexity branch * Add parameters to enable leaping and AC * Unify terrain and traversability settings for leaping and AC * Disable linear mm, add test fixture to gbp and begin porting tests * Update gbp to pass tests, tweak some refinement parameters * Beginning modifications to re-enable leaping: readd Jinv damping, remove cartesian feedback * Soften knee hard stop and add slight front liftoff period in contact schedule * Update quad_logger scripts to handle trajectory data, state GRFs, better fig exporting, and clean and add comments * Update quad logger legend locations and figure limits * Update trajectory publisher to correctly publish traj state, remove deprecated support for publishing the entire trajectory * Update QuadKD to enforce knee-above-hip, use accurate joint limits, and select knee position to minimize error * Update nmpc gains, u_max, solve times to old values, use forward euler dynamics, disable hard complex bounds, disable foot dynamics, remove front foot liftoff period * Update robot driver to publish current traj state, quad utils to permit this * Nightly commit - re-add foot traj modification, complex elements * Increase friction, change current_plan_index from 0 to -1 * Update complimentarity filter and add debug statements * Fix bug in velocity comp filter * Add nmpc and lp parameter changes for 1.5m/s trotting in hw * Add foot height constraint framework, data not yet passed. Revert casadi funcs to 4/20 commit but with terrain added * Add terrain height and slope into nlp, solves but fails on steps * Mid-debugging overconstraining of foot positions * Refactor nmpc param loading, constructor, quadnlp constructor, test that OG casadi fcns work * Add vectors for dimension of cost terms and include in cost calculations. Not tested with complex yet * Add new casadi funcs which match devel's (except using closed form integration for linear acc rather than Euler) * Remove feet_in_simple param, begin correcting foot constraints * Set LP update rate to 300Hz to match mean solve time, tweak warm start * Remove unnecessary changes from devel * Clean up NMPC tests and primary working functions * Pass linting checks * Complex elements work but foot still overconstrained if allowed to move * Clean up comments and PR review refactors * Make parameter loading a member of the PlannerConfig struct, integrate in test fixture and ros interface * Add todo comment * Squashed commit of the following: commit af732b46f705250058b3fae1ad38987588499859 Author: Joe Norby <[email protected]> Date: Tue May 3 19:48:36 2022 -0400 Add todo comment commit 4fca314d6988789199778653e36585f4efbee3f7 Author: Joe Norby <[email protected]> Date: Tue May 3 19:45:35 2022 -0400 Make parameter loading a member of the PlannerConfig struct, integrate in test fixture and ros interface * Reduce catkin tests parallel jobs * Move node handle constructor to text fixture body (attempt to fix CircleCI bug), remove some print statements * Add logging and plotting support for NLP data * Foot state constraints are cleaner and not infeasible but still not solving, likely a cost bug * Switch to piecewise FOH foot control, fixes overconstraint issue! * Begin debugging terrain height constraint, need to try debug mode * Update logging.launch to take bag_name as an arg * Relax foot height constraint and make clearance a function of foot vel, solves better but not perfect * Repair nmpc traj lifting and evaluating constraints * Support variable horizon planning * Readd stabilization of foot plan (which is important), relax mm constraint, move updateHorizonLength to its own function * Increase foot tracking weights to incentivize changing body pose, fix bug in joint vel constraint bounds * Add step_25cm world * Add low pass filter on new foothold positions * Add batch sim script * Add launchfile to issue a standing command * Better align step_20cm grid map with world * Change settings to match devel * Change nmpc print and AC settings to standard values * Readd NMPCController test * Clean up comments in nmpc_controller.yaml * Remove unnecessary LPF on foot position which caused failed tests * Respect linting checks * Bugfixes for variable horizon support (don't change foot plan length) * Readd index update of body and grf plan in local planner Co-authored-by: Yanhao Yang <[email protected]> Co-authored-by: Justin Yim <[email protected]> Co-authored-by: RML group <[email protected]> Co-authored-by: Robomechanics Lab <[email protected]> * solve namespace issue, but still has problems * Resolve multi-robot tf tree and rviz namespaces * Respect linting checks * Resolve test issuses * merge with devel, resolve conflicts, catkin test succeeds, lint respected * Commit to test CI * Have multi robot plan just call planning.launch, fix logging scripts to support a namespace arg * solve single and multiple robots launch issue * rename quad_plan.launch * started refactor branch to work on quad_utils and quad_msgs * added a readme to describe the quad util package * added quad_msgs, quad_utils readme, and delete BodyFroceEstimate.msg * remove RobotStateTrajectory, modified quad_msgs/README * delete unused msgs in CMakeList * delete trajectory_publisher and related functions * delete body_force_estimator and contact_detection packages * delete nmpc dependency on quad_msgs/RobotStateTrajectory.h * launch file cleanup * delete local_planner dependency on quad_msgs/RobotStateTrajectory.h * exclude contact/body force.yaml in load_params.launch file * solve replay issue, reorganize the rviz profile * Updated quad_simulator readme and removed unused launch files. * readme tweaks. * Changed math_utils fxns to pass inputs by const * Fix lint * Local Planner Refactor (#266) * clean local planner * Fix test * Fix test again * README + small cleanups * Format * Add info Co-authored-by: Alex Stutt <[email protected]> * Global Body Planner Refactor (#264) * Update README.md * Update parameter formatting in GBP, clean some unused ones * Remove temp file * Update README.md * Remove deprecated functions from planning_utils * Streamlining gbp refactors, adding vertical velocity params * Fix small bugs in param loading and valid state checking * Update connect stance time so that max vel = planning_config.h_nom * Update leaping traversability checks to yield better takeoff locations * Update goal state to only require x/y position * Update README.md * Remove a few more unnecessary parameters and update traversability to permit planning in rough terrain * Update README.md * add documentation to planning_utils * temp changes * Update tests to be more robust * Respect linting checks * intermediate commit * finish planning_uutils documentation, add enum * respect lint * Tweak comment to trigger CI * respect lint * Update README.md Co-authored-by: jrenaf <[email protected]> * Update main Readme (#272) * Update README.md * Update README.md * Add front page figure * Update README.md * Update README.md * Update README.md * Update README.md * divide param launch * reorganize param * Fix local planner timestep bug and readd trajectory publisher (#274) * Readd trajectory publisher * Fix global plan timestamp setting in local planner * Readd gazebo plugins * Add correct plugins back in and begin implementing global param reading * Update visualization and logging to fully support multi-robot operation * Update plotjuggler to add robot_1 namespace * Split topic yamls into global and robot topics * Update remaining parameter setting and checking, tweak some leaping parameters for good measure * Add arg to load robot params in circleci script * Update README.md * Remove ekf estimator package (now in robot driver) and deprecated launch files * Update documentation (#278) * Add images of topic structure * Update promo figure * Add files via upload * Update README.md * Update README.md * Add files via upload * Add files via upload * Update promo figure * update gbp and utils documentation * fixed image formatting * fixed links in documentation * readme formatting Co-authored-by: Alex Stutt <[email protected]> * Rename global planner (#280) * Add images of topic structure * Update promo figure * Add files via upload * Update README.md * Update README.md * Add files via upload * Add files via upload * Update promo figure * update gbp and utils documentation * fixed image formatting * Rename Fast Global Motion Planner to Global Body Planner for Legged Robots (gbpl) * Update GBP readme with new name * Update README.md * Update README.md * Update README.md Co-authored-by: Alex Stutt <[email protected]> * cleaned out all osqp stuff (#281) * Update README.md Co-authored-by: Justin Yim <[email protected]> Co-authored-by: Yanhao Yang <[email protected]> Co-authored-by: Robomechanics Lab <[email protected]> Co-authored-by: justinyim <[email protected]> Co-authored-by: ardalantj <[email protected]> Co-authored-by: Yanhao Yang <[email protected]> Co-authored-by: Bill Yu <[email protected]> Co-authored-by: jrenaf <[email protected]> Co-authored-by: Jiming Ren <[email protected]> Co-authored-by: RML group <[email protected]> Co-authored-by: qishuny <[email protected]> Co-authored-by: Alex Stutt <[email protected]>

* Attempt to handle slopes by predicting z_max * Switch build system to catkin_tools, switch spirit* to quad* * Fix timeout in catkin_tools install * Reduce jobs in circle ci to avoid compiling issues * Initial commit * Fix spirit namespace in parsing scripts * fix sdls matrix inverse by comparing with the max eigen value, change nmpc controller test to a trotting at the same point * Add restart flag to mblink converter * Increase update rate of mblink converter to 500Hz * Update model.config * Update README.md * Update processLog.m * Update LICENSE * Update LICENSE.txt * Update LICENSE * Update global_body_planner.yaml * Update planning_utils.h * Update CMakeLists.txt Removed from line 31: include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../external/QuadSDK/SDK/thirdparty) * Change a few more namespacing instances, remove .travis.yml * Delete parseSpiritBag.m * Finish initial LegControllerTemplate integration * Add joint controller (needs cleaning) * Switch back to grf controller * Switch leg controller to leg controller interface, implement leg controller template as abstract class * Fix leg controller test bug * add panic variable, better warm start * fix inverse dynamics, fix gait round up problem * fix inverse dynamics, add hip clearance for footstep planner, revert plan index and current position for twist planner * set the control and estimation rate to 500Hz so as to match the real robot * Switch FastTerrainMap queries from O(n^2) to O(1) * Add getNearestValidFoothold function * Add traversability mask layer to terrain map * Switch to flying trot * Fix bugs in merge (missed some namespaces) * Switch foot markers to spheres * Fix timestamp bug from merge * fix swing foot ID * fix ID again * Begin cleanup of global body planner spin * Add GlobalBodyPlan class and clean up global_body_planner.cpp interface * Remove matlab gen msg * About to begin switch to Eigen for state * cleaned up the mpc stuff and removed dependencies * removed additional extra files going to write unitest for computeFootPos next * Added spirit_rotors.sdf with rotor inertias and launch flag to switch spirit.sdf/spirit_rotors.sdf. Also spirit.sdf upper link masses now match spirit.urdf (from inertial identification). * Correct bugs in gbp actions, add tests, update sim rate, add features to fast terrain map * removed global footstep planner as well as qpoases and osqp * First commit after gbp overhaul with valid leap plans, update some sim update rates, increase slope penalty on footholds * Fix some bugs in the merge * Reduce traversability threshold * clean up pull request * fix ipopt building error * delete qpOASES * specify coinbrew version * Fix out of range bug in FTM, connect timing bug in GBP, update sim timesteps for worlds * Reasonable GBP performance * Fix bug in NMPC test, rename time_ahead to first_element_duration * online update swing foot trajectory * fix merge issues * init nominal prev foot postion, fix interp when z duration = 0, update pass foothold more frequently * limit acceleration of swing foot * Clean up GBP formatting * Remove NMPC test requirement to solve problem given CIs limitations * better swing foot acceleration limit, fix acceleration interplation at the end, smaller feedback on velocity, consider toe radius * fix init of grf position * PR updates (mostly refactoring for cleanliness and const protection) * Rename GlobalBodyPlan functions * Unify stance and swing foot planning (#199) * Initial commit moving to computeFootPlan, not compiled yet * computeFootPlan works, beginning refactoring * Fix impact acceleration bug, switch to MultiFootState for past footholds, clean LFP * Fix timestamp for foot message, clean Co-authored-by: Yanhao Yang <[email protected]> Co-authored-by: Yanhao Yang <[email protected]> * intermediate commit * Finihsed adding tests * intermediate commit before rebase * added pipeline fix * Addressed pr comments * Improve simulation and nmpc (#202) * Improve simulation friction, contact, and physics * Sync new nmpc updates * fix typo in warmstart initialization * Update ipopt options, update discretization * Fix another typo, and hopefully fix the coinbrew building error * update the rotor sdf Co-authored-by: Joe Norby <[email protected]> * Reformat to Google Style for C++ and Add CI Linting (#205) * Reformat all cpp, h, hpp files to google style, implement linting script and add to tests (one error inserted for CI testing) * Make scripts folder for cleaner root, fix test formatting error * Fix CircleCI script path * Format updates to pass linting checks * Update logging and planning launch files, remove global footstep planner (#209) * Update live plot to show ground truth grf, uniform colors and labels (#210) * Add teleop_twist_joy and configure to robot driver control modes (#211) * Add teleop_twist_joy and configure to robot driver control modes * Remove teleop_twist_joy tests of enable button, add cmd_vel to visualization * Remove EKF_tests_2021_12_3 * add grf plan initialization * update initialization * Add stand mode for local planner, remove twist body planner (#217) * Add stand mode for local planner, remove twist body planner * Fix weird formatting changes * Change stand threshold variables to ROS params, add cleaner statement of conditions * Update threshold for end of global plan to ROS param * Added teleop-twist-joy to setup_deps since joystick doesn't respond without this install. * Remove stray lines Co-authored-by: Justin Yim <[email protected]> * Improve swing foot plan (#214) * fix bugs, fix foothold heuristics, better swing foot acc limitation * remove online swing foot update, revert pd gains * Foothold nominal position minimize the maximum distance from hip plan MPC will not modify body and grf plan if fail to solve * fix centrifugal tracking term * Clean * Clean * Remove unused files and functions * Keep the original style * Fix some comments and warning info * Pass test * Resolve review requests * code format * Revert stand pose * Add getNextLiftoffIndex Use contact schedule for end_of_stance Move computeSwingApex to cpp * Fix test All the parameters should match the server... Co-authored-by: Joe Norby <[email protected]> * Clean unused or duplicate params in local planner and nmpc controller (#220) * Add stand mode for local planner, remove twist body planner * Fix weird formatting changes * Change stand threshold variables to ROS params, add cleaner statement of conditions * Update threshold for end of global plan to ROS param * Added teleop-twist-joy to setup_deps since joystick doesn't respond without this install. * Remove local_body_planner params (they are in nmpc yaml) and horizon length/timestep from nmpc (they are in local planner) * Remove stray lines * Update params in CMPC test and remove adaptive complexity test (will come in later PR) * Update local planner test with new param Co-authored-by: Justin Yim <[email protected]> * multiple robot spawning working * Add dash profile for rqt and roslaunch support * intermediate commit * Fix end of stance bug (#224) * Fix end of stance bug * Add comment clarifying logic * Add tcpnodelay to logging, sim control, and sim contact state publishing (#225) * Add tcpnodelay to controller plugin, contact state publisher, and logging scripts, change local plan timestamp to publish time not state time * Use auto for ros::Time declaration * Add CMU text to spirit.urdf and spirit_meshes.urdf * Realign xml text * Add tcpnodelay hint to sim command topic, may actually affect performance (should improve) * Add state traces to RVizInterface (#230) * multi robot sim working * cleaned up launch file * intermediate commit * cleaned up launch files * modified launch file * minor changes * intermediate commit * multi robot teleop working * Adjust local planner to support arbitrary horizons and match length of body and foot plans (#221) * Add stand mode for local planner, remove twist body planner * Fix weird formatting changes * Change stand threshold variables to ROS params, add cleaner statement of conditions * Update threshold for end of global plan to ROS param * Added teleop-twist-joy to setup_deps since joystick doesn't respond without this install. * Remove local_body_planner params (they are in nmpc yaml) and horizon length/timestep from nmpc (they are in local planner) * Remove stray lines * Update params in CMPC test and remove adaptive complexity test (will come in later PR) * Update local planner test with new param * Modify horizon lengths in local planner, nmpc, and tests * Update nmpc controller to support arbitrary horizons, change RVizInterface to show one frame per gait period * Add comment clarifying indexing difference * Fix body height bug in LFP * Update NMPC temporal weights to saturate at a given parameter (was unbounded before), reduce surface normal filter radius Co-authored-by: Justin Yim <[email protected]> * addressed pr comments * Add joint data view (#238) * Add accurate joint limits and linear motor model to gazebo (#236) * Add accurate joint limits and linear motor model to gazebo * Add soft joint limit for knees * fixed formatting errors * fixed formatting * formatting * address pr comments * Add gap worlds and handle missing map data (#237) * Add gaps and pit worlds, add x and y layers to maps, add traversability mask option * Update rest of repo to use z_inpainted, mark areas with differences between z and z_inpainted as untraversable * Add buffer of untraversability around areas with no information, expand foothold search radius from 0.2 to 0.25 * Improve contact stability in simulation (#239) * Improve contact stability: Format contact state publisher Modify contact kp and kd setting to improve contact stability Modify simulation solver setting to improve accuracy * Synchronize changes to the new world files * Fix urdf * Fix indexing bug in gbp replanning (#240) * Update terrain to match mocap space, add some extra worlds (#241) * Remove slope from traversability, add parkour world * Make terrains fit mocap space, add some other terrains, remove pit (buggy) * Update ode solver settings for new worlds * Add abstract hardware interface and refactor Robot Driver (#242) * Add abstract hardware interface class and spirit interface, refactor robot_driver to include robot name and folders for controllers and interfaces, remove open loop robot controller and leg override * Remove deprecated launch files * Correct some comments * Add include for hardware interfaces class in robot driver * Add user_rx_data to RobotDriver, Update is_hw to is_hardware * Make user data var names more specific * Add joint state publisher to robot driver, undo incorrect imu acc mapping * Switch /mcu/state/* to /state/* to differentiate from GR topics, remove robot_proxy and ground_truth_publisher Co-authored-by: RML group <[email protected]> * addressed comments * Add utility function to load multiple sets of casadi code * Respect linting checks * roslaunch robot in arguments * able to stand up * a1 is working * change robot_ to robot_name_, reformat * Refactor quad logger, include reference state and GRFs (#248) * Update quad_logger scripts to handle trajectory data, state GRFs, better fig exporting, and clean and add comments * Update quad logger legend locations and figure limits * Update trajectory publisher to correctly publish traj state, remove deprecated support for publishing the entire trajectory * Add trajectory_publisher to planning.launch * Add IPOPT install fix for CI * Quad logger bugfix for empty data structs * Updated Ghost license from Avik and updated quad-software/LICENSE to CMU & RML. (#254) Co-authored-by: Justin Yim <[email protected]> * Add Doxygen-updating Github workflow (#255) * Remove matplotlibcpp support * Update repo name in Doxygen * Add doxygen workflow * Add devel to Doxygen updated branches * Remove matplotlibcpp comments from gbp * Fix complementary filter in Robot Driver (#253) * Update quad_logger scripts to handle trajectory data, state GRFs, better fig exporting, and clean and add comments * Update quad logger legend locations and figure limits * Update trajectory publisher to correctly publish traj state, remove deprecated support for publishing the entire trajectory * Add trajectory_publisher to planning.launch * Add IPOPT install fix for CI * Quad logger bugfix for empty data structs * Fix bug in velocity comp filter * Second-order filters * Bug fix Co-authored-by: Robomechanics Lab <[email protected]> Co-authored-by: Yanhao Yang <[email protected]> * Finalize complementary filter bug fixes (#258) * Finalize * Move the acceleration flip to the spirit interface. * Add framework for adaptive complexity, GBP updates, and many miscellaneous tweaks (#256) * Add motor model * Start adding state lifting * First draft of centroidal and full state validity checking * Start passing constraint violations out of validity checking functions * First commit with working validity checking * Add hard coded local plan * Leaping with local planner, still have multiple finite elements during the flight phase, add closed form linear dynamics * Initial commit for adaptive complexity * Updating nmpc member variables and params * Streamline debug print statements, switch to more flexible validity checking * Implement maximum height at liftoff * update constraint bounds * Start adding utils test * Add adaptive complexity functions * Update contact sequence and state weights for leap * Continue updates * Nearly finished with compute_nnz_jac_g * Add phase dependent reachability back in * Finish updating nnz functions, still buggy * Move gen code to new folder, fix compute nnz bug, eval_f, bounds * Implement eval_g * Finish main virtual functions * Reformat quad_nlp.cpp and switch to VectorXd instead of MatrixXd where relevant * Remove accidental cold start in update_complexity_schedule * Add indexing vectors to prep for merge with indexing functions * Add support for world frame foot pos in local planner * First pass at merging with nmpc indexing * fix bugs, fix foothold heuristics, better swing foot acc limitation * Continue pre-merge with nmpc_indexing * Remove first_step check form test * Update eval functions and unit test * More bugfixes, passes nnz checks for jac and hess * Fix nmpc_controller test to set complexity schedule length to N+1 * Fix bugs in simple model and switch to world frame foot positions (complex still fails) * remove online swing foot update, revert pd gains * Re-add GBP planning utils updates from merge * Fix some bugs in the merge (formatting, quad kd tests, planning.launch) * Foothold nominal position minimize the maximum distance from hip plan MPC will not modify body and grf plan if fail to solve * Update logging and planning launch files, remove global footstep planner * fix centrifugal tracking term * Fix vector slicing bug - complex sometimes solves, mostly does not * Clean * Add copy constructor for quadNLP * update sdf to increase mu to 1 * Continue adding to shift initial guess * Finish adding warm start for null vars, bug in setup somewhere * constraint indexing fixed, use correct parameters (may still be structure bug) * Add flexibility for initial state * Update constraint funcs, fix small complexity bugs * Fix kinematics bug, add constraints back, increase ma57_pre_alloc * Readd contact schedule and state update to test for to match shift initial guess * Clean * Remove unused files and functions * Keep the original style * Fix some comments and warning info * Add stand mode for local planner, remove twist body planner * Pass test * Resolve review requests * code format * Update warm start to leave new complex decision variables unchanged from last solve * Revert stand pose * Begin simulation testing * Fix weird formatting changes * Fix merge bug * Change to update structure and update initial guess so we can handle changing complexity (still bugs) * Disable warm start if problem structure got more complex * Begin unifying kinematic constraint evaluation between local planner and nlp * Change stand threshold variables to ROS params, add cleaner statement of conditions * Update threshold for end of global plan to ROS param * Added teleop-twist-joy to setup_deps since joystick doesn't respond without this install. * Remove local_body_planner params (they are in nmpc yaml) and horizon length/timestep from nmpc (they are in local planner) * Remove stray lines * Update params in CMPC test and remove adaptive complexity test (will come in later PR) * Update gen functions to match QuadKD * Add getNextLiftoffIndex Use contact schedule for end_of_stance Move computeSwingApex to cpp * Fix test All the parameters should match the server... * Implement evalLiftedTrajectoryConstraints * Update local planner test with new param * Modify horizon lengths in local planner, nmpc, and tests * Update to use equal horizon for foot and body plan * Begin debugging solve fails, increase sim friction * Turn of require_init for better solves * Turn off hip threshold, turn on slack vars, reduce normals vector filter, increase GBP clearance to plan over step * Fix end of stance bug, add a few parameter changes for more feasible leaps * Begin adding panic var for constraints * Finish first pass of constraint relaxations * Add separate panic weigts for constraints * Added parameters for hard and soft bounds, about to edit quadNLP internals to use * Mid transition to relaxed constraints * First pass at soft constraint penalty with hard constraint, runs but not tested for performance * Bugs mostly solved, takes a while to solve though * Nightly commit * Remove takeoff state weight, enforce back leg reachability, add tcpnodelay to controller plugin * More debugging, always enforce hard joint limits * Start adding adaptive case * Refactor GBP terrain height functions, add grid_map support * Add traversability heuristic to gbp * Increase filter size for surface normals (may reverse this), add minimum foot clearance during swing * Add state traces to RVizInterface * First successful leap - front legs lead rear, reduce friction * Start softening all constraints * Finish implementing relaxation of all complex constraints but leave only knee relaxed, solves pretty well but fails fk * Tweak weights, much better performance. Verified constraint jacobian * Add cartesian space PD feedback for swing legs * Remove unnecessary private vars * Reduce jacobian inv damping, align world more accurately, use only cartesian joint PD, tracking looking much better * Add kinematic cost to valid footholds penalizing deviations from nominal and previous * Add accurate joint limits and linear motor model to gazebo * Add gaps and pit worlds, add x and y layers to maps, add traversability mask option * Update rest of repo to use z_inpainted, mark areas with differences between z and z_inpainted as untraversable * Add soft joint limit for knees * Change starting location in quad_gazebo * Add buffer of untraversability around areas with no information, expand foothold search radius from 0.2 to 0.25 * Add lognorm velocity sampling to GBP, restrict replanning to connect phases, tweak vel parameters * Remove slope from traversability, add parkour world * Reduce cartesian feedback gains, add stiff pd for knee hard stop, relax buffer in GBP leap * Make terrains fit mocap space, add some other terrains, remove pit (buggy) * Update ode solver settings for new worlds * Fix indexing bug in gbp replanning * Squashed commit of the following: commit 14a26dfd3d0541733438a170e3c2d41e61d9dca7 Author: jcnorby <[email protected]> Date: Fri Apr 15 08:46:40 2022 -0400 Squashed commit of the following: commit 88276122a535e4e1047b1cbedeab0bba3c304b8e Author: Joe Norby <[email protected]> Date: Thu Apr 14 19:25:07 2022 -0400 Add abstract hardware interface and refactor Robot Driver (#242) * Add abstract hardware interface class and spirit interface, refactor robot_driver to include robot name and folders for controllers and interfaces, remove open loop robot controller and leg override * Remove deprecated launch files * Correct some comments * Add include for hardware interfaces class in robot driver * Add user_rx_data to RobotDriver, Update is_hw to is_hardware * Make user data var names more specific * Add joint state publisher to robot driver, undo incorrect imu acc mapping * Switch /mcu/state/* to /state/* to differentiate from GR topics, remove robot_proxy and ground_truth_publisher Co-authored-by: RML group <[email protected]> commit f96ab7550ac96c63fb110b19f6e063061f82e83f Author: jcnorby <[email protected]> Date: Fri Apr 15 08:41:39 2022 -0400 Squashed commit of the following: commit e3b179ae3f65dbd996dcd25e33332caa5bb4e27c Author: jcnorby <[email protected]> Date: Thu Apr 14 17:00:44 2022 -0400 Fix bug in setting current state and in evaluating constraints, add terrain constraint, update constraint name map commit fdb2b2a0dcffea09a792558a8e6717e1083d0f09 Author: Joe Norby <[email protected]> Date: Wed Apr 13 22:41:50 2022 -0400 Simplify warm start, correct application of foot constraitnts, add terrain variable commit 172247e7b579d78e8053e80e9f706c1ef5554bde Author: jcnorby <[email protected]> Date: Wed Apr 13 18:01:02 2022 -0400 Switch to ZOH foot input, revert to N-1 control vars and remove u1 from finite elements, update conditions for foot dynamics and constraints commit 277720bee311225cabe7efd6e4fe2b74ced75faf Author: jcnorby <[email protected]> Date: Tue Apr 12 18:02:56 2022 -0400 Fix bug which turned foot EOM off, apply new foot traj online (still violates constraints) commit de5990352dc15515012c7e7ab7196430e9feabca Author: jcnorby <[email protected]> Date: Mon Apr 11 12:42:28 2022 -0400 Turn on foot EOM constraints during swing commit a3fd2e7c0f6f88efe13302771c99f53e641dd652 Author: Joe Norby <[email protected]> Date: Thu Apr 7 23:02:31 2022 -0400 Clean up print statements commit 451c43e7089ea4e1ef7360c5ec2c6181565bf196 Merge: 0e94b60e d2b2e18a Author: Joe Norby <[email protected]> Date: Thu Apr 7 21:54:57 2022 -0400 Merge branch 'melodic_devel_adaptive_complexity_add_foot_state_correct' into melodic_devel_adaptive_complexity_add_foot_state commit d2b2e18a93394efcf7a38088caec9d67e05a1433 Author: Joe Norby <[email protected]> Date: Thu Apr 7 21:53:47 2022 -0400 Add terminal input to cost, recovers previous solve time commit 393337c0a16b33b78eaf9de626ad0bdecc2c878c Author: Joe Norby <[email protected]> Date: Thu Apr 7 20:53:36 2022 -0400 Correct foot state warm start and cost commit 0e94b60e21ddbef22415d5caa537f04f4c1430e6 Author: jcnorby <[email protected]> Date: Thu Apr 7 15:51:22 2022 -0400 Start updating cost to include control at last iteration commit 6b58687abf38115515f3cad4d09d53fab43f3474 Merge: a1a19e9a 14f0624c Author: jcnorby <[email protected]> Date: Thu Apr 7 13:37:29 2022 -0400 Merge branch 'melodic_devel_adaptive_complexity' into melodic_devel_adaptive_complexity_add_foot_state commit a1a19e9af9c73675553bec1f0fb6785e036a415c Author: Joe Norby <[email protected]> Date: Wed Apr 6 23:10:14 2022 -0400 Fix tail mode bug, a few more indexing issues commit 6df542a7976d1a6398e1814b1913674f701fc765 Author: Joe Norby <[email protected]> Date: Tue Apr 5 21:21:10 2022 -0400 Initial commit wit foot state vars, midway through porting commit 52a26803419a0d97f9bc965f9d69e5fb4890e6df Merge: 14f0624c dd517bb5 Author: jcnorby <[email protected]> Date: Mon Apr 11 12:43:43 2022 -0400 Merge branch 'devel' into melodic_devel_adaptive_complexity commit dd517bb5c54166340e6b6d42893d690fe7a7d0d4 Author: Joe Norby <[email protected]> Date: Sat Apr 9 11:55:02 2022 -0400 Update terrain to match mocap space, add some extra worlds (#241) * Remove slope from traversability, add parkour world * Make terrains fit mocap space, add some other terrains, remove pit (buggy) * Update ode solver settings for new worlds commit 4be6347d5717532a7ebb10eeefd0605a5462d931 Author: Joe Norby <[email protected]> Date: Thu Apr 7 19:27:06 2022 -0400 Fix indexing bug in gbp replanning (#240) commit 14f0624cae1b31524271dda6e282b49690d94c13 Merge: b3963391 e4c7a964 Author: jcnorby <[email protected]> Date: Thu Apr 7 13:21:54 2022 -0400 Merge branch 'melodic_devel_mocap_terrain' into melodic_devel_adaptive_complexity commit b39633911f8135d4eb93b635312788e7035deec0 Merge: 5ca1d105 399836d2 Author: jcnorby <[email protected]> Date: Thu Apr 7 13:15:23 2022 -0400 Merge branch 'devel' into melodic_devel_adaptive_complexity commit 5ca1d10534770a30d53a610383ba1cc15ed9cd72 Author: Joe Norby <[email protected]> Date: Thu Mar 31 17:55:15 2022 -0400 Add velocity constraint back in, relax knee constraint, tighten soft joint limits, soften cartesian PD commit 5a9487d164462903c61721a95890a46718bbd485 Merge: a8d357cc a509af75 Author: Joe Norby <[email protected]> Date: Wed Mar 30 18:23:25 2022 -0400 Merge branch 'melodic_devel_adaptive_complexity' of github.com:robomechanics/quad-software into melodic_devel_adaptive_complexity commit a509af75ccca8b028bfc028a7cb667dcd9e24acf Author: jcnorby <[email protected]> Date: Mon Mar 28 13:11:05 2022 -0400 Readd hip height constraint on foot trajectory, speed up sim commit a711fda92e0767c4a2bbf71f5dad8fa65bd1a89a Merge: 262d4a16 69841722 Author: jcnorby <[email protected]> Date: Mon Mar 28 12:39:39 2022 -0400 Merge branch 'melodic_devel_add_joint_data_to_liveplot' into melodic_devel_adaptive_complexity commit 698417222b2edaf461b4a9d1482dde96abb427d5 Author: jcnorby <[email protected]> Date: Mon Mar 28 12:11:37 2022 -0400 Add joint data view commit 262d4a16708f4694d477c81205f0b37ca06d5453 Author: jcnorby <[email protected]> Date: Mon Mar 28 11:56:01 2022 -0400 Lower max GRFs in NMPC and remove some print statements commit 6d3b39195817a12c13a2688b93e4d0b205028e3d Merge: f23165a4 15bef6bb Author: jcnorby <[email protected]> Date: Mon Mar 28 10:41:07 2022 -0400 Merge branch 'melodic_devel_correct_sim_kin_dyn_limits' into melodic_devel_adaptive_complexity commit a8d357cc777bcc343af97f1d430de8daa8d23551 Merge: 977e9812 50749b59 Author: Joe Norby <[email protected]> Date: Sun Mar 27 21:04:37 2022 -0400 Merge branch 'melodic_devel_mapping_set_untraversable_regions' into melodic_devel_adaptive_complexity * Merged devel, squashed commit of the following: commit 88276122a535e4e1047b1cbedeab0bba3c304b8e Author: Joe Norby <[email protected]> Date: Thu Apr 14 19:25:07 2022 -0400 Add abstract hardware interface and refactor Robot Driver (#242) * Add abstract hardware interface class and spirit interface, refactor robot_driver to include robot name and folders for controllers and interfaces, remove open loop robot controller and leg override * Remove deprecated launch files * Correct some comments * Add include for hardware interfaces class in robot driver * Add user_rx_data to RobotDriver, Update is_hw to is_hardware * Make user data var names more specific * Add joint state publisher to robot driver, undo incorrect imu acc mapping * Switch /mcu/state/* to /state/* to differentiate from GR topics, remove robot_proxy and ground_truth_publisher Co-authored-by: RML group <[email protected]> commit dd517bb5c54166340e6b6d42893d690fe7a7d0d4 Author: Joe Norby <[email protected]> Date: Sat Apr 9 11:55:02 2022 -0400 Update terrain to match mocap space, add some extra worlds (#241) * Remove slope from traversability, add parkour world * Make terrains fit mocap space, add some other terrains, remove pit (buggy) * Update ode solver settings for new worlds commit 4be6347d5717532a7ebb10eeefd0605a5462d931 Author: Joe Norby <[email protected]> Date: Thu Apr 7 19:27:06 2022 -0400 Fix indexing bug in gbp replanning (#240) * Add additional terrains and update starting position * Expand traversability filter * Initial commit benchmarking timing performance before removing slack variables * Update settings to match current adaptive complexity branch * Add parameters to enable leaping and AC * Unify terrain and traversability settings for leaping and AC * Disable linear mm, add test fixture to gbp and begin porting tests * Update gbp to pass tests, tweak some refinement parameters * Beginning modifications to re-enable leaping: readd Jinv damping, remove cartesian feedback * Soften knee hard stop and add slight front liftoff period in contact schedule * Update quad_logger scripts to handle trajectory data, state GRFs, better fig exporting, and clean and add comments * Update quad logger legend locations and figure limits * Update trajectory publisher to correctly publish traj state, remove deprecated support for publishing the entire trajectory * Update QuadKD to enforce knee-above-hip, use accurate joint limits, and select knee position to minimize error * Update nmpc gains, u_max, solve times to old values, use forward euler dynamics, disable hard complex bounds, disable foot dynamics, remove front foot liftoff period * Update robot driver to publish current traj state, quad utils to permit this * Nightly commit - re-add foot traj modification, complex elements * Increase friction, change current_plan_index from 0 to -1 * Update complimentarity filter and add debug statements * Fix bug in velocity comp filter * Add nmpc and lp parameter changes for 1.5m/s trotting in hw * Add foot height constraint framework, data not yet passed. Revert casadi funcs to 4/20 commit but with terrain added * Add terrain height and slope into nlp, solves but fails on steps * Mid-debugging overconstraining of foot positions * Refactor nmpc param loading, constructor, quadnlp constructor, test that OG casadi fcns work * Add vectors for dimension of cost terms and include in cost calculations. Not tested with complex yet * Add new casadi funcs which match devel's (except using closed form integration for linear acc rather than Euler) * Remove feet_in_simple param, begin correcting foot constraints * Set LP update rate to 300Hz to match mean solve time, tweak warm start * Remove unnecessary changes from devel * Clean up NMPC tests and primary working functions * Pass linting checks * Clean up comments and PR review refactors * Make parameter loading a member of the PlannerConfig struct, integrate in test fixture and ros interface * Add todo comment * Reduce catkin tests parallel jobs * Move node handle constructor to text fixture body (attempt to fix CircleCI bug), remove some print statements * More cleaning for PR * Reduce nmpc max solve time * Parameter updates for successful hardware deployment * Final merge updates - fix global plan state publishing, make traversability names more clear, enforce legbases stay over terrain in GBP * Update names to quad-sdk, update README with construction notice Co-authored-by: Yanhao Yang <[email protected]> Co-authored-by: Justin Yim <[email protected]> Co-authored-by: RML group <[email protected]> Co-authored-by: Robomechanics Lab <[email protected]> * Fix small bug in checking isInMap (switch to using grid_map) (#260) * Update to c++14 (#262) * removing unneeded files * removing commented out .sdf sections. * remove unused simulator plugins * Permit variable horizon lengths in Local Planner, add NLP diagnostics, add working mixed complexity solving. (#263) * Add motor model * Start adding state lifting * First draft of centroidal and full state validity checking * Start passing constraint violations out of validity checking functions * First commit with working validity checking * Add hard coded local plan * Leaping with local planner, still have multiple finite elements during the flight phase, add closed form linear dynamics * Initial commit for adaptive complexity * Updating nmpc member variables and params * Streamline debug print statements, switch to more flexible validity checking * Implement maximum height at liftoff * update constraint bounds * Start adding utils test * Add adaptive complexity functions * Update contact sequence and state weights for leap * Continue updates * Nearly finished with compute_nnz_jac_g * Add phase dependent reachability back in * Finish updating nnz functions, still buggy * Move gen code to new folder, fix compute nnz bug, eval_f, bounds * Implement eval_g * Finish main virtual functions * Reformat quad_nlp.cpp and switch to VectorXd instead of MatrixXd where relevant * Remove accidental cold start in update_complexity_schedule * Add indexing vectors to prep for merge with indexing functions * Add support for world frame foot pos in local planner * First pass at merging with nmpc indexing * fix bugs, fix foothold heuristics, better swing foot acc limitation * Continue pre-merge with nmpc_indexing * Remove first_step check form test * Update eval functions and unit test * More bugfixes, passes nnz checks for jac and hess * Fix nmpc_controller test to set complexity schedule length to N+1 * Fix bugs in simple model and switch to world frame foot positions (complex still fails) * remove online swing foot update, revert pd gains * Re-add GBP planning utils updates from merge * Fix some bugs in the merge (formatting, quad kd tests, planning.launch) * Foothold nominal position minimize the maximum distance from hip plan MPC will not modify body and grf plan if fail to solve * Update logging and planning launch files, remove global footstep planner * fix centrifugal tracking term * Fix vector slicing bug - complex sometimes solves, mostly does not * Clean * Add copy constructor for quadNLP * update sdf to increase mu to 1 * Continue adding to shift initial guess * Finish adding warm start for null vars, bug in setup somewhere * constraint indexing fixed, use correct parameters (may still be structure bug) * Add flexibility for initial state * Update constraint funcs, fix small complexity bugs * Fix kinematics bug, add constraints back, increase ma57_pre_alloc * Readd contact schedule and state update to test for to match shift initial guess * Clean * Remove unused files and functions * Keep the original style * Fix some comments and warning info * Add stand mode for local planner, remove twist body planner * Pass test * Resolve review requests * code format * Update warm start to leave new complex decision variables unchanged from last solve * Revert stand pose * Begin simulation testing * Fix weird formatting changes * Fix merge bug * Change to update structure and update initial guess so we can handle changing complexity (still bugs) * Disable warm start if problem structure got more complex * Begin unifying kinematic constraint evaluation between local planner and nlp * Change stand threshold variables to ROS params, add cleaner statement of conditions * Update threshold for end of global plan to ROS param * Added teleop-twist-joy to setup_deps since joystick doesn't respond without this install. * Remove local_body_planner params (they are in nmpc yaml) and horizon length/timestep from nmpc (they are in local planner) * Remove stray lines * Update params in CMPC test and remove adaptive complexity test (will come in later PR) * Update gen functions to match QuadKD * Add getNextLiftoffIndex Use contact schedule for end_of_stance Move computeSwingApex to cpp * Fix test All the parameters should match the server... * Implement evalLiftedTrajectoryConstraints * Update local planner test with new param * Modify horizon lengths in local planner, nmpc, and tests * Update to use equal horizon for foot and body plan * Begin debugging solve fails, increase sim friction * Turn of require_init for better solves * Turn off hip threshold, turn on slack vars, reduce normals vector filter, increase GBP clearance to plan over step * Fix end of stance bug, add a few parameter changes for more feasible leaps * Begin adding panic var for constraints * Finish first pass of constraint relaxations * Add separate panic weigts for constraints * Added parameters for hard and soft bounds, about to edit quadNLP internals to use * Mid transition to relaxed constraints * First pass at soft constraint penalty with hard constraint, runs but not tested for performance * Bugs mostly solved, takes a while to solve though * Nightly commit * Remove takeoff state weight, enforce back leg reachability, add tcpnodelay to controller plugin * More debugging, always enforce hard joint limits * Start adding adaptive case * Refactor GBP terrain height functions, add grid_map support * Add traversability heuristic to gbp * Increase filter size for surface normals (may reverse this), add minimum foot clearance during swing * Add state traces to RVizInterface * First successful leap - front legs lead rear, reduce friction * Start softening all constraints * Finish implementing relaxation of all complex constraints but leave only knee relaxed, solves pretty well but fails fk * Tweak weights, much better performance. Verified constraint jacobian * Add cartesian space PD feedback for swing legs * Remove unnecessary private vars * Reduce jacobian inv damping, align world more accurately, use only cartesian joint PD, tracking looking much better * Add kinematic cost to valid footholds penalizing deviations from nominal and previous * Add accurate joint limits and linear motor model to gazebo * Add gaps and pit worlds, add x and y layers to maps, add traversability mask option * Update rest of repo to use z_inpainted, mark areas with differences between z and z_inpainted as untraversable * Add soft joint limit for knees * Change starting location in quad_gazebo * Add buffer of untraversability around areas with no information, expand foothold search radius from 0.2 to 0.25 * Add lognorm velocity sampling to GBP, restrict replanning to connect phases, tweak vel parameters * Remove slope from traversability, add parkour world * Reduce cartesian feedback gains, add stiff pd for knee hard stop, relax buffer in GBP leap * Make terrains fit mocap space, add some other terrains, remove pit (buggy) * Update ode solver settings for new worlds * Fix indexing bug in gbp replanning * Squashed commit of the following: commit 14a26dfd3d0541733438a170e3c2d41e61d9dca7 Author: jcnorby <[email protected]> Date: Fri Apr 15 08:46:40 2022 -0400 Squashed commit of the following: commit 88276122a535e4e1047b1cbedeab0bba3c304b8e Author: Joe Norby <[email protected]> Date: Thu Apr 14 19:25:07 2022 -0400 Add abstract hardware interface and refactor Robot Driver (#242) * Add abstract hardware interface class and spirit interface, refactor robot_driver to include robot name and folders for controllers and interfaces, remove open loop robot controller and leg override * Remove deprecated launch files * Correct some comments * Add include for hardware interfaces class in robot driver * Add user_rx_data to RobotDriver, Update is_hw to is_hardware * Make user data var names more specific * Add joint state publisher to robot driver, undo incorrect imu acc mapping * Switch /mcu/state/* to /state/* to differentiate from GR topics, remove robot_proxy and ground_truth_publisher Co-authored-by: RML group <[email protected]> commit f96ab7550ac96c63fb110b19f6e063061f82e83f Author: jcnorby <[email protected]> Date: Fri Apr 15 08:41:39 2022 -0400 Squashed commit of the following: commit e3b179ae3f65dbd996dcd25e33332caa5bb4e27c Author: jcnorby <[email protected]> Date: Thu Apr 14 17:00:44 2022 -0400 Fix bug in setting current state and in evaluating constraints, add terrain constraint, update constraint name map commit fdb2b2a0dcffea09a792558a8e6717e1083d0f09 Author: Joe Norby <[email protected]> Date: Wed Apr 13 22:41:50 2022 -0400 Simplify warm start, correct application of foot constraitnts, add terrain variable commit 172247e7b579d78e8053e80e9f706c1ef5554bde Author: jcnorby <[email protected]> Date: Wed Apr 13 18:01:02 2022 -0400 Switch to ZOH foot input, revert to N-1 control vars and remove u1 from finite elements, update conditions for foot dynamics and constraints commit 277720bee311225cabe7efd6e4fe2b74ced75faf Author: jcnorby <[email protected]> Date: Tue Apr 12 18:02:56 2022 -0400 Fix bug which turned foot EOM off, apply new foot traj online (still violates constraints) commit de5990352dc15515012c7e7ab7196430e9feabca Author: jcnorby <[email protected]> Date: Mon Apr 11 12:42:28 2022 -0400 Turn on foot EOM constraints during swing commit a3fd2e7c0f6f88efe13302771c99f53e641dd652 Author: Joe Norby <[email protected]> Date: Thu Apr 7 23:02:31 2022 -0400 Clean up print statements commit 451c43e7089ea4e1ef7360c5ec2c6181565bf196 Merge: 0e94b60e d2b2e18a Author: Joe Norby <[email protected]> Date: Thu Apr 7 21:54:57 2022 -0400 Merge branch 'melodic_devel_adaptive_complexity_add_foot_state_correct' into melodic_devel_adaptive_complexity_add_foot_state commit d2b2e18a93394efcf7a38088caec9d67e05a1433 Author: Joe Norby <[email protected]> Date: Thu Apr 7 21:53:47 2022 -0400 Add terminal input to cost, recovers previous solve time commit 393337c0a16b33b78eaf9de626ad0bdecc2c878c Author: Joe Norby <[email protected]> Date: Thu Apr 7 20:53:36 2022 -0400 Correct foot state warm start and cost commit 0e94b60e21ddbef22415d5caa537f04f4c1430e6 Author: jcnorby <[email protected]> Date: Thu Apr 7 15:51:22 2022 -0400 Start updating cost to include control at last iteration commit 6b58687abf38115515f3cad4d09d53fab43f3474 Merge: a1a19e9a 14f0624c Author: jcnorby <[email protected]> Date: Thu Apr 7 13:37:29 2022 -0400 Merge branch 'melodic_devel_adaptive_complexity' into melodic_devel_adaptive_complexity_add_foot_state commit a1a19e9af9c73675553bec1f0fb6785e036a415c Author: Joe Norby <[email protected]> Date: Wed Apr 6 23:10:14 2022 -0400 Fix tail mode bug, a few more indexing issues commit 6df542a7976d1a6398e1814b1913674f701fc765 Author: Joe Norby <[email protected]> Date: Tue Apr 5 21:21:10 2022 -0400 Initial commit wit foot state vars, midway through porting commit 52a26803419a0d97f9bc965f9d69e5fb4890e6df Merge: 14f0624c dd517bb5 Author: jcnorby <[email protected]> Date: Mon Apr 11 12:43:43 2022 -0400 Merge branch 'devel' into melodic_devel_adaptive_complexity commit dd517bb5c54166340e6b6d42893d690fe7a7d0d4 Author: Joe Norby <[email protected]> Date: Sat Apr 9 11:55:02 2022 -0400 Update terrain to match mocap space, add some extra worlds (#241) * Remove slope from traversability, add parkour world * Make terrains fit mocap space, add some other terrains, remove pit (buggy) * Update ode solver settings for new worlds commit 4be6347d5717532a7ebb10eeefd0605a5462d931 Author: Joe Norby <[email protected]> Date: Thu Apr 7 19:27:06 2022 -0400 Fix indexing bug in gbp replanning (#240) commit 14f0624cae1b31524271dda6e282b49690d94c13 Merge: b3963391 e4c7a964 Author: jcnorby <[email protected]> Date: Thu Apr 7 13:21:54 2022 -0400 Merge branch 'melodic_devel_mocap_terrain' into melodic_devel_adaptive_complexity commit b39633911f8135d4eb93b635312788e7035deec0 Merge: 5ca1d105 399836d2 Author: jcnorby <[email protected]> Date: Thu Apr 7 13:15:23 2022 -0400 Merge branch 'devel' into melodic_devel_adaptive_complexity commit 5ca1d10534770a30d53a610383ba1cc15ed9cd72 Author: Joe Norby <[email protected]> Date: Thu Mar 31 17:55:15 2022 -0400 Add velocity constraint back in, relax knee constraint, tighten soft joint limits, soften cartesian PD commit 5a9487d164462903c61721a95890a46718bbd485 Merge: a8d357cc a509af75 Author: Joe Norby <[email protected]> Date: Wed Mar 30 18:23:25 2022 -0400 Merge branch 'melodic_devel_adaptive_complexity' of github.com:robomechanics/quad-software into melodic_devel_adaptive_complexity commit a509af75ccca8b028bfc028a7cb667dcd9e24acf Author: jcnorby <[email protected]> Date: Mon Mar 28 13:11:05 2022 -0400 Readd hip height constraint on foot trajectory, speed up sim commit a711fda92e0767c4a2bbf71f5dad8fa65bd1a89a Merge: 262d4a16 69841722 Author: jcnorby <[email protected]> Date: Mon Mar 28 12:39:39 2022 -0400 Merge branch 'melodic_devel_add_joint_data_to_liveplot' into melodic_devel_adaptive_complexity commit 698417222b2edaf461b4a9d1482dde96abb427d5 Author: jcnorby <[email protected]> Date: Mon Mar 28 12:11:37 2022 -0400 Add joint data view commit 262d4a16708f4694d477c81205f0b37ca06d5453 Author: jcnorby <[email protected]> Date: Mon Mar 28 11:56:01 2022 -0400 Lower max GRFs in NMPC and remove some print statements commit 6d3b39195817a12c13a2688b93e4d0b205028e3d Merge: f23165a4 15bef6bb Author: jcnorby <[email protected]> Date: Mon Mar 28 10:41:07 2022 -0400 Merge branch 'melodic_devel_correct_sim_kin_dyn_limits' into melodic_devel_adaptive_complexity commit a8d357cc777bcc343af97f1d430de8daa8d23551 Merge: 977e9812 50749b59 Author: Joe Norby <[email protected]> Date: Sun Mar 27 21:04:37 2022 -0400 Merge branch 'melodic_devel_mapping_set_untraversable_regions' into melodic_devel_adaptive_complexity * Merged devel, squashed commit of the following: commit 88276122a535e4e1047b1cbedeab0bba3c304b8e Author: Joe Norby <[email protected]> Date: Thu Apr 14 19:25:07 2022 -0400 Add abstract hardware interface and refactor Robot Driver (#242) * Add abstract hardware interface class and spirit interface, refactor robot_driver to include robot name and folders for controllers and interfaces, remove open loop robot controller and leg override * Remove deprecated launch files * Correct some comments * Add include for hardware interfaces class in robot driver * Add user_rx_data to RobotDriver, Update is_hw to is_hardware * Make user data var names more specific * Add joint state publisher to robot driver, undo incorrect imu acc mapping * Switch /mcu/state/* to /state/* to differentiate from GR topics, remove robot_proxy and ground_truth_publisher Co-authored-by: RML group <[email protected]> commit dd517bb5c54166340e6b6d42893d690fe7a7d0d4 Author: Joe Norby <[email protected]> Date: Sat Apr 9 11:55:02 2022 -0400 Update terrain to match mocap space, add some extra worlds (#241) * Remove slope from traversability, add parkour world * Make terrains fit mocap space, add some other terrains, remove pit (buggy) * Update ode solver settings for new worlds commit 4be6347d5717532a7ebb10eeefd0605a5462d931 Author: Joe Norby <[email protected]> Date: Thu Apr 7 19:27:06 2022 -0400 Fix indexing bug in gbp replanning (#240) * Add additional terrains and update starting position * Expand traversability filter * Initial commit benchmarking timing performance before removing slack variables * Update settings to match current adaptive complexity branch * Add parameters to enable leaping and AC * Unify terrain and traversability settings for leaping and AC * Disable linear mm, add test fixture to gbp and begin porting tests * Update gbp to pass tests, tweak some refinement parameters * Beginning modifications to re-enable leaping: readd Jinv damping, remove cartesian feedback * Soften knee hard stop and add slight front liftoff period in contact schedule * Update quad_logger scripts to handle trajectory data, state GRFs, better fig exporting, and clean and add comments * Update quad logger legend locations and figure limits * Update trajectory publisher to correctly publish traj state, remove deprecated support for publishing the entire trajectory * Update QuadKD to enforce knee-above-hip, use accurate joint limits, and select knee position to minimize error * Update nmpc gains, u_max, solve times to old values, use forward euler dynamics, disable hard complex bounds, disable foot dynamics, remove front foot liftoff period * Update robot driver to publish current traj state, quad utils to permit this * Nightly commit - re-add foot traj modification, complex elements * Increase friction, change current_plan_index from 0 to -1 * Update complimentarity filter and add debug statements * Fix bug in velocity comp filter * Add nmpc and lp parameter changes for 1.5m/s trotting in hw * Add foot height constraint framework, data not yet passed. Revert casadi funcs to 4/20 commit but with terrain added * Add terrain height and slope into nlp, solves but fails on steps * Mid-debugging overconstraining of foot positions * Refactor nmpc param loading, constructor, quadnlp constructor, test that OG casadi fcns work * Add vectors for dimension of cost terms and include in cost calculations. Not tested with complex yet * Add new casadi funcs which match devel's (except using closed form integration for linear acc rather than Euler) * Remove feet_in_simple param, begin correcting foot constraints * Set LP update rate to 300Hz to match mean solve time, tweak warm start * Remove unnecessary changes from devel * Clean up NMPC tests and primary working functions * Pass linting checks * Complex elements work but foot still overconstrained if allowed to move * Clean up comments and PR review refactors * Make parameter loading a member of the PlannerConfig struct, integrate in test fixture and ros interface * Add todo comment * Squashed commit of the following: commit af732b46f705250058b3fae1ad38987588499859 Author: Joe Norby <[email protected]> Date: Tue May 3 19:48:36 2022 -0400 Add todo comment commit 4fca314d6988789199778653e36585f4efbee3f7 Author: Joe Norby <[email protected]> Date: Tue May 3 19:45:35 2022 -0400 Make parameter loading a member of the PlannerConfig struct, integrate in test fixture and ros interface * Reduce catkin tests parallel jobs * Move node handle constructor to text fixture body (attempt to fix CircleCI bug), remove some print statements * Add logging and plotting support for NLP data * Foot state constraints are cleaner and not infeasible but still not solving, likely a cost bug * Switch to piecewise FOH foot control, fixes overconstraint issue! * Begin debugging terrain height constraint, need to try debug mode * Update logging.launch to take bag_name as an arg * Relax foot height constraint and make clearance a function of foot vel, solves better but not perfect * Repair nmpc traj lifting and evaluating constraints * Support variable horizon planning * Readd stabilization of foot plan (which is important), relax mm constraint, move updateHorizonLength to its own function * Increase foot tracking weights to incentivize changing body pose, fix bug in joint vel constraint bounds * Add step_25cm world * Add low pass filter on new foothold positions * Add batch sim script * Add launchfile to issue a standing command * Better align step_20cm grid map with world * Change settings to match devel * Change nmpc print and AC settings to standard values * Readd NMPCController test * Clean up comments in nmpc_controller.yaml * Remove unnecessary LPF on foot position which caused failed tests * Respect linting checks * Bugfixes for variable horizon support (don't change foot plan length) * Readd index update of body and grf plan in local planner Co-authored-by: Yanhao Yang <[email protected]> Co-authored-by: Justin Yim <[email protected]> Co-authored-by: RML group <[email protected]> Co-authored-by: Robomechanics Lab <[email protected]> * solve namespace issue, but still has problems * Resolve multi-robot tf tree and rviz namespaces * Respect linting checks * Resolve test issuses * merge with devel, resolve conflicts, catkin test succeeds, lint respected * Commit to test CI * Have multi robot plan just call planning.launch, fix logging scripts to support a namespace arg * solve single and multiple robots launch issue * rename quad_plan.launch * started refactor branch to work on quad_utils and quad_msgs * added a readme to describe the quad util package * added quad_msgs, quad_utils readme, and delete BodyFroceEstimate.msg * remove RobotStateTrajectory, modified quad_msgs/README * delete unused msgs in CMakeList * delete trajectory_publisher and related functions * delete body_force_estimator and contact_detection packages * delete nmpc dependency on quad_msgs/RobotStateTrajectory.h * launch file cleanup * delete local_planner dependency on quad_msgs/RobotStateTrajectory.h * exclude contact/body force.yaml in load_params.launch file * solve replay issue, reorganize the rviz profile * Updated quad_simulator readme and removed unused launch files. * readme tweaks. * Changed math_utils fxns to pass inputs by const * Fix lint * Local Planner Refactor (#266) * clean local planner * Fix test * Fix test again * README + small cleanups * Format * Add info Co-authored-by: Alex Stutt <[email protected]> * Global Body Planner Refactor (#264) * Update README.md * Update parameter formatting in GBP, clean some unused ones * Remove temp file * Update README.md * Remove deprecated functions from planning_utils * Streamlining gbp refactors, adding vertical velocity params * Fix small bugs in param loading and valid state checking * Update connect stance time so that max vel = planning_config.h_nom * Update leaping traversability checks to yield better takeoff locations * Update goal state to only require x/y position * Update README.md * Remove a few more unnecessary parameters and update traversability to permit planning in rough terrain * Update README.md * add documentation to planning_utils * temp changes * Update tests to be more robust * Respect linting checks * intermediate commit * finish planning_uutils documentation, add enum * respect lint * Tweak comment to trigger CI * respect lint * Update README.md Co-authored-by: jrenaf <[email protected]> * added Ardalan's refactor to robot driver * fix lint * change robot driver cmake list format * change cmake list format again * Update main Readme (#272) * Update README.md * Update README.md * Add front page figure * Update README.md * Update README.md * Update README.md * Update README.md * divide param launch * reorganize param * Fix local planner timestep bug and readd trajectory publisher (#274) * Readd trajectory publisher * Fix global plan timestamp setting in local planner * Readd gazebo plugins * Add correct plugins back in and begin implementing global param reading * Update visualization and logging to fully support multi-robot operation * Update plotjuggler to add robot_1 namespace * Split topic yamls into global and robot topics * Update remaining parameter setting and checking, tweak some leaping parameters for good measure * Add arg to load robot params in circleci script * Update README.md * in refactor branch move CF estimator from robot driver to its own class * Remove ekf estimator package (now in robot driver) and deprecated launch files * Update documentation (#278) * Add images of topic structure * Update promo figure * Add files via upload * Update README.md * Update README.md * Add files via upload * Add files via upload * Update promo figure * update gbp and utils documentation * fixed image formatting * fixed links in documentation * readme formatting Co-authored-by: Alex Stutt <[email protected]> * change function input, fix lint * Rename global planner (#280) * Add images of topic structure * Update promo figure * Add files via upload * Update README.md * Update README.md * Add files via upload * Add files via upload * Update promo figure * update gbp and utils documentation * fixed image formatting * Rename Fast Global Motion Planner to Global Body Planner for Legged Robots (gbpl) * Update GBP readme with new name * Update README.md * Update README.md * Update README.md Co-authored-by: Alex Stutt <[email protected]> * cleaned out all osqp stuff (#281) * Additional changes to match devel * Additional changes to estimator inheritance. * Update README.md * fix format * additional format fix * Fixed params loading issue. Rewrite comments * additional changes to comments * Minor documentation updates (#287) * removed old world file, added some pics for docs * Added Unitree A1 license. * pics of another world Co-authored-by: Justin Yim <[email protected]> Co-authored-by: Joe Norby <[email protected]> * Squashed commit of the following: commit 1c05a0faa0f06be71220914555e7079aa5afb5d7 Author: Alex Stutt <[email protected]> Date: Fri Jun 10 10:57:45 2022 -0700 Minor documentation updates (#287) * removed old world file, added some pics for docs * Added Unitree A1 license. * pics of another world Co-authored-by: Justin Yim <[email protected]> Co-authored-by: Joe Norby <[email protected]> commit bae9ff0237784b692dbef41daa82a4e6c32688e3 Merge: a0b9f9e3 7cdb9756 Author: Bill Yu <[email protected]> Date: Mon Jun 6 08:55:03 2022 -0400 Merge pull request #271 from robomechanics/melodic_devel_test_robot_driver Robot driver refactoring commit 7cdb975670af7e28726e63f2886af1f18491c5b3 Author: qishuny <[email protected]> Date: Fri Jun 3 18:48:57 2022 -0400 additional changes to comments commit b577932ee5182376565512925aba127c02185053 Author: qishuny <[email protected]> Date: Fri Jun 3 18:47:13 2022 -0400 Fixed params loading issue. Rewrite comments commit 4f83a42b41ec1079def70620524cfe1f1fdbff21 Author: qishuny <[email protected]> Date: Fri Jun 3 16:37:59 2022 -0400 additional format fix commit 139550f125bb5b8e9ebcbca1479d33cbe24bce98 Merge: cf2c9574 a0b9f9e3 Author: qishuny <[email protected]> Date: Fri Jun 3 11:08:04 2022 -0400 Merge remote-tracking branch 'origin/devel' into melodic_devel_test_robot_driver commit cf2c9574a4cf338ac7b88fc9813d1bf34277df96 Author: qishuny <[email protected]> Date: Fri Jun 3 11:07:46 2022 -0400 fix format commit a0b9f9e351f41812f1d4a4cd66b8fd47ab5ed83d Author: Joe Norby <[email protected]> Date: Fri Jun 3 09:15:45 2022 -0400 Update README.md commit b48c939243994d410968832b21e8944f62483262 Author: qishuny <[email protected]> Date: Thu Jun 2 11:12:42 2022 -0400 Additional changes to estimator inheritance. commit 351de7d2974285971af6f07ffd65ab7e9e755feb Merge: 82c665d2 2e3962c7 Author: qishuny <[email protected]> Date: Wed May 25 14:54:56 2022 -0400 Merge remote-tracking branch 'origin/devel' into melodic_devel_test_robot_driver commit 82c665d29176248d4d961aa4def17717ba97a273 Author: qishuny <[email protected]> Date: Tue May 24 21:26:47 2022 -0400 change function input, fix lint commit 05a077a15491afae38a2508492cf75dc656805c5 Author: qishuny <[email protected]> Date: Tue May 24 00:26:32 2022 -0400 in refactor branch move CF estimator from robot driver to its own class commit 81a5c9963b12db4b18e1152326eea3cbe3fc3c36 Merge: 6fda82a0 88c4f1c3 Author: qishuny <[email protected]> Date: Mon May 23 23:37:42 2022 -0400 Merge remote-tracking branch 'origin/devel' into melodic_devel_test_robot_driver commit 6fda82a0fd2f336ef95e907dfba5646027f54c0c Merge: 7fca4c1d a6e25dac Author: qishuny <[email protected]> Date: Mon May 23 08:51:04 2022 -0400 Merge remote-tracking branch 'origin/devel' into melodic_devel_test_robot_driver commit 7fca4c1dce53c426fd3ef4db677ce4888e1402ad Author: qishuny <[email protected]> Date: Sat May 21 21:01:36 2022 -0400 change cmake list format again commit 5d07d7c294bcd0b93de4d7e8efb554918a3bce9b Author: qishuny <[email protected]> Date: Sat May 21 21:00:27 2022 -0400 change robot driver cmake list format commit d9ff47dc9d60c4e1e1350931056edd8cc7780aa0 Author: qishuny <[email protected]> Date: Sat May 21 02:04:38 2022 -0400 fix lint commit 8ff802c4a2cbe657bc5d4ae80d47b15dbd4c244e Author: qishuny <[email protected]> Date: Sat May 21 01:52:36 2022 -0400 added Ardalan's refactor to robot driver * Fix parameter setting to allow non-spirit robot descriptions (#286) * Add permissions to doxygen action and remove PRs from action triggers (only pushes) (#293) * NMPC Refactor (#265) * Initial commit cleaning nmpc_controller, also adds landing support to FLP and changes same_plan_index to plan_index_diff * Add MATLAB scripts * Add readme * add documentation * respect lint * Clear out some comments * Disable AC by default * Respect linking checks * Reduce solve tolerance back to standard value * Switch linear solver form ma57 (commercial solver) to ma27 (free solver) Co-authored-by: Yanhao Yang <[email protected]> Co-authored-by: jrenaf <[email protected]> Co-authored-by: Bill Yu <[email protected]> Co-authored-by: Robomechanics Lab <[email protected]> Co-authored-by: Yanhao Yang <[email protected]> Co-authored-by: justinyim <[email protected]> Co-authored-by: ardalantj <[email protected]> Co-authored-by: Justin Yim <[email protected]> Co-authored-by: Yanhao Yang <[email protected]> Co-authored-by: Bill Yu <[email protected]> Co-authored-by: jrenaf <[email protected]> Co-authored-by: Jiming Ren <[email protected]> Co-authored-by: RML group <[email protected]> Co-authored-by: qishuny <[email protected]> Co-authored-by: Alex Stutt <[email protected]>

jcnorby and others added 11 commits May 23, 2022 09:41

Add images of topic structure

4f259d5

Update promo figure

4343473

Add files via upload

48835e5

Update README.md

fc5a932

Update README.md

ccb906f

Add files via upload

172d7eb

Add files via upload

890ac5e

Update promo figure

0948730

update gbp and utils documentation

73e525d

fixed image formatting

3b971a3

Rename Fast Global Motion Planner to Global Body Planner for Legged R…

eed17c3

…obots (gbpl)

jcnorby requested review from astutt and jrenaf May 24, 2022 16:20

jcnorby added 2 commits May 24, 2022 12:21

Update GBP readme with new name

95defbc

Merge branch 'devel' into melodic_devel_rename_global_planner

0695287

jcnorby changed the base branch from main to devel May 24, 2022 16:34

jcnorby requested a review from ardalantj May 25, 2022 00:36

ardalantj approved these changes May 25, 2022

View reviewed changes

astutt approved these changes May 25, 2022

View reviewed changes

global_body_planner/README.md Outdated Show resolved Hide resolved

jcnorby added 3 commits May 24, 2022 21:17

Update README.md

b5ce67e

Update README.md

a419d3f

Update README.md

9e8b1a6

jcnorby merged commit 076f0a6 into devel May 25, 2022

jcnorby deleted the melodic_devel_rename_global_planner branch May 25, 2022 02:06

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Rename global planner #280

Rename global planner #280

jcnorby commented May 24, 2022

Rename global planner #280

Rename global planner #280

Conversation

jcnorby commented May 24, 2022