├── .github └── workflows │ └── build.yml ├── .gitignore ├── CITATION.bib ├── CITATION.cff ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake ├── CheckAvxSupport.cmake ├── FindOrFetch.cmake ├── MpcOptions.cmake ├── MujocoHarden.cmake ├── MujocoLinkOptions.cmake └── MujocoMacOS.cmake ├── docs ├── CONTRIBUTING.md ├── DIRECT.md ├── ESTIMATORS.md ├── GUI.md ├── OVERVIEW.md └── assets │ ├── banner.png │ └── gui.png ├── mjpc ├── CMakeLists.txt ├── agent.cc ├── agent.h ├── app.cc ├── app.h ├── array_safety.h ├── direct │ ├── direct.cc │ ├── direct.h │ ├── model_parameters.cc │ ├── model_parameters.h │ └── trajectory.h ├── estimators │ ├── batch.cc │ ├── batch.h │ ├── estimator.h │ ├── include.cc │ ├── include.h │ ├── kalman.cc │ ├── kalman.h │ ├── unscented.cc │ └── unscented.h ├── grpc │ ├── CMakeLists.txt │ ├── agent.proto │ ├── agent_server.cc │ ├── agent_service.cc │ ├── agent_service.h │ ├── agent_service_test.cc │ ├── direct.proto │ ├── direct_server.cc │ ├── direct_service.cc │ ├── direct_service.h │ ├── filter.proto │ ├── filter_server.cc │ ├── filter_service.cc │ ├── filter_service.h │ ├── grpc_agent_util.cc │ ├── grpc_agent_util.h │ ├── ui_agent_server.cc │ ├── ui_agent_service.cc │ └── ui_agent_service.h ├── interface.cc ├── interface.h ├── macos_gui.mm ├── main.cc ├── norm.cc ├── norm.h ├── planners │ ├── cost_derivatives.cc │ ├── cost_derivatives.h │ ├── cross_entropy │ │ ├── planner.cc │ │ └── planner.h │ ├── gradient │ │ ├── gradient.cc │ │ ├── gradient.h │ │ ├── planner.cc │ │ ├── planner.h │ │ ├── policy.cc │ │ ├── policy.h │ │ ├── settings.h │ │ ├── spline_mapping.cc │ │ └── spline_mapping.h │ ├── ilqg │ │ ├── backward_pass.cc │ │ ├── backward_pass.h │ │ ├── boxqp.h │ │ ├── planner.cc │ │ ├── planner.h │ │ ├── policy.cc │ │ ├── policy.h │ │ └── settings.h │ ├── ilqs │ │ ├── planner.cc │ │ └── planner.h │ ├── include.cc │ ├── include.h │ ├── model_derivatives.cc │ ├── model_derivatives.h │ ├── planner.cc │ ├── planner.h │ ├── policy.h │ ├── robust │ │ ├── robust_planner.cc │ │ └── robust_planner.h │ ├── sample_gradient │ │ ├── planner.cc │ │ └── planner.h │ └── sampling │ │ ├── planner.cc │ │ ├── planner.h │ │ ├── policy.cc │ │ └── policy.h ├── simulate.cc ├── simulate.h ├── spline │ ├── spline.cc │ └── spline.h ├── states │ ├── state.cc │ └── state.h ├── task.cc ├── task.h ├── tasks │ ├── CMakeLists.txt │ ├── acrobot │ │ ├── acrobot.cc │ │ ├── acrobot.h │ │ ├── acrobot.xml.patch │ │ └── task.xml │ ├── allegro │ │ ├── allegro.cc │ │ ├── allegro.h │ │ ├── right_hand.xml.patch │ │ └── task.xml │ ├── bimanual │ │ ├── aloha.patch │ │ ├── handover │ │ │ ├── handover.cc │ │ │ ├── handover.h │ │ │ └── task.xml │ │ ├── insert │ │ │ ├── insert.cc │ │ │ ├── insert.h │ │ │ └── task.xml │ │ └── reorient │ │ │ ├── reorient.cc │ │ │ ├── reorient.h │ │ │ └── task.xml │ ├── cartpole │ │ ├── cartpole.cc │ │ ├── cartpole.h │ │ ├── cartpole.xml.patch │ │ └── task.xml │ ├── common.xml │ ├── common_assets │ │ ├── connector │ │ │ ├── mcX_f.stl │ │ │ ├── mcX_f_collision_mcX_f_MESH.stl │ │ │ ├── mcX_m.stl │ │ │ └── mcX_m_collision_mcX_m_MESH.stl │ │ ├── connector_f.xml │ │ ├── connector_m.xml │ │ ├── cube.xml.patch │ │ ├── reorientation_cross.xml │ │ ├── reorientation_cube.xml │ │ └── reorientation_cube_textures │ │ │ ├── fileback.png │ │ │ ├── filedown.png │ │ │ ├── filefront.png │ │ │ ├── fileleft.png │ │ │ ├── fileright.png │ │ │ ├── fileup.png │ │ │ ├── grayback.png │ │ │ ├── graydown.png │ │ │ ├── grayfront.png │ │ │ ├── grayleft.png │ │ │ ├── grayright.png │ │ │ └── grayup.png │ ├── fingers │ │ ├── fingers.cc │ │ ├── fingers.h │ │ └── task.xml │ ├── humanoid │ │ ├── humanoid.xml.patch │ │ ├── interact │ │ │ ├── contact_keyframe.cc │ │ │ ├── contact_keyframe.h │ │ │ ├── interact.cc │ │ │ ├── interact.h │ │ │ ├── motion_strategy.cc │ │ │ ├── motion_strategy.h │ │ │ ├── scenes │ │ │ │ ├── armchair.xml │ │ │ │ ├── bed.xml │ │ │ │ ├── bench.xml │ │ │ │ ├── dining_chair.xml │ │ │ │ ├── stool.xml │ │ │ │ └── table.xml │ │ │ ├── strategies │ │ │ │ ├── armchair_cross_leg.json │ │ │ │ ├── armchair_relax.json │ │ │ │ ├── bench_lie_down_get_up.json │ │ │ │ ├── bench_shuffle.json │ │ │ │ ├── stand_sit_stand.json │ │ │ │ ├── stool_rotate.json │ │ │ │ └── table_climb.json │ │ │ └── task.xml │ │ ├── stand │ │ │ ├── stand.cc │ │ │ ├── stand.h │ │ │ └── task.xml │ │ ├── tracking │ │ │ ├── keyframes │ │ │ │ ├── CMU-CMU-02-02_04_poses.xml │ │ │ │ ├── CMU-CMU-103-103_08_poses.xml │ │ │ │ ├── CMU-CMU-108-108_13_poses.xml │ │ │ │ ├── CMU-CMU-137-137_40_poses.xml │ │ │ │ ├── CMU-CMU-87-87_01_poses.xml │ │ │ │ ├── CMU-CMU-88-88_06_poses.xml │ │ │ │ ├── CMU-CMU-88-88_07_poses.xml │ │ │ │ ├── CMU-CMU-88-88_08_poses.xml │ │ │ │ ├── CMU-CMU-88-88_09_poses.xml │ │ │ │ ├── CMU-CMU-90-90_19_poses.xml │ │ │ │ └── README.md │ │ │ ├── task.xml │ │ │ ├── tracking.cc │ │ │ └── tracking.h │ │ └── walk │ │ │ ├── task.xml │ │ │ ├── walk.cc │ │ │ └── walk.h │ ├── manipulation │ │ ├── common.cc │ │ ├── common.h │ │ ├── manipulation.cc │ │ ├── manipulation.h │ │ ├── merge_panda_robotiq.py │ │ └── task_panda_bring.xml │ ├── op3 │ │ ├── op3.xml.patch │ │ ├── stand.cc │ │ ├── stand.h │ │ └── task.xml │ ├── panda │ │ ├── panda.cc │ │ ├── panda.h │ │ ├── panda.xml.patch │ │ └── task.xml │ ├── particle │ │ ├── particle.cc │ │ ├── particle.h │ │ ├── particle.xml.patch │ │ ├── task.xml │ │ └── task_timevarying.xml │ ├── quadrotor │ │ ├── gates.xml │ │ ├── quadrotor.cc │ │ ├── quadrotor.h │ │ ├── quadrotor.xml.patch │ │ └── task.xml │ ├── quadruped │ │ ├── a1.xml.patch │ │ ├── assets │ │ │ ├── fractal.xml │ │ │ ├── fractal_noise.png │ │ │ └── martian.png │ │ ├── quadruped.cc │ │ ├── quadruped.h │ │ ├── task_flat.xml │ │ └── task_hill.xml │ ├── rubik │ │ ├── cube_3x3x3.xml.patch │ │ ├── solve.cc │ │ ├── solve.h │ │ ├── task.xml │ │ └── transition_model.xml.patch │ ├── shadow_reorient │ │ ├── hand.cc │ │ ├── hand.h │ │ └── task.xml │ ├── swimmer │ │ ├── swimmer.cc │ │ ├── swimmer.h │ │ ├── swimmer.xml.patch │ │ └── task.xml │ ├── tasks.cc │ ├── tasks.h │ └── walker │ │ ├── task.xml │ │ ├── walker.cc │ │ ├── walker.h │ │ └── walker.xml.patch ├── test │ ├── CMakeLists.txt │ ├── agent │ │ ├── CMakeLists.txt │ │ ├── agent_test.cc │ │ ├── agent_utilities_test.cc │ │ ├── cost_derivatives_test.cc │ │ ├── norm_test.cc │ │ ├── rollout_test.cc │ │ ├── threadpool_test.cc │ │ └── trajectory_test.cc │ ├── direct │ │ ├── CMakeLists.txt │ │ ├── direct_force_test.cc │ │ ├── direct_optimize_test.cc │ │ ├── direct_parameter_test.cc │ │ ├── direct_sensor_test.cc │ │ ├── direct_trajectory_test.cc │ │ └── direct_utilities_test.cc │ ├── estimator │ │ ├── CMakeLists.txt │ │ ├── batch_filter_test.cc │ │ ├── batch_prior_test.cc │ │ ├── kalman_test.cc │ │ └── unscented_test.cc │ ├── gradient_planner │ │ ├── CMakeLists.txt │ │ ├── cubic_test.cc │ │ ├── gradient_planner_test.cc │ │ ├── gradient_test.cc │ │ ├── linear_test.cc │ │ └── zero_test.cc │ ├── ilqg_planner │ │ ├── CMakeLists.txt │ │ ├── backward_pass_test.cc │ │ └── ilqg_test.cc │ ├── load.cc │ ├── load.h │ ├── lqr.cc │ ├── lqr.h │ ├── planners │ │ └── robust │ │ │ ├── CMakeLists.txt │ │ │ └── robust_planner_test.cc │ ├── sampling_planner │ │ ├── CMakeLists.txt │ │ └── sampling_planner_test.cc │ ├── simulation.cc │ ├── simulation.h │ ├── spline │ │ ├── CMakeLists.txt │ │ └── spline_test.cc │ ├── state │ │ ├── CMakeLists.txt │ │ └── state_test.cc │ ├── tasks │ │ ├── CMakeLists.txt │ │ └── task_test.cc │ ├── testdata │ │ ├── box.xml │ │ ├── common.xml │ │ ├── estimator │ │ │ ├── box │ │ │ │ ├── task0.xml │ │ │ │ ├── task2D.xml │ │ │ │ ├── task3Drot.xml │ │ │ │ └── task3Drot2.xml │ │ │ └── particle │ │ │ │ ├── particle.xml │ │ │ │ ├── task.xml │ │ │ │ ├── task1D.xml │ │ │ │ ├── task1D_damped.xml │ │ │ │ ├── task1D_framepos.xml │ │ │ │ └── task_imu.xml │ │ ├── particle.xml │ │ ├── particle_residual.h │ │ └── particle_task.xml │ └── utilities │ │ ├── CMakeLists.txt │ │ └── utilities_test.cc ├── testspeed.cc ├── testspeed.h ├── testspeed_app.cc ├── threadpool.cc ├── threadpool.h ├── trajectory.cc ├── trajectory.h ├── utilities.cc └── utilities.h └── python ├── mujoco_mpc ├── __init__.py ├── agent.py ├── agent_test.py ├── demos │ ├── agent │ │ ├── cartpole.py │ │ └── cartpole_gui.py │ ├── direct │ │ ├── api_examples │ │ │ ├── box_drop_smoother.py │ │ │ ├── cartpole_trajopt.py │ │ │ ├── particle_smoother.py │ │ │ └── particle_trajopt.py │ │ ├── direct_optimizer.py │ │ ├── particle_parameter.py │ │ └── particle_trajopt.py │ ├── filter │ │ ├── block_drop.py │ │ └── particle_drop.py │ └── predictive_sampling │ │ ├── cube_orientation.py │ │ ├── particle.py │ │ └── predictive_sampling.py ├── direct.py ├── direct_test.py ├── filter.py ├── filter_test.py ├── mjpc_parameters.py ├── mjx │ ├── multi_plan.ipynb │ ├── predictive_sampling.py │ ├── tasks │ │ ├── bimanual │ │ │ └── handover.py │ │ ├── insert.py │ │ └── instruction.py │ └── visualize.py └── ui_agent_test.py ├── setup.py ├── setup_direct.py └── setup_filter.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Don't commit binaries 2 | *.a 3 | *.asm 4 | *.dll 5 | *.dylib 6 | *.exe 7 | *.lib 8 | *.out 9 | *.o 10 | *.so 11 | *.so.* 12 | # Clang cache 13 | .cache/ 14 | # Exclude editor config 15 | .vscode/ 16 | .vs/ 17 | # Exclude temporary folders 18 | *.egg-info/ 19 | .eggs/ 20 | build/ 21 | build_cmake/ 22 | # Exclude macOS folder attributes 23 | .DS_Store 24 | # Exclude macOS Info.framework.plist 25 | Info.framework.plist 26 | # Python byte-compiled / optimized / DLL files 27 | __pycache__/ 28 | *.py[cod] 29 | *$py.class 30 | # MuJoCo 31 | MUJOCO_LOG.TXT 32 | MJDATA.TXT 33 | MJMODEL.TXT 34 | # Python 35 | python/dist/ 36 | -------------------------------------------------------------------------------- /CITATION.bib: -------------------------------------------------------------------------------- 1 | @article{howell2022, 2 | title={{Predictive Sampling: Real-time Behaviour Synthesis with MuJoCo}}, 3 | author={Howell, Taylor and Gileadi, Nimrod and Tunyasuvunakool, Saran and Zakka, Kevin and Erez, Tom and Tassa, Yuval}, 4 | archivePrefix={arXiv}, 5 | eprint={2212.00541}, 6 | primaryClass={cs.RO}, 7 | url={https://arxiv.org/abs/2212.00541}, 8 | doi={10.48550/arXiv.2212.00541}, 9 | year={2022}, 10 | month={dec} 11 | } 12 | -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | cff-version: 1.2.0 2 | authors: 3 | - family-names: Howell 4 | given-names: Taylor 5 | - family-names: Gileadi 6 | given-names: Nimrod 7 | - family-names: Tunyasuvunakool 8 | given-names: Saran 9 | - family-names: Zakka 10 | given-names: Kevin 11 | - family-names: Erez 12 | given-names: Tom 13 | - family-names: Tassa 14 | given-names: Yuval 15 | date-released: "2022-12-01" 16 | message: "If you use this software, please cite it as below." 17 | preferred-citation: 18 | authors: 19 | - family-names: Howell 20 | given-names: Taylor 21 | - family-names: Gileadi 22 | given-names: Nimrod 23 | - family-names: Tunyasuvunakool 24 | given-names: Saran 25 | - family-names: Zakka 26 | given-names: Kevin 27 | - family-names: Erez 28 | given-names: Tom 29 | - family-names: Tassa 30 | given-names: Yuval 31 | date-published: "2022-12-01" 32 | doi: 10.48550/ARXIV.2212.00541 33 | month: 12 34 | publisher: 35 | name: arXiv 36 | title: "Predictive Sampling: Real-time Behaviour Synthesis with MuJoCo" 37 | type: article 38 | url: "https://arxiv.org/abs/2212.00541" 39 | year: 2022 40 | title: "MuJoCo MPC (MJPC)" 41 | url: "https://github.com/google-deepmind/mujoco_mpc" 42 | -------------------------------------------------------------------------------- /cmake/CheckAvxSupport.cmake: -------------------------------------------------------------------------------- 1 | # Copyright 2021 DeepMind Technologies Limited 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # https://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | include(CheckCSourceCompiles) 16 | 17 | # Assigns compiler options to the given variable based on availability of AVX. 18 | function(get_avx_compile_options OUTPUT_VAR) 19 | message(VERBOSE "Checking if AVX is available...") 20 | 21 | if(MSVC) 22 | set(CMAKE_REQUIRED_FLAGS "/arch:AVX") 23 | elseif(WIN32) 24 | # Abseil LTS 20230125.0 assumes that AVX implies PCLMUL on Windows. 25 | set(CMAKE_REQUIRED_FLAGS "-mavx" "-mpclmul") 26 | else() 27 | set(CMAKE_REQUIRED_FLAGS "-mavx") 28 | endif() 29 | 30 | if(APPLE AND "x86_64" IN_LIST CMAKE_OSX_ARCHITECTURES) 31 | message(STATUS "Building x86_64 on macOS, forcing CAN_BUILD_AVX to TRUE.") 32 | set(CAN_BUILD_AVX TRUE) 33 | else() 34 | check_c_source_compiles( 35 | " 36 | #include 37 | int main(int argc, char* argv[]) { 38 | __m256d ymm; 39 | return 0; 40 | } 41 | " 42 | CAN_BUILD_AVX 43 | ) 44 | endif() 45 | 46 | if(CAN_BUILD_AVX) 47 | message(VERBOSE "Checking if AVX is available... AVX available.") 48 | set("${OUTPUT_VAR}" 49 | ${CMAKE_REQUIRED_FLAGS} 50 | PARENT_SCOPE 51 | ) 52 | else() 53 | message(VERBOSE "Checking if AVX is available... AVX not available.") 54 | set("${OUTPUT_VAR}" PARENT_SCOPE) 55 | endif() 56 | endfunction() 57 | -------------------------------------------------------------------------------- /cmake/MujocoHarden.cmake: -------------------------------------------------------------------------------- 1 | # Copyright 2022 DeepMind Technologies Limited 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # https://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | option(MUJOCO_HARDEN "Enable build hardening for MuJoCo." OFF) 16 | if(MUJOCO_HARDEN 17 | AND NOT 18 | CMAKE_CXX_COMPILER_ID 19 | MATCHES 20 | ".*Clang.*" 21 | ) 22 | message(FATAL_ERROR "MUJOCO_HARDEN is only supported when building with Clang") 23 | endif() 24 | 25 | if(MUJOCO_HARDEN) 26 | set(MUJOCO_HARDEN_COMPILE_OPTIONS -D_FORTIFY_SOURCE=2 -fstack-protector) 27 | if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") 28 | set(MUJOCO_HARDEN_LINK_OPTIONS -Wl,-bind_at_load) 29 | elseif(${CMAKE_SYSTEM_NAME} MATCHES "Linux") 30 | set(MUJOCO_HARDEN_LINK_OPTIONS -Wl,-z,relro -Wl,-z,now) 31 | endif() 32 | else() 33 | set(MUJOCO_HARDEN_COMPILE_OPTIONS "") 34 | set(MUJOCO_HARDEN_LINK_OPTIONS "") 35 | endif() 36 | -------------------------------------------------------------------------------- /cmake/MujocoLinkOptions.cmake: -------------------------------------------------------------------------------- 1 | # Copyright 2021 DeepMind Technologies Limited 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # https://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | include(CheckCSourceCompiles) 16 | 17 | # Gets the appropriate linker options for building MuJoCo, based on features available on the 18 | # linker. 19 | function(get_mujoco_extra_link_options OUTPUT_VAR) 20 | if(MSVC) 21 | set(EXTRA_LINK_OPTIONS /OPT:REF /OPT:ICF=5) 22 | else() 23 | set(EXTRA_LINK_OPTIONS) 24 | 25 | if(WIN32) 26 | set(CMAKE_REQUIRED_FLAGS "-fuse-ld=lld-link") 27 | check_c_source_compiles("int main() {}" SUPPORTS_LLD) 28 | if(SUPPORTS_LLD) 29 | set(EXTRA_LINK_OPTIONS 30 | ${EXTRA_LINK_OPTIONS} 31 | -fuse-ld=lld-link 32 | -Wl,/OPT:REF 33 | -Wl,/OPT:ICF 34 | ) 35 | endif() 36 | else() 37 | set(CMAKE_REQUIRED_FLAGS "-fuse-ld=lld") 38 | check_c_source_compiles("int main() {}" SUPPORTS_LLD) 39 | if(SUPPORTS_LLD) 40 | set(EXTRA_LINK_OPTIONS ${EXTRA_LINK_OPTIONS} -fuse-ld=lld) 41 | else() 42 | set(CMAKE_REQUIRED_FLAGS "-fuse-ld=gold") 43 | check_c_source_compiles("int main() {}" SUPPORTS_GOLD) 44 | if(SUPPORTS_GOLD) 45 | set(EXTRA_LINK_OPTIONS ${EXTRA_LINK_OPTIONS} -fuse-ld=gold) 46 | endif() 47 | endif() 48 | 49 | set(CMAKE_REQUIRED_FLAGS ${EXTRA_LINK_OPTIONS} "-Wl,--gc-sections") 50 | check_c_source_compiles("int main() {}" SUPPORTS_GC_SECTIONS) 51 | if(SUPPORTS_GC_SECTIONS) 52 | set(EXTRA_LINK_OPTIONS ${EXTRA_LINK_OPTIONS} -Wl,--gc-sections) 53 | else() 54 | set(CMAKE_REQUIRED_FLAGS ${EXTRA_LINK_OPTIONS} "-Wl,-dead_strip") 55 | check_c_source_compiles("int main() {}" SUPPORTS_DEAD_STRIP) 56 | if(SUPPORTS_DEAD_STRIP) 57 | set(EXTRA_LINK_OPTIONS ${EXTRA_LINK_OPTIONS} -Wl,-dead_strip) 58 | endif() 59 | endif() 60 | endif() 61 | endif() 62 | 63 | set("${OUTPUT_VAR}" 64 | ${EXTRA_LINK_OPTIONS} 65 | PARENT_SCOPE 66 | ) 67 | endfunction() 68 | -------------------------------------------------------------------------------- /cmake/MujocoMacOS.cmake: -------------------------------------------------------------------------------- 1 | # Copyright 2022 DeepMind Technologies Limited 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # https://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | if(APPLE) 16 | # Target the oldest version of macOS that is still supported by Apple. 17 | # We follow https://endoflife.date/macos, which considers a version to become 18 | # unsupported the first time it is excluded from a macOS security update 19 | # (e.g. https://github.com/endoflife-date/endoflife.date/issues/1602). 20 | set(MUJOCO_MACOSX_VERSION_MIN 11) 21 | 22 | # We are setting the -mmacosx-version-min compiler flag directly rather than using the 23 | # CMAKE_OSX_DEPLOYMENT_TARGET variable since we do not want to affect choice of SDK, 24 | # and also we only want to apply the version restriction locally. 25 | set(MUJOCO_MACOS_COMPILE_OPTIONS -mmacosx-version-min=${MUJOCO_MACOSX_VERSION_MIN} 26 | -Werror=partial-availability -Werror=unguarded-availability 27 | ) 28 | set(MUJOCO_MACOS_LINK_OPTIONS -mmacosx-version-min=${MUJOCO_MACOSX_VERSION_MIN} 29 | -Wl,-no_weak_imports 30 | ) 31 | else() 32 | set(MUJOCO_MACOS_COMPILE_OPTIONS "") 33 | set(MUJOCO_MACOS_LINK_OPTIONS "") 34 | endif() 35 | 36 | function(enforce_mujoco_macosx_min_version) 37 | if(APPLE) 38 | add_compile_options(${MUJOCO_MACOS_COMPILE_OPTIONS}) 39 | add_link_options(${MUJOCO_MACOS_LINK_OPTIONS}) 40 | endif() 41 | endfunction() 42 | -------------------------------------------------------------------------------- /docs/GUI.md: -------------------------------------------------------------------------------- 1 | # Graphical User Interface 2 | 3 | - [Graphical User Interface](#graphical-user-interface) 4 | - [Overview](#overview) 5 | - [User Guide](#user-guide) 6 | 7 | ## Overview 8 | 9 | The MJPC GUI is built on top of MuJoCo's `simulate` application with a few additional features. The below screenshot shows a capture of the GUI for the `walker` task. 10 | 11 | ![GUI](assets/gui.png) 12 | 13 | ## User Guide 14 | 15 | - Press `F1` to bring up a help pane that describes how to use the GUI. 16 | - The MJPC GUI is an extension of MuJoCo's native `simulate` viewer, with the same keyboard shortcuts and mouse functionality. 17 | - `+` speeds up the simulation, resulting in fewer planning steps per simulation step. 18 | - `-` slows down the simulation, resulting in more planning steps per simulation step. 19 | - The `simulate` viewer enables drag-and-drop interaction with simulated objects to apply forces or torques. 20 | - Double-click on a body to select it. 21 | - `Ctrl + left drag` applies a torque to the selected object, resulting in rotation. 22 | - `Ctrl + right drag` applies a force to the selected object in the `(x,z)` plane, resulting in translation. 23 | - `Ctrl + Shift + right drag` applies a force to the selected object in the `(x,y)` plane. 24 | - MJPC adds three keyboard shortcuts: 25 | - The `Enter` key starts and stops the planner. 26 | - The `\` key starts and stops the controller (sending actions from the planner to the model). 27 | - The `9` key turns the traces on/off. 28 | -------------------------------------------------------------------------------- /docs/assets/banner.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/google-deepmind/mujoco_mpc/ff572a21e7c2bf9fda62e1862a758da7e9a8719b/docs/assets/banner.png -------------------------------------------------------------------------------- /docs/assets/gui.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/google-deepmind/mujoco_mpc/ff572a21e7c2bf9fda62e1862a758da7e9a8719b/docs/assets/gui.png -------------------------------------------------------------------------------- /mjpc/app.h: -------------------------------------------------------------------------------- 1 | // Copyright 2021 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_APP_H_ 16 | #define MJPC_APP_H_ 17 | 18 | #include 19 | #include 20 | 21 | #include "mjpc/simulate.h" // mjpc fork 22 | #include "mjpc/task.h" 23 | 24 | namespace mjpc { 25 | class MjpcApp { 26 | public: 27 | MjpcApp(std::vector> tasks, int task_id = 0); 28 | MjpcApp(const MjpcApp&) = delete; 29 | MjpcApp& operator=(const MjpcApp&) = delete; 30 | ~MjpcApp(); 31 | 32 | void Start(); 33 | 34 | ::mujoco::Simulate* Sim(); 35 | }; 36 | 37 | void StartApp(std::vector> tasks, int task_id = 0); 38 | 39 | } // namespace mjpc 40 | 41 | #endif // MJPC_APP_H_ 42 | -------------------------------------------------------------------------------- /mjpc/direct/model_parameters.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2023 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "mjpc/direct/model_parameters.h" 16 | 17 | #include 18 | #include 19 | 20 | namespace mjpc { 21 | 22 | // Loads all available ModelParameters 23 | std::vector> LoadModelParameters() { 24 | // model parameters 25 | std::vector> model_parameters; 26 | 27 | // add model parameters 28 | model_parameters.emplace_back(new mjpc::Particle1DDampedParameters()); 29 | model_parameters.emplace_back(new mjpc::Particle1DFramePosParameters()); 30 | 31 | return model_parameters; 32 | } 33 | 34 | } // namespace mjpc 35 | -------------------------------------------------------------------------------- /mjpc/estimators/include.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include "mjpc/estimators/batch.h" 18 | #include "mjpc/estimators/estimator.h" 19 | #include "mjpc/estimators/include.h" 20 | 21 | namespace mjpc { 22 | 23 | const char kEstimatorNames[] = 24 | "Ground Truth\n" 25 | "Kalman\n" 26 | "Unscented\n" 27 | "Batch"; 28 | 29 | // load all available estimators 30 | std::vector> LoadEstimators() { 31 | // planners 32 | std::vector> estimators; 33 | 34 | // add estimators 35 | estimators.emplace_back(new mjpc::GroundTruth()); // ground truth state 36 | estimators.emplace_back(new mjpc::Kalman()); // extended Kalman filter 37 | estimators.emplace_back(new mjpc::Unscented()); // unscented Kalman filter 38 | estimators.emplace_back(new mjpc::Batch()); // recursive batch filter 39 | 40 | return estimators; 41 | } 42 | 43 | } // namespace mjpc 44 | -------------------------------------------------------------------------------- /mjpc/estimators/include.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_ESTIMATORS_INCLUDE_H_ 16 | #define MJPC_ESTIMATORS_INCLUDE_H_ 17 | 18 | #include 19 | #include 20 | 21 | #include "mjpc/estimators/estimator.h" 22 | #include "mjpc/estimators/kalman.h" 23 | #include "mjpc/estimators/unscented.h" 24 | 25 | namespace mjpc { 26 | 27 | // Estimator names, separated by '\n'. 28 | extern const char kEstimatorNames[]; 29 | 30 | // Loads all available estimators 31 | std::vector> LoadEstimators(); 32 | 33 | } // namespace mjpc 34 | 35 | #endif // MJPC_ESTIMATORS_INCLUDE_H_ 36 | -------------------------------------------------------------------------------- /mjpc/grpc/agent_server.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2023 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | // Startup code for `Agent` server. 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | // DEEPMIND INTERNAL IMPORT 26 | #include 27 | #include 28 | #include 29 | 30 | #include "mjpc/grpc/agent_service.h" 31 | #include "mjpc/tasks/tasks.h" 32 | 33 | ABSL_FLAG(int32_t, mjpc_port, 10000, "port to listen on"); 34 | ABSL_FLAG(int32_t, mjpc_workers, -1, 35 | "number of worker threads for MJPC planning. -1 means use the number " 36 | "of available hardware threads."); 37 | 38 | int main(int argc, char** argv) { 39 | absl::ParseCommandLine(argc, argv); 40 | int port = absl::GetFlag(FLAGS_mjpc_port); 41 | 42 | std::string server_address = absl::StrCat("[::]:", port); 43 | 44 | std::shared_ptr server_credentials = 45 | grpc::experimental::LocalServerCredentials(LOCAL_TCP); 46 | grpc::ServerBuilder builder; 47 | builder.AddListeningPort(server_address, server_credentials); 48 | 49 | mjpc::agent_grpc::AgentService service(mjpc::GetTasks(), 50 | absl::GetFlag(FLAGS_mjpc_workers)); 51 | builder.SetMaxReceiveMessageSize(40 * 1024 * 1024); 52 | builder.RegisterService(&service); 53 | 54 | std::unique_ptr server(builder.BuildAndStart()); 55 | LOG(INFO) << "Server listening on " << server_address; 56 | 57 | // Keep the program running until the server shuts down. 58 | server->Wait(); 59 | 60 | return 0; 61 | } 62 | -------------------------------------------------------------------------------- /mjpc/grpc/direct_server.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2023 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | // Startup code for `Direct` server. 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | // DEEPMIND INTERNAL IMPORT 26 | #include 27 | #include 28 | #include 29 | 30 | #include "mjpc/grpc/direct_service.h" 31 | 32 | ABSL_FLAG(int32_t, mjpc_port, 10000, "port to listen on"); 33 | 34 | int main(int argc, char** argv) { 35 | absl::ParseCommandLine(argc, argv); 36 | int port = absl::GetFlag(FLAGS_mjpc_port); 37 | 38 | std::string server_address = absl::StrCat("[::]:", port); 39 | 40 | std::shared_ptr server_credentials = 41 | grpc::experimental::LocalServerCredentials(LOCAL_TCP); 42 | grpc::ServerBuilder builder; 43 | builder.AddListeningPort(server_address, server_credentials); 44 | 45 | mjpc::direct_grpc::DirectService service; 46 | builder.SetMaxReceiveMessageSize(40 * 1024 * 1024); 47 | builder.RegisterService(&service); 48 | 49 | std::unique_ptr server(builder.BuildAndStart()); 50 | LOG(INFO) << "Server listening on " << server_address; 51 | 52 | // Keep the program running until the server shuts down. 53 | server->Wait(); 54 | 55 | return 0; 56 | } 57 | -------------------------------------------------------------------------------- /mjpc/grpc/filter.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2023 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto3"; 16 | 17 | package filter; 18 | 19 | service StateEstimation { 20 | // Initialize Filter 21 | rpc Init(InitRequest) returns (InitResponse); 22 | // Reset Filter 23 | rpc Reset(ResetRequest) returns (ResetResponse); 24 | // Filter measurement update 25 | rpc Update(UpdateRequest) returns (UpdateResponse); 26 | // Filter state 27 | rpc State(StateRequest) returns (StateResponse); 28 | // Filter covariance 29 | rpc Covariance(CovarianceRequest) returns (CovarianceResponse); 30 | // Filter noise 31 | rpc Noise(NoiseRequest) returns (NoiseResponse); 32 | } 33 | 34 | message MjModel { 35 | optional bytes mjb = 1; 36 | optional string xml = 2; 37 | } 38 | 39 | message InitRequest { 40 | optional MjModel model = 1; 41 | } 42 | 43 | message InitResponse {} 44 | 45 | message ResetRequest {} 46 | 47 | message ResetResponse {} 48 | 49 | message UpdateRequest { 50 | repeated double ctrl = 1 [packed = true]; 51 | repeated double sensor = 2 [packed = true]; 52 | optional int32 mode = 3; 53 | 54 | } 55 | 56 | message UpdateResponse {} 57 | 58 | message State { 59 | repeated double state = 1 [packed = true]; 60 | optional double time = 2; 61 | repeated double qfrc = 3 [packed = true]; 62 | } 63 | 64 | message StateRequest { 65 | State state = 1; 66 | } 67 | 68 | message StateResponse { 69 | State state = 1; 70 | } 71 | 72 | message Covariance { 73 | repeated double covariance = 1 [packed = true]; 74 | optional int32 dimension = 2; 75 | } 76 | 77 | message CovarianceRequest { 78 | Covariance covariance = 1; 79 | } 80 | 81 | message CovarianceResponse { 82 | Covariance covariance = 1; 83 | } 84 | 85 | message Noise { 86 | repeated double process = 1 [packed = true]; 87 | repeated double sensor = 2 [packed = true]; 88 | } 89 | 90 | message NoiseRequest { 91 | Noise noise = 1; 92 | } 93 | 94 | message NoiseResponse { 95 | Noise noise = 1; 96 | } 97 | -------------------------------------------------------------------------------- /mjpc/grpc/filter_server.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2023 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | // Startup code for `Filter` server. 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | // DEEPMIND INTERNAL IMPORT 27 | #include 28 | #include 29 | #include 30 | #include "mjpc/grpc/filter_service.h" 31 | 32 | ABSL_FLAG(int32_t, mjpc_port, 10000, "port to listen on"); 33 | 34 | int main(int argc, char** argv) { 35 | absl::ParseCommandLine(argc, argv); 36 | int port = absl::GetFlag(FLAGS_mjpc_port); 37 | 38 | std::string server_address = absl::StrCat("[::]:", port); 39 | 40 | std::shared_ptr server_credentials = 41 | grpc::experimental::LocalServerCredentials(LOCAL_TCP); 42 | grpc::ServerBuilder builder; 43 | builder.AddListeningPort(server_address, server_credentials); 44 | 45 | filter_grpc::FilterService service; 46 | builder.SetMaxReceiveMessageSize(40 * 1024 * 1024); 47 | builder.RegisterService(&service); 48 | 49 | std::unique_ptr server(builder.BuildAndStart()); 50 | LOG(INFO) << "Server listening on " << server_address; 51 | 52 | // Keep the program running until the server shuts down. 53 | server->Wait(); 54 | 55 | return 0; 56 | } 57 | -------------------------------------------------------------------------------- /mjpc/grpc/ui_agent_server.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2023 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | // A version of the MJPC UI that serves the AgentService API. 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | // DEEPMIND INTERNAL IMPORT 28 | #include 29 | #include 30 | #include 31 | 32 | #include "mjpc/app.h" 33 | #include "mjpc/tasks/tasks.h" 34 | #include "mjpc/grpc/ui_agent_service.h" 35 | 36 | ABSL_FLAG(int32_t, mjpc_port, 10000, "port to listen on"); 37 | ABSL_FLAG(int32_t, mjpc_workers, -1, 38 | "Not used. Here for compatibility with agent_server.cc"); 39 | 40 | int main(int argc, char** argv) { 41 | absl::ParseCommandLine(argc, argv); 42 | int port = absl::GetFlag(FLAGS_mjpc_port); 43 | 44 | std::string server_address = absl::StrCat("[::]:", port); 45 | 46 | std::shared_ptr server_credentials = 47 | grpc::experimental::LocalServerCredentials(LOCAL_TCP); 48 | grpc::ServerBuilder builder; 49 | builder.AddListeningPort(server_address, server_credentials); 50 | 51 | mjpc::MjpcApp app(mjpc::GetTasks()); 52 | mjpc::agent_grpc::UiAgentService service(app.Sim()); 53 | builder.SetMaxReceiveMessageSize(40 * 1024 * 1024); 54 | builder.RegisterService(&service); 55 | 56 | std::unique_ptr server(builder.BuildAndStart()); 57 | LOG(INFO) << "Server listening on " << server_address; 58 | 59 | app.Start(); 60 | server->Shutdown(absl::ToChronoTime(absl::Now() + absl::Seconds(1))); 61 | server->Wait(); 62 | 63 | return 0; 64 | } 65 | -------------------------------------------------------------------------------- /mjpc/interface.h: -------------------------------------------------------------------------------- 1 | // Copyright 2021 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_MJPC_INTERFACE_H_ 16 | #define MJPC_MJPC_INTERFACE_H_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "mjpc/agent.h" 23 | #include "mjpc/task.h" 24 | #include "mjpc/threadpool.h" 25 | #include "mjpc/utilities.h" 26 | 27 | 28 | namespace mjpc { 29 | class AgentRunner{ 30 | public: 31 | ~AgentRunner(); 32 | explicit AgentRunner(const mjModel* model, std::shared_ptr task); 33 | void Step(mjData* data); 34 | void Residual(const mjModel* model, mjData* data); 35 | private: 36 | Agent agent_; 37 | ThreadPool agent_plan_pool_; 38 | std::atomic_bool exit_request_ = false; 39 | std::atomic_int ui_load_request_ = 0; 40 | }; 41 | 42 | } // namespace mjpc 43 | 44 | extern "C" void destroy_policy(); 45 | extern "C" void create_policy_from_task_id(const mjModel* model, int task_id); 46 | extern "C" void create_policy(const mjModel* model, 47 | std::shared_ptr task); 48 | extern "C" void step_policy(mjData* data); 49 | extern "C" void set_weights(double* weights); 50 | 51 | #endif // MJPC_MJPC_INTERFACE_H_ 52 | -------------------------------------------------------------------------------- /mjpc/macos_gui.mm: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | std::string GetSavePath(const char* filename) { 21 | NSSavePanel* panel = [NSSavePanel savePanel]; 22 | NSURL* userDocumentsDir = [NSFileManager.defaultManager URLsForDirectory:NSDocumentDirectory 23 | inDomains:NSUserDomainMask].firstObject; 24 | [panel setDirectoryURL:userDocumentsDir]; 25 | [panel setNameFieldStringValue:[NSString stringWithUTF8String:filename]]; 26 | if ([panel runModal] == NSModalResponseOK) { 27 | std::ostringstream s; 28 | s << [panel.URL.path cStringUsingEncoding:NSUTF8StringEncoding]; 29 | return s.str(); 30 | } else { 31 | return ""; 32 | } 33 | } 34 | 35 | #ifdef __AVX__ 36 | void DisplayErrorDialogBox(const char* title, const char* msg) { 37 | NSAlert *alert = [[[NSAlert alloc] init] autorelease]; 38 | [alert setMessageText:[NSString stringWithUTF8String:title]]; 39 | [alert setInformativeText:[NSString stringWithUTF8String:msg]]; 40 | [alert setAlertStyle:NSAlertStyleCritical]; 41 | [alert addButtonWithTitle:@"Exit"]; 42 | [alert runModal]; 43 | } 44 | #endif 45 | -------------------------------------------------------------------------------- /mjpc/main.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2021 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include "mjpc/app.h" 24 | #include "mjpc/tasks/tasks.h" 25 | 26 | ABSL_FLAG(std::string, task, "Quadruped Flat", 27 | "Which model to load on startup."); 28 | 29 | // machinery for replacing command line error by a macOS dialog box 30 | // when running under Rosetta 31 | #if defined(__APPLE__) && defined(__AVX__) 32 | extern void DisplayErrorDialogBox(const char* title, const char* msg); 33 | static const char* rosetta_error_msg = nullptr; 34 | __attribute__((used, visibility("default"))) 35 | extern "C" void _mj_rosettaError(const char* msg) { 36 | rosetta_error_msg = msg; 37 | } 38 | #endif 39 | 40 | // run event loop 41 | int main(int argc, char** argv) { 42 | // display an error if running on macOS under Rosetta 2 43 | #if defined(__APPLE__) && defined(__AVX__) 44 | if (rosetta_error_msg) { 45 | DisplayErrorDialogBox("Rosetta 2 is not supported", rosetta_error_msg); 46 | std::exit(1); 47 | } 48 | #endif 49 | absl::ParseCommandLine(argc, argv); 50 | 51 | std::string task_name = absl::GetFlag(FLAGS_task); 52 | auto tasks = mjpc::GetTasks(); 53 | int task_id = -1; 54 | for (int i = 0; i < tasks.size(); i++) { 55 | if (absl::EqualsIgnoreCase(task_name, tasks[i]->Name())) { 56 | task_id = i; 57 | break; 58 | } 59 | } 60 | if (task_id == -1) { 61 | std::cerr << "Invalid --task flag: '" << task_name 62 | << "'. Valid values:\n"; 63 | for (int i = 0; i < tasks.size(); i++) { 64 | std::cerr << " " << tasks[i]->Name() << "\n"; 65 | } 66 | mju_error("Invalid --task flag."); 67 | } 68 | 69 | mjpc::StartApp(tasks, task_id); // start with quadruped flat 70 | return 0; 71 | } 72 | -------------------------------------------------------------------------------- /mjpc/norm.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_NORM_H_ 16 | #define MJPC_NORM_H_ 17 | 18 | namespace mjpc { 19 | 20 | // maximum number of norm parameters 21 | inline constexpr int kMaxNormParameters = 3; 22 | 23 | // norm types 24 | enum NormType : int { 25 | kNull = -1, 26 | kQuadratic = 0, 27 | kL22 = 1, 28 | kL2 = 2, 29 | kCosh = 3, 30 | // retain ordering for backward compatibility 31 | kPowerLoss = 5, 32 | kSmoothAbsLoss = 6, 33 | kSmoothAbs2Loss = 7, 34 | kRectifyLoss = 8, 35 | }; 36 | 37 | // norm's number of parameters 38 | int NormParameterDimension(int type); 39 | 40 | // evaluate norm; optionally, gradient and Hessian 41 | double Norm(double *g, double *H, const double *x, const double *params, int n, 42 | NormType type); 43 | 44 | } // namespace mjpc 45 | 46 | #endif // MJPC_NORM_H_ 47 | -------------------------------------------------------------------------------- /mjpc/planners/gradient/gradient.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_PLANNERS_GRADIENT_GRADIENT_H_ 16 | #define MJPC_PLANNERS_GRADIENT_GRADIENT_H_ 17 | 18 | #include 19 | 20 | #include "mjpc/planners/cost_derivatives.h" 21 | #include "mjpc/planners/gradient/policy.h" 22 | #include "mjpc/planners/model_derivatives.h" 23 | 24 | namespace mjpc { 25 | 26 | // data and methods to perform gradient descent 27 | class Gradient { 28 | public: 29 | // constructor 30 | Gradient() = default; 31 | 32 | // destructor 33 | ~Gradient() = default; 34 | 35 | // ----- methods ----- // 36 | 37 | // allocate memory 38 | void Allocate(int dim_state_derivative, int dim_action, int T); 39 | 40 | // reset memory to zeros 41 | void Reset(int dim_state_derivative, int dim_action, int T); 42 | 43 | // compute gradient at one time step 44 | int GradientStep(int n, int m, const double *Wx, const double *A, 45 | const double *B, const double *cx, const double *cu, 46 | double *Vx, double *du, double *Qx, double *Qu); 47 | 48 | // compute gradient for entire trajectory 49 | int Compute(GradientPolicy *p, const ModelDerivatives *md, 50 | const CostDerivatives *cd, int dim_state_derivative, 51 | int dim_action, int T); 52 | 53 | // ----- members ----- // 54 | std::vector Vx; // cost-to-go gradient 55 | double dV[2]; // cost-to-go error 56 | std::vector Qx; // objetive state gradient 57 | std::vector Qu; // cost action gradient 58 | }; 59 | 60 | } // namespace mjpc 61 | 62 | #endif // MJPC_PLANNERS_GRADIENT_GRADIENT_H_ 63 | -------------------------------------------------------------------------------- /mjpc/planners/gradient/policy.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_PLANNERS_GRADIENT_POLICY_H_ 16 | #define MJPC_PLANNERS_GRADIENT_POLICY_H_ 17 | 18 | #include 19 | 20 | #include 21 | #include "mjpc/planners/policy.h" 22 | #include "mjpc/spline/spline.h" 23 | #include "mjpc/task.h" 24 | 25 | namespace mjpc { 26 | 27 | // policy for gradient descent planner 28 | class GradientPolicy : public Policy { 29 | public: 30 | // constructor 31 | GradientPolicy() = default; 32 | 33 | // destructor 34 | ~GradientPolicy() override = default; 35 | 36 | // ----- methods ----- // 37 | 38 | // allocate memory 39 | void Allocate(const mjModel* model, const Task& task, int horizon) override; 40 | 41 | // reset memory to zeros 42 | void Reset(int horizon, 43 | const double* initial_repeated_action = nullptr) override; 44 | 45 | // compute action from policy 46 | // state is not used 47 | void Action(double* action, const double* state, double time) const override; 48 | 49 | // copy policy 50 | void CopyFrom(const GradientPolicy& policy, int horizon); 51 | 52 | // copy parameters 53 | void CopyParametersFrom(const std::vector& src_parameters, 54 | const std::vector& src_times); 55 | 56 | // ----- members ----- // 57 | const mjModel* model; 58 | 59 | std::vector k; // action improvement 60 | 61 | std::vector parameters; 62 | std::vector parameter_update; 63 | std::vector times; 64 | int num_parameters; 65 | int num_spline_points; 66 | mjpc::spline::SplineInterpolation representation; 67 | }; 68 | 69 | } // namespace mjpc 70 | 71 | #endif // MJPC_PLANNERS_GRADIENT_POLICY_H_ 72 | -------------------------------------------------------------------------------- /mjpc/planners/gradient/settings.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_PLANNERS_GRADIENT_SETTINGS_H_ 16 | #define MJPC_PLANNERS_GRADIENT_SETTINGS_H_ 17 | 18 | namespace mjpc { 19 | 20 | // GradientPlanner settings 21 | struct GradientPlannerSettings { 22 | int max_rollout = 1; // maximum number of planner iterations 23 | double min_linesearch_step = 1.0e-8; // minimum step size for line search 24 | double fd_tolerance = 1.0e-5; // finite-difference tolerance 25 | double fd_mode = 0; // type of finite difference; 0: one-side, 1: centered 26 | int action_limits = 1; // flag 27 | }; 28 | 29 | } // namespace mjpc 30 | 31 | #endif // MJPC_PLANNERS_GRADIENT_SETTINGS_H_ 32 | -------------------------------------------------------------------------------- /mjpc/planners/ilqg/boxqp.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_PLANNERS_ILQG_BOXQP_H_ 16 | #define MJPC_PLANNERS_ILQG_BOXQP_H_ 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | namespace mjpc { 24 | 25 | // ----- boxQP data ----- // 26 | // min 0.5 res' H res + res' g 27 | // st lower <= res <= upper 28 | class BoxQP { 29 | public: 30 | // constructor 31 | BoxQP() = default; 32 | 33 | // destructor 34 | ~BoxQP() = default; 35 | 36 | // allocate memory 37 | void Allocate(int n) { 38 | // size memory 39 | res.resize(n); 40 | R.resize(n * (n + 7)); 41 | H.resize(n * n); 42 | g.resize(n); 43 | lower.resize(n); 44 | upper.resize(n); 45 | index.resize(n); 46 | 47 | // reset for warmstart 48 | mju_zero(res.data(), n); 49 | } 50 | 51 | // ----- members ----- // 52 | std::vector res; // solution 53 | std::vector R; // factorization 54 | std::vector index; // free indices 55 | std::vector H; // SPD matrix 56 | std::vector g; // bias 57 | std::vector lower; // lower bounds 58 | std::vector upper; // upper bounds 59 | }; 60 | 61 | } // namespace mjpc 62 | 63 | #endif // MJPC_PLANNERS_ILQG_BOXQP_H_ 64 | -------------------------------------------------------------------------------- /mjpc/planners/ilqg/policy.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_PLANNERS_ILQG_POLICY_H_ 16 | #define MJPC_PLANNERS_ILQG_POLICY_H_ 17 | 18 | #include 19 | 20 | #include 21 | #include "mjpc/planners/policy.h" 22 | #include "mjpc/task.h" 23 | #include "mjpc/trajectory.h" 24 | 25 | namespace mjpc { 26 | 27 | // iLQG policy 28 | class iLQGPolicy : public Policy { 29 | public: 30 | // constructor 31 | iLQGPolicy() = default; 32 | 33 | // destructor 34 | ~iLQGPolicy() override = default; 35 | 36 | // allocate memory 37 | void Allocate(const mjModel* model, const Task& task, int horizon) override; 38 | 39 | // reset memory to zeros 40 | void Reset(int horizon, 41 | const double* initial_repeated_action = nullptr) override; 42 | 43 | // set action from policy 44 | // if state == nullptr, return the nominal action without a feedback term 45 | void Action(double* action, const double* state, double time) const override; 46 | 47 | // copy policy 48 | void CopyFrom(const iLQGPolicy& policy, int horizon); 49 | 50 | public: 51 | // ----- members ----- // 52 | const mjModel* model; 53 | 54 | Trajectory trajectory; // reference trajectory 55 | std::vector feedback_gain; // (T * dim_action * dim_state_derivative) 56 | std::vector action_improvement; // (T * dim_action) 57 | 58 | // scratch space 59 | mutable std::vector state_scratch; // dim_state 60 | mutable std::vector action_scratch; // dim_action 61 | 62 | // interpolation 63 | mutable std::vector feedback_gain_scratch; 64 | mutable std::vector state_interp; 65 | int representation; 66 | double feedback_scaling; 67 | }; 68 | 69 | } // namespace mjpc 70 | 71 | #endif // MJPC_PLANNERS_ILQG_POLICY_H_ 72 | -------------------------------------------------------------------------------- /mjpc/planners/ilqg/settings.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_PLANNERS_ILQG_SETTINGS_H_ 16 | #define MJPC_PLANNERS_ILQG_SETTINGS_H_ 17 | 18 | namespace mjpc { 19 | 20 | // iLQG settings 21 | struct iLQGSettings { 22 | double min_linesearch_step = 1.0e-3; // minimum step size for line search 23 | double fd_tolerance = 1.0e-6; // finite difference tolerance 24 | double fd_mode = 0; // type of finite difference; 0: one-sided, 1: centered 25 | double min_regularization = 1.0e-6; // minimum regularization value 26 | double max_regularization = 1.0e6; // maximum regularization value 27 | int regularization_type = 0; // 0: control; 1: feedback; 2: value; 3: none 28 | int max_regularization_iterations = 29 | 5; // maximum number of regularization updates per iteration 30 | int action_limits = 1; // flag 31 | int nominal_feedback_scaling = 1; // flag 32 | int verbose = 0; // print optimizer info 33 | }; 34 | 35 | } // namespace mjpc 36 | 37 | #endif // MJPC_PLANNERS_ILQG_SETTINGS_H_ 38 | -------------------------------------------------------------------------------- /mjpc/planners/include.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "mjpc/planners/include.h" 16 | 17 | #include 18 | #include 19 | 20 | #include "mjpc/planners/cross_entropy/planner.h" 21 | #include "mjpc/planners/gradient/planner.h" 22 | #include "mjpc/planners/ilqg/planner.h" 23 | #include "mjpc/planners/ilqs/planner.h" 24 | #include "mjpc/planners/planner.h" 25 | #include "mjpc/planners/robust/robust_planner.h" 26 | #include "mjpc/planners/sample_gradient/planner.h" 27 | #include "mjpc/planners/sampling/planner.h" 28 | 29 | namespace mjpc { 30 | const char kPlannerNames[] = 31 | "Sampling\n" 32 | "Gradient\n" 33 | "iLQG\n" 34 | "iLQS\n" 35 | "Robust Sampling\n" 36 | "Cross Entropy\n" 37 | "Sample Gradient"; 38 | 39 | // load all available planners 40 | std::vector> LoadPlanners() { 41 | // planners 42 | std::vector> planners; 43 | 44 | planners.emplace_back(new mjpc::SamplingPlanner); 45 | planners.emplace_back(new mjpc::GradientPlanner); 46 | planners.emplace_back(new mjpc::iLQGPlanner); 47 | planners.emplace_back(new mjpc::iLQSPlanner); 48 | planners.emplace_back( 49 | new RobustPlanner(std::make_unique())); 50 | planners.emplace_back(new mjpc::CrossEntropyPlanner); 51 | planners.emplace_back(new mjpc::SampleGradientPlanner); 52 | return planners; 53 | } 54 | 55 | } // namespace mjpc 56 | -------------------------------------------------------------------------------- /mjpc/planners/include.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_PLANNERS_INCLUDE_H_ 16 | #define MJPC_PLANNERS_INCLUDE_H_ 17 | 18 | #include 19 | #include 20 | 21 | #include "mjpc/planners/planner.h" 22 | 23 | namespace mjpc { 24 | 25 | // planner types 26 | enum PlannerType : int { 27 | kSamplingPlanner = 0, 28 | kGradientPlanner, 29 | kILQGPlanner, 30 | kILQSPlanner, 31 | kRobustPlanner, 32 | kCrossEntropyPlanner, 33 | kSampleGradientPlanner, 34 | }; 35 | 36 | // Planner names, separated by '\n'. 37 | extern const char kPlannerNames[]; 38 | 39 | // Loads all available planners 40 | std::vector> LoadPlanners(); 41 | 42 | } // namespace mjpc 43 | 44 | #endif // MJPC_PLANNERS_INCLUDE_H_ 45 | -------------------------------------------------------------------------------- /mjpc/planners/model_derivatives.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_PLANNERS_MODEL_DERIVATIVES_H_ 16 | #define MJPC_PLANNERS_MODEL_DERIVATIVES_H_ 17 | 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | #include "mjpc/threadpool.h" 24 | #include "mjpc/utilities.h" 25 | 26 | namespace mjpc { 27 | 28 | // data and methods for model derivatives 29 | class ModelDerivatives { 30 | public: 31 | // constructor 32 | ModelDerivatives() = default; 33 | 34 | // destructor 35 | ~ModelDerivatives() = default; 36 | 37 | // allocate memory 38 | void Allocate(int dim_state_derivative, int dim_action, int dim_sensor, 39 | int T); 40 | 41 | // reset memory to zeros 42 | void Reset(int dim_state_derivative, int dim_action, int dim_sensor, int T); 43 | 44 | // compute derivatives at all time steps 45 | void Compute(const mjModel* m, const std::vector& data, 46 | const double* x, const double* u, const double* h, int dim_state, 47 | int dim_state_derivative, int dim_action, int dim_sensor, int T, 48 | double tol, int mode, ThreadPool& pool, int skip = 0); 49 | 50 | // Jacobians 51 | std::vector A; // model Jacobians wrt state 52 | // (T * dim_state_derivative * dim_state_derivative) 53 | std::vector B; // model Jacobians wrt action 54 | // (T * dim_state_derivative * dim_action) 55 | std::vector C; // output Jacobians wrt state 56 | // (T * dim_sensor * dim_state_derivative) 57 | std::vector D; // output Jacobians wrt action 58 | // (T * dim_sensor * dim_action) 59 | 60 | // indices 61 | std::vector evaluate_; 62 | std::vector interpolate_; 63 | }; 64 | 65 | } // namespace mjpc 66 | 67 | #endif // MJPC_PLANNERS_MODEL_DERIVATIVES_H_ 68 | -------------------------------------------------------------------------------- /mjpc/planners/planner.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "mjpc/planners/planner.h" 16 | 17 | #include 18 | 19 | #include 20 | #include "mjpc/utilities.h" 21 | 22 | namespace mjpc { 23 | void Planner::ResizeMjData(const mjModel* model, int num_threads) { 24 | int new_size = std::max(1, num_threads); 25 | if (data_.size() > new_size) { 26 | data_.erase(data_.begin() + new_size, data_.end()); 27 | } else { 28 | data_.reserve(new_size); 29 | while (data_.size() < new_size) { 30 | data_.push_back(MakeUniqueMjData(mj_makeData(model))); 31 | } 32 | } 33 | } 34 | } // namespace mjpc 35 | -------------------------------------------------------------------------------- /mjpc/planners/policy.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_PLANNERS_POLICY_H_ 16 | #define MJPC_PLANNERS_POLICY_H_ 17 | 18 | #include 19 | #include "mjpc/task.h" 20 | 21 | namespace mjpc { 22 | 23 | // virtual policy 24 | class Policy { 25 | public: 26 | // destructor 27 | virtual ~Policy() = default; 28 | 29 | // allocate memory 30 | virtual void Allocate(const mjModel* model, const Task& task, 31 | int horizon) = 0; 32 | 33 | // reset memory to zeros 34 | virtual void Reset(int horizon, 35 | const double* initial_repeated_action = nullptr) = 0; 36 | 37 | // set action from policy 38 | // for policies that have a feedback term, passing nullptr for state turns 39 | // the feedback term off and returns the nominal action for that time 40 | virtual void Action(double* action, const double* state, 41 | double time) const = 0; 42 | }; 43 | 44 | } // namespace mjpc 45 | 46 | #endif // MJPC_PLANNERS_POLICY_H_ 47 | -------------------------------------------------------------------------------- /mjpc/planners/sampling/policy.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "mjpc/planners/sampling/policy.h" 16 | 17 | #include 18 | #include 19 | #include 20 | #include "mjpc/spline/spline.h" 21 | #include "mjpc/task.h" 22 | #include "mjpc/trajectory.h" 23 | #include "mjpc/utilities.h" 24 | 25 | namespace mjpc { 26 | 27 | using mjpc::spline::TimeSpline; 28 | 29 | // allocate memory 30 | void SamplingPolicy::Allocate(const mjModel* model, const Task& task, 31 | int horizon) { 32 | // model 33 | this->model = model; 34 | 35 | // spline points 36 | num_spline_points = GetNumberOrDefault(kMaxTrajectoryHorizon, model, 37 | "sampling_spline_points"); 38 | 39 | plan = TimeSpline(/*dim=*/model->nu); 40 | plan.Reserve(num_spline_points); 41 | } 42 | 43 | // reset memory to zeros 44 | void SamplingPolicy::Reset(int horizon, const double* initial_repeated_action) { 45 | plan.Clear(); 46 | if (initial_repeated_action != nullptr) { 47 | plan.AddNode(0, absl::MakeConstSpan(initial_repeated_action, model->nu)); 48 | } 49 | } 50 | 51 | // set action from policy 52 | void SamplingPolicy::Action(double* action, const double* state, 53 | double time) const { 54 | CHECK(action != nullptr); 55 | plan.Sample(time, absl::MakeSpan(action, model->nu)); 56 | 57 | // Clamp controls 58 | Clamp(action, model->actuator_ctrlrange, model->nu); 59 | } 60 | 61 | // copy policy 62 | void SamplingPolicy::CopyFrom(const SamplingPolicy& policy, int horizon) { 63 | this->plan = policy.plan; 64 | num_spline_points = policy.num_spline_points; 65 | } 66 | 67 | // copy parameters 68 | void SamplingPolicy::SetPlan(const TimeSpline& plan) { 69 | this->plan = plan; 70 | } 71 | 72 | } // namespace mjpc 73 | -------------------------------------------------------------------------------- /mjpc/planners/sampling/policy.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_PLANNERS_SAMPLING_POLICY_H_ 16 | #define MJPC_PLANNERS_SAMPLING_POLICY_H_ 17 | 18 | #include 19 | #include "mjpc/planners/policy.h" 20 | #include "mjpc/spline/spline.h" 21 | #include "mjpc/task.h" 22 | 23 | namespace mjpc { 24 | 25 | // policy for sampling planner 26 | class SamplingPolicy : public Policy { 27 | public: 28 | // constructor 29 | SamplingPolicy() = default; 30 | 31 | // destructor 32 | ~SamplingPolicy() override = default; 33 | 34 | // ----- methods ----- // 35 | 36 | // allocate memory 37 | void Allocate(const mjModel* model, const Task& task, int horizon) override; 38 | 39 | // reset memory to zeros 40 | void Reset(int horizon, 41 | const double* initial_repeated_action = nullptr) override; 42 | 43 | // set action from policy 44 | void Action(double* action, const double* state, double time) const override; 45 | 46 | // copy policy 47 | void CopyFrom(const SamplingPolicy& policy, int horizon); 48 | 49 | // copy parameters 50 | void SetPlan(const mjpc::spline::TimeSpline& plan); 51 | 52 | // ----- members ----- // 53 | const mjModel* model; 54 | mjpc::spline::TimeSpline plan; 55 | int num_spline_points; 56 | }; 57 | 58 | } // namespace mjpc 59 | 60 | #endif // MJPC_PLANNERS_SAMPLING_POLICY_H_ 61 | -------------------------------------------------------------------------------- /mjpc/tasks/acrobot/acrobot.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "mjpc/tasks/acrobot/acrobot.h" 16 | 17 | #include 18 | 19 | #include 20 | #include "mjpc/task.h" 21 | #include "mjpc/utilities.h" 22 | 23 | namespace mjpc { 24 | std::string Acrobot::XmlPath() const { 25 | return GetModelPath("acrobot/task.xml"); 26 | } 27 | std::string Acrobot::Name() const { return "Acrobot"; } 28 | 29 | // ---------- Residuals for acrobot task --------- 30 | // Number of residuals: 5 31 | // Residual (0-1): Distance from tip to goal 32 | // Residual (2-3): Joint velocity 33 | // Residual (4): Control 34 | // ----------------------------------------------- 35 | void Acrobot::ResidualFn::Residual(const mjModel* model, const mjData* data, 36 | double* residual) const { 37 | // ---------- Residual (0-1) ---------- 38 | mjtNum* goal_xpos = &data->site_xpos[3 * 0]; 39 | mjtNum* tip_xpos = &data->site_xpos[3 * 1]; 40 | residual[0] = goal_xpos[2] - tip_xpos[2]; 41 | residual[1] = goal_xpos[0] - tip_xpos[0]; 42 | 43 | // ---------- Residual (2-3) ---------- 44 | residual[2] = data->qvel[0]; 45 | residual[3] = data->qvel[1]; 46 | 47 | // ---------- Residual (4) ---------- 48 | residual[4] = data->ctrl[0]; 49 | } 50 | 51 | } // namespace mjpc 52 | -------------------------------------------------------------------------------- /mjpc/tasks/acrobot/acrobot.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_TASKS_ACROBOT_ACROBOT_H_ 16 | #define MJPC_TASKS_ACROBOT_ACROBOT_H_ 17 | 18 | #include 19 | #include 20 | #include "mjpc/task.h" 21 | 22 | namespace mjpc { 23 | class Acrobot : public Task { 24 | public: 25 | std::string Name() const override; 26 | std::string XmlPath() const override; 27 | 28 | class ResidualFn : public BaseResidualFn { 29 | public: 30 | explicit ResidualFn(const Acrobot* task) : BaseResidualFn(task) {} 31 | // ---------- Residuals for acrobot task --------- 32 | // Number of residuals: 5 33 | // Residual (0-1): Distance from tip to goal 34 | // Residual (2-3): Joint velocity 35 | // Residual (4): Control 36 | // ----------------------------------------------- 37 | void Residual(const mjModel* model, const mjData* data, 38 | double* residual) const override; 39 | }; 40 | 41 | Acrobot() : residual_(this) {} 42 | 43 | protected: 44 | std::unique_ptr ResidualLocked() const override { 45 | return std::make_unique(this); 46 | } 47 | ResidualFn* InternalResidual() override { return &residual_; } 48 | 49 | private: 50 | ResidualFn residual_; 51 | }; 52 | } // namespace mjpc 53 | 54 | #endif // MJPC_TASKS_ACROBOT_ACROBOT_H_ 55 | -------------------------------------------------------------------------------- /mjpc/tasks/acrobot/acrobot.xml.patch: -------------------------------------------------------------------------------- 1 | diff --git a/acrobot_modified.xml b/acrobot_modified.xml 2 | --- a/acrobot_modified.xml 3 | +++ b/acrobot_modified.xml 4 | @@ -6,22 +6,18 @@ 5 | IEEE control systems 15, no. 1 (1995): 49-55. 6 | --> 7 | 8 | - 9 | - 10 | - 11 | - 12 | 13 | 14 | 15 | 16 | 17 | - 21 | 22 | 23 | 24 | - 25 | + 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /mjpc/tasks/acrobot/task.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /mjpc/tasks/allegro/allegro.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_TASKS_ALLEGRO_ALLEGRO_H_ 16 | #define MJPC_TASKS_ALLEGRO_ALLEGRO_H_ 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include "mjpc/task.h" 23 | 24 | namespace mjpc { 25 | class Allegro : public Task { 26 | public: 27 | std::string Name() const override; 28 | std::string XmlPath() const override; 29 | 30 | class ResidualFn : public BaseResidualFn { 31 | public: 32 | explicit ResidualFn(const Allegro *task) : BaseResidualFn(task) {} 33 | void Residual(const mjModel *model, const mjData *data, 34 | double *residual) const override; 35 | }; 36 | Allegro() : residual_(this) {} 37 | 38 | // Reset the cube into the hand if it's on the floor 39 | void TransitionLocked(mjModel *model, mjData *data) override; 40 | 41 | protected: 42 | std::unique_ptr ResidualLocked() const override { 43 | return std::make_unique(this); 44 | } 45 | ResidualFn *InternalResidual() override { return &residual_; } 46 | 47 | private: 48 | ResidualFn residual_; 49 | }; 50 | } // namespace mjpc 51 | 52 | #endif // MJPC_TASKS_ALLEGRO_ALLEGRO_H_ 53 | -------------------------------------------------------------------------------- /mjpc/tasks/allegro/right_hand.xml.patch: -------------------------------------------------------------------------------- 1 | diff --git a/right_hand_modified.xml b/right_hand_modified.xml 2 | --- a/right_hand_modified.xml 3 | +++ b/right_hand_modified.xml 4 | @@ -6,7 +6,7 @@ 5 | 6 | 7 | 8 | - 9 | + 10 | 11 | 12 | 13 | @@ -124,8 +124,9 @@ 14 | 15 | 16 | 17 | - 18 | + 19 | 20 | + 21 | 22 | 23 | 24 | @@ -148,6 +149,7 @@ 25 | 26 | 27 | 28 | + 29 | 30 | 31 | 32 | @@ -257,4 +259,12 @@ 33 | 34 | 35 | 36 | + 37 | + 38 | + 39 | + 40 | + 41 | + 42 | + 43 | + 44 | 45 | -------------------------------------------------------------------------------- /mjpc/tasks/bimanual/handover/handover.h: -------------------------------------------------------------------------------- 1 | // Copyright 2024 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_MJPC_TASKS_BIMANUAL_HANDOVER_HANDOVER_H_ 16 | #define MJPC_MJPC_TASKS_BIMANUAL_HANDOVER_HANDOVER_H_ 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include "mjpc/task.h" 23 | 24 | namespace mjpc::aloha { 25 | class Handover : public Task { 26 | public: 27 | std::string Name() const override; 28 | std::string XmlPath() const override; 29 | 30 | class ResidualFn : public BaseResidualFn { 31 | public: 32 | explicit ResidualFn(const Handover* task) : BaseResidualFn(task) {} 33 | void Residual(const mjModel* model, const mjData* data, 34 | double* residual) const override; 35 | }; 36 | 37 | Handover() : residual_(this) {} 38 | void TransitionLocked(mjModel* model, mjData* data) override; 39 | 40 | protected: 41 | std::unique_ptr ResidualLocked() const override { 42 | return std::make_unique(this); 43 | } 44 | ResidualFn* InternalResidual() override { return &residual_; } 45 | double last_solve_time = 0; 46 | 47 | private: 48 | ResidualFn residual_; 49 | }; 50 | } // namespace mjpc::aloha 51 | 52 | 53 | #endif // MJPC_MJPC_TASKS_BIMANUAL_HANDOVER_HANDOVER_H_ 54 | -------------------------------------------------------------------------------- /mjpc/tasks/bimanual/insert/insert.h: -------------------------------------------------------------------------------- 1 | // Copyright 2024 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_MJPC_TASKS_BIMANUAL_INSERT_INSERT_H_ 16 | #define MJPC_MJPC_TASKS_BIMANUAL_INSERT_INSERT_H_ 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include "mjpc/task.h" 23 | 24 | namespace mjpc::aloha { 25 | class Insert : public Task { 26 | public: 27 | std::string Name() const override; 28 | std::string XmlPath() const override; 29 | 30 | class ResidualFn : public BaseResidualFn { 31 | public: 32 | explicit ResidualFn(const Insert* task) : BaseResidualFn(task) {} 33 | void Residual(const mjModel* model, const mjData* data, 34 | double* residual) const override; 35 | }; 36 | 37 | Insert() : residual_(this) {} 38 | void TransitionLocked(mjModel* model, mjData* data) override; 39 | 40 | protected: 41 | std::unique_ptr ResidualLocked() const override { 42 | return std::make_unique(this); 43 | } 44 | ResidualFn* InternalResidual() override { return &residual_; } 45 | double last_solve_time = 0; 46 | 47 | private: 48 | ResidualFn residual_; 49 | }; 50 | } // namespace mjpc::aloha 51 | 52 | 53 | #endif // MJPC_MJPC_TASKS_BIMANUAL_INSERT_INSERT_H_ 54 | -------------------------------------------------------------------------------- /mjpc/tasks/bimanual/reorient/reorient.h: -------------------------------------------------------------------------------- 1 | // Copyright 2024 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_MJPC_TASKS_BIMANUAL_REORIENT_REORIENT_H_ 16 | #define MJPC_MJPC_TASKS_BIMANUAL_REORIENT_REORIENT_H_ 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include "mjpc/task.h" 23 | 24 | namespace mjpc::aloha { 25 | class Reorient : public Task { 26 | public: 27 | std::string Name() const override; 28 | std::string XmlPath() const override; 29 | 30 | class ResidualFn : public BaseResidualFn { 31 | public: 32 | explicit ResidualFn(const Reorient* task) : BaseResidualFn(task) {} 33 | void Residual(const mjModel* model, const mjData* data, 34 | double* residual) const override; 35 | }; 36 | 37 | Reorient() : residual_(this) {} 38 | void TransitionLocked(mjModel* model, mjData* data) override; 39 | 40 | protected: 41 | std::unique_ptr ResidualLocked() const override { 42 | return std::make_unique(this); 43 | } 44 | ResidualFn* InternalResidual() override { return &residual_; } 45 | double last_solve_time = 0; 46 | 47 | private: 48 | ResidualFn residual_; 49 | }; 50 | } // namespace mjpc::aloha 51 | 52 | 53 | #endif // MJPC_MJPC_TASKS_BIMANUAL_REORIENT_REORIENT_H_ 54 | -------------------------------------------------------------------------------- /mjpc/tasks/cartpole/cartpole.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "mjpc/tasks/cartpole/cartpole.h" 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include "mjpc/task.h" 22 | #include "mjpc/utilities.h" 23 | 24 | namespace mjpc { 25 | std::string Cartpole::XmlPath() const { 26 | return GetModelPath("cartpole/task.xml"); 27 | } 28 | std::string Cartpole::Name() const { return "Cartpole"; } 29 | 30 | // ------- Residuals for cartpole task ------ 31 | // Vertical: Pole angle cosine should be -1 32 | // Centered: Cart should be at goal position 33 | // Velocity: Pole angular velocity should be small 34 | // Control: Control should be small 35 | // ------------------------------------------ 36 | void Cartpole::ResidualFn::Residual(const mjModel* model, const mjData* data, 37 | double* residual) const { 38 | // ---------- Vertical ---------- 39 | residual[0] = std::cos(data->qpos[1]) - 1; 40 | 41 | // ---------- Centered ---------- 42 | residual[1] = data->qpos[0] - parameters_[0]; 43 | 44 | // ---------- Velocity ---------- 45 | residual[2] = data->qvel[1]; 46 | 47 | // ---------- Control ---------- 48 | residual[3] = data->ctrl[0]; 49 | } 50 | 51 | } // namespace mjpc 52 | -------------------------------------------------------------------------------- /mjpc/tasks/cartpole/cartpole.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_TASKS_CARTPOLE_CARTPOLE_H_ 16 | #define MJPC_TASKS_CARTPOLE_CARTPOLE_H_ 17 | 18 | #include 19 | 20 | #include 21 | #include "mjpc/task.h" 22 | 23 | namespace mjpc { 24 | class Cartpole : public Task { 25 | public: 26 | std::string Name() const override; 27 | std::string XmlPath() const override; 28 | 29 | class ResidualFn : public BaseResidualFn { 30 | public: 31 | explicit ResidualFn(const Cartpole* task) : BaseResidualFn(task) {} 32 | // ------- Residuals for cartpole task ------ 33 | // Number of residuals: 4 34 | // Residual (0): distance from vertical 35 | // Residual (1): distance from goal 36 | // Residual (2): angular velocity 37 | // Residual (3): control 38 | // ------------------------------------------ 39 | void Residual(const mjModel* model, const mjData* data, 40 | double* residual) const override; 41 | }; 42 | 43 | Cartpole() : residual_(this) {} 44 | 45 | protected: 46 | std::unique_ptr ResidualLocked() const override { 47 | return std::make_unique(this); 48 | } 49 | ResidualFn* InternalResidual() override { return &residual_; } 50 | 51 | private: 52 | ResidualFn residual_; 53 | }; 54 | } // namespace mjpc 55 | 56 | #endif // MJPC_TASKS_CARTPOLE_CARTPOLE_H_ 57 | -------------------------------------------------------------------------------- /mjpc/tasks/cartpole/cartpole.xml.patch: -------------------------------------------------------------------------------- 1 | diff --git a/cartpole_modified.xml b/cartpole_modified.xml 2 | --- a/cartpole_modified.xml 3 | +++ b/cartpole_modified.xml 4 | @@ -1,10 +1,6 @@ 5 | - 6 | - 7 | - 8 | - 9 | - 10 | - 38 | -------------------------------------------------------------------------------- /mjpc/tasks/cartpole/task.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /mjpc/tasks/common.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /mjpc/tasks/common_assets/connector/mcX_f.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/google-deepmind/mujoco_mpc/ff572a21e7c2bf9fda62e1862a758da7e9a8719b/mjpc/tasks/common_assets/connector/mcX_f.stl -------------------------------------------------------------------------------- /mjpc/tasks/common_assets/connector/mcX_f_collision_mcX_f_MESH.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/google-deepmind/mujoco_mpc/ff572a21e7c2bf9fda62e1862a758da7e9a8719b/mjpc/tasks/common_assets/connector/mcX_f_collision_mcX_f_MESH.stl -------------------------------------------------------------------------------- /mjpc/tasks/common_assets/connector/mcX_m.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/google-deepmind/mujoco_mpc/ff572a21e7c2bf9fda62e1862a758da7e9a8719b/mjpc/tasks/common_assets/connector/mcX_m.stl -------------------------------------------------------------------------------- /mjpc/tasks/common_assets/connector/mcX_m_collision_mcX_m_MESH.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/google-deepmind/mujoco_mpc/ff572a21e7c2bf9fda62e1862a758da7e9a8719b/mjpc/tasks/common_assets/connector/mcX_m_collision_mcX_m_MESH.stl -------------------------------------------------------------------------------- /mjpc/tasks/common_assets/cube.xml.patch: -------------------------------------------------------------------------------- 1 | mjpc/tasks/cube.xml.patch--- common_assets/reorientation_cube.xml 2024-02-11 18:42:07 2 | +++ hand/cube.xml 2024-02-12 14:52:27 3 | @@ -19,9 +19,9 @@ 4 | 5 | 6 | 7 | - 8 | + 9 | 10 | - 11 | + 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /mjpc/tasks/common_assets/reorientation_cross.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /mjpc/tasks/common_assets/reorientation_cube.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 10 | 11 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /mjpc/tasks/common_assets/reorientation_cube_textures/fileback.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/google-deepmind/mujoco_mpc/ff572a21e7c2bf9fda62e1862a758da7e9a8719b/mjpc/tasks/common_assets/reorientation_cube_textures/fileback.png -------------------------------------------------------------------------------- /mjpc/tasks/common_assets/reorientation_cube_textures/filedown.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/google-deepmind/mujoco_mpc/ff572a21e7c2bf9fda62e1862a758da7e9a8719b/mjpc/tasks/common_assets/reorientation_cube_textures/filedown.png -------------------------------------------------------------------------------- /mjpc/tasks/common_assets/reorientation_cube_textures/filefront.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/google-deepmind/mujoco_mpc/ff572a21e7c2bf9fda62e1862a758da7e9a8719b/mjpc/tasks/common_assets/reorientation_cube_textures/filefront.png -------------------------------------------------------------------------------- /mjpc/tasks/common_assets/reorientation_cube_textures/fileleft.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/google-deepmind/mujoco_mpc/ff572a21e7c2bf9fda62e1862a758da7e9a8719b/mjpc/tasks/common_assets/reorientation_cube_textures/fileleft.png -------------------------------------------------------------------------------- /mjpc/tasks/common_assets/reorientation_cube_textures/fileright.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/google-deepmind/mujoco_mpc/ff572a21e7c2bf9fda62e1862a758da7e9a8719b/mjpc/tasks/common_assets/reorientation_cube_textures/fileright.png -------------------------------------------------------------------------------- /mjpc/tasks/common_assets/reorientation_cube_textures/fileup.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/google-deepmind/mujoco_mpc/ff572a21e7c2bf9fda62e1862a758da7e9a8719b/mjpc/tasks/common_assets/reorientation_cube_textures/fileup.png -------------------------------------------------------------------------------- /mjpc/tasks/common_assets/reorientation_cube_textures/grayback.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/google-deepmind/mujoco_mpc/ff572a21e7c2bf9fda62e1862a758da7e9a8719b/mjpc/tasks/common_assets/reorientation_cube_textures/grayback.png -------------------------------------------------------------------------------- /mjpc/tasks/common_assets/reorientation_cube_textures/graydown.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/google-deepmind/mujoco_mpc/ff572a21e7c2bf9fda62e1862a758da7e9a8719b/mjpc/tasks/common_assets/reorientation_cube_textures/graydown.png -------------------------------------------------------------------------------- /mjpc/tasks/common_assets/reorientation_cube_textures/grayfront.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/google-deepmind/mujoco_mpc/ff572a21e7c2bf9fda62e1862a758da7e9a8719b/mjpc/tasks/common_assets/reorientation_cube_textures/grayfront.png -------------------------------------------------------------------------------- /mjpc/tasks/common_assets/reorientation_cube_textures/grayleft.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/google-deepmind/mujoco_mpc/ff572a21e7c2bf9fda62e1862a758da7e9a8719b/mjpc/tasks/common_assets/reorientation_cube_textures/grayleft.png -------------------------------------------------------------------------------- /mjpc/tasks/common_assets/reorientation_cube_textures/grayright.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/google-deepmind/mujoco_mpc/ff572a21e7c2bf9fda62e1862a758da7e9a8719b/mjpc/tasks/common_assets/reorientation_cube_textures/grayright.png -------------------------------------------------------------------------------- /mjpc/tasks/common_assets/reorientation_cube_textures/grayup.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/google-deepmind/mujoco_mpc/ff572a21e7c2bf9fda62e1862a758da7e9a8719b/mjpc/tasks/common_assets/reorientation_cube_textures/grayup.png -------------------------------------------------------------------------------- /mjpc/tasks/fingers/fingers.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "mjpc/tasks/fingers/fingers.h" 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | #include "mjpc/task.h" 22 | #include "mjpc/utilities.h" 23 | 24 | namespace mjpc { 25 | std::string Fingers::XmlPath() const { 26 | return GetModelPath("fingers/task.xml"); 27 | } 28 | std::string Fingers::Name() const { return "FreeFingers"; } 29 | 30 | void Fingers::ResidualFn::Residual(const mjModel* model, const mjData* data, 31 | double* residual) const { 32 | int counter = 0; 33 | 34 | // reach 35 | double* finger_a = SensorByName(model, data, "finger_a"); 36 | double* box = SensorByName(model, data, "object"); 37 | mju_sub3(residual + counter, finger_a, box); 38 | counter += 3; 39 | double* finger_b = SensorByName(model, data, "finger_b"); 40 | mju_sub3(residual + counter, finger_b, box); 41 | counter += 3; 42 | 43 | // bring 44 | for (int i=0; i < 3; i++) { 45 | double* object = SensorByName(model, data, std::to_string(i).c_str()); 46 | double* target = SensorByName(model, data, 47 | (std::to_string(i) + "t").c_str()); 48 | residual[counter++] = mju_dist3(object, target); 49 | } 50 | 51 | // control 52 | for (int i=0; i < model->nu; i++) { 53 | residual[counter++] = data->ctrl[i]; 54 | } 55 | 56 | CheckSensorDim(model, counter); 57 | } 58 | } // namespace mjpc 59 | -------------------------------------------------------------------------------- /mjpc/tasks/fingers/fingers.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_MJPC_TASKS_FINGERS_FINGERS_H_ 16 | #define MJPC_MJPC_TASKS_FINGERS_FINGERS_H_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include "mjpc/task.h" 22 | 23 | namespace mjpc { 24 | class Fingers : public Task { 25 | public: 26 | std::string Name() const override; 27 | std::string XmlPath() const override; 28 | class ResidualFn : public mjpc::BaseResidualFn { 29 | public: 30 | explicit ResidualFn(const Fingers* task) : mjpc::BaseResidualFn(task) {} 31 | void Residual(const mjModel* model, const mjData* data, 32 | double* residual) const override; 33 | }; 34 | Fingers() : residual_(this) {} 35 | 36 | protected: 37 | std::unique_ptr ResidualLocked() const override { 38 | return std::make_unique(this); 39 | } 40 | ResidualFn* InternalResidual() override { return &residual_; } 41 | 42 | private: 43 | ResidualFn residual_; 44 | }; 45 | } // namespace mjpc 46 | 47 | 48 | #endif // MJPC_MJPC_TASKS_FINGERS_FINGERS_H_ 49 | -------------------------------------------------------------------------------- /mjpc/tasks/humanoid/interact/scenes/armchair.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 29 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /mjpc/tasks/humanoid/interact/scenes/bed.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 27 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /mjpc/tasks/humanoid/interact/scenes/bench.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 31 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /mjpc/tasks/humanoid/interact/scenes/dining_chair.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 30 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /mjpc/tasks/humanoid/interact/scenes/stool.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /mjpc/tasks/humanoid/interact/scenes/table.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /mjpc/tasks/humanoid/stand/stand.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_TASKS_HUMANOID_STAND_TASK_H_ 16 | #define MJPC_TASKS_HUMANOID_STAND_TASK_H_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include "mjpc/task.h" 22 | 23 | namespace mjpc { 24 | namespace humanoid { 25 | 26 | class Stand : public Task { 27 | public: 28 | class ResidualFn : public mjpc::BaseResidualFn { 29 | public: 30 | explicit ResidualFn(const Stand* task) : mjpc::BaseResidualFn(task) {} 31 | 32 | // ------------------ Residuals for humanoid stand task ------------ 33 | // Number of residuals: 6 34 | // Residual (0): control 35 | // Residual (1): COM_xy - average(feet position)_xy 36 | // Residual (2): torso_xy - COM_xy 37 | // Residual (3): head_z - feet^{(i)}_position_z - height_goal 38 | // Residual (4): velocity COM_xy 39 | // Residual (5): joint velocity 40 | // Number of parameters: 1 41 | // Parameter (0): height_goal 42 | // ---------------------------------------------------------------- 43 | void Residual(const mjModel* model, const mjData* data, 44 | double* residual) const override; 45 | }; 46 | 47 | Stand() : residual_(this) {} 48 | 49 | std::string Name() const override; 50 | std::string XmlPath() const override; 51 | 52 | protected: 53 | std::unique_ptr ResidualLocked() const override { 54 | return std::make_unique(this); 55 | } 56 | ResidualFn* InternalResidual() override { return &residual_; } 57 | 58 | private: 59 | ResidualFn residual_; 60 | }; 61 | 62 | } // namespace humanoid 63 | } // namespace mjpc 64 | 65 | #endif // MJPC_TASKS_HUMANOID_STAND_TASK_H_ 66 | -------------------------------------------------------------------------------- /mjpc/tasks/humanoid/stand/task.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /mjpc/tasks/humanoid/tracking/keyframes/README.md: -------------------------------------------------------------------------------- 1 | * The data in this directory was obtained from mocap.cs.cmu.edu. 2 | * The database was created with funding from NSF EIA-0196217. 3 | -------------------------------------------------------------------------------- /mjpc/tasks/humanoid/walk/walk.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_TASKS_HUMANOID_WALK_TASK_H_ 16 | #define MJPC_TASKS_HUMANOID_WALK_TASK_H_ 17 | 18 | #include 19 | #include "mjpc/task.h" 20 | 21 | namespace mjpc { 22 | namespace humanoid { 23 | 24 | class Walk : public Task { 25 | public: 26 | class ResidualFn : public mjpc::BaseResidualFn { 27 | public: 28 | explicit ResidualFn(const Walk* task) : mjpc::BaseResidualFn(task) {} 29 | 30 | // ------------------ Residuals for humanoid walk task ------------ 31 | // Number of residuals: 32 | // Residual (0): torso height 33 | // Residual (1): pelvis-feet aligment 34 | // Residual (2): balance 35 | // Residual (3): upright 36 | // Residual (4): posture 37 | // Residual (5): walk 38 | // Residual (6): move feet 39 | // Residual (7): control 40 | // Number of parameters: 41 | // Parameter (0): torso height goal 42 | // Parameter (1): speed goal 43 | // ---------------------------------------------------------------- 44 | void Residual(const mjModel* model, const mjData* data, 45 | double* residual) const override; 46 | }; 47 | 48 | Walk() : residual_(this) {} 49 | 50 | std::string Name() const override; 51 | std::string XmlPath() const override; 52 | 53 | protected: 54 | std::unique_ptr ResidualLocked() const override { 55 | return std::make_unique(this); 56 | } 57 | ResidualFn* InternalResidual() override { return &residual_; } 58 | 59 | private: 60 | ResidualFn residual_; 61 | }; 62 | 63 | } // namespace humanoid 64 | } // namespace mjpc 65 | 66 | #endif // MJPC_TASKS_HUMANOID_WALK_TASK_H_ 67 | -------------------------------------------------------------------------------- /mjpc/tasks/manipulation/manipulation.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_MJPC_TASKS_MANIPULATION_MANIPULATION_H_ 16 | #define MJPC_MJPC_TASKS_MANIPULATION_MANIPULATION_H_ 17 | 18 | 19 | #include 20 | #include "mjpc/task.h" 21 | #include "mjpc/tasks/manipulation/common.h" 22 | 23 | namespace mjpc::manipulation { 24 | class Bring : public Task { 25 | public: 26 | std::string Name() const override; 27 | std::string XmlPath() const override; 28 | class ResidualFn : public mjpc::BaseResidualFn { 29 | public: 30 | explicit ResidualFn(const Bring* task, ModelValues values) 31 | : mjpc::BaseResidualFn(task), model_vals_(std::move(values)) {} 32 | 33 | void Residual(const mjModel* model, const mjData* data, 34 | double* residual) const override; 35 | private: 36 | friend class Bring; 37 | ModelValues model_vals_; 38 | }; 39 | 40 | Bring() : residual_(this, ModelValues()) {} 41 | void TransitionLocked(mjModel* model, mjData* data) override; 42 | void ResetLocked(const mjModel* model) override; 43 | 44 | protected: 45 | std::unique_ptr ResidualLocked() const override { 46 | return std::make_unique(this, residual_.model_vals_); 47 | } 48 | ResidualFn* InternalResidual() override { return &residual_; } 49 | 50 | private: 51 | ResidualFn residual_; 52 | }; 53 | } // namespace mjpc::manipulation 54 | 55 | 56 | #endif // MJPC_MJPC_TASKS_MANIPULATION_MANIPULATION_H_ 57 | -------------------------------------------------------------------------------- /mjpc/tasks/op3/stand.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_TASKS_OP3_STAND_H_ 16 | #define MJPC_TASKS_OP3_STAND_H_ 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include "mjpc/task.h" 23 | 24 | namespace mjpc { 25 | 26 | class OP3 : public Task { 27 | public: 28 | std::string Name() const override; 29 | std::string XmlPath() const override; 30 | 31 | class ResidualFn : public BaseResidualFn { 32 | public: 33 | explicit ResidualFn(const OP3* task, int current_mode = 0) 34 | : BaseResidualFn(task), current_mode_(current_mode) {} 35 | // ------- Residuals for OP3 task ------------ 36 | // Residual(0): height - feet height 37 | // Residual(1): balance 38 | // Residual(2): center of mass xy velocity 39 | // Residual(3): ctrl - ctrl_nominal 40 | // Residual(4): upright 41 | // Residual(5): joint velocity 42 | // ------------------------------------------- 43 | void Residual(const mjModel* model, const mjData* data, 44 | double* residual) const override; 45 | 46 | private: 47 | friend class OP3; 48 | int current_mode_; 49 | 50 | // modes 51 | enum OP3Mode { 52 | kModeStand = 0, 53 | kModeHandstand, 54 | }; 55 | }; 56 | 57 | OP3() : residual_(this) {} 58 | void TransitionLocked(mjModel* model, mjData* data) override; 59 | 60 | // default height goals 61 | constexpr static double kModeHeight[2] = {0.38, 0.57}; 62 | 63 | protected: 64 | std::unique_ptr ResidualLocked() const override { 65 | return std::make_unique(this, residual_.current_mode_); 66 | } 67 | ResidualFn* InternalResidual() override { return &residual_; } 68 | 69 | private: 70 | ResidualFn residual_; 71 | }; 72 | 73 | } // namespace mjpc 74 | 75 | #endif // MJPC_TASKS_OP3_STAND_H_ 76 | -------------------------------------------------------------------------------- /mjpc/tasks/panda/panda.h: -------------------------------------------------------------------------------- 1 | // Copyright 2022 DeepMind Technologies Limited 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef MJPC_MJPC_TASKS_PANDA_PANDA_H_ 16 | #define MJPC_MJPC_TASKS_PANDA_PANDA_H_ 17 | 18 | #include 19 | #include 20 | #include "mjpc/task.h" 21 | 22 | namespace mjpc { 23 | class Panda : public Task { 24 | public: 25 | std::string Name() const override; 26 | std::string XmlPath() const override; 27 | class ResidualFn : public mjpc::BaseResidualFn { 28 | public: 29 | explicit ResidualFn(const Panda* task) : mjpc::BaseResidualFn(task) {} 30 | void Residual(const mjModel* model, const mjData* data, 31 | double* residual) const override; 32 | }; 33 | Panda() : residual_(this) {} 34 | void TransitionLocked(mjModel* model, mjData* data) override; 35 | 36 | protected: 37 | std::unique_ptr ResidualLocked() const override { 38 | return std::make_unique(this); 39 | } 40 | ResidualFn* InternalResidual() override { return &residual_; } 41 | 42 | private: 43 | ResidualFn residual_; 44 | }; 45 | } // namespace mjpc 46 | 47 | 48 | #endif // MJPC_MJPC_TASKS_PANDA_PANDA_H_ 49 | -------------------------------------------------------------------------------- /mjpc/tasks/particle/particle.xml.patch: -------------------------------------------------------------------------------- 1 | diff --git a/particle_modified.xml b/particle_modified.xml 2 | --- a/particle_modified.xml 3 | +++ b/particle_modified.xml 4 | @@ -1,9 +1,5 @@ 5 | - 6 | - 7 | - 8 | - 9 | - 10 | -