forked from mayataka/robotoc
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
add terrain examples and update README
- Loading branch information
Showing
10 changed files
with
813 additions
and
28 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -5,4 +5,5 @@ pybind11-build/ | |
| __pycache__/ | ||
| *.mp4 | ||
| *.log | ||
| *.gif | ||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,38 @@ | ||
| ## Examples | ||
| Examples of these solvers are found in `examples` directory. | ||
| Further explanations are found at https://mayataka.github.io/robotoc/page_examples.html. | ||
|
|
||
| ### Optimal control example with fixed contact timings | ||
|
|
||
| - Configuration-space and task-space optimal control for a robot manipulator iiwa14 using `UnconstrOCPSolver` (`iiwa14/config_space_ocp.cpp`, `iiwa14/task_space_ocp.cpp`, or `iiwa14/python/config_space_ocp.py`): | ||
|
|
||
| <img src="https://raw.githubusercontent.com/wiki/mayataka/robotoc/images/config_ocp.gif" width="100"> | ||
| <img src="https://raw.githubusercontent.com/wiki/mayataka/robotoc/images/task_ocp.gif" width="100"> | ||
|
|
||
|
|
||
| - Crawl, trot, pacing, and bounding gaits of quadruped ANYmal using `OCPSolver` (yellow arrows denote contact forces and blue polyhedrons denote linearized friction cone constraints) with fixed contact timings (e.g., `anymal/crawl.cpp` or `anymal/python/crawl.py`): | ||
|
|
||
| <img src="https://raw.githubusercontent.com/wiki/mayataka/robotoc/images/walking.gif" width="180"> | ||
| <img src="https://raw.githubusercontent.com/wiki/mayataka/robotoc/images/trotting.gif" width="180"> | ||
| <img src="https://raw.githubusercontent.com/wiki/mayataka/robotoc/images/pacing.gif" width="180"> | ||
| <img src="https://raw.githubusercontent.com/wiki/mayataka/robotoc/images/bounding.gif" width="180"> | ||
|
|
||
| ### Switching time optimization (STO) examples | ||
| - `OCPSolver` for the switching time optimization (STO) problem, which optimizes the trajectory and the contact timings simultaneously (`anymal/python/jumping_sto.py` and `icub/python/jumping_sto.py`): | ||
|
|
||
| <img src="https://raw.githubusercontent.com/wiki/mayataka/robotoc/images/jumping_sto.gif" width="250"> | ||
| <img src="https://raw.githubusercontent.com/wiki/mayataka/robotoc/images/icub.gif" width="230"> | ||
|
|
||
| ### Whole-body MPC examples | ||
| - The following example implementations of whole-body MPC are provided: | ||
| - `MPCCrawl` : MPC with `OCPSolver` for the crawl gait of quadrupedal robots. | ||
| - `MPCTrot` : MPC with `OCPSolver` for the trot gait of quadrupedal robots. | ||
| - `MPCPace` : MPC with `OCPSolver` for the pace gait of quadrupedal robots. | ||
| - `MPCFlyingTrot` : MPC with `OCPSolver` for the flying trot gait of quadrupedal robots. | ||
| - `MPCJump` : MPC with `OCPSolver` for the jump motion of quadrupedal or bipedal robots. | ||
| - `MPCBipedWalk` : MPC with `OCPSolver` for the walking motion of bipedal robots. | ||
| - You can find the simulations of these MPC at `a1/mpc`, `anymal/mpc`, and `icub/mpc` (you need to install [PyBullet](https://pybullet.org/wordpress/), e.g., by `pip install pybullet`): | ||
|
|
||
| <img src="https://raw.githubusercontent.com/wiki/mayataka/robotoc/images/a1_trotting.gif" width="250"> | ||
| <img src="https://raw.githubusercontent.com/wiki/mayataka/robotoc/images/anymal_crawling.gif" width="250"> | ||
| <img src="https://raw.githubusercontent.com/wiki/mayataka/robotoc/images/icub_walking.gif" width="240"> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,61 @@ | ||
| import robotoc | ||
| import numpy as np | ||
| from a1_simulator import A1Simulator | ||
|
|
||
|
|
||
| path_to_urdf = '../a1_description/urdf/a1.urdf' | ||
| contact_frames = ['FL_foot', 'RL_foot', 'FR_foot', 'RR_foot'] | ||
| contact_types = [robotoc.ContactType.PointContact for i in contact_frames] | ||
| baumgarte_time_step = 0.05 | ||
| robot = robotoc.Robot(path_to_urdf, robotoc.BaseJointType.FloatingBase, | ||
| contact_frames, contact_types, baumgarte_time_step) | ||
| LF_foot_id, LH_foot_id, RF_foot_id, RH_foot_id = robot.contact_frames() | ||
|
|
||
| step_length = np.array([0.2, 0, 0]) | ||
| yaw_cmd = 0.0 | ||
|
|
||
| step_height = 0.1 | ||
| swing_time = 0.25 | ||
| # stance_time = 0 | ||
| stance_time = 0.05 | ||
| swing_start_time = 0.5 | ||
|
|
||
| vcom_cmd = step_length / swing_time | ||
| yaw_rate_cmd = yaw_cmd / swing_time | ||
|
|
||
| T = 0.5 | ||
| N = 18 | ||
| max_steps = 3 | ||
| nthreads = 4 | ||
| mpc = robotoc.MPCCrawl(robot, T, N, max_steps, nthreads) | ||
|
|
||
| planner = robotoc.CrawlFootStepPlanner(robot) | ||
| raibert_gain = 0.2 | ||
| planner.set_gait_pattern(vcom_cmd, yaw_rate_cmd, swing_time, swing_time+stance_time, raibert_gain) | ||
| mpc.set_gait_pattern(planner, step_height, swing_time, stance_time, swing_start_time) | ||
|
|
||
| q = np.array([0, 0, 0.3181, 0, 0, 0, 1, | ||
| 0.0, 0.67, -1.3, | ||
| 0.0, 0.67, -1.3, | ||
| 0.0, 0.67, -1.3, | ||
| 0.0, 0.67, -1.3]) | ||
| v = np.zeros(robot.dimv()) | ||
| t = 0.0 | ||
| option_init = robotoc.SolverOptions() | ||
| option_init.max_iter = 10 | ||
|
|
||
| mpc.init(t, q, v, option_init) | ||
| option_mpc = robotoc.SolverOptions() | ||
| option_mpc.max_iter = 2 # MPC iterations | ||
| mpc.set_solver_options(option_mpc) | ||
|
|
||
| sim_time_step = 0.0025 # 400 Hz MPC | ||
| sim_start_time = 0.0 | ||
| sim_end_time = 10.0 | ||
| sim = A1Simulator(path_to_urdf, sim_time_step, sim_start_time, sim_end_time) | ||
|
|
||
| q0 = q.copy() | ||
| q0[2] += 0.1 # to avoid penetration at the initial configuraion | ||
| sim.set_camera(3.0, 15, 8, q[0:3]+np.array([0.0, 1.5, 0.2])) | ||
| sim.run_simulation(mpc, q0, v, feedback_delay=True, verbose=False, terrain=True, record=False) | ||
| # sim.run_simulation(mpc, q, v, verbose=False, record=True, terrain=True, record_name='a1_crawl_terrain.mp4') |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,61 @@ | ||
| import robotoc | ||
| import numpy as np | ||
| from a1_simulator import A1Simulator | ||
|
|
||
|
|
||
| path_to_urdf = '../a1_description/urdf/a1.urdf' | ||
| contact_frames = ['FL_foot', 'RL_foot', 'FR_foot', 'RR_foot'] | ||
| contact_types = [robotoc.ContactType.PointContact for i in contact_frames] | ||
| baumgarte_time_step = 0.05 | ||
| robot = robotoc.Robot(path_to_urdf, robotoc.BaseJointType.FloatingBase, | ||
| contact_frames, contact_types, baumgarte_time_step) | ||
| LF_foot_id, LH_foot_id, RF_foot_id, RH_foot_id = robot.contact_frames() | ||
|
|
||
| step_length = np.array([0.15, 0, 0]) | ||
| yaw_cmd = 0.0 | ||
|
|
||
| step_height = 0.1 | ||
| swing_time = 0.25 | ||
| stance_time = 0 | ||
| # stance_time = 0.05 | ||
| swing_start_time = 0.5 | ||
|
|
||
| vcom_cmd = step_length / swing_time | ||
| yaw_rate_cmd = yaw_cmd / swing_time | ||
|
|
||
| T = 0.5 | ||
| N = 18 | ||
| max_steps = 3 | ||
| nthreads = 4 | ||
| mpc = robotoc.MPCTrot(robot, T, N, max_steps, nthreads) | ||
|
|
||
| planner = robotoc.TrotFootStepPlanner(robot) | ||
| raibert_gain = 0.2 | ||
| planner.set_gait_pattern(vcom_cmd, yaw_rate_cmd, swing_time, swing_time+stance_time, raibert_gain) | ||
| mpc.set_gait_pattern(planner, step_height, swing_time, stance_time, swing_start_time) | ||
|
|
||
| q = np.array([0, 0, 0.3181, 0, 0, 0, 1, | ||
| 0.0, 0.67, -1.3, | ||
| 0.0, 0.67, -1.3, | ||
| 0.0, 0.67, -1.3, | ||
| 0.0, 0.67, -1.3]) | ||
| v = np.zeros(robot.dimv()) | ||
| t = 0.0 | ||
| option_init = robotoc.SolverOptions() | ||
| option_init.max_iter = 10 | ||
|
|
||
| mpc.init(t, q, v, option_init) | ||
| option_mpc = robotoc.SolverOptions() | ||
| option_mpc.max_iter = 2 # MPC iterations | ||
| mpc.set_solver_options(option_mpc) | ||
|
|
||
| sim_time_step = 0.0025 # 400 Hz MPC | ||
| sim_start_time = 0.0 | ||
| sim_end_time = 10.0 | ||
| sim = A1Simulator(path_to_urdf, sim_time_step, sim_start_time, sim_end_time) | ||
|
|
||
| q0 = q.copy() | ||
| q0[2] += 0.1 # to avoid penetration at the initial configuraion | ||
| sim.set_camera(3.0, 15, 8, q[0:3]+np.array([0.0, 1.5, 0.2])) | ||
| sim.run_simulation(mpc, q0, v, feedback_delay=True, verbose=False, terrain=True, record=False) | ||
| # sim.run_simulation(mpc, q, v, verbose=False, record=True, terrain=True, record_name='a1_trot_terrain.mp4') |
Oops, something went wrong.