Skip to content

Commit

Permalink
Quad-SDK Soft Release - Refactors and Bugfixes (#294)
Browse files Browse the repository at this point in the history 
* 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]>
  • Loading branch information
@jcnorby @justinyim
@ardalantj @yanhaoy @qishuny @jrenaf @robomechanicslab @astutt
13 people authored Jun 14, 2022
1 parent 89154df commit b04d4f7
Show file tree
Hide file tree
Showing 53 changed files with 74,979 additions and 70,615 deletions.
5 changes: 2 additions & 3 deletions .github/workflows/doxygen.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,14 +6,13 @@ on:
- main
- devel
- melodic_devel_gh_pages_doxygen_updates
pull_request:
branches:
- main
page_build:

jobs:
doxygen:
runs-on: ubuntu-latest
permissions:
contents: write
steps:
- uses: actions/checkout@v1
- name: Build documentation
Expand Down
Binary file added doc/flat.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added doc/gap_40cm.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added doc/parkour_local_min.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added doc/rough_25cm.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added doc/rough_40cm_huge.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added doc/slope_20.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added doc/step_20cm.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
2 changes: 2 additions & 0 deletions local_planner/include/local_planner/local_footstep_planner.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -106,11 +106,13 @@ class LocalFootstepPlanner {
/**
* @brief Compute the contact schedule based on the current phase
* @param[in] current_plan_index_ Current index in the plan
* @param[in] body_plan Current body plan
* @param[in] ref_primitive_plan_ Reference primitive plan
* @param[in] control_mode Control mode
* @param[out] contact_schedule 2D array of contact states
*/
void computeContactSchedule(int current_plan_index,
const Eigen::MatrixXd &body_plan,
const Eigen::VectorXi &ref_primitive_plan_,
int control_mode,
std::vector<std::vector<bool>> &contact_schedule);
Expand Down
4 changes: 2 additions & 2 deletions local_planner/include/local_planner/local_planner.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -264,8 +264,8 @@ class LocalPlanner {
/// Time duration to the next plan index
double first_element_duration_;

/// If the current solving is duplicated in the same index
bool same_plan_index_;
/// Difference in plan index from last solve
int plan_index_diff_;

/// Toe radius
double toe_radius_;
Expand Down
27 changes: 14 additions & 13 deletions local_planner/src/local_footstep_planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,8 +84,9 @@ void LocalFootstepPlanner::getFootPositionsBodyFrame(
}

void LocalFootstepPlanner::computeContactSchedule(
int current_plan_index, const Eigen::VectorXi &ref_primitive_plan,
int control_mode, std::vector<std::vector<bool>> &contact_schedule) {
int current_plan_index, const Eigen::MatrixXd &body_plan,
const Eigen::VectorXi &ref_primitive_plan, int control_mode,
std::vector<std::vector<bool>> &contact_schedule) {
// Compute the current phase in the nominal contact schedule
int phase = current_plan_index % period_;

Expand All @@ -106,19 +107,19 @@ void LocalFootstepPlanner::computeContactSchedule(
for (int i = 0; i < horizon_length_; i++) {
// Leaping and landing
if (ref_primitive_plan(i) == LEAP_STANCE) {
int leading_leg_liftoff_period = 0;
int leading_leg_liftoff_idx =
std::min(i + leading_leg_liftoff_period, horizon_length_ - 1);

if (ref_primitive_plan(leading_leg_liftoff_idx) == FLIGHT) {
contact_schedule.at(i) = {false, true, false, true};
} else {
contact_schedule.at(i) = {true, true, true, true};
} else if (ref_primitive_plan(i) == FLIGHT) {
// Flight, check that min landing height is exceeded
double min_landing_height = 0.3;
double current_height =
body_plan(i, 2) - terrain_grid_.atPosition(
"z_inpainted", body_plan.row(i).head<2>(),
grid_map::InterpolationMethods::INTER_NEAREST);
if (current_height < min_landing_height && body_plan(i, 8) < 0) {
contact_schedule.at(i) = {true, true, true, true};
} else {
contact_schedule.at(i) = {false, false, false, false};
}
} else if (ref_primitive_plan(i) == FLIGHT) {
// Flight
std::fill(contact_schedule.at(i).begin(), contact_schedule.at(i).end(),
false);
} else if (ref_primitive_plan(i) == LAND_STANCE) {
contact_schedule.at(i) = {true, true, true, true};
}
Expand Down
12 changes: 6 additions & 6 deletions local_planner/src/local_planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,8 +101,8 @@ LocalPlanner::LocalPlanner(ros::NodeHandle nh)
// Initialize the time duration to the next plan index
first_element_duration_ = dt_;

// Initialize the plan index boolean
same_plan_index_ = true;
// Initialize the plan index diff
plan_index_diff_ = 0;

// Initialize the plan index
current_plan_index_ = 0;
Expand Down Expand Up @@ -238,7 +238,7 @@ void LocalPlanner::getReference() {
int previous_plan_index = current_plan_index_;
quad_utils::getPlanIndex(initial_timestamp_, dt_, current_plan_index_,
first_element_duration_);
same_plan_index_ = previous_plan_index == current_plan_index_;
plan_index_diff_ = current_plan_index_ - previous_plan_index;

// Get the current body and foot positions into Eigen
current_state_ = quad_utils::bodyStateMsgToEigen(robot_state_msg_->body);
Expand Down Expand Up @@ -411,7 +411,7 @@ void LocalPlanner::getReference() {
}
} else {
// Only shift the foot position if it's a solve for a new plan index
if (!same_plan_index_) {
if (plan_index_diff_ > 0) {
body_plan_.topRows(N_ - 1) = body_plan_.bottomRows(N_ - 1);
grf_plan_.topRows(N_ - 2) = grf_plan_.bottomRows(N_ - 2);

Expand Down Expand Up @@ -441,7 +441,7 @@ bool LocalPlanner::computeLocalPlan() {

// Compute the contact schedule
local_footstep_planner_->computeContactSchedule(
current_plan_index_, ref_primitive_plan_, control_mode_,
current_plan_index_, body_plan_, ref_primitive_plan_, control_mode_,
contact_schedule_);

// Compute the new footholds if we have a valid existing plan (i.e. if
Expand Down Expand Up @@ -478,7 +478,7 @@ bool LocalPlanner::computeLocalPlan() {
if (!local_body_planner_nonlinear_->computeLegPlan(
current_full_state, ref_body_plan_, grf_positions_body,
grf_positions_world, foot_velocities_world_, contact_schedule_,
ref_ground_height_, first_element_duration_, same_plan_index_,
ref_ground_height_, first_element_duration_, plan_index_diff_,
terrain_grid_, body_plan_, grf_plan_))
return false;

Expand Down
3 changes: 2 additions & 1 deletion nmpc_controller/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,8 @@ add_library(nmpc_controller
src/quad_nlp_utils.cpp
src/quad_nlp.cpp
src/quad_nlp_utils.cpp
src/nmpc_controller.cpp
src/adaptive_complexity_utils.cpp
src/nmpc_controller.cpp
)
target_link_libraries(nmpc_controller
Eigen3::Eigen
Expand Down
160 changes: 160 additions & 0 deletions nmpc_controller/README.md
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,160 @@
# NMPC Controller

## Overview

This package implements a nonlinear model predictive controller (NMPC) for agile quadrupedal navigation. The package produces ground reaction force (GRF) and body state plans which guide the robot from its current state to track the desired trajectory according to the contact schedule. The primary algorithm is collocation trajectory planning constructed by [CasADi] supporting multiple dynamics model (default is a centroidal dynamics one) and is solved by [IPOPT] through a customed C++ interface to produce GRF plans considering contact schedule.

### License

The source code is released under a [MIT License](quad-sdk/LICENSE).

**Author: Yanhao Yang<br />
Affiliation: [Robomechanics Lab](https://www.cmu.edu/me/robomechanicslab/)<br />
Maintainers: Yanhao Yang ([email protected]), Joe Norby ([email protected]), and Jiming Ren ([email protected])**

The NMPC Controller package has been tested under [ROS] Melodic 18.04.
This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed.

### Publications

If you use this work in an academic context, please cite the following publication(s):

* J. Norby, Y. Yang, A. Tajbakhsh et al., “Quad-SDK,” 2022. \[Online\]. Available: https://github.com/robomechanics/quad-software ([paper])

@misc{norby2022quad,
title={{Quad-SDK}},
url={https://github.com/robomechanics/quad-software},
author={Norby, Joseph and Yang, Yanhao and Tajbakhsh, Ardalan and Ren, Jiming and Yim, Justin K. and Stutt, Alexandra and Yu, Qishun and Johnson, Aaron M.},
year={2022}}

### Unit Tests

Run the unit tests with

catkin run_tests nmpc_controller

## Usage

It is a C++ class that can be included in other ROS packages.

## Config files

* **nmpc_controller.yaml** Sets NMPC hyperparameters.

#### Parameters

* **`panic_weights`** (double, default: 200.0)

The linear panalty of the state panic variables.

* **`constraint_panic_weights`** (double, default: 20.0)

The linear panalty of the constraint panic variables.

* **`Q_temporal_factor`** (double, default: 10.0)

The temporal factor of the state cost weight.

* **`R_temporal_factor`** (double, default: 1.0)

The temporal factor of the control cost weight.

* **`friction_coefficient`** (double, default: 0.3)

The friction coefficient for GRF.

* **`enable_variable_horizon`** (bool, default: false)

Turn on varialbe horzion or not.

* **`min_horizon_length`** (int, default: 10)

Minimum horizon length when it's varaible.

* **`enable_mixed_complexity`** (bool, default: false)

Enable mixed complexity or not

* **`enable_adaptive_complexity`** (bool, default: false)

Enable adaptive complexity or not.

* **`fixed_complex_idxs`** (vector, default: [])

Fixed complex finite element index.

* **`fixed_complex_head`** (int, default: 0)

Fixed complex finite element number at the head.

* **`fixed_complex_tail`** (int, default: 0)

Fixed complex finite element number at the tail.

#### Parameters for each type of model

* **`x_dim`** (int, default: 12)

State dimension.

* **`u_dim`** (int, default: 12)

Control dimension.

* **`g_dim`** (int, default: 28)

Constraint dimension.

* **`x_weights`** (vector, default: [5.0, 5.0, 5.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.2, 0.05, 0.05, 0.01])

State cost weights.

* **`u_weights`** (vector, default: [!!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5])

Control cost weights.

* **`x_lb`** (vector, default: [!!float -2e19, !!float -2e19, 0, -3.14159, -3.14159, -3.14159, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19])

State lower bound.

* **`x_ub`** (vector, default: [!!float 2e19, !!float 2e19, !!float 2e19, 3.14159, 3.14159, 3.14159, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19])

State upper bound.

* **`x_lb_soft`** (vector, default: [!!float -2e19, !!float -2e19, 0, -3.14159, -3.14159, -3.14159, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19])

Soft state lower bound.

* **`x_ub_soft`** (vector, default: [!!float 2e19, !!float 2e19, !!float 2e19, 3.14159, 3.14159, 3.14159, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19])

Soft state upper bound.

* **`u_lb`** (vector, default: [!!float -2e19, !!float -2e19, 10, !!float -2e19, !!float -2e19, 10, !!float -2e19, !!float -2e19, 10, !!float -2e19, !!float -2e19, 10])

Control lower bound.

* **`u_ub`** (vector, default: [!!float 2e19, !!float 2e19, 250, !!float 2e19, !!float 2e19, 250, !!float 2e19, !!float 2e19, 250, !!float 2e19, !!float 2e19, 250])

Control upper bound.

* **`g_lb`** (vector, default: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19])

Constraint lower bound.

* **`g_ub`** (vector, default: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])

Constraint upper bound.

## Bugs & Feature Requests

Please report bugs and request features using the [Issue Tracker](https://github.com/robomechanics/quad-sdk/issues).


[paper]: https://leggedrobots.org/assets/pdfs/paper22.pdf
[ROS]: http://www.ros.org
[rviz]: http://wiki.ros.org/rviz
[Eigen]: http://eigen.tuxfamily.org
[std_srvs/Trigger]: http://docs.ros.org/api/std_srvs/html/srv/Trigger.html
[sensor_msgs/Temperature]: http://docs.ros.org/api/sensor_msgs/html/msg/Temperature.html
[CasADi]: https://web.casadi.org/
[IPOPT]: https://coin-or.github.io/Ipopt/
Loading

0 comments on commit b04d4f7

Please sign in to comment.