├── .clang-format ├── .github └── workflows │ └── ungar-ci.yml ├── .gitignore ├── CMakeLists.txt ├── LICENSE.md ├── README.md ├── UngarConfig.cmake.in ├── data └── robots │ ├── a1_description │ ├── README.md │ ├── meshes │ │ ├── calf.dae │ │ ├── hip.dae │ │ ├── thigh.dae │ │ ├── thigh_mirror.dae │ │ ├── trunk.dae │ │ └── trunk_A1.png │ ├── srdf │ │ └── a1.srdf │ └── urdf │ │ └── a1.urdf │ ├── anymal_b_description │ ├── README.md │ ├── meshes │ │ ├── base │ │ │ ├── anymal_base.dae │ │ │ └── base_uv_texture.jpg │ │ ├── foot │ │ │ ├── anymal_foot.dae │ │ │ └── carbon_uv_texture.jpg │ │ ├── hip │ │ │ ├── anymal_hip_l.dae │ │ │ └── anymal_hip_r.dae │ │ ├── shank │ │ │ ├── anymal_shank_l.dae │ │ │ └── anymal_shank_r.dae │ │ └── thigh │ │ │ └── anymal_thigh.dae │ ├── robots │ │ ├── anymal-kinova.urdf │ │ └── anymal.urdf │ └── srdf │ │ ├── anymal-kinova.srdf │ │ └── anymal.srdf │ ├── cassie_description │ ├── meshes │ │ ├── achilles-rod.stl │ │ ├── foot-crank.stl │ │ ├── foot.stl │ │ ├── heel-spring.stl │ │ ├── hip-pitch.stl │ │ ├── hip-roll.stl │ │ ├── hip-yaw.stl │ │ ├── knee-spring.stl │ │ ├── knee.stl │ │ ├── pelvis.stl │ │ ├── plantar-rod.stl │ │ ├── shin.stl │ │ └── tarsus.stl │ ├── robots │ │ ├── cassie.sdf │ │ ├── cassie.world │ │ ├── cassie_reduced.sdf │ │ ├── cassie_simple.sdf │ │ ├── cassie_v2.sdf │ │ ├── cassie_wo_gearbox_spring.sdf │ │ └── model.config │ └── srdf │ │ └── cassie_v2.srdf │ ├── hyq_description │ ├── README.md │ ├── meshes │ │ ├── leg │ │ │ ├── hipassembly.dae │ │ │ ├── lowerleg.dae │ │ │ └── upperleg.dae │ │ └── trunk │ │ │ └── trunk.dae │ ├── robots │ │ └── hyq_no_sensors.urdf │ └── srdf │ │ └── hyq.srdf │ ├── icub_description │ ├── README.md │ ├── meshes │ │ └── upmc │ │ │ ├── collision │ │ │ ├── icub_simple_collision_chest.dae │ │ │ ├── icub_simple_collision_head.dae │ │ │ ├── icub_simple_collision_l_ankle_1.dae │ │ │ ├── icub_simple_collision_l_arm.dae │ │ │ ├── icub_simple_collision_l_foot.dae │ │ │ ├── icub_simple_collision_l_forearm.dae │ │ │ ├── icub_simple_collision_l_hand.dae │ │ │ ├── icub_simple_collision_l_hip_1.dae │ │ │ ├── icub_simple_collision_l_hip_2.dae │ │ │ ├── icub_simple_collision_l_shank.dae │ │ │ ├── icub_simple_collision_l_shoulder_1.dae │ │ │ ├── icub_simple_collision_l_shoulder_2.dae │ │ │ ├── icub_simple_collision_l_thigh.dae │ │ │ ├── icub_simple_collision_lap_belt_1.dae │ │ │ ├── icub_simple_collision_neck_1.dae │ │ │ ├── icub_simple_collision_neck_2.dae │ │ │ ├── icub_simple_collision_r_ankle_1.dae │ │ │ ├── icub_simple_collision_r_arm.dae │ │ │ ├── icub_simple_collision_r_foot.dae │ │ │ ├── icub_simple_collision_r_forearm.dae │ │ │ ├── icub_simple_collision_r_hand.dae │ │ │ ├── icub_simple_collision_r_hip_1.dae │ │ │ ├── icub_simple_collision_r_hip_2.dae │ │ │ ├── icub_simple_collision_r_shank.dae │ │ │ ├── icub_simple_collision_r_shoulder_1.dae │ │ │ ├── icub_simple_collision_r_shoulder_2.dae │ │ │ ├── icub_simple_collision_r_thigh.dae │ │ │ └── icub_simple_collision_root_link.dae │ │ │ └── visual │ │ │ ├── icub_chest.dae │ │ │ ├── icub_head.dae │ │ │ ├── icub_l_ankle_1.dae │ │ │ ├── icub_l_arm.dae │ │ │ ├── icub_l_foot.dae │ │ │ ├── icub_l_forearm.dae │ │ │ ├── icub_l_hand.dae │ │ │ ├── icub_l_hip_1.dae │ │ │ ├── icub_l_hip_2.dae │ │ │ ├── icub_l_shank.dae │ │ │ ├── icub_l_shoulder_1.dae │ │ │ ├── icub_l_shoulder_2.dae │ │ │ ├── icub_l_thigh.dae │ │ │ ├── icub_lap_belt_1.dae │ │ │ ├── icub_neck_1.dae │ │ │ ├── icub_neck_2.dae │ │ │ ├── icub_r_ankle_1.dae │ │ │ ├── icub_r_arm.dae │ │ │ ├── icub_r_foot.dae │ │ │ ├── icub_r_forearm.dae │ │ │ ├── icub_r_hand.dae │ │ │ ├── icub_r_hip_1.dae │ │ │ ├── icub_r_hip_2.dae │ │ │ ├── icub_r_shank.dae │ │ │ ├── icub_r_shoulder_1.dae │ │ │ ├── icub_r_shoulder_2.dae │ │ │ ├── icub_r_thigh.dae │ │ │ └── icub_root_link.dae │ ├── robots │ │ ├── icub.urdf │ │ └── icub_reduced.urdf │ └── srdf │ │ └── icub.srdf │ ├── kinova_description │ ├── README.md │ ├── meshes │ │ ├── arm.dae │ │ ├── base.dae │ │ ├── finger_distal.dae │ │ ├── finger_proximal.dae │ │ ├── forearm.dae │ │ ├── hand_2finger.dae │ │ ├── jaco_front_hatch_support_v2.dae │ │ ├── jaco_mounting_block.dae │ │ ├── ring_big.dae │ │ ├── ring_small.dae │ │ ├── shoulder.dae │ │ ├── wrist_spherical_1.dae │ │ └── wrist_spherical_2.dae │ ├── robots │ │ └── kinova.urdf │ └── srdf │ │ └── kinova.srdf │ ├── laikago_description │ ├── meshes │ │ ├── calf.mtl │ │ ├── calf.obj │ │ ├── hip.mtl │ │ ├── hip.obj │ │ ├── thigh.mtl │ │ ├── thigh.obj │ │ ├── thigh_mirror.mtl │ │ ├── thigh_mirror.obj │ │ ├── trunk.mtl │ │ ├── trunk.obj │ │ └── trunk_origin.mtl │ └── urdf │ │ └── laikago.urdf │ ├── panda_description │ ├── LICENSE │ ├── README.md │ ├── meshes │ │ ├── collision │ │ │ ├── finger.stl │ │ │ ├── hand.stl │ │ │ ├── link0.stl │ │ │ ├── link1.stl │ │ │ ├── link2.stl │ │ │ ├── link3.stl │ │ │ ├── link4.stl │ │ │ ├── link5.stl │ │ │ ├── link6.stl │ │ │ └── link7.stl │ │ └── visual │ │ │ ├── finger.dae │ │ │ ├── hand.dae │ │ │ ├── link0.dae │ │ │ ├── link1.dae │ │ │ ├── link2.dae │ │ │ ├── link3.dae │ │ │ ├── link4.dae │ │ │ ├── link5.dae │ │ │ ├── link6.dae │ │ │ └── link7.dae │ └── urdf │ │ └── panda.urdf │ └── talos_description │ ├── README.md │ ├── meshes │ ├── arm │ │ ├── arm_1.stl │ │ ├── arm_1_collision.STL │ │ ├── arm_2.stl │ │ ├── arm_2_collision.STL │ │ ├── arm_3.stl │ │ ├── arm_3_collision.STL │ │ ├── arm_4.stl │ │ ├── arm_4_collision.STL │ │ ├── arm_5.stl │ │ ├── arm_5_collision.STL │ │ ├── arm_6.stl │ │ ├── arm_6_collision.STL │ │ ├── arm_7.STL │ │ └── arm_7_collision.STL │ ├── gripper │ │ ├── base_link.STL │ │ ├── base_link_collision.STL │ │ ├── fingertip.STL │ │ ├── fingertip_collision.STL │ │ ├── gripper_motor_double.STL │ │ ├── gripper_motor_double_collision.STL │ │ ├── gripper_motor_single.STL │ │ ├── gripper_motor_single_collision.STL │ │ ├── inner_double.STL │ │ ├── inner_double_collision.STL │ │ ├── inner_single.STL │ │ └── inner_single_collision.STL │ ├── head │ │ ├── head_1.stl │ │ ├── head_1_collision.stl │ │ ├── head_2.stl │ │ └── head_2_collision.stl │ ├── sensors │ │ └── orbbec │ │ │ └── orbbec.STL │ ├── torso │ │ ├── base_link.STL │ │ ├── base_link_collision.STL │ │ ├── torso_1.STL │ │ ├── torso_2.STL │ │ └── torso_2_collision.STL │ └── v2 │ │ ├── ankle_X_collision.stl │ │ ├── ankle_X_lo_res.stl │ │ ├── ankle_Y_collision.stl │ │ ├── ankle_Y_lo_res.stl │ │ ├── hip_x_collision.stl │ │ ├── hip_x_lo_res.stl │ │ ├── hip_y_collision.stl │ │ ├── hip_y_lo_res.stl │ │ ├── hip_z_collision.stl │ │ ├── hip_z_lo_res.stl │ │ ├── knee_collision.stl │ │ └── knee_lo_res.stl │ ├── robots │ ├── talos_full_v2.urdf │ ├── talos_full_v2_box.urdf │ ├── talos_left_arm.urdf │ ├── talos_reduced.urdf │ ├── talos_reduced_box.urdf │ └── talos_reduced_corrected.urdf │ └── srdf │ └── talos.srdf ├── example ├── CMakeLists.txt ├── autodiff │ └── function.example.cpp ├── mpc │ ├── quadrotor.example.cpp │ ├── quadruped.example.cpp │ └── rc_car.example.cpp ├── rbd │ ├── quantity.example.cpp │ └── robot.example.cpp ├── variable.example.cpp └── variable_map.example.cpp ├── external ├── CMakeLists.txt └── config │ ├── CppAD │ └── CMakeLists.txt.in │ ├── CppADCodeGen │ └── CMakeLists.txt.in │ ├── abseil-cpp │ └── CMakeLists.txt.in │ ├── boost │ └── CMakeLists.txt.in │ ├── console_bridge │ └── CMakeLists.txt.in │ ├── eigen │ ├── CMakeLists.txt.in │ └── eigen-3.4.0.zip │ ├── finite-diff │ └── CMakeLists.txt.in │ ├── googletest │ └── CMakeLists.txt.in │ ├── hana │ ├── CMakeLists.txt.in │ └── hana-boost-1.84.0.zip │ ├── osqp-cpp │ └── CMakeLists.txt.in │ ├── osqp │ └── CMakeLists.txt.in │ ├── pinocchio │ └── CMakeLists.txt.in │ ├── preprocessor │ ├── CMakeLists.txt.in │ └── preprocessor-1.84.0-ungar.zip │ ├── spdlog │ └── CMakeLists.txt.in │ ├── tinyxml2 │ └── CMakeLists.txt.in │ ├── urdfdom │ └── CMakeLists.txt.in │ └── urdfdom_headers │ └── CMakeLists.txt.in ├── include └── ungar │ ├── assert.hpp │ ├── autodiff │ ├── data_types.hpp │ ├── function.hpp │ ├── support │ │ └── quaternion.hpp │ └── vector_composer.hpp │ ├── data_types.hpp │ ├── io │ └── logging.hpp │ ├── mvariable.hpp │ ├── mvariable_lazy_map.hpp │ ├── optimization │ ├── backtracking_line_search.hpp │ ├── concepts.hpp │ ├── soft_equality_constraint.hpp │ ├── soft_inequality_constraint.hpp │ └── soft_sqp.hpp │ ├── rbd │ ├── evaluator.hpp │ ├── getter.hpp │ ├── quantities │ │ ├── centroidal_momentum.hpp │ │ ├── centroidal_momentum_matrix.hpp │ │ ├── com_acceleration.hpp │ │ ├── com_position.hpp │ │ ├── com_velocity.hpp │ │ ├── composite_rigid_body_inertia.hpp │ │ ├── frames.hpp │ │ ├── generalized_accelerations.hpp │ │ ├── generalized_gravity.hpp │ │ ├── joint_space_inertia_matrix.hpp │ │ ├── joint_space_inertia_matrix_inverse.hpp │ │ ├── joint_torques.hpp │ │ ├── kinetic_energy.hpp │ │ ├── nonlinear_effects.hpp │ │ ├── potential_energy.hpp │ │ └── quantities.hpp │ ├── quantity.hpp │ └── robot.hpp │ ├── utils │ ├── defaulted.hpp │ ├── integral_map.hpp │ ├── macros │ │ └── for_each.hpp │ ├── passkey.hpp │ └── utils.hpp │ ├── variable.hpp │ ├── variable_lazy_map.hpp │ └── variable_map.hpp └── test ├── CMakeLists.txt ├── autodiff └── function.test.cpp ├── optimization └── soft_sqp.test.cpp ├── rbd └── robot.test.cpp ├── utils └── utils.test.cpp └── variable.test.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: Google 4 | AccessModifierOffset: -2 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveAssignments: true 7 | AlignConsecutiveDeclarations: false 8 | AlignEscapedNewlinesLeft: true 9 | AlignOperands: true 10 | AlignTrailingComments: true 11 | AllowAllParametersOfDeclarationOnNextLine: true 12 | AllowShortBlocksOnASingleLine: false 13 | AllowShortCaseLabelsOnASingleLine: false 14 | AllowShortFunctionsOnASingleLine: None 15 | AllowShortIfStatementsOnASingleLine: true 16 | AllowShortLoopsOnASingleLine: true 17 | AlwaysBreakAfterDefinitionReturnType: None 18 | AlwaysBreakBeforeMultilineStrings: true 19 | AlwaysBreakTemplateDeclarations: true 20 | BinPackArguments: false 21 | BinPackParameters: false 22 | BraceWrapping: 23 | AfterClass: false 24 | AfterControlStatement: false 25 | AfterEnum: false 26 | AfterFunction: false 27 | AfterNamespace: false 28 | AfterObjCDeclaration: false 29 | AfterStruct: false 30 | AfterUnion: false 31 | BeforeCatch: false 32 | BeforeElse: false 33 | IndentBraces: false 34 | BreakBeforeBinaryOperators: None 35 | BreakBeforeBraces: Attach 36 | BreakBeforeTernaryOperators: true 37 | BreakConstructorInitializersBeforeComma: false 38 | ColumnLimit: 100 39 | CommentPragmas: '(.+\n*.+)*' 40 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 41 | ConstructorInitializerIndentWidth: 4 42 | ContinuationIndentWidth: 4 43 | Cpp11BracedListStyle: true 44 | DerivePointerAlignment: false 45 | DisableFormat: false 46 | ExperimentalAutoDetectBinPacking: false 47 | ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] 48 | IncludeCategories: 49 | - Regex: '^<.*\.h>' 50 | Priority: 2 51 | - Regex: '^<.*' 52 | Priority: 1 53 | - Regex: '.*' 54 | Priority: 3 55 | IndentCaseLabels: true 56 | IndentWidth: 4 57 | IndentWrappedFunctionNames: false 58 | KeepEmptyLinesAtTheStartOfBlocks: true 59 | MacroBlockBegin: '' 60 | MacroBlockEnd: '' 61 | MaxEmptyLinesToKeep: 1 62 | NamespaceIndentation: None 63 | ObjCBlockIndentWidth: 2 64 | ObjCSpaceAfterProperty: false 65 | ObjCSpaceBeforeProtocolList: false 66 | PenaltyBreakBeforeFirstCallParameter: 1 67 | PenaltyBreakComment: 300 68 | PenaltyBreakFirstLessLess: 120 69 | PenaltyBreakString: 1000 70 | PenaltyExcessCharacter: 1000000 71 | PenaltyReturnTypeOnItsOwnLine: 200 72 | SortIncludes: true 73 | PointerAlignment: Left 74 | SpaceAfterCStyleCast: false 75 | SpaceBeforeAssignmentOperators: true 76 | SpaceBeforeParens: ControlStatements 77 | SpaceInEmptyParentheses: false 78 | SpacesBeforeTrailingComments: 2 79 | SpacesInAngles: false 80 | SpacesInContainerLiterals: true 81 | SpacesInCStyleCastParentheses: false 82 | SpacesInParentheses: false 83 | SpacesInSquareBrackets: false 84 | Standard: Auto 85 | TabWidth: 4 86 | UseTab: Never 87 | ... 88 | -------------------------------------------------------------------------------- /.github/workflows/ungar-ci.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: 4 | push: 5 | pull_request: 6 | 7 | env: 8 | BUILD_TYPE: Release 9 | 10 | jobs: 11 | ci: 12 | strategy: 13 | fail-fast: false 14 | matrix: 15 | os: [ ubuntu-latest ] 16 | compiler: [ gcc-11, gcc-13 ] 17 | 18 | runs-on: ${{ matrix.os }} 19 | steps: 20 | - name: Check out Ungar 21 | uses: actions/checkout@v3 22 | 23 | - name: Install compiler 24 | id: install_compiler 25 | uses: rlalik/setup-cpp-compiler@v1.2 26 | with: 27 | compiler: ${{ matrix.compiler }} 28 | 29 | - name: Configure CMake 30 | run: cmake -DUNGAR_BUILD_TESTS=ON 31 | -DUNGAR_BUILD_EXAMPLES=ON 32 | -DUNGAR_ENABLE_LOGGING=ON 33 | -DUNGAR_ENABLE_AUTODIFF=ON 34 | -DUNGAR_ENABLE_OPTIMIZATION=ON 35 | -B ${{github.workspace}}/build 36 | -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} 37 | -DCMAKE_C_COMPILER=${{ steps.install_compiler.outputs.cc }} 38 | -DCMAKE_CXX_COMPILER=${{ steps.install_compiler.outputs.cxx }} 39 | 40 | - name: Build 41 | run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} 42 | 43 | - name: Test 44 | working-directory: ${{github.workspace}}/build 45 | run: ctest -C ${{env.BUILD_TYPE}} 46 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | /build/ 2 | /.cache/ 3 | /.vscode/ 4 | -------------------------------------------------------------------------------- /UngarConfig.cmake.in: -------------------------------------------------------------------------------- 1 | include(CMakeFindDependencyMacro) 2 | 3 | @PACKAGE_INIT@ 4 | 5 | # ############################################################################## 6 | # Core modules. 7 | # ############################################################################## 8 | # Eigen. 9 | find_dependency(Eigen3 3.4.0 REQUIRED NO_MODULE) 10 | # Hana. 11 | find_dependency(Hana REQUIRED) 12 | # Preprocessor. 13 | find_dependency(Preprocessor REQUIRED) 14 | 15 | # ############################################################################## 16 | # Optional modules. 17 | # ############################################################################## 18 | if(@UNGAR_ENABLE_LOGGING@) 19 | # spdlog. 20 | find_dependency(spdlog REQUIRED) 21 | endif() 22 | 23 | if(@UNGAR_ENABLE_AUTODIFF@) 24 | # FiniteDiff. 25 | find_dependency(FiniteDiff REQUIRED) 26 | 27 | # CppAD. 28 | find_library(CppAD_LIBRARY cppad_lib REQUIRED) 29 | find_path(CppAD_INCLUDE_DIR cppad/cppad.hpp REQUIRED) 30 | 31 | add_library(CppAD SHARED IMPORTED GLOBAL) 32 | add_library(CppAD::CppAD ALIAS CppAD) 33 | target_include_directories(CppAD INTERFACE ${CppAD_INCLUDE_DIR}) 34 | 35 | get_filename_component(CppAD_LIBRARY_NAME ${CppAD_LIBRARY} NAME) 36 | get_filename_component(CppAD_LIBRARY_REAL_PATH ${CppAD_LIBRARY} REALPATH) 37 | 38 | set_target_properties(CppAD PROPERTIES IMPORTED_SONAME ${CppAD_LIBRARY_NAME}) 39 | set_target_properties(CppAD PROPERTIES IMPORTED_LOCATION 40 | ${CppAD_LIBRARY_REAL_PATH}) 41 | 42 | # CppADCodeGen. 43 | find_path(CppADCodeGen_INCLUDE_DIR cppad/cg.hpp REQUIRED) 44 | 45 | add_library(CppADCodeGen INTERFACE IMPORTED GLOBAL) 46 | add_library(CppADCodeGen::CppADCodeGen ALIAS CppADCodeGen) 47 | target_include_directories(CppADCodeGen INTERFACE ${CppADCodeGen_INCLUDE_DIR}) 48 | endif() 49 | 50 | if(@UNGAR_ENABLE_OPTIMIZATION@) 51 | # Abseil. 52 | find_dependency(absl REQUIRED) 53 | # OSQP. 54 | find_dependency(osqp REQUIRED) 55 | # osqp-cpp. 56 | find_dependency(osqp-cpp REQUIRED) 57 | endif() 58 | 59 | if(@UNGAR_ENABLE_PINOCCHIO@) 60 | # Abseil. 61 | find_dependency(pinocchio REQUIRED) 62 | endif() 63 | 64 | include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") 65 | -------------------------------------------------------------------------------- /data/robots/a1_description/README.md: -------------------------------------------------------------------------------- 1 | # Unitree A1 2 | 3 | upstream: https://github.com/unitreerobotics/unitree_ros/tree/master/robots/a1_description 4 | license: MPL-2.0 5 | 6 | 7 | ### Modifications: 8 | 9 | - Removed Gazebo and transmission tags 10 | - Added SRDF 11 | -------------------------------------------------------------------------------- /data/robots/a1_description/meshes/trunk_A1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/a1_description/meshes/trunk_A1.png -------------------------------------------------------------------------------- /data/robots/anymal_b_description/README.md: -------------------------------------------------------------------------------- 1 | # ANYmal B Robot Description 2 | 3 | upstream: https://github.com/ANYbotics/anymal_b_simple_description 4 | license: BSD-3-Clause 5 | -------------------------------------------------------------------------------- /data/robots/anymal_b_description/meshes/base/base_uv_texture.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/anymal_b_description/meshes/base/base_uv_texture.jpg -------------------------------------------------------------------------------- /data/robots/anymal_b_description/meshes/foot/carbon_uv_texture.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/anymal_b_description/meshes/foot/carbon_uv_texture.jpg -------------------------------------------------------------------------------- /data/robots/cassie_description/meshes/achilles-rod.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/cassie_description/meshes/achilles-rod.stl -------------------------------------------------------------------------------- /data/robots/cassie_description/meshes/foot-crank.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/cassie_description/meshes/foot-crank.stl -------------------------------------------------------------------------------- /data/robots/cassie_description/meshes/foot.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/cassie_description/meshes/foot.stl -------------------------------------------------------------------------------- /data/robots/cassie_description/meshes/heel-spring.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/cassie_description/meshes/heel-spring.stl -------------------------------------------------------------------------------- /data/robots/cassie_description/meshes/hip-pitch.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/cassie_description/meshes/hip-pitch.stl -------------------------------------------------------------------------------- /data/robots/cassie_description/meshes/hip-roll.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/cassie_description/meshes/hip-roll.stl -------------------------------------------------------------------------------- /data/robots/cassie_description/meshes/hip-yaw.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/cassie_description/meshes/hip-yaw.stl -------------------------------------------------------------------------------- /data/robots/cassie_description/meshes/knee-spring.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/cassie_description/meshes/knee-spring.stl -------------------------------------------------------------------------------- /data/robots/cassie_description/meshes/knee.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/cassie_description/meshes/knee.stl -------------------------------------------------------------------------------- /data/robots/cassie_description/meshes/pelvis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/cassie_description/meshes/pelvis.stl -------------------------------------------------------------------------------- /data/robots/cassie_description/meshes/plantar-rod.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/cassie_description/meshes/plantar-rod.stl -------------------------------------------------------------------------------- /data/robots/cassie_description/meshes/shin.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/cassie_description/meshes/shin.stl -------------------------------------------------------------------------------- /data/robots/cassie_description/meshes/tarsus.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/cassie_description/meshes/tarsus.stl -------------------------------------------------------------------------------- /data/robots/cassie_description/robots/cassie.world: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 0.0005 22 | 2000 23 | 24 | 25 | model://sun 26 | 27 | 28 | model://ground_plane 29 | 30 | 31 | model://cassie 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /data/robots/cassie_description/robots/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Cassie 5 | 1.0 6 | cassie.sdf 7 | 8 | -------------------------------------------------------------------------------- /data/robots/cassie_description/srdf/cassie_v2.srdf: -------------------------------------------------------------------------------- 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 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | -------------------------------------------------------------------------------- /data/robots/hyq_description/README.md: -------------------------------------------------------------------------------- 1 | # HyQ Description 2 | 3 | upstream: https://github.com/iit-DLSLab/hyq-description 4 | license: Apache License 2.0 5 | -------------------------------------------------------------------------------- /data/robots/icub_description/README.md: -------------------------------------------------------------------------------- 1 | # iCub models 2 | 3 | upstream: https://github.com/robotology/icub-models/tree/master/iCub 4 | license: CC-BY-SA-4.0 5 | -------------------------------------------------------------------------------- /data/robots/icub_description/meshes/upmc/collision/icub_simple_collision_l_ankle_1.dae: -------------------------------------------------------------------------------- 1 | 2 | 2013-11-15T10:34:58.3384482013-11-15T10:34:58.338453Y_UP 3 | 4 | 5 | 6 | 0.0 0.0 0.0 1.00.0 0.0 0.0 1.00.5696 0.5696 0.5696 1.00.11 0.11 0.11 1.0235.01.01.0 7 | 8 | 9 | 10 | 1 11 | 12 | 13 | 14 | 1 15 | 16 | 17 | 18 | 19 | 20 | -0.009375691 -0.02263504 -0.03149998 0.009375691 -0.02263504 -0.03149998 0.02263504 0.009375691 -0.03149998 0.00937581 0.02263498 0.03149998 0.009375691 -0.02263504 0.03149998 0.02263504 0.009375691 0.03149998 0.02263504 -0.009375691 -0.03149998 -0.02263504 -0.009375751 -0.03149998 -0.02263504 0.009375691 -0.03149998 -0.009375691 0.02263504 -0.03149998 0.009375751 0.02263504 -0.03149998 0.02263504 -0.009375751 0.03149998 -0.009375751 -0.02263498 0.03149998 -0.02263504 -0.009375691 0.03149998 -0.02263498 0.009375751 0.03149998 -0.009375631 0.02263504 0.03149998 21 | 22 | 23 | 24 | 25 | 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0.7172765 0.2970977 0.6302378 0.7144383 -0.2959074 0.6339915 0.7172765 -0.2970977 -0.6302378 0.7144383 0.2959074 -0.6339915 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0.2959074 -0.7144383 0.6339915 0.2970977 -0.7172765 -0.6302378 -0.7172765 0.2970977 0.6302378 -0.7144383 0.2959074 -0.6339915 -0.7116306 -0.2947478 -0.6376842 -0.7172765 -0.2970977 0.6302378 -0.2947478 -0.7116306 -0.6376842 -0.2970977 -0.7172765 0.6302378 0.2947478 0.7116306 -0.6376842 0.2970977 0.7172765 0.6302378 -0.2970977 0.7172765 0.6302378 -0.2959074 0.7144383 -0.6339915 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 3 3 3 3 3 3 4 3 3 3 3 3 3 4 4 4 4 4 4 4 36 |

9 0 10 0 8 0 10 1 2 1 8 1 2 2 7 2 8 2 2 3 6 3 7 3 6 4 1 4 7 4 1 5 0 5 7 5 5 6 11 7 6 8 2 9 14 10 3 10 15 10 14 11 5 11 3 11 14 12 13 12 5 12 13 13 11 13 5 13 13 14 12 14 11 14 12 15 4 15 11 15 11 7 4 16 1 17 6 8 14 18 8 19 7 20 13 21 0 22 12 23 13 21 7 20 10 24 3 25 5 6 2 9 15 26 9 27 8 19 14 18 3 25 10 24 9 27 15 26 12 23 0 22 1 17 4 16

37 | 38 | 39 | 1 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /data/robots/icub_description/meshes/upmc/collision/icub_simple_collision_l_hip_1.dae: -------------------------------------------------------------------------------- 1 | 2 | 2013-11-15T10:34:57.2131462013-11-15T10:34:57.213153Y_UP 3 | 4 | 5 | 6 | 0.0 0.0 0.0 1.00.0 0.0 0.0 1.00.48 0.48 0.48 1.00.5 0.5 0.5 1.050.01.01.0 7 | 8 | 9 | 10 | 1 11 | 12 | 13 | 14 | 1 15 | 16 | 17 | 18 | 19 | 20 | -0.01454192 0.0351535 0.03100006 -0.03510737 0.01456099 0.03100006 0.01454192 -0.0351535 0.03100006 0.03510737 -0.01456099 0.03100006 0.03510737 0.01456099 0.03100006 -0.03510743 -0.01456093 0.04399995 -0.01454204 -0.0351535 0.04399995 -0.01454192 -0.0351535 0.03100006 -0.03510737 -0.01456105 0.03100006 0.01454198 0.0351535 0.03100006 0.03510743 0.01456099 0.04399995 0.03510737 -0.01456105 0.04399995 0.01454186 -0.03515356 0.04399995 -0.03510737 0.01456111 0.04399995 -0.01454186 0.03515356 0.04399995 0.01454204 0.0351535 0.04399995 21 | 22 | 23 | 24 | 25 | -0.2891934 -0.6950591 0.6581927 -0.2973418 -0.7172155 -0.6302073 0.2973418 -0.7172155 -0.6302073 0.2891934 -0.6950591 0.6581927 -0.6959136 -0.286935 0.6582537 -0.6959136 0.286935 0.6582537 -0.7173376 0.2968536 -0.6302988 -0.7173376 -0.2968536 -0.6302988 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0.7173376 0.2968536 -0.6302988 0.2973418 0.7172155 -0.6302073 0.2891934 0.6950591 0.6581927 0.6959136 0.286935 0.6582537 -0.2973418 0.7172155 -0.6302073 -0.2891934 0.6950591 0.6581927 0.6959136 -0.286935 0.6582537 0.7173376 -0.2968536 -0.6302988 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 4 4 3 3 3 3 3 3 3 3 3 3 3 3 4 4 4 4 4 4 36 |

6 0 7 1 2 2 12 3 5 4 13 5 1 6 8 7 1 8 0 8 9 8 1 9 9 9 4 9 8 10 1 10 4 10 8 11 4 11 3 11 8 12 3 12 2 12 8 13 2 13 7 13 15 14 14 14 13 14 10 15 15 15 13 15 10 16 13 16 5 16 11 17 10 17 5 17 6 18 11 18 5 18 6 19 12 19 11 19 5 4 8 7 7 1 6 0 4 20 9 21 15 22 10 23 0 24 14 25 15 22 9 21 14 25 0 24 1 6 13 5 10 23 11 26 3 27 4 20 12 3 2 2 3 27 11 26

37 | 38 | 39 | 1 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /data/robots/icub_description/meshes/upmc/collision/icub_simple_collision_l_hip_2.dae: -------------------------------------------------------------------------------- 1 | 2 | 2013-11-15T10:34:57.3955982013-11-15T10:34:57.395603Y_UP 3 | 4 | 5 | 6 | 0.0 0.0 0.0 1.00.0 0.0 0.0 1.00.48 0.48 0.48 1.00.5 0.5 0.5 1.050.01.01.0 7 | 8 | 9 | 10 | 1 11 | 12 | 13 | 14 | 1 15 | 16 | 17 | 18 | 19 | 20 | 0.01186317 0.02864021 -0.03749996 -0.02864027 0.01186317 -0.03749996 0.01186317 -0.02864027 -0.03749996 0.01186323 0.02864021 0.03749996 0.01186311 -0.02864027 0.03749996 0.02864027 0.01186311 0.03749996 0.02864027 0.01186317 -0.03749996 0.02864027 -0.01186317 -0.03749996 -0.01186317 -0.02864027 -0.03749996 -0.02864027 -0.01186317 -0.03749996 -0.01186317 0.02864027 -0.03749996 0.02864021 -0.01186317 0.03749996 -0.01186323 -0.02864021 0.03749996 -0.02864027 -0.01186311 0.03749996 -0.02864021 0.01186323 0.03749996 -0.01186305 0.02864027 0.03749996 21 | 22 | 23 | 24 | 25 | 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0.2970977 0.7172765 0.6302378 0.2945952 0.7112644 -0.6381726 -0.2958769 0.7142552 -0.6342357 -0.2970977 0.7172765 0.6302378 -0.7112644 0.2945952 -0.6381726 -0.7172765 0.2970977 0.6302378 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 -0.7172765 -0.2970977 0.6302378 -0.7112644 -0.2945952 -0.6381726 -0.2945952 -0.7112644 -0.6381726 -0.2970977 -0.7172765 0.6302378 0.7172765 0.2970977 0.6302378 0.7172765 -0.2970977 0.6302378 0.7112644 -0.2945952 -0.6381726 0.7112644 0.2945952 -0.6381726 0.2970977 -0.7172765 0.6302378 0.2945952 -0.7112644 -0.6381726 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 3 3 3 3 3 3 4 4 3 3 3 3 3 3 4 4 4 4 4 4 36 |

10 0 0 0 1 0 1 1 0 1 6 1 9 2 1 2 6 2 9 3 6 3 7 3 9 4 7 4 8 4 7 5 2 5 8 5 3 6 0 7 10 8 15 9 15 9 10 8 1 10 14 11 3 12 15 12 14 12 5 13 3 13 14 13 5 14 14 14 13 14 5 15 13 15 11 15 13 16 12 16 11 16 12 17 4 17 11 17 13 18 9 19 8 20 12 21 5 22 11 23 7 24 6 25 11 23 4 26 2 27 7 24 12 21 8 20 2 27 4 26 14 11 1 10 9 19 13 18 6 25 0 7 3 6 5 22

37 | 38 | 39 | 1 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /data/robots/icub_description/meshes/upmc/collision/icub_simple_collision_l_shoulder_1.dae: -------------------------------------------------------------------------------- 1 | 2 | 2013-11-15T10:34:59.5149472013-11-15T10:34:59.514955Y_UP 3 | 4 | 5 | 6 | 0.0 0.0 0.0 1.00.0 0.0 0.0 1.00.48 0.48 0.48 1.00.5 0.5 0.5 1.050.01.01.0 7 | 8 | 9 | 10 | 1 11 | 12 | 13 | 14 | 1 15 | 16 | 17 | 18 | 19 | 20 | 0.01186317 0.02864021 0.0667401 -0.01186317 0.02864027 0.0667401 -0.02864027 0.01186317 0.0667401 -0.02864021 -0.01186317 0.0667401 0.01186317 -0.02864027 0.0667401 0.02864027 -0.01186317 0.0667401 0.02864027 0.01186317 0.0667401 -0.01186317 -0.02864027 0.0667401 0.02864027 0.01186311 0.0777399 0.02864021 -0.01186317 0.0777399 0.01186311 -0.02864027 0.0777399 -0.01186323 -0.02864021 0.0777399 -0.02864027 -0.01186311 0.0777399 -0.02864021 0.01186323 0.0777399 -0.01186305 0.02864027 0.0777399 0.01186323 0.02864021 0.0777399 21 | 22 | 23 | 24 | 25 | -0.2970977 0.7172765 0.6302378 -0.2892239 0.6959746 -0.6572161 -0.7172765 0.2970977 -0.6302378 -0.6959746 0.2892239 0.6572161 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 -0.7172765 -0.2970977 0.6302378 -0.696646 -0.2875759 -0.6572161 -0.2798547 -0.6756493 -0.6819971 -0.2970977 -0.7172765 0.6302378 0.2970977 -0.7172765 0.6302378 0.2798547 -0.6756493 -0.6819971 0.6756493 -0.2798547 -0.6819971 0.7172765 -0.2970977 0.6302378 0.6756493 0.2798547 -0.6819971 0.7172765 0.2970977 0.6302378 0.2970977 0.7172765 0.6302378 0.2798547 0.6756493 -0.6819971 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 4 3 3 3 3 3 3 3 3 3 3 3 3 4 4 4 4 4 4 4 36 |

14 0 1 1 2 2 13 3 14 4 13 4 15 4 13 5 8 5 15 5 13 6 12 6 8 6 12 7 9 7 8 7 12 8 11 8 9 8 11 9 10 9 9 9 1 10 0 10 2 10 2 11 0 11 6 11 3 12 2 12 6 12 5 13 3 13 6 13 5 14 7 14 3 14 5 15 4 15 7 15 12 16 3 17 7 18 11 19 10 20 4 21 5 22 9 23 13 3 2 2 3 17 12 16 9 23 5 22 6 24 8 25 15 26 0 27 1 1 14 0 11 19 7 18 4 21 10 20 0 27 15 26 8 25 6 24

37 | 38 | 39 | 1 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /data/robots/icub_description/meshes/upmc/collision/icub_simple_collision_l_shoulder_2.dae: -------------------------------------------------------------------------------- 1 | 2 | 2013-11-15T10:34:59.3308832013-11-15T10:34:59.330894Y_UP 3 | 4 | 5 | 0 0.02999997 -0.02949988 -0.02121317 0.02121317 -0.02949988 -0.02999997 0 -0.02949988 0.02121317 -0.02121317 -0.02949988 0.02121317 0.02121317 -0.02949988 0 0.02999997 0.02949988 -0.02121311 0.02121323 0.02949988 0.02999997 0 0.02949988 0.02121317 0.02121317 0.02949988 0.02999997 0 -0.02949988 0 -0.02999997 -0.02949988 -0.02121317 -0.02121317 -0.02949988 0.02121317 -0.02121323 0.02949988 0 -0.02999997 0.02949988 -0.02121323 -0.02121311 0.02949988 -0.02999997 0 0.02949988 6 | 7 | 8 | 9 | 10 | 0 0.7763603 0.6302378 0 0.7683645 -0.6399732 -0.543321 0.543321 -0.6399732 -0.548967 0.548967 0.6302378 -0.7763603 0 0.6302378 -0.7683645 0 -0.6399732 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0.548967 0.548967 0.6302378 0.543321 0.543321 -0.6399732 -0.548967 -0.548967 0.6302378 -0.543321 -0.543321 -0.6399732 0 -0.7683645 -0.6399732 0 -0.7763603 0.6302378 0.548967 -0.548967 0.6302378 0.5461593 -0.5461288 -0.6351512 0.7723624 0 -0.6351512 0.7763603 0 0.6302378 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 4 4 3 3 3 3 3 3 3 3 3 3 3 3 4 4 4 4 4 4 21 |

5 0 0 1 1 2 6 3 15 4 6 3 1 2 2 5 1 6 0 6 4 6 4 7 2 7 1 7 4 8 9 8 2 8 9 9 11 9 2 9 9 10 3 10 11 10 3 11 10 11 11 11 8 12 5 12 6 12 8 13 6 13 15 13 7 14 8 14 15 14 14 15 7 15 15 15 14 16 12 16 7 16 14 17 13 17 12 17 0 1 5 0 8 18 4 19 14 20 11 21 10 22 13 23 12 24 3 25 9 26 7 27 14 20 15 4 2 5 11 21 7 27 9 26 4 19 8 18 13 23 10 22 3 25 12 24

22 | 23 | 24 | 1 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /data/robots/icub_description/meshes/upmc/collision/icub_simple_collision_neck_1.dae: -------------------------------------------------------------------------------- 1 | 2 | 2013-11-15T10:34:57.5810202013-11-15T10:34:57.581030Y_UP 3 | 4 | 5 | 6 | 0.0 0.0 0.0 1.00.0 0.0 0.0 1.00.48 0.48 0.48 1.00.5 0.5 0.5 1.050.01.01.0 7 | 8 | 9 | 10 | 1 11 | 12 | 13 | 14 | 1 15 | 16 | 17 | 18 | 19 | 20 | 0 0.01499992 -0.03849995 -0.01060652 0.01060652 -0.03849995 0 -0.01499992 -0.03849995 0.01499992 0 -0.03849995 0.01060652 0.01060652 -0.03849995 0 0.01499992 0.03849995 -0.01060646 0.01060652 0.03849995 0.01499992 0 0.03849995 0.01060652 0.01060652 0.03849995 0.01060652 -0.01060652 -0.03849995 -0.01060652 -0.01060652 -0.03849995 -0.01499992 0 -0.03849995 0.01060652 -0.01060652 0.03849995 0 -0.01499992 0.03849995 -0.01060652 -0.01060646 0.03849995 -0.01499992 0 0.03849995 21 | 22 | 23 | 24 | 25 | 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0.548967 -0.548967 0.6302378 0.5468001 -0.5468001 -0.6339915 0.7733085 0 -0.6339915 0.7763603 0 0.6302378 0 -0.7748345 0.6321299 0 -0.7763603 -0.6302378 0 0.7763603 -0.6302378 0 0.7748345 0.6321299 0.548967 0.548967 0.6302378 0.5468001 0.5468001 -0.6339915 -0.5478988 -0.5478988 0.6321299 -0.548967 -0.548967 -0.6302378 -0.548967 0.548967 -0.6302378 -0.5478988 0.5478988 0.6321299 -0.7763603 0 0.6302378 -0.7733085 0 -0.6339915 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 3 3 3 3 3 3 3 3 3 3 3 3 4 4 4 4 4 4 4 4 36 |

8 0 5 0 6 0 15 1 8 1 6 1 15 2 7 2 8 2 15 3 14 3 7 3 14 4 12 4 7 4 14 5 13 5 12 5 0 6 4 6 1 6 4 7 11 7 1 7 4 8 3 8 11 8 3 9 10 9 11 9 3 10 9 10 10 10 9 11 2 11 10 11 12 12 9 13 3 14 7 15 13 16 2 17 9 13 12 12 0 18 5 19 8 20 4 21 13 16 14 22 10 23 2 17 5 19 0 18 1 24 6 25 7 15 3 14 4 21 8 20 15 26 11 27 10 23 14 22 15 26 6 25 1 24 11 27

37 | 38 | 39 | 1 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /data/robots/icub_description/meshes/upmc/collision/icub_simple_collision_neck_2.dae: -------------------------------------------------------------------------------- 1 | 2 | 2013-11-15T10:34:57.0299072013-11-15T10:34:57.029915Y_UP 3 | 4 | 5 | 6 | 0.0 0.0 0.0 1.00.0 0.0 0.0 1.00.48 0.48 0.48 1.00.5 0.5 0.5 1.050.01.01.0 7 | 8 | 9 | 10 | 1 11 | 12 | 13 | 14 | 1 15 | 16 | 17 | 18 | 19 | 20 | 0 0.01499992 -0.01649999 -0.01060652 0.01060652 -0.01649999 0 -0.01499992 -0.01649999 0 0.01499992 0.01649999 -0.01060646 0.01060652 0.01649999 0 -0.01499992 0.01649999 0.01499992 0 0.01649999 0.01060652 0.01060652 0.01649999 0.01060652 0.01060652 -0.01649999 0.01499992 0 -0.01649999 0.01060652 -0.01060652 -0.01649999 -0.01060652 -0.01060652 -0.01649999 -0.01499992 0 -0.01649999 0.01060652 -0.01060652 0.01649999 -0.01060652 -0.01060646 0.01649999 -0.01499992 0 0.01649999 21 | 22 | 23 | 24 | 25 | 0.7763603 0 0.6302378 0.7692191 0 -0.6389355 0.5439314 0.5439314 -0.6389355 0.548967 0.548967 0.6302378 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0.548967 -0.548967 0.6302378 0.5439314 -0.5439314 -0.6389355 0 -0.7727897 0.6346324 0 -0.7763603 -0.6302378 0 0.7763603 -0.6302378 0 0.7727897 0.6346324 -0.5464645 -0.5464339 0.6346324 -0.548967 -0.548967 -0.6302378 -0.548967 0.548967 -0.6302378 -0.5464645 0.5464339 0.6346324 -0.7763603 0 0.6302378 -0.7692191 0 -0.6389355 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 4 3 3 3 3 3 3 3 3 3 3 3 3 4 4 4 4 4 4 4 36 |

6 0 9 1 8 2 7 3 7 4 3 4 4 4 7 5 4 5 15 5 6 6 7 6 15 6 14 7 6 7 15 7 14 8 13 8 6 8 14 9 5 9 13 9 1 10 0 10 8 10 8 11 12 11 1 11 8 12 9 12 12 12 9 13 11 13 12 13 9 14 10 14 11 14 10 15 2 15 11 15 13 16 10 17 9 1 6 0 5 18 2 19 10 17 13 16 0 20 3 21 7 3 8 2 5 18 14 22 11 23 2 19 3 21 0 20 1 24 4 25 14 22 15 26 12 27 11 23 15 26 4 25 1 24 12 27

37 | 38 | 39 | 1 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /data/robots/icub_description/meshes/upmc/collision/icub_simple_collision_r_ankle_1.dae: -------------------------------------------------------------------------------- 1 | 2 | 2013-11-15T10:35:00.7033932013-11-15T10:35:00.703400Y_UP 3 | 4 | 5 | 6 | 0.0 0.0 0.0 1.00.0 0.0 0.0 1.00.5696 0.5696 0.5696 1.00.11 0.11 0.11 1.0235.01.01.0 7 | 8 | 9 | 10 | 1 11 | 12 | 13 | 14 | 1 15 | 16 | 17 | 18 | 19 | 20 | -0.009375691 -0.02263504 -0.03149998 0.009375691 -0.02263504 -0.03149998 0.02263504 0.009375691 -0.03149998 0.00937581 0.02263498 0.03149998 0.009375691 -0.02263504 0.03149998 0.02263504 0.009375691 0.03149998 0.02263504 -0.009375691 -0.03149998 -0.02263504 -0.009375751 -0.03149998 -0.02263504 0.009375691 -0.03149998 -0.009375691 0.02263504 -0.03149998 0.009375751 0.02263504 -0.03149998 0.02263504 -0.009375751 0.03149998 -0.009375751 -0.02263498 0.03149998 -0.02263504 -0.009375691 0.03149998 -0.02263498 0.009375751 0.03149998 -0.009375631 0.02263504 0.03149998 21 | 22 | 23 | 24 | 25 | 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0.7172765 0.2970977 0.6302378 0.7144383 -0.2959074 0.6339915 0.7172765 -0.2970977 -0.6302378 0.7144383 0.2959074 -0.6339915 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0.2959074 -0.7144383 0.6339915 0.2970977 -0.7172765 -0.6302378 -0.7172765 0.2970977 0.6302378 -0.7144383 0.2959074 -0.6339915 -0.7116306 -0.2947478 -0.6376842 -0.7172765 -0.2970977 0.6302378 -0.2947478 -0.7116306 -0.6376842 -0.2970977 -0.7172765 0.6302378 0.2947478 0.7116306 -0.6376842 0.2970977 0.7172765 0.6302378 -0.2970977 0.7172765 0.6302378 -0.2959074 0.7144383 -0.6339915 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 3 3 3 3 3 3 4 3 3 3 3 3 3 4 4 4 4 4 4 4 36 |

9 0 10 0 8 0 10 1 2 1 8 1 2 2 7 2 8 2 2 3 6 3 7 3 6 4 1 4 7 4 1 5 0 5 7 5 5 6 11 7 6 8 2 9 14 10 3 10 15 10 14 11 5 11 3 11 14 12 13 12 5 12 13 13 11 13 5 13 13 14 12 14 11 14 12 15 4 15 11 15 11 7 4 16 1 17 6 8 14 18 8 19 7 20 13 21 0 22 12 23 13 21 7 20 10 24 3 25 5 6 2 9 15 26 9 27 8 19 14 18 3 25 10 24 9 27 15 26 12 23 0 22 1 17 4 16

37 | 38 | 39 | 1 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /data/robots/icub_description/meshes/upmc/collision/icub_simple_collision_r_hip_1.dae: -------------------------------------------------------------------------------- 1 | 2 | 2013-11-15T10:35:00.3281272013-11-15T10:35:00.328133Y_UP 3 | 4 | 5 | 6 | 0.0 0.0 0.0 1.00.0 0.0 0.0 1.00.48 0.48 0.48 1.00.5 0.5 0.5 1.050.01.01.0 7 | 8 | 9 | 10 | 1 11 | 12 | 13 | 14 | 1 15 | 16 | 17 | 18 | 19 | 20 | -0.01454192 0.0351535 -0.04399995 -0.03510737 0.01456099 -0.04399995 0.01454192 -0.0351535 -0.04399995 0.03510737 -0.01456099 -0.04399995 0.03510737 0.01456099 -0.04399995 -0.03510743 -0.01456093 -0.03100006 -0.01454204 -0.0351535 -0.03100006 -0.01454192 -0.0351535 -0.04399995 -0.03510737 -0.01456105 -0.04399995 0.01454198 0.0351535 -0.04399995 0.03510743 0.01456099 -0.03100006 0.03510737 -0.01456105 -0.03100006 0.01454186 -0.03515356 -0.03100006 -0.03510737 0.01456111 -0.03100006 -0.01454186 0.03515356 -0.03100006 0.01454204 0.0351535 -0.03100006 21 | 22 | 23 | 24 | 25 | -0.2891934 -0.6950591 0.6581927 -0.2973418 -0.7172155 -0.6302073 0.2973418 -0.7172155 -0.6302073 0.2891934 -0.6950591 0.6581927 -0.6959136 -0.286935 0.6582537 -0.6959136 0.286935 0.6582537 -0.7173376 0.2968536 -0.6302988 -0.7173376 -0.2968536 -0.6302988 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0.7173376 0.2968536 -0.6302988 0.2973418 0.7172155 -0.6302073 0.2891934 0.6950591 0.6581927 0.6959136 0.286935 0.6582537 -0.2973418 0.7172155 -0.6302073 -0.2891934 0.6950591 0.6581927 0.6959136 -0.286935 0.6582537 0.7173376 -0.2968536 -0.6302988 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 4 4 3 3 3 3 3 3 3 3 3 3 3 3 4 4 4 4 4 4 36 |

6 0 7 1 2 2 12 3 5 4 13 5 1 6 8 7 1 8 0 8 9 8 1 9 9 9 4 9 8 10 1 10 4 10 8 11 4 11 3 11 8 12 3 12 2 12 8 13 2 13 7 13 15 14 14 14 13 14 10 15 15 15 13 15 10 16 13 16 5 16 11 17 10 17 5 17 6 18 11 18 5 18 6 19 12 19 11 19 5 4 8 7 7 1 6 0 4 20 9 21 15 22 10 23 0 24 14 25 15 22 9 21 14 25 0 24 1 6 13 5 10 23 11 26 3 27 4 20 12 3 2 2 3 27 11 26

37 | 38 | 39 | 1 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /data/robots/icub_description/meshes/upmc/collision/icub_simple_collision_r_hip_2.dae: -------------------------------------------------------------------------------- 1 | 2 | 2013-11-15T10:34:57.9623792013-11-15T10:34:57.962389Y_UP 3 | 4 | 5 | 6 | 0.0 0.0 0.0 1.00.0 0.0 0.0 1.00.48 0.48 0.48 1.00.5 0.5 0.5 1.050.01.01.0 7 | 8 | 9 | 10 | 1 11 | 12 | 13 | 14 | 1 15 | 16 | 17 | 18 | 19 | 20 | 0.01186317 0.02864021 -0.03749996 -0.02864027 0.01186317 -0.03749996 0.01186317 -0.02864027 -0.03749996 0.01186323 0.02864021 0.03749996 0.01186311 -0.02864027 0.03749996 0.02864027 0.01186311 0.03749996 0.02864027 0.01186317 -0.03749996 0.02864027 -0.01186317 -0.03749996 -0.01186317 -0.02864027 -0.03749996 -0.02864027 -0.01186317 -0.03749996 -0.01186317 0.02864027 -0.03749996 0.02864021 -0.01186317 0.03749996 -0.01186323 -0.02864021 0.03749996 -0.02864027 -0.01186311 0.03749996 -0.02864021 0.01186323 0.03749996 -0.01186305 0.02864027 0.03749996 21 | 22 | 23 | 24 | 25 | 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0.2970977 0.7172765 0.6302378 0.2945952 0.7112644 -0.6381726 -0.2958769 0.7142552 -0.6342357 -0.2970977 0.7172765 0.6302378 -0.7112644 0.2945952 -0.6381726 -0.7172765 0.2970977 0.6302378 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 -0.7172765 -0.2970977 0.6302378 -0.7112644 -0.2945952 -0.6381726 -0.2945952 -0.7112644 -0.6381726 -0.2970977 -0.7172765 0.6302378 0.7172765 0.2970977 0.6302378 0.7172765 -0.2970977 0.6302378 0.7112644 -0.2945952 -0.6381726 0.7112644 0.2945952 -0.6381726 0.2970977 -0.7172765 0.6302378 0.2945952 -0.7112644 -0.6381726 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 3 3 3 3 3 3 4 4 3 3 3 3 3 3 4 4 4 4 4 4 36 |

10 0 0 0 1 0 1 1 0 1 6 1 9 2 1 2 6 2 9 3 6 3 7 3 9 4 7 4 8 4 7 5 2 5 8 5 3 6 0 7 10 8 15 9 15 9 10 8 1 10 14 11 3 12 15 12 14 12 5 13 3 13 14 13 5 14 14 14 13 14 5 15 13 15 11 15 13 16 12 16 11 16 12 17 4 17 11 17 13 18 9 19 8 20 12 21 5 22 11 23 7 24 6 25 11 23 4 26 2 27 7 24 12 21 8 20 2 27 4 26 14 11 1 10 9 19 13 18 6 25 0 7 3 6 5 22

37 | 38 | 39 | 1 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /data/robots/icub_description/meshes/upmc/collision/icub_simple_collision_r_shoulder_1.dae: -------------------------------------------------------------------------------- 1 | 2 | 2013-11-15T10:35:01.8320702013-11-15T10:35:01.832075Y_UP 3 | 4 | 5 | 6 | 0.0 0.0 0.0 1.00.0 0.0 0.0 1.00.48 0.48 0.48 1.00.5 0.5 0.5 1.050.01.01.0 7 | 8 | 9 | 10 | 1 11 | 12 | 13 | 14 | 1 15 | 16 | 17 | 18 | 19 | 20 | 0.01186317 0.02864021 -0.0777399 -0.01186317 0.02864027 -0.0777399 -0.02864027 0.01186317 -0.0777399 -0.02864021 -0.01186317 -0.0777399 0.01186317 -0.02864027 -0.0777399 0.02864027 -0.01186317 -0.0777399 0.02864027 0.01186317 -0.0777399 -0.01186317 -0.02864027 -0.0777399 0.02864027 0.01186311 -0.0667401 0.02864021 -0.01186317 -0.0667401 0.01186311 -0.02864027 -0.0667401 -0.01186323 -0.02864021 -0.0667401 -0.02864027 -0.01186311 -0.0667401 -0.02864021 0.01186323 -0.0667401 -0.01186305 0.02864027 -0.0667401 0.01186323 0.02864021 -0.0667401 21 | 22 | 23 | 24 | 25 | -0.2970977 0.7172765 0.6302378 -0.2892239 0.6959746 -0.6572161 -0.7172765 0.2970977 -0.6302378 -0.6959746 0.2892239 0.6572161 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 -0.7172765 -0.2970977 0.6302378 -0.696646 -0.2875759 -0.6572161 -0.2798547 -0.6756493 -0.6819971 -0.2970977 -0.7172765 0.6302378 0.2970977 -0.7172765 0.6302378 0.2798547 -0.6756493 -0.6819971 0.6756493 -0.2798547 -0.6819971 0.7172765 -0.2970977 0.6302378 0.6756493 0.2798547 -0.6819971 0.7172765 0.2970977 0.6302378 0.2970977 0.7172765 0.6302378 0.2798547 0.6756493 -0.6819971 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 4 3 3 3 3 3 3 3 3 3 3 3 3 4 4 4 4 4 4 4 36 |

14 0 1 1 2 2 13 3 14 4 13 4 15 4 13 5 8 5 15 5 13 6 12 6 8 6 12 7 9 7 8 7 12 8 11 8 9 8 11 9 10 9 9 9 1 10 0 10 2 10 2 11 0 11 6 11 3 12 2 12 6 12 5 13 3 13 6 13 5 14 7 14 3 14 5 15 4 15 7 15 12 16 3 17 7 18 11 19 10 20 4 21 5 22 9 23 13 3 2 2 3 17 12 16 9 23 5 22 6 24 8 25 15 26 0 27 1 1 14 0 11 19 7 18 4 21 10 20 0 27 15 26 8 25 6 24

37 | 38 | 39 | 1 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /data/robots/icub_description/meshes/upmc/collision/icub_simple_collision_r_shoulder_2.dae: -------------------------------------------------------------------------------- 1 | 2 | 2013-11-15T10:35:02.0174162013-11-15T10:35:02.017422Y_UP 3 | 4 | 5 | 0 0.02999997 -0.02949988 -0.02121317 0.02121317 -0.02949988 -0.02999997 0 -0.02949988 0.02121317 -0.02121317 -0.02949988 0.02121317 0.02121317 -0.02949988 0 0.02999997 0.02949988 -0.02121311 0.02121323 0.02949988 0.02999997 0 0.02949988 0.02121317 0.02121317 0.02949988 0.02999997 0 -0.02949988 0 -0.02999997 -0.02949988 -0.02121317 -0.02121317 -0.02949988 0.02121317 -0.02121323 0.02949988 0 -0.02999997 0.02949988 -0.02121323 -0.02121311 0.02949988 -0.02999997 0 0.02949988 6 | 7 | 8 | 9 | 10 | 0 0.7763603 0.6302378 0 0.7763603 -0.6302378 -0.548967 0.548967 -0.6302378 -0.548967 0.548967 0.6302378 -0.7763603 0 0.6302378 -0.7763603 0 -0.6302378 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0.548967 0.548967 0.6302378 0.548967 0.548967 -0.6302378 -0.548967 -0.548967 0.6302378 -0.548967 -0.548967 -0.6302378 0 -0.7763603 -0.6302378 0 -0.7763603 0.6302378 0.548967 -0.548967 0.6302378 0.548967 -0.548967 -0.6302378 0.7763603 0 -0.6302378 0.7763603 0 0.6302378 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 4 4 3 3 3 3 3 3 3 3 3 3 3 3 4 4 4 4 4 4 21 |

5 0 0 1 1 2 6 3 15 4 6 3 1 2 2 5 1 6 0 6 4 6 4 7 2 7 1 7 4 8 9 8 2 8 9 9 11 9 2 9 9 10 3 10 11 10 3 11 10 11 11 11 8 12 5 12 6 12 8 13 6 13 15 13 7 14 8 14 15 14 14 15 7 15 15 15 14 16 12 16 7 16 14 17 13 17 12 17 0 1 5 0 8 18 4 19 14 20 11 21 10 22 13 23 12 24 3 25 9 26 7 27 14 20 15 4 2 5 11 21 7 27 9 26 4 19 8 18 13 23 10 22 3 25 12 24

22 | 23 | 24 | 1 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /data/robots/icub_description/meshes/upmc/visual/icub_l_hand.dae: -------------------------------------------------------------------------------- 1 | 2 | 2013-11-15T10:34:58.6848882013-11-15T10:34:58.684900Y_UP 3 | 4 | 5 | 6 | 0.0 0.0 0.0 1.00.0 0.0 0.0 1.00.4224001 0.5056 0.64 1.00.25 0.25 0.25 1.050.01.01.0 7 | 8 | 9 | 10 | 1 11 | 12 | 13 | 14 | 1 15 | 16 | 17 | 18 | 19 | 20 | 0.0 0.0 0.0 1.00.0 0.0 0.0 1.00.4096 0.5056 0.4672001 1.00.25 0.25 0.25 1.050.01.01.0 21 | 22 | 23 | 24 | 1 25 | 26 | 27 | 28 | 1 29 | 30 | 31 | 32 | 33 | 34 | 0.06899994 0.0324999 -0.01199996 0.06899994 -0.0324999 -0.01199996 5.960464e-08 -0.0324999 -0.01199996 5.960464e-08 0.0324999 -0.01199996 0.06899994 0.0324999 0.01199996 0.06899994 -0.0324999 0.01199996 0 -0.03249984 0.01199996 5.960464e-08 0.0324999 0.01199996 35 | 36 | 37 | 38 | 39 | 0 0 -1 0 0 1 1 0 0 -4.31918e-07 -1 1.24177e-06 -1 4.58498e-07 -1.24177e-06 0 1 0 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 4 4 4 4 4 50 |

4 1 7 1 6 1 5 1 0 2 4 2 5 2 1 2 1 3 5 3 6 3 2 3 2 4 6 4 7 4 3 4 4 5 0 5 3 5 7 5

51 | 52 | 53 | 54 | 55 | 4 56 |

0 0 1 0 2 0 3 0

57 | 58 | 59 | 1 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /data/robots/icub_description/meshes/upmc/visual/icub_r_hand.dae: -------------------------------------------------------------------------------- 1 | 2 | 2013-11-15T10:34:58.8740782013-11-15T10:34:58.874090Y_UP 3 | 4 | 5 | 6 | 0.0 0.0 0.0 1.00.0 0.0 0.0 1.00.4224001 0.5056 0.64 1.00.25 0.25 0.25 1.050.01.01.0 7 | 8 | 9 | 10 | 1 11 | 12 | 13 | 14 | 1 15 | 16 | 17 | 18 | 19 | 20 | 0.0 0.0 0.0 1.00.0 0.0 0.0 1.00.4096 0.5056 0.4672001 1.00.25 0.25 0.25 1.050.01.01.0 21 | 22 | 23 | 24 | 1 25 | 26 | 27 | 28 | 1 29 | 30 | 31 | 32 | 33 | 34 | -5.960464e-08 0.0324999 -0.01199996 -5.960464e-08 -0.0324999 -0.01199996 -0.06899994 -0.0324999 -0.01199996 -0.06899994 0.0324999 -0.01199996 -5.960464e-08 0.0324999 0.01199996 -5.960464e-08 -0.0324999 0.01199996 -0.069 -0.03249984 0.01199996 -0.06899994 0.0324999 0.01199996 35 | 36 | 37 | 38 | 39 | 0 0 -1 0 0 1 1 0 0 -4.31918e-07 -1 1.24177e-06 -1 4.58498e-07 -1.24177e-06 0 1 0 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 4 4 4 4 4 50 |

0 0 1 0 2 0 3 0 0 2 4 2 5 2 1 2 1 3 5 3 6 3 2 3 2 4 6 4 7 4 3 4 4 5 0 5 3 5 7 5

51 | 52 | 53 | 54 | 55 | 4 56 |

4 1 7 1 6 1 5 1

57 | 58 | 59 | 1 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /data/robots/icub_description/meshes/upmc/visual/icub_root_link.dae: -------------------------------------------------------------------------------- 1 | 2 | 2013-11-15T10:35:01.2360832013-11-15T10:35:01.236093Y_UP 3 | 4 | 5 | 6 | 0.0 0.0 0.0 1.00.0 0.0 0.0 1.00.4224001 0.5056 0.64 1.00.25 0.25 0.25 1.050.01.01.0 7 | 8 | 9 | 10 | 1 11 | 12 | 13 | 14 | 1 15 | 16 | 17 | 18 | 19 | 20 | 0.0 0.0 0.0 1.00.0 0.0 0.0 1.00.4096 0.5056 0.4672001 1.00.25 0.25 0.25 1.050.01.01.0 21 | 22 | 23 | 24 | 1 25 | 26 | 27 | 28 | 1 29 | 30 | 31 | 32 | 33 | 34 | 0.03799988 0.02349996 -0.1572149 0.03799988 -0.02349996 -0.1572149 -0.02599988 -0.02349996 -0.1572149 -0.02599988 0.02349996 -0.1572149 0.03799988 0.02349996 -0.04278509 0.03799988 -0.02349996 -0.04278509 -0.026 -0.02349996 -0.04278509 -0.02599988 0.02349996 -0.04278509 35 | 36 | 37 | 38 | 39 | 0 0 -1 0 0 1 1 0 0 0 -1 0 -1 6.73724e-07 -2.76719e-07 0 1 0 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 4 4 4 4 4 50 |

4 1 7 1 6 1 5 1 0 2 4 2 5 2 1 2 1 3 5 3 6 3 2 3 2 4 6 4 7 4 3 4 4 5 0 5 3 5 7 5

51 | 52 | 53 | 54 | 55 | 4 56 |

0 0 1 0 2 0 3 0

57 | 58 | 59 | 1 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /data/robots/kinova_description/README.md: -------------------------------------------------------------------------------- 1 | # Kinova ROS 2 | 3 | upstream: https://github.com/Kinovarobotics/kinova-ros/tree/melodic-devel/kinova_description 4 | license: BSD 5 | -------------------------------------------------------------------------------- /data/robots/kinova_description/srdf/kinova.srdf: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /data/robots/laikago_description/meshes/calf.mtl: -------------------------------------------------------------------------------- 1 | # 2 | # Wavefront material file 3 | # Converted by Meshlab Group 4 | # 5 | 6 | newmtl material_0 7 | Ka 1.000000 1.000000 1.000000 8 | Kd 0.640000 0.640000 0.640000 9 | Ks 0.500000 0.500000 0.500000 10 | Tr 1.000000 11 | illum 2 12 | Ns 0.000000 13 | -------------------------------------------------------------------------------- /data/robots/laikago_description/meshes/hip.mtl: -------------------------------------------------------------------------------- 1 | # 2 | # Wavefront material file 3 | # Converted by Meshlab Group 4 | # 5 | 6 | newmtl material_0 7 | Ka 1.000000 1.000000 1.000000 8 | Kd 0.640000 0.640000 0.640000 9 | Ks 0.500000 0.500000 0.500000 10 | Tr 1.000000 11 | illum 2 12 | Ns 0.000000 13 | -------------------------------------------------------------------------------- /data/robots/laikago_description/meshes/thigh.mtl: -------------------------------------------------------------------------------- 1 | # 2 | # Wavefront material file 3 | # Converted by Meshlab Group 4 | # 5 | 6 | newmtl material_0 7 | Ka 1.000000 1.000000 1.000000 8 | Kd 0.640000 0.640000 0.640000 9 | Ks 0.500000 0.500000 0.500000 10 | Tr 1.000000 11 | illum 2 12 | Ns 0.000000 13 | -------------------------------------------------------------------------------- /data/robots/laikago_description/meshes/thigh_mirror.mtl: -------------------------------------------------------------------------------- 1 | # 2 | # Wavefront material file 3 | # Converted by Meshlab Group 4 | # 5 | 6 | newmtl material_0 7 | Ka 1.000000 1.000000 1.000000 8 | Kd 0.640000 0.640000 0.640000 9 | Ks 0.500000 0.500000 0.500000 10 | Tr 1.000000 11 | illum 2 12 | Ns 0.000000 13 | -------------------------------------------------------------------------------- /data/robots/laikago_description/meshes/trunk.mtl: -------------------------------------------------------------------------------- 1 | # File produced by Open Asset Import Library (http://www.assimp.sf.net) 2 | # (assimp v5.0.639693989) 3 | 4 | newmtl MaterialFBXASC032FBXASC03528 5 | Kd 1.0 0.423529411765 0.0392156862745 6 | Ka 1.0 0.423529411765 0.0392156862745 7 | Ks 0 0 0 8 | Ke 0 0 0 9 | Tf 1 1 1 10 | d 1 11 | Ni 1 12 | Ns 2 13 | illum 2 14 | 15 | newmtl MaterialFBXASC032FBXASC03529 16 | Kd 1.0 0.423529411765 0.0392156862745 17 | Ka 1.0 0.423529411765 0.0392156862745 18 | Ks 0 0 0 19 | Ke 0 0 0 20 | Tf 1 1 1 21 | d 1 22 | Ni 1 23 | Ns 2 24 | illum 2 25 | 26 | newmtl MaterialFBXASC032FBXASC03530 27 | Kd 1.0 0.423529411765 0.0392156862745 28 | Ka 1.0 0.423529411765 0.0392156862745 29 | Ks 0 0 0 30 | Ke 0 0 0 31 | Tf 1 1 1 32 | d 1 33 | Ni 1 34 | Ns 2 35 | illum 2 36 | -------------------------------------------------------------------------------- /data/robots/laikago_description/meshes/trunk_origin.mtl: -------------------------------------------------------------------------------- 1 | # File produced by Open Asset Import Library (http://www.assimp.sf.net) 2 | # (assimp v5.0.639693989) 3 | 4 | newmtl MaterialFBXASC032FBXASC03528 5 | Kd 0.933333 0.392157 0.211765 6 | Ka 0.933333 0.392157 0.211765 7 | Ks 0 0 0 8 | Ke 0 0 0 9 | Tf 1 1 1 10 | d 1 11 | Ni 1 12 | Ns 2 13 | illum 2 14 | 15 | newmtl MaterialFBXASC032FBXASC03529 16 | Kd 0.203922 0.203922 0.203922 17 | Ka 0.203922 0.203922 0.203922 18 | Ks 0 0 0 19 | Ke 0 0 0 20 | Tf 1 1 1 21 | d 1 22 | Ni 1 23 | Ns 2 24 | illum 2 25 | 26 | newmtl MaterialFBXASC032FBXASC03530 27 | Kd 0.50196099 0.50196099 0.50196099 28 | Ka 0.50196099 0.50196099 0.50196099 29 | Ks 0 0 0 30 | Ke 0 0 0 31 | Tf 1 1 1 32 | d 1 33 | Ni 1 34 | Ns 2 35 | illum 2 36 | -------------------------------------------------------------------------------- /data/robots/panda_description/README.md: -------------------------------------------------------------------------------- 1 | # Panda ign 2 | 3 | upstream: https://github.com/frankaemika/franka_ros/tree/noetic-devel/franka_description 4 | license: Apache 2.0 5 | -------------------------------------------------------------------------------- /data/robots/panda_description/meshes/collision/finger.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/panda_description/meshes/collision/finger.stl -------------------------------------------------------------------------------- /data/robots/panda_description/meshes/collision/hand.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/panda_description/meshes/collision/hand.stl -------------------------------------------------------------------------------- /data/robots/panda_description/meshes/collision/link0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/panda_description/meshes/collision/link0.stl -------------------------------------------------------------------------------- /data/robots/panda_description/meshes/collision/link1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/panda_description/meshes/collision/link1.stl -------------------------------------------------------------------------------- /data/robots/panda_description/meshes/collision/link2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/panda_description/meshes/collision/link2.stl -------------------------------------------------------------------------------- /data/robots/panda_description/meshes/collision/link3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/panda_description/meshes/collision/link3.stl -------------------------------------------------------------------------------- /data/robots/panda_description/meshes/collision/link4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/panda_description/meshes/collision/link4.stl -------------------------------------------------------------------------------- /data/robots/panda_description/meshes/collision/link5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/panda_description/meshes/collision/link5.stl -------------------------------------------------------------------------------- /data/robots/panda_description/meshes/collision/link6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/panda_description/meshes/collision/link6.stl -------------------------------------------------------------------------------- /data/robots/panda_description/meshes/collision/link7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/panda_description/meshes/collision/link7.stl -------------------------------------------------------------------------------- /data/robots/talos_description/README.md: -------------------------------------------------------------------------------- 1 | # Talos Data 2 | 3 | upstream: https://github.com/stack-of-tasks/talos-data 4 | license: LGPL-3.0 5 | -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/arm/arm_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/arm/arm_1.stl -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/arm/arm_1_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/arm/arm_1_collision.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/arm/arm_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/arm/arm_2.stl -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/arm/arm_2_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/arm/arm_2_collision.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/arm/arm_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/arm/arm_3.stl -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/arm/arm_3_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/arm/arm_3_collision.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/arm/arm_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/arm/arm_4.stl -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/arm/arm_4_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/arm/arm_4_collision.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/arm/arm_5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/arm/arm_5.stl -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/arm/arm_5_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/arm/arm_5_collision.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/arm/arm_6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/arm/arm_6.stl -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/arm/arm_6_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/arm/arm_6_collision.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/arm/arm_7.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/arm/arm_7.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/arm/arm_7_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/arm/arm_7_collision.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/gripper/base_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/gripper/base_link.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/gripper/base_link_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/gripper/base_link_collision.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/gripper/fingertip.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/gripper/fingertip.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/gripper/fingertip_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/gripper/fingertip_collision.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/gripper/gripper_motor_double.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/gripper/gripper_motor_double.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/gripper/gripper_motor_double_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/gripper/gripper_motor_double_collision.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/gripper/gripper_motor_single.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/gripper/gripper_motor_single.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/gripper/gripper_motor_single_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/gripper/gripper_motor_single_collision.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/gripper/inner_double.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/gripper/inner_double.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/gripper/inner_double_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/gripper/inner_double_collision.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/gripper/inner_single.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/gripper/inner_single.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/gripper/inner_single_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/gripper/inner_single_collision.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/head/head_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/head/head_1.stl -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/head/head_1_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/head/head_1_collision.stl -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/head/head_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/head/head_2.stl -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/head/head_2_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/head/head_2_collision.stl -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/sensors/orbbec/orbbec.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/sensors/orbbec/orbbec.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/torso/base_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/torso/base_link.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/torso/base_link_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/torso/base_link_collision.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/torso/torso_1.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/torso/torso_1.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/torso/torso_2.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/torso/torso_2.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/torso/torso_2_collision.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/torso/torso_2_collision.STL -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/v2/ankle_X_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/v2/ankle_X_collision.stl -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/v2/ankle_X_lo_res.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/v2/ankle_X_lo_res.stl -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/v2/ankle_Y_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/v2/ankle_Y_collision.stl -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/v2/ankle_Y_lo_res.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/v2/ankle_Y_lo_res.stl -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/v2/hip_x_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/v2/hip_x_collision.stl -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/v2/hip_x_lo_res.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/v2/hip_x_lo_res.stl -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/v2/hip_y_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/v2/hip_y_collision.stl -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/v2/hip_y_lo_res.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/v2/hip_y_lo_res.stl -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/v2/hip_z_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/v2/hip_z_collision.stl -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/v2/hip_z_lo_res.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/v2/hip_z_lo_res.stl -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/v2/knee_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/v2/knee_collision.stl -------------------------------------------------------------------------------- /data/robots/talos_description/meshes/v2/knee_lo_res.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/data/robots/talos_description/meshes/v2/knee_lo_res.stl -------------------------------------------------------------------------------- /example/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(ungar.variable.example variable.example.cpp) 2 | target_link_libraries(ungar.variable.example ungar::ungar) 3 | 4 | add_executable(ungar.variable_map.example variable_map.example.cpp) 5 | target_link_libraries(ungar.variable_map.example ungar::ungar) 6 | 7 | if(UNGAR_ENABLE_AUTODIFF) 8 | add_executable(ungar.function.example autodiff/function.example.cpp) 9 | target_link_libraries(ungar.function.example ungar::ungar) 10 | endif() 11 | 12 | if(UNGAR_ENABLE_OPTIMIZATION) 13 | add_executable(ungar.quadrotor.example mpc/quadrotor.example.cpp) 14 | target_link_libraries(ungar.quadrotor.example ungar::ungar) 15 | 16 | add_executable(ungar.quadruped.example mpc/quadruped.example.cpp) 17 | target_link_libraries(ungar.quadruped.example ungar::ungar) 18 | 19 | add_executable(ungar.rc_car.example mpc/rc_car.example.cpp) 20 | target_link_libraries(ungar.rc_car.example ungar::ungar) 21 | endif() 22 | 23 | if(UNGAR_ENABLE_PINOCCHIO) 24 | add_executable(ungar.quantity.example rbd/quantity.example.cpp) 25 | target_link_libraries(ungar.quantity.example ungar::ungar) 26 | 27 | add_executable(ungar.robot.example rbd/robot.example.cpp) 28 | target_link_libraries(ungar.robot.example ungar::ungar) 29 | endif() 30 | 31 | # Add examples to the test targets if testing is enabled. 32 | if(UNGAR_BUILD_TESTS) 33 | add_test(NAME ungar.variable.example COMMAND ungar.variable.example) 34 | add_test(NAME ungar.variable_map.example COMMAND ungar.variable_map.example) 35 | 36 | if(UNGAR_ENABLE_AUTODIFF) 37 | add_test(NAME ungar.function.example COMMAND ungar.function.example) 38 | endif() 39 | 40 | if(UNGAR_ENABLE_OPTIMIZATION) 41 | add_test(NAME ungar.quadrotor.example COMMAND ungar.quadrotor.example) 42 | add_test(NAME ungar.quadruped.example COMMAND ungar.quadruped.example) 43 | add_test(NAME ungar.rc_car.example COMMAND ungar.rc_car.example) 44 | endif() 45 | 46 | if(UNGAR_ENABLE_PINOCCHIO) 47 | add_test(NAME ungar.quantity.example COMMAND ungar.quantity.example) 48 | add_test(NAME ungar.robot.example COMMAND ungar.robot.example) 49 | endif() 50 | endif() 51 | -------------------------------------------------------------------------------- /example/autodiff/function.example.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/example/autodiff/function.example.cpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #include "ungar/autodiff/function.hpp" 28 | #include "ungar/variable_map.hpp" 29 | 30 | int main() { 31 | using namespace Ungar; 32 | 33 | /*************** POINT MASS ***************/ 34 | /*************** Define numeric invariants as integral constants. ***************/ 35 | constexpr auto N = 30_c; // Discrete time horizon. 36 | 37 | /*************** Define "leaf" variables. ***************/ 38 | // Positions are 3-dimensional vectors, masses are scalars, etc. 39 | UNGAR_VARIABLE(position, 3); // := p 40 | UNGAR_VARIABLE(linear_velocity, 3); // := pDot 41 | UNGAR_VARIABLE(force, 3); // := f 42 | UNGAR_VARIABLE(mass, 1); // := m 43 | UNGAR_VARIABLE(state_cost_weight, 1); // := wx 44 | UNGAR_VARIABLE(input_cost_weight, 1); // := wu 45 | 46 | /*************** Define "branch" variables. ***************/ 47 | // States are stacked poses and velocities, while inputs are forces applied 48 | // to the point mass. 49 | UNGAR_VARIABLE(x) <<= (position, linear_velocity); // x := [p pDot] 50 | UNGAR_VARIABLE(u) <<= force; // u := f 51 | UNGAR_VARIABLE(X) <<= (N + 1_c) * x; // X := [x0 x1 ... xN] 52 | UNGAR_VARIABLE(U) <<= N * u; // U := [u0 u1 ... uN-1] 53 | 54 | UNGAR_VARIABLE(xRef) <<= (position, linear_velocity); // xRef := [p pDot] 55 | UNGAR_VARIABLE(XRef) <<= (N + 1_c) * xRef; // XRef := [xRef0 xRef1 ... xRefN] 56 | 57 | UNGAR_VARIABLE(decision_variables) <<= (X, U); 58 | UNGAR_VARIABLE(parameters) <<= (mass, XRef, state_cost_weight, input_cost_weight); 59 | UNGAR_VARIABLE(variables) <<= (decision_variables, parameters); 60 | 61 | /*************** Generate derivatives for functions of variable maps. ***************/ 62 | const Autodiff::Function::Blueprint functionBlueprint{ 63 | [&](const VectorXad& xp, VectorXad& y) { 64 | // Create variables maps on existing arrays of data. 65 | const auto vars = MakeVariableLazyMap(xp, variables); 66 | 67 | // You can further split the stacked independent variables and parameters. 68 | // const auto decVars = 69 | // MakeVariableLazyMap(varsMap.Get(decision_variables), decision_variables); 70 | // const auto params = MakeVariableLazyMap(varsMap.Get(parameters), parameters); 71 | 72 | y = VectorXr::Zero(1); 73 | for (auto k : enumerate(N)) { 74 | y[0] += vars.Get(state_cost_weight) * 75 | (vars.Get(x, k) - vars.Get(xRef, k)).squaredNorm() + 76 | vars.Get(input_cost_weight) * vars.Get(u, k).squaredNorm(); 77 | } 78 | y[0] += 79 | vars.Get(state_cost_weight) * (vars.Get(x, N) - vars.Get(xRef, N)).squaredNorm(); 80 | }, 81 | decision_variables.Size(), 82 | parameters.Size(), 83 | "function_example"sv, 84 | EnabledDerivatives::JACOBIAN | EnabledDerivatives::HESSIAN}; 85 | const auto function = Autodiff::MakeFunction(functionBlueprint, true); 86 | 87 | auto vars = MakeVariableMap(variables); 88 | vars.Get().setRandom(); 89 | const VectorXr value = function(vars.Get()); 90 | const SparseMatrix jacobian = function.Jacobian(vars.Get()); 91 | const SparseMatrix hessian = function.Hessian(vars.Get()); 92 | 93 | UNGAR_ASSERT(value.size() == function.DependentVariableSize() && value.size() == 1); 94 | UNGAR_ASSERT(function.TestJacobian(vars.Get())); 95 | UNGAR_ASSERT(function.TestHessian(vars.Get())); 96 | 97 | return 0; 98 | } 99 | -------------------------------------------------------------------------------- /example/variable_map.example.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/example/variable_map.example.cpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #include "ungar/variable_map.hpp" 28 | 29 | int main() { 30 | using namespace Ungar; 31 | 32 | /*************** QUADROTOR MODEL ***************/ 33 | /*************** Define numeric invariants as integral constants. ***************/ 34 | constexpr auto N = 30_c; // Discrete time horizon. 35 | constexpr auto NUM_ROTORS = 4_c; 36 | 37 | /*************** Define "leaf" variables. ***************/ 38 | // Positions are 3-dimensional vectors, orientations are unit quaternions, 39 | // rotor speeds are scalars, etc. 40 | constexpr auto position = var_c<"position", 3>; // := p 41 | constexpr auto orientation = var_c<"orientation", Q>; // := q 42 | constexpr auto linear_velocity = var_c<"linear_velocity", 3>; // := pDot 43 | constexpr auto angular_velocity = var_c<"angular_velocity", 3>; // := omega 44 | constexpr auto rotor_speed = var_c<"rotor_speed", 1>; // := r 45 | 46 | /*************** Define "branch" variables. ***************/ 47 | // States are stacked poses and velocities, while inputs are stacked rotor 48 | // speeds: one for each rotor. 49 | constexpr auto x = var_c<"x"> <<= 50 | (position, orientation, linear_velocity, angular_velocity); // x := [p q pDot omega] 51 | constexpr auto u = var_c<"u"> <<= NUM_ROTORS * rotor_speed; // u := [r0 r1 r2 r3] 52 | constexpr auto X = var_c<"X"> <<= (N + 1_c) * x; // X := [x0 x1 ... xN] 53 | constexpr auto U = var_c<"U"> <<= N * u; // U := [u0 u1 ... uN-1] 54 | 55 | constexpr auto variables = var_c<"variables"> <<= (X, U); 56 | 57 | /*************** Instantiate and use variable maps. ***************/ 58 | // Access information about variables at compile time. 59 | static_assert(x.Size() == 13); 60 | static_assert(variables(x, 0).Index() == 0); // [{x0} x1 x2 ... xN u0 ... ] 61 | static_assert(variables(x, 1).Index() == 13); // [ x0 {x1} x2 ... xN u0 ... ] 62 | static_assert(variables(x, 2).Index() == 26); // [ x0 x1 {x2} ... xN u0 ... ] 63 | static_assert(variables(u, 0).Index() == X.Size()); // [ x0 x1 x2 ... xN {u0} ... ] 64 | 65 | // Create maps over contiguous arrays of data. 66 | auto vars = MakeVariableMap(variables); // [ X u0 u1 ... uN-1 ] 67 | vars.Get(u, 0).setZero(); // [ X 0000 u1 ... uN-1 ] 68 | vars.Get(u, 1).setOnes(); // [ X 0000 1111 ... uN-1 ] 69 | vars.Get(u, N - 1).setLinSpaced(2.0, 8.0); // [ X 0000 1111 ... 2468 ] 70 | UNGAR_LOG(info, "u0 = {}", vars.Get(u, 0)); 71 | UNGAR_LOG(info, "u1 = {}", vars.Get(u, 1)); 72 | UNGAR_LOG(info, "uN-1 = {}", vars.Get(u, N - 1)); 73 | 74 | static_assert(std::same_as); 75 | static_assert(std::same_as&>); 76 | static_assert(std::same_as&>); 77 | static_assert(std::same_as>&>); 78 | 79 | return 0; 80 | } 81 | -------------------------------------------------------------------------------- /external/config/CppAD/CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | 3 | project(ExternalCppAD NONE) 4 | 5 | include(ExternalProject) 6 | 7 | if(NOT @CppAD_CMAKE_FLAGS@ STREQUAL "") 8 | message(STATUS "CppAD additional CMake flags: @CppAD_CMAKE_FLAGS@") 9 | endif() 10 | 11 | ExternalProject_Add( 12 | CppAD-20230000.0 13 | URL https://github.com/coin-or/CppAD/archive/refs/tags/20230000.0.zip 14 | URL_HASH 15 | SHA256=a5a6b32e026c01f71dba580552176cd9fc69b72ec3156c9438552d42068ac54a 16 | CMAKE_CACHE_ARGS 17 | -DCMAKE_CXX_STANDARD:STRING=20 18 | -Dcppad_prefix:STRING=@CMAKE_CURRENT_BINARY_DIR@/CppAD/install 19 | @CppAD_CMAKE_FLAGS@ 20 | UPDATE_DISCONNECTED 1 21 | DOWNLOAD_EXTRACT_TIMESTAMP 1) 22 | -------------------------------------------------------------------------------- /external/config/CppADCodeGen/CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | 3 | project(ExternalCppADCodeGen NONE) 4 | 5 | include(ExternalProject) 6 | 7 | if(NOT @CppADCodeGen_CMAKE_FLAGS@ STREQUAL "") 8 | message( 9 | STATUS "CppADCodeGen additional CMake flags: @CppADCodeGen_CMAKE_FLAGS@") 10 | endif() 11 | 12 | ExternalProject_Add( 13 | CppADCodeGen-v2.4.3 14 | URL https://github.com/fdevinc/CppADCodeGen/archive/refs/tags/v2.4.3-ungar.zip 15 | URL_HASH 16 | SHA256=63adcd082b6de959763460797619dcdef92353498b8ccdf0a312ae0a91dd1d3a 17 | CMAKE_CACHE_ARGS 18 | -DCMAKE_CXX_STANDARD:STRING=20 19 | -DCPPAD_HOME:STRING=@CPPAD_ROOT_DIRECTORY@/include 20 | -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/CppADCodeGen/install 21 | -DCPPADCODEGEN_BUILD_TESTS:BOOL=OFF 22 | @CppADCodeGen_CMAKE_FLAGS@ 23 | UPDATE_DISCONNECTED 1 24 | DOWNLOAD_EXTRACT_TIMESTAMP 1) 25 | -------------------------------------------------------------------------------- /external/config/abseil-cpp/CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | 3 | project(ExternalAbseil NONE) 4 | 5 | include(ExternalProject) 6 | 7 | if(NOT @absl_CMAKE_FLAGS@ STREQUAL "") 8 | message(STATUS "Abseil additional CMake flags: @absl_CMAKE_FLAGS@") 9 | endif() 10 | 11 | ExternalProject_Add( 12 | abseil-cpp-20230802.1 13 | URL https://github.com/abseil/abseil-cpp/archive/refs/tags/20230802.1.zip 14 | URL_HASH 15 | SHA256=497ebdc3a4885d9209b9bd416e8c3f71e7a1fb8af249f6c2a80b7cbeefcd7e21 16 | CMAKE_CACHE_ARGS 17 | -DCMAKE_CXX_STANDARD:STRING=20 18 | -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/abseil-cpp/install 19 | @absl_CMAKE_FLAGS@ 20 | UPDATE_DISCONNECTED 1 21 | DOWNLOAD_EXTRACT_TIMESTAMP 1) 22 | -------------------------------------------------------------------------------- /external/config/boost/CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | 3 | project(ExternalBoost NONE) 4 | 5 | include(ExternalProject) 6 | 7 | if(NOT @Boost_CMAKE_FLAGS@ STREQUAL "") 8 | message(WARN "The following Boost additional CMake flags were provided, but \ 9 | they will be ignored: @Boost_CMAKE_FLAGS@") 10 | endif() 11 | 12 | ExternalProject_Add( 13 | boost-1.84.0 14 | URL https://boostorg.jfrog.io/artifactory/main/release/1.84.0/source/boost_1_84_0.tar.bz2 15 | URL_HASH 16 | SHA256=cc4b893acf645c9d4b698e9a0f08ca8846aa5d6c68275c14c3e7949c24109454 17 | BUILD_IN_SOURCE TRUE 18 | CONFIGURE_COMMAND 19 | ./bootstrap.sh --with-libraries=filesystem,system,test,serialization 20 | --prefix=@CMAKE_CURRENT_BINARY_DIR@/boost/install 21 | BUILD_COMMAND "" 22 | INSTALL_COMMAND ./b2 install link=static threading=multi runtime-link=static 23 | cxxflags=-fpic 24 | UPDATE_DISCONNECTED 1 25 | DOWNLOAD_EXTRACT_TIMESTAMP 1) 26 | -------------------------------------------------------------------------------- /external/config/console_bridge/CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | 3 | project(ExternalConsoleBridge NONE) 4 | 5 | include(ExternalProject) 6 | 7 | if(NOT @console_bridge_CMAKE_FLAGS@ STREQUAL "") 8 | message( 9 | STATUS "console_bridge additional CMake flags: @console_bridge_CMAKE_FLAGS@" 10 | ) 11 | endif() 12 | 13 | ExternalProject_Add( 14 | console_bridge-1.0.2-ungar 15 | URL https://github.com/fdevinc/console_bridge/archive/refs/tags/1.0.2-ungar.zip 16 | URL_HASH 17 | SHA256=0fadd85047247953a64f7dc01f00f0da683aaab556fed7ec704d0a910e623b8e 18 | CMAKE_CACHE_ARGS 19 | -DCMAKE_CXX_STANDARD:STRING=20 20 | -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/console_bridge/install 21 | @console_bridge_CMAKE_FLAGS@ 22 | UPDATE_DISCONNECTED 1 23 | DOWNLOAD_EXTRACT_TIMESTAMP 1) 24 | -------------------------------------------------------------------------------- /external/config/eigen/CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | 3 | project(ExternalEigen NONE) 4 | 5 | include(ExternalProject) 6 | 7 | if(NOT @Eigen3_CMAKE_FLAGS@ STREQUAL "") 8 | message(STATUS "Eigen additional CMake flags: @Eigen3_CMAKE_FLAGS@") 9 | endif() 10 | 11 | ExternalProject_Add( 12 | eigen-3.4.0 13 | URL @BUNDLED_EIGEN_FILENAME@ 14 | URL_HASH 15 | SHA256=1ccaabbfe870f60af3d6a519c53e09f3dcf630207321dffa553564a8e75c4fc8 16 | CMAKE_CACHE_ARGS 17 | -DCMAKE_CXX_STANDARD:STRING=20 18 | -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/eigen/install 19 | -DEIGEN_BUILD_DOC:BOOL=OFF 20 | -DBUILD_TESTING:BOOL=OFF 21 | -DEIGEN_LEAVE_TEST_IN_ALL_TARGET:BOOL=OFF 22 | -DEIGEN_BUILD_PKGCONFIG:BOOL=OFF 23 | @Eigen3_CMAKE_FLAGS@ 24 | UPDATE_DISCONNECTED 1 25 | DOWNLOAD_EXTRACT_TIMESTAMP 1) 26 | -------------------------------------------------------------------------------- /external/config/eigen/eigen-3.4.0.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/external/config/eigen/eigen-3.4.0.zip -------------------------------------------------------------------------------- /external/config/finite-diff/CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | 3 | project(ExternalFiniteDiff NONE) 4 | 5 | include(ExternalProject) 6 | 7 | if(NOT @FiniteDiff_CMAKE_FLAGS@ STREQUAL "") 8 | message(STATUS "FiniteDiff additional CMake flags: @FiniteDiff_CMAKE_FLAGS@") 9 | endif() 10 | 11 | ExternalProject_Add( 12 | finite-diff-v1.0.2-ungar 13 | URL https://github.com/fdevinc/finite-diff/archive/refs/tags/v1.0.2-ungar.zip 14 | URL_HASH 15 | SHA256=9583bc34ab446d2313ded912fc791b7ad39e853d27f29dc78438bd3223cd2a85 16 | CMAKE_CACHE_ARGS 17 | -DCMAKE_POLICY_DEFAULT_CMP0074:STRING=NEW 18 | -DCMAKE_CXX_STANDARD:STRING=20 19 | -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/finite-diff/install 20 | -DFINITE_DIFF_INSTALL:BOOL=ON 21 | -DEigen3_ROOT:STRING=@EIGEN3_ROOT_DIRECTORY@ 22 | -Dspdlog_ROOT:STRING=@SPDLOG_ROOT_DIRECTORY@ 23 | @FiniteDiff_CMAKE_FLAGS@ 24 | UPDATE_DISCONNECTED 1 25 | DOWNLOAD_EXTRACT_TIMESTAMP 1) 26 | -------------------------------------------------------------------------------- /external/config/googletest/CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | 3 | project(ExternalGoogleTest NONE) 4 | 5 | include(ExternalProject) 6 | 7 | if(NOT @GTest_CMAKE_FLAGS@ STREQUAL "") 8 | message(STATUS "GoogleTest additional CMake flags: @GTest_CMAKE_FLAGS@") 9 | endif() 10 | 11 | ExternalProject_Add( 12 | googletest-v1.14.0 13 | URL https://github.com/google/googletest/archive/refs/tags/v1.14.0.zip 14 | URL_HASH 15 | SHA256=1f357c27ca988c3f7c6b4bf68a9395005ac6761f034046e9dde0896e3aba00e4 16 | CMAKE_CACHE_ARGS 17 | -DCMAKE_CXX_STANDARD:STRING=20 -DBUILD_GMOCK:BOOL=OFF 18 | -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/googletest/install 19 | @GTest_CMAKE_FLAGS@ 20 | UPDATE_DISCONNECTED 1 21 | DOWNLOAD_EXTRACT_TIMESTAMP 1) 22 | -------------------------------------------------------------------------------- /external/config/hana/CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | 3 | project(ExternalHana NONE) 4 | 5 | include(ExternalProject) 6 | 7 | if(NOT @Hana_CMAKE_FLAGS@ STREQUAL "") 8 | message(STATUS "Hana additional CMake flags: @Hana_CMAKE_FLAGS@") 9 | endif() 10 | 11 | ExternalProject_Add( 12 | hana-1.84.0 13 | URL @BUNDLED_HANA_FILENAME@ 14 | URL_HASH 15 | SHA256=ca29f3af12099394cbf52421e9e624254d60fb87412fcfa11576773d53a23ded 16 | CMAKE_CACHE_ARGS 17 | -DCMAKE_CXX_STANDARD:STRING=20 18 | -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/hana/install 19 | -DBOOST_HANA_ENABLE_CONCEPT_CHECKS:BOOL=ON 20 | -DBOOST_HANA_ENABLE_DEBUG_MODE:BOOL=OFF 21 | -DBOOST_HANA_ENABLE_STRING_UDL:BOOL=OFF 22 | -DBOOST_HANA_ENABLE_EXCEPTIONS:BOOL=ON 23 | @Hana_CMAKE_FLAGS@ 24 | UPDATE_DISCONNECTED 1 25 | DOWNLOAD_EXTRACT_TIMESTAMP 1) 26 | -------------------------------------------------------------------------------- /external/config/hana/hana-boost-1.84.0.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/external/config/hana/hana-boost-1.84.0.zip -------------------------------------------------------------------------------- /external/config/osqp-cpp/CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | 3 | project(ExternalOSQPCpp NONE) 4 | 5 | include(ExternalProject) 6 | 7 | if(NOT @osqp-cpp_CMAKE_FLAGS@ STREQUAL "") 8 | message(STATUS "osqp-cpp additional CMake flags: @osqp-cpp_CMAKE_FLAGS@") 9 | endif() 10 | 11 | ExternalProject_Add( 12 | osqp-cpp-v0.5.1-ungar 13 | URL https://github.com/fdevinc/osqp-cpp/archive/refs/tags/v0.5.1-ungar.zip 14 | URL_HASH 15 | SHA256=7d46a394ebdd1532c0eeaa4e3f798600ef9fbc2de4f4325d513289d92563150a 16 | CMAKE_CACHE_ARGS 17 | -DCMAKE_POLICY_DEFAULT_CMP0074:STRING=NEW 18 | -DCMAKE_CXX_STANDARD:STRING=20 19 | -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/osqp-cpp/install 20 | -DOSQP_CPP_INSTALL:BOOL=ON 21 | -Dabsl_ROOT:STRING=@ABSL_ROOT_DIRECTORY@ 22 | -DEigen3_ROOT:STRING=@EIGEN3_ROOT_DIRECTORY@ 23 | -Dosqp_ROOT:STRING=@OSQP_ROOT_DIRECTORY@ 24 | @osqp-cpp_CMAKE_FLAGS@ 25 | UPDATE_DISCONNECTED 1 26 | DOWNLOAD_EXTRACT_TIMESTAMP 1) 27 | -------------------------------------------------------------------------------- /external/config/osqp/CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | 3 | project(ExternalOSQP NONE) 4 | 5 | include(ExternalProject) 6 | 7 | if(NOT @osqp_CMAKE_FLAGS@ STREQUAL "") 8 | message(STATUS "OSQP additional CMake flags: @osqp_CMAKE_FLAGS@") 9 | endif() 10 | 11 | # OSQP integrates QLDL as a Git submodule, so it is convenient to download it 12 | # using Git rather than using the URL method. 13 | ExternalProject_Add( 14 | osqp-v0.6.3 15 | GIT_REPOSITORY https://github.com/osqp/osqp.git 16 | GIT_TAG v0.6.3 17 | GIT_SHALLOW TRUE 18 | GIT_PROGRESS TRUE 19 | CMAKE_CACHE_ARGS 20 | -DCMAKE_CXX_STANDARD:STRING=20 21 | -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/osqp/install 22 | @osqp_CMAKE_FLAGS@ 23 | UPDATE_DISCONNECTED 1) 24 | -------------------------------------------------------------------------------- /external/config/pinocchio/CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | 3 | project(ExternalPinocchio NONE) 4 | 5 | include(ExternalProject) 6 | 7 | if(NOT @pinocchio_CMAKE_FLAGS@ STREQUAL "") 8 | message(STATUS "Pinocchio additional CMake flags: @pinocchio_CMAKE_FLAGS@") 9 | endif() 10 | 11 | # Pinocchio uses Git submodules, so it is convenient to download it using Git 12 | # rather than using the URL method. 13 | ExternalProject_Add( 14 | pinocchio-v2.7.0 15 | GIT_REPOSITORY https://github.com/stack-of-tasks/pinocchio.git 16 | GIT_TAG v2.7.0 17 | GIT_SHALLOW TRUE 18 | GIT_PROGRESS TRUE 19 | CMAKE_CACHE_ARGS 20 | -DCMAKE_POLICY_DEFAULT_CMP0074:STRING=NEW 21 | -DCMAKE_CXX_STANDARD:STRING=20 22 | -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/pinocchio/install 23 | -DCMAKE_SHARED_LINKER_FLAGS:STRING=-Wl,--disable-new-dtags 24 | -DBUILD_PYTHON_INTERFACE:BOOL=OFF 25 | -DBUILD_WITH_URDF_SUPPORT:BOOL=ON 26 | -DBUILD_WITH_CODEGEN_SUPPORT:BOOL=ON 27 | -DBUILD_TESTING:BOOL=OFF 28 | -DBoost_NO_WARN_NEW_VERSIONS:STRING=1 29 | -DBoost_ROOT:STRING=@BOOST_ROOT_DIRECTORY@ 30 | -DEigen3_ROOT:STRING=@EIGEN3_ROOT_DIRECTORY@ 31 | -Dconsole_bridge_ROOT:STRING=@CONSOLE_BRIDGE_ROOT_DIRECTORY@ 32 | -Durdfdom_headers_ROOT:STRING=@URDFDOM_HEADERS_ROOT_DIRECTORY@ 33 | -Durdfdom_ROOT:STRING=@URDFDOM_ROOT_DIRECTORY@ 34 | @pinocchio_CMAKE_FLAGS@ 35 | UPDATE_DISCONNECTED 1) 36 | -------------------------------------------------------------------------------- /external/config/preprocessor/CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | 3 | project(ExternalPreprocessor NONE) 4 | 5 | include(ExternalProject) 6 | 7 | if(NOT @Preprocessor_CMAKE_FLAGS@ STREQUAL "") 8 | message( 9 | STATUS "Preprocessor additional CMake flags: @Preprocessor_CMAKE_FLAGS@") 10 | endif() 11 | 12 | ExternalProject_Add( 13 | preprocessor-1.84.0-ungar 14 | URL @BUNDLED_PREPROCESSOR_FILENAME@ 15 | URL_HASH 16 | SHA256=6c5a4bf474c1ee5355d426a0a890d96186361eb66b73666bb09dc4062270435f 17 | CMAKE_CACHE_ARGS 18 | -DCMAKE_CXX_STANDARD:STRING=20 19 | -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/preprocessor/install 20 | -DBUILD_TESTING:BOOL=OFF 21 | -DBOOST_PREPROCESSOR_INSTALL:BOOL=ON 22 | @Preprocessor_CMAKE_FLAGS@ 23 | UPDATE_DISCONNECTED 1 24 | DOWNLOAD_EXTRACT_TIMESTAMP 1) 25 | -------------------------------------------------------------------------------- /external/config/preprocessor/preprocessor-1.84.0-ungar.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fdevinc/ungar/db9cc701082262aed3379f714eb20eb9f62d3185/external/config/preprocessor/preprocessor-1.84.0-ungar.zip -------------------------------------------------------------------------------- /external/config/spdlog/CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | 3 | project(ExternalSpdLog NONE) 4 | 5 | include(ExternalProject) 6 | 7 | if(NOT @spdlog_CMAKE_FLAGS@ STREQUAL "") 8 | message(STATUS "spdlog additional CMake flags: @spdlog_CMAKE_FLAGS@") 9 | endif() 10 | 11 | ExternalProject_Add( 12 | spdlog-v1.9.2 13 | URL https://github.com/gabime/spdlog/archive/refs/tags/v1.9.2.zip 14 | URL_HASH 15 | SHA256=130bd593c33e2e2abba095b551db6a05f5e4a5a19c03ab31256c38fa218aa0a6 16 | CMAKE_CACHE_ARGS 17 | -DCMAKE_CXX_STANDARD:STRING=20 18 | -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/spdlog/install 19 | -DSPDLOG_BUILD_SHARED:BOOL=OFF 20 | -DSPDLOG_BUILD_ALL:BOOL=OFF 21 | @spdlog_CMAKE_FLAGS@ 22 | UPDATE_DISCONNECTED 1 23 | DOWNLOAD_EXTRACT_TIMESTAMP 1) 24 | -------------------------------------------------------------------------------- /external/config/tinyxml2/CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | 3 | project(ExternalTinyXML2 NONE) 4 | 5 | include(ExternalProject) 6 | 7 | if(NOT @TinyXML2_CMAKE_FLAGS@ STREQUAL "") 8 | message(STATUS "TinyXML2 additional CMake flags: @TinyXML2_CMAKE_FLAGS@") 9 | endif() 10 | 11 | ExternalProject_Add( 12 | tinyxml2-10.0.0 13 | URL https://github.com/leethomason/tinyxml2/archive/refs/tags/10.0.0.zip 14 | URL_HASH 15 | SHA256=6c60b185d3570d37196d5bde17ca4f63b44a9936b515450db75ac8de5e560910 16 | CMAKE_CACHE_ARGS 17 | -DCMAKE_CXX_STANDARD:STRING=20 -DCMAKE_POSITION_INDEPENDENT_CODE:BOOL=ON 18 | -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/tinyxml2/install 19 | -DBUILD_SHARED_LIBS:BOOL=OFF @TinyXML2_CMAKE_FLAGS@ 20 | UPDATE_DISCONNECTED 1 21 | DOWNLOAD_EXTRACT_TIMESTAMP 1) 22 | -------------------------------------------------------------------------------- /external/config/urdfdom/CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | 3 | project(ExternalURDFDom NONE) 4 | 5 | include(ExternalProject) 6 | 7 | if(NOT @urdfdom_CMAKE_FLAGS@ STREQUAL "") 8 | message(STATUS "urdfdom additional CMake flags: @urdfdom_CMAKE_FLAGS@") 9 | endif() 10 | 11 | ExternalProject_Add( 12 | urdfdom-4.0.0 13 | URL https://github.com/ros/urdfdom/archive/refs/tags/4.0.0.zip 14 | URL_HASH 15 | SHA256=a74b9d38e2b018bd72dc663499a6dec14994243c9029733ea89c4c63a463483b 16 | CMAKE_CACHE_ARGS 17 | -DCMAKE_POLICY_DEFAULT_CMP0074:STRING=NEW 18 | -DCMAKE_CXX_STANDARD:STRING=20 19 | -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/urdfdom/install 20 | -DTinyXML2_ROOT:STRING=@TINYXML2_ROOT_DIRECTORY@ 21 | -Dconsole_bridge_ROOT:STRING=@CONSOLE_BRIDGE_ROOT_DIRECTORY@ 22 | -Durdfdom_headers_ROOT:STRING=@URDFDOM_HEADERS_ROOT_DIRECTORY@ 23 | @urdfdom_CMAKE_FLAGS@ 24 | UPDATE_DISCONNECTED 1 25 | DOWNLOAD_EXTRACT_TIMESTAMP 1) 26 | -------------------------------------------------------------------------------- /external/config/urdfdom_headers/CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | 3 | project(ExternalURDFDomHeaders NONE) 4 | 5 | include(ExternalProject) 6 | 7 | if(NOT @urdfdom_headers_CMAKE_FLAGS@ STREQUAL "") 8 | message( 9 | STATUS 10 | "urdfdom_headers additional CMake flags: @urdfdom_headers_CMAKE_FLAGS@") 11 | endif() 12 | 13 | ExternalProject_Add( 14 | urdfdom_headers-1.1.1 15 | URL https://github.com/ros/urdfdom_headers/archive/refs/tags/1.1.1.zip 16 | URL_HASH 17 | SHA256=dde77e3dd96ffa41e2ee0a20bddcd6ef05863e95ce0143ede77130d8cd46c644 18 | CMAKE_CACHE_ARGS 19 | -DCMAKE_CXX_STANDARD:STRING=20 20 | -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/urdfdom_headers/install 21 | @urdfdom_headers_CMAKE_FLAGS@ 22 | UPDATE_DISCONNECTED 1 23 | DOWNLOAD_EXTRACT_TIMESTAMP 1) 24 | -------------------------------------------------------------------------------- /include/ungar/assert.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/assert.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__ASSERT_HPP_ 28 | #define _UNGAR__ASSERT_HPP_ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #ifdef UNGAR_CONFIG_ENABLE_LOGGING 35 | #include "ungar/io/logging.hpp" 36 | #endif 37 | 38 | namespace Ungar { 39 | 40 | inline constexpr long expect(long exp, long c) { 41 | if (exp == c) [[likely]] { 42 | return c; 43 | } else { 44 | return exp; 45 | } 46 | } 47 | 48 | inline void assertion_failed( 49 | char const* expr, const std::source_location location = std::source_location::current()) { 50 | #ifdef UNGAR_CONFIG_ENABLE_LOGGING 51 | UNGAR_LOG(error, 52 | "{}:{}: {}: Assertion `{}` failed.", 53 | location.file_name(), 54 | location.line(), 55 | location.function_name(), 56 | expr); 57 | #else 58 | std::fprintf(stderr, 59 | "%s:%u: %s: Assertion `%s` failed.", 60 | location.file_name(), 61 | location.line(), 62 | location.function_name(), 63 | expr); 64 | #endif 65 | std::abort(); 66 | } 67 | 68 | inline void assertion_failed_msg( 69 | char const* expr, 70 | char const* msg, 71 | const std::source_location location = std::source_location::current()) { 72 | #ifdef UNGAR_CONFIG_ENABLE_LOGGING 73 | UNGAR_LOG(error, 74 | "{}:{}: {}: Assertion `{} && \"{}\"` failed.", 75 | location.file_name(), 76 | location.line(), 77 | location.function_name(), 78 | expr, 79 | msg); 80 | #else 81 | std::fprintf(stderr, 82 | "%s:%u: %s: Assertion `%s && \"%s\"` failed.", 83 | location.file_name(), 84 | location.line(), 85 | location.function_name(), 86 | expr, 87 | msg); 88 | #endif 89 | std::abort(); 90 | } 91 | 92 | } // namespace Ungar 93 | 94 | #define UNGAR_LIKELY(x) ::Ungar::expect(x, 1) 95 | #define UNGAR_UNLIKELY(x) ::Ungar::expect(x, 0) 96 | 97 | #ifdef UNGAR_CONFIG_ENABLE_RELEASE_MODE 98 | #define UNGAR_ASSERT(expr) ((void)0) 99 | #define UNGAR_ASSERT_MSG(expr, msg) ((void)0) 100 | #define UNGAR_ASSERT_EXPLICIT(expr) ((void)0) 101 | #define UNGAR_ASSERT_EXPLICIT_MSG(expr, msg) ((void)0) 102 | #else 103 | #define UNGAR_ASSERT(expr) (UNGAR_LIKELY(!!(expr)) ? ((void)0) : ::Ungar::assertion_failed(#expr)) 104 | #define UNGAR_ASSERT_MSG(expr, msg) \ 105 | (UNGAR_LIKELY(!!(expr)) ? ((void)0) : ::Ungar::assertion_failed_msg(#expr, msg)) 106 | #define UNGAR_ASSERT_EXPLICIT(expr) UNGAR_ASSERT(static_cast(expr)) 107 | #define UNGAR_ASSERT_EXPLICIT_MSG(expr, msg) UNGAR_ASSERT_MSG(static_cast(expr), msg) 108 | #endif 109 | 110 | #endif /* _UNGAR__ASSERT_HPP_ */ 111 | -------------------------------------------------------------------------------- /include/ungar/autodiff/vector_composer.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/autodiff/vector_composer.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__AUTODIFF__VECTOR_COMPOSER_HPP_ 28 | #define _UNGAR__AUTODIFF__VECTOR_COMPOSER_HPP_ 29 | 30 | #include "ungar/autodiff/data_types.hpp" 31 | 32 | namespace Ungar { 33 | namespace Autodiff { 34 | 35 | class VectorComposer { 36 | public: 37 | VectorComposer() : _impl{} { 38 | } 39 | 40 | template // clang-format off 41 | requires std::same_as 42 | VectorComposer& operator<<( 43 | const Eigen::MatrixBase<_Vector>& vector) { // clang-format on 44 | _impl.emplace_back(vector); 45 | return *this; 46 | } 47 | 48 | VectorComposer& operator<<(const ad_scalar_t& scalar) { 49 | _impl.emplace_back((VectorXad{1_idx} << scalar).finished()); 50 | return *this; 51 | } 52 | 53 | void Clear() { 54 | _impl.clear(); 55 | } 56 | 57 | index_t Size() const { 58 | index_t size = 0_idx; 59 | for (const VectorXad& el : _impl) { 60 | size += el.size(); 61 | } 62 | return size; 63 | } 64 | 65 | VectorXad Compose() const { 66 | VectorXad composedOutput{Size()}; 67 | for (index_t i = 0; const VectorXad& el : _impl) { 68 | composedOutput.segment(i, el.size()) = el; 69 | i += el.size(); 70 | } 71 | return composedOutput; 72 | } 73 | 74 | private: 75 | std::vector _impl; 76 | }; 77 | 78 | } // namespace Autodiff 79 | } // namespace Ungar 80 | 81 | #endif /* _UNGAR__AUTODIFF__VECTOR_COMPOSER_HPP_ */ 82 | -------------------------------------------------------------------------------- /include/ungar/io/logging.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/io/logging.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__IO__LOGGING_HPP_ 28 | #define _UNGAR__IO__LOGGING_HPP_ 29 | 30 | #ifdef UNGAR_CONFIG_ENABLE_LOGGING 31 | 32 | #ifndef UNGAR_CONFIG_ENABLE_RELEASE_MODE 33 | #define SPDLOG_ACTIVE_LEVEL SPDLOG_LEVEL_TRACE 34 | #endif 35 | 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | namespace Ungar { 44 | namespace Internal { 45 | 46 | class Logger { 47 | public: 48 | static constexpr const char* NAME = "ungar"; 49 | 50 | private: 51 | Logger() = delete; 52 | Logger(const Logger&) = delete; 53 | Logger(Logger&&) = delete; 54 | Logger& operator=(const Logger&) = delete; 55 | Logger& operator=(Logger&&) = delete; 56 | 57 | static auto InitializeLogger() { 58 | static bool initialized = false; 59 | if (initialized) { 60 | throw std::logic_error("Can initialize Ungar's logger only once."); 61 | } 62 | 63 | auto logger = spdlog::stdout_color_mt(NAME); 64 | #ifndef UNGAR_CONFIG_ENABLE_RELEASE_MODE 65 | logger->set_level(spdlog::level::trace); 66 | #endif 67 | logger->set_pattern("[%H:%M:%S.%e] [%n] [%^%l%$] %v"); 68 | 69 | initialized = true; 70 | return logger; 71 | } 72 | 73 | public: 74 | static auto& Get() { 75 | static auto logger = InitializeLogger(); 76 | return logger; 77 | } 78 | }; 79 | 80 | } // namespace Internal 81 | } // namespace Ungar 82 | 83 | #define _UNGAR_LOG_IMPL_trace SPDLOG_LOGGER_TRACE 84 | #define _UNGAR_LOG_IMPL_debug SPDLOG_LOGGER_DEBUG 85 | #define _UNGAR_LOG_IMPL_info SPDLOG_LOGGER_INFO 86 | #define _UNGAR_LOG_IMPL_warn SPDLOG_LOGGER_WARN 87 | #define _UNGAR_LOG_IMPL_error SPDLOG_LOGGER_ERROR 88 | #define _UNGAR_LOG_IMPL_critical SPDLOG_LOGGER_CRITICAL 89 | #define UNGAR_LOG(level, ...) _UNGAR_LOG_IMPL_##level(::Ungar::Internal::Logger::Get(), __VA_ARGS__) 90 | 91 | namespace fmt { 92 | 93 | template // clang-format off 94 | requires boost::hana::Struct<_HanaStruct>::value 95 | struct formatter<_HanaStruct> { // clang-format on 96 | char presentation = 'v'; 97 | 98 | constexpr auto parse(format_parse_context& ctx) -> decltype(ctx.begin()) { 99 | auto it = ctx.begin(); 100 | const auto end = ctx.end(); 101 | if (it != end && (*it == 'v' || *it == 'c')) { 102 | presentation = *(it++); 103 | } 104 | 105 | if (it != end && *it != '}') { 106 | throw format_error( 107 | "Invalid format: the options available are 'v' (\"verbose\") or 'c' " 108 | "(\"compact\")."); 109 | } 110 | 111 | return it; 112 | } 113 | 114 | template 115 | auto format(const _HanaStruct& hanaStruct, _FormatContext& ctx) -> decltype(ctx.out()) { 116 | namespace hana = boost::hana; 117 | 118 | auto separator = [&](auto i) -> const char* { 119 | if constexpr (i == 0UL) { 120 | return presentation == 'v' ? "\t" : ""; 121 | } else { 122 | return presentation == 'v' ? ",\n\t" : ", "; 123 | } 124 | }; 125 | 126 | std::string buffer; 127 | hana::length(hanaStruct).times.with_index([&](auto i) { 128 | std::ignore = presentation == 'v' ? fmt::format_to(std::back_inserter(buffer), 129 | "{}{} = {}", 130 | separator(i), 131 | hana::keys(hanaStruct)[i].c_str(), 132 | hana::members(hanaStruct)[i]) 133 | : fmt::format_to(std::back_inserter(buffer), 134 | "{}{}", 135 | separator(i), 136 | hana::members(hanaStruct)[i]); 137 | }); 138 | 139 | if (presentation == 'v') { 140 | return fmt::format_to(ctx.out(), "\n{{\n{}\n}}", buffer); 141 | } else { 142 | return fmt::format_to(ctx.out(), "{{ {} }}", buffer); 143 | } 144 | } 145 | }; 146 | 147 | } // namespace fmt 148 | 149 | #else 150 | #error \ 151 | "Ungar's logging functionalities are disabled. To enable them, set the CMake flag UNGAR_ENABLE_LOGGING to 'ON'." 152 | #endif 153 | 154 | #endif /* _UNGAR__IO__LOGGING_HPP_ */ 155 | -------------------------------------------------------------------------------- /include/ungar/optimization/soft_equality_constraint.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/optimization/soft_equality_constraint.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__OPTIMIZATION__SOFT_EQUALITY_CONSTRAINT_HPP_ 28 | #define _UNGAR__OPTIMIZATION__SOFT_EQUALITY_CONSTRAINT_HPP_ 29 | 30 | #include "ungar/data_types.hpp" 31 | 32 | namespace Ungar { 33 | 34 | class SoftEqualityConstraint { 35 | public: 36 | constexpr SoftEqualityConstraint(const real_t rhs, const real_t stiffness = 1.0) 37 | : _rhs{rhs}, _stiffness{stiffness} { 38 | } 39 | 40 | template 41 | _Scalar Evaluate(const _Scalar& lhs) const { 42 | return 0.5 * _stiffness * pow(lhs - _rhs, 2); 43 | } 44 | 45 | template 46 | _Scalar Evaluate(const RefToConstVectorX<_Scalar>& lhs) const { 47 | return 0.5 * _stiffness * (lhs.array() - _rhs).square().sum(); 48 | } 49 | 50 | private: 51 | real_t _rhs, _stiffness; 52 | }; 53 | 54 | } // namespace Ungar 55 | 56 | #endif /* _UNGAR__OPTIMIZATION__SOFT_EQUALITY_CONSTRAINT_HPP_ */ 57 | -------------------------------------------------------------------------------- /include/ungar/rbd/evaluator.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/rbd/evaluator.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__RBD__EVALUATOR_HPP_ 28 | #define _UNGAR__RBD__EVALUATOR_HPP_ 29 | 30 | #include "ungar/data_types.hpp" 31 | #include "ungar/utils/macros/for_each.hpp" 32 | 33 | namespace Ungar { 34 | 35 | namespace RBD { 36 | 37 | template 38 | struct Evaluator { 39 | static_assert(dependent_false, 40 | "The evaluator for the given quantity was not included or does not exist."); 41 | }; 42 | 43 | #define _UNGAR_MAKE_EVALUATOR_HELPER_1(arg) , const auto& arg 44 | #define _UNGAR_MAKE_EVALUATOR_HELPER_2(arg) , arg 45 | #define UNGAR_MAKE_EVALUATOR(quantity, algorithm, firstArg, ...) \ 46 | template \ 47 | struct Evaluator<::Ungar::RBD::Quantities::quantity, _Scalar> { \ 48 | void At(const auto& firstArg UNGAR_FOR_EACH(_UNGAR_MAKE_EVALUATOR_HELPER_1, \ 49 | __VA_ARGS__)) { \ 50 | ::pinocchio::algorithm( \ 51 | model, \ 52 | data, \ 53 | firstArg UNGAR_FOR_EACH(_UNGAR_MAKE_EVALUATOR_HELPER_2, __VA_ARGS__)); \ 54 | } \ 55 | \ 56 | const ::pinocchio ::ModelTpl<_Scalar>& model; \ 57 | ::pinocchio ::DataTpl<_Scalar>& data; \ 58 | } 59 | 60 | } // namespace RBD 61 | } // namespace Ungar 62 | 63 | #endif /* _UNGAR__RBD__EVALUATOR_HPP_ */ 64 | -------------------------------------------------------------------------------- /include/ungar/rbd/getter.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/rbd/getter.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__RBD__GETTER_HPP_ 28 | #define _UNGAR__RBD__GETTER_HPP_ 29 | 30 | #include "ungar/data_types.hpp" 31 | 32 | namespace Ungar { 33 | 34 | namespace RBD { 35 | 36 | template 37 | struct Getter { 38 | static_assert(dependent_false, 39 | "The getter for the given quantity was not included or does not exist."); 40 | }; 41 | 42 | #define UNGAR_MAKE_GETTER(quantity, dataMember) \ 43 | template \ 44 | struct Getter<::Ungar::RBD::Quantities::quantity, _Scalar> { \ 45 | const auto& Get() const { \ 46 | return data.dataMember; \ 47 | } \ 48 | \ 49 | auto& Get() { \ 50 | return data.dataMember; \ 51 | } \ 52 | \ 53 | const ::pinocchio ::ModelTpl<_Scalar>& model; \ 54 | ::pinocchio ::DataTpl<_Scalar>& data; \ 55 | } 56 | 57 | } // namespace RBD 58 | } // namespace Ungar 59 | 60 | #endif /* _UNGAR__RBD__GETTER_HPP_ */ 61 | -------------------------------------------------------------------------------- /include/ungar/rbd/quantities/centroidal_momentum.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/rbd/quantities/centroidal_momentum.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__RBD__QUANTITIES__CENTROIDAL_MOMENTUM_HPP_ 28 | #define _UNGAR__RBD__QUANTITIES__CENTROIDAL_MOMENTUM_HPP_ 29 | 30 | #include "ungar/rbd/quantity.hpp" 31 | 32 | #include "pinocchio/algorithm/centroidal.hpp" 33 | 34 | namespace Ungar { 35 | namespace RBD { 36 | namespace Quantities { 37 | 38 | UNGAR_MAKE_QUANTITY(centroidal_momentum); 39 | 40 | } 41 | 42 | UNGAR_MAKE_EVALUATOR(centroidal_momentum, ccrba, q, v); 43 | UNGAR_MAKE_GETTER(centroidal_momentum, hg); 44 | 45 | } // namespace RBD 46 | } // namespace Ungar 47 | 48 | #endif /* _UNGAR__RBD__QUANTITIES__CENTROIDAL_MOMENTUM_HPP_ */ 49 | -------------------------------------------------------------------------------- /include/ungar/rbd/quantities/centroidal_momentum_matrix.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/rbd/quantities/centroidal_momentum_matrix.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__RBD__QUANTITIES__CENTROIDAL_MOMENTUM_MATRIX_HPP_ 28 | #define _UNGAR__RBD__QUANTITIES__CENTROIDAL_MOMENTUM_MATRIX_HPP_ 29 | 30 | #include "ungar/rbd/quantity.hpp" 31 | 32 | #include "pinocchio/algorithm/crba.hpp" 33 | 34 | namespace Ungar { 35 | namespace RBD { 36 | namespace Quantities { 37 | 38 | UNGAR_MAKE_QUANTITY(centroidal_momentum_matrix); 39 | 40 | } 41 | 42 | UNGAR_MAKE_EVALUATOR(centroidal_momentum_matrix, crbaMinimal, q); 43 | UNGAR_MAKE_GETTER(centroidal_momentum_matrix, Ag); 44 | 45 | } // namespace RBD 46 | } // namespace Ungar 47 | 48 | #endif /* _UNGAR__RBD__QUANTITIES__CENTROIDAL_MOMENTUM_MATRIX_HPP_ */ 49 | -------------------------------------------------------------------------------- /include/ungar/rbd/quantities/com_acceleration.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/rbd/quantities/com_acceleration.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__RBD__QUANTITIES__COM_ACCELERATION_HPP_ 28 | #define _UNGAR__RBD__QUANTITIES__COM_ACCELERATION_HPP_ 29 | 30 | #include "ungar/rbd/quantity.hpp" 31 | 32 | #include "pinocchio/algorithm/center-of-mass.hpp" 33 | 34 | namespace Ungar { 35 | namespace RBD { 36 | namespace Quantities { 37 | 38 | UNGAR_MAKE_QUANTITY(com_acceleration); 39 | 40 | } 41 | 42 | UNGAR_MAKE_EVALUATOR(com_acceleration, centerOfMass, q, v, a); 43 | UNGAR_MAKE_GETTER(com_acceleration, acom.front()); 44 | 45 | } // namespace RBD 46 | } // namespace Ungar 47 | 48 | #endif /* _UNGAR__RBD__QUANTITIES__COM_ACCELERATION_HPP_ */ 49 | -------------------------------------------------------------------------------- /include/ungar/rbd/quantities/com_position.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/rbd/quantities/com_position.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__RBD__QUANTITIES__COM_POSITION_HPP_ 28 | #define _UNGAR__RBD__QUANTITIES__COM_POSITION_HPP_ 29 | 30 | #include "ungar/rbd/quantity.hpp" 31 | 32 | #include "pinocchio/algorithm/center-of-mass.hpp" 33 | 34 | namespace Ungar { 35 | namespace RBD { 36 | namespace Quantities { 37 | 38 | UNGAR_MAKE_QUANTITY(com_position); 39 | 40 | } 41 | 42 | UNGAR_MAKE_EVALUATOR(com_position, centerOfMass, q); 43 | UNGAR_MAKE_GETTER(com_position, com.front()); 44 | 45 | } // namespace RBD 46 | } // namespace Ungar 47 | 48 | #endif /* _UNGAR__RBD__QUANTITIES__COM_POSITION_HPP_ */ 49 | -------------------------------------------------------------------------------- /include/ungar/rbd/quantities/com_velocity.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/rbd/quantities/com_velocity.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__RBD__QUANTITIES__COM_VELOCITY_HPP_ 28 | #define _UNGAR__RBD__QUANTITIES__COM_VELOCITY_HPP_ 29 | 30 | #include "ungar/rbd/quantity.hpp" 31 | 32 | #include "pinocchio/algorithm/center-of-mass.hpp" 33 | 34 | namespace Ungar { 35 | namespace RBD { 36 | namespace Quantities { 37 | 38 | UNGAR_MAKE_QUANTITY(com_velocity); 39 | 40 | } 41 | 42 | UNGAR_MAKE_EVALUATOR(com_velocity, centerOfMass, q, v); 43 | UNGAR_MAKE_GETTER(com_velocity, vcom.front()); 44 | 45 | } // namespace RBD 46 | } // namespace Ungar 47 | 48 | #endif /* _UNGAR__RBD__QUANTITIES__COM_VELOCITY_HPP_ */ 49 | -------------------------------------------------------------------------------- /include/ungar/rbd/quantities/composite_rigid_body_inertia.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/rbd/quantities/composite_rigid_body_inertia.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__RBD__QUANTITIES__COMPOSITE_RIGID_BODY_INERTIA_HPP_ 28 | #define _UNGAR__RBD__QUANTITIES__COMPOSITE_RIGID_BODY_INERTIA_HPP_ 29 | 30 | #include "ungar/rbd/quantity.hpp" 31 | 32 | #include "pinocchio/algorithm/centroidal.hpp" 33 | 34 | namespace Ungar { 35 | namespace RBD { 36 | namespace Quantities { 37 | 38 | UNGAR_MAKE_QUANTITY(composite_rigid_body_inertia); 39 | 40 | } 41 | 42 | UNGAR_MAKE_EVALUATOR(composite_rigid_body_inertia, ccrba, q, v); 43 | UNGAR_MAKE_GETTER(composite_rigid_body_inertia, Ig); 44 | 45 | } // namespace RBD 46 | } // namespace Ungar 47 | 48 | #endif /* _UNGAR__RBD__QUANTITIES__COMPOSITE_RIGID_BODY_INERTIA_HPP_ */ 49 | -------------------------------------------------------------------------------- /include/ungar/rbd/quantities/frames.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/rbd/quantities/frames.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__RBD__QUANTITIES__FRAMES_HPP_ 28 | #define _UNGAR__RBD__QUANTITIES__FRAMES_HPP_ 29 | 30 | #include "ungar/rbd/quantity.hpp" 31 | 32 | #include "pinocchio/algorithm/frames.hpp" 33 | 34 | namespace Ungar { 35 | namespace RBD { 36 | namespace Quantities { 37 | 38 | UNGAR_MAKE_QUANTITY(frames); 39 | 40 | } 41 | 42 | UNGAR_MAKE_EVALUATOR(frames, framesForwardKinematics, q); 43 | UNGAR_MAKE_GETTER(frames, oMf); 44 | 45 | } // namespace RBD 46 | } // namespace Ungar 47 | 48 | #endif /* _UNGAR__RBD__QUANTITIES__FRAMES_HPP_ */ 49 | -------------------------------------------------------------------------------- /include/ungar/rbd/quantities/generalized_accelerations.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/rbd/quantities/generalized_accelerations.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__RBD__QUANTITIES__GENERALIZED_ACCELERATIONS_HPP_ 28 | #define _UNGAR__RBD__QUANTITIES__GENERALIZED_ACCELERATIONS_HPP_ 29 | 30 | #include "ungar/rbd/quantity.hpp" 31 | 32 | #include "pinocchio/algorithm/aba.hpp" 33 | 34 | namespace Ungar { 35 | namespace RBD { 36 | namespace Quantities { 37 | 38 | UNGAR_MAKE_QUANTITY(generalized_accelerations); 39 | 40 | } 41 | 42 | UNGAR_MAKE_EVALUATOR(generalized_accelerations, aba, q, v, tau); 43 | UNGAR_MAKE_GETTER(generalized_accelerations, ddq); 44 | 45 | } // namespace RBD 46 | } // namespace Ungar 47 | 48 | #endif /* _UNGAR__RBD__QUANTITIES__GENERALIZED_ACCELERATIONS_HPP_ */ 49 | -------------------------------------------------------------------------------- /include/ungar/rbd/quantities/generalized_gravity.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/rbd/quantities/generalized_gravity.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__RBD__QUANTITIES__GENERALIZED_GRAVITY_HPP_ 28 | #define _UNGAR__RBD__QUANTITIES__GENERALIZED_GRAVITY_HPP_ 29 | 30 | #include "ungar/rbd/quantity.hpp" 31 | 32 | #include "pinocchio/algorithm/rnea.hpp" 33 | 34 | namespace Ungar { 35 | namespace RBD { 36 | namespace Quantities { 37 | 38 | UNGAR_MAKE_QUANTITY(generalized_gravity); 39 | 40 | } 41 | 42 | UNGAR_MAKE_EVALUATOR(generalized_gravity, computeGeneralizedGravity, q); 43 | UNGAR_MAKE_GETTER(generalized_gravity, g); 44 | 45 | } // namespace RBD 46 | } // namespace Ungar 47 | 48 | #endif /* _UNGAR__RBD__QUANTITIES__GENERALIZED_GRAVITY_HPP_ */ 49 | -------------------------------------------------------------------------------- /include/ungar/rbd/quantities/joint_space_inertia_matrix.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/rbd/quantities/joint_space_inertia_matrix.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__RBD__QUANTITIES__JOINT_SPACE_INERTIA_MATRIX_HPP_ 28 | #define _UNGAR__RBD__QUANTITIES__JOINT_SPACE_INERTIA_MATRIX_HPP_ 29 | 30 | #include "ungar/rbd/quantity.hpp" 31 | 32 | #include "pinocchio/algorithm/crba.hpp" 33 | 34 | namespace Ungar { 35 | namespace RBD { 36 | namespace Quantities { 37 | 38 | UNGAR_MAKE_QUANTITY(joint_space_inertia_matrix); 39 | 40 | } 41 | 42 | UNGAR_MAKE_EVALUATOR(joint_space_inertia_matrix, crba, q); 43 | UNGAR_MAKE_GETTER(joint_space_inertia_matrix, M); 44 | 45 | } // namespace RBD 46 | } // namespace Ungar 47 | 48 | #endif /* _UNGAR__RBD__QUANTITIES__JOINT_SPACE_INERTIA_MATRIX_HPP_ */ 49 | -------------------------------------------------------------------------------- /include/ungar/rbd/quantities/joint_space_inertia_matrix_inverse.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/rbd/quantities/joint_space_inertia_matrix_inverse.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__RBD__QUANTITIES__JOINT_SPACE_INERTIA_MATRIX_INVERSE_HPP_ 28 | #define _UNGAR__RBD__QUANTITIES__JOINT_SPACE_INERTIA_MATRIX_INVERSE_HPP_ 29 | 30 | #include "ungar/rbd/quantity.hpp" 31 | 32 | #include "pinocchio/algorithm/aba.hpp" 33 | 34 | namespace Ungar { 35 | namespace RBD { 36 | namespace Quantities { 37 | 38 | UNGAR_MAKE_QUANTITY(joint_space_inertia_matrix_inverse); 39 | 40 | } 41 | 42 | UNGAR_MAKE_EVALUATOR(joint_space_inertia_matrix_inverse, computeMinverse, q); 43 | UNGAR_MAKE_GETTER(joint_space_inertia_matrix_inverse, Minv); 44 | 45 | } // namespace RBD 46 | } // namespace Ungar 47 | 48 | #endif /* _UNGAR__RBD__QUANTITIES__JOINT_SPACE_INERTIA_MATRIX_INVERSE_HPP_ */ 49 | -------------------------------------------------------------------------------- /include/ungar/rbd/quantities/joint_torques.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/rbd/quantities/joint_torques.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__RBD__QUANTITIES__JOINT_TORQUES_HPP_ 28 | #define _UNGAR__RBD__QUANTITIES__JOINT_TORQUES_HPP_ 29 | 30 | #include "ungar/rbd/quantity.hpp" 31 | 32 | #include "pinocchio/algorithm/rnea.hpp" 33 | 34 | namespace Ungar { 35 | namespace RBD { 36 | namespace Quantities { 37 | 38 | UNGAR_MAKE_QUANTITY(joint_torques); 39 | 40 | } 41 | 42 | UNGAR_MAKE_EVALUATOR(joint_torques, rnea, q, v, a); 43 | UNGAR_MAKE_GETTER(joint_torques, tau); 44 | 45 | } // namespace RBD 46 | } // namespace Ungar 47 | 48 | #endif /* _UNGAR__RBD__QUANTITIES__JOINT_TORQUES_HPP_ */ 49 | -------------------------------------------------------------------------------- /include/ungar/rbd/quantities/kinetic_energy.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/rbd/quantities/kinetic_energy.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__RBD__QUANTITIES__KINETIC_ENERGY_HPP_ 28 | #define _UNGAR__RBD__QUANTITIES__KINETIC_ENERGY_HPP_ 29 | 30 | #include "ungar/rbd/quantity.hpp" 31 | 32 | #include "pinocchio/algorithm/energy.hpp" 33 | 34 | namespace Ungar { 35 | namespace RBD { 36 | namespace Quantities { 37 | 38 | UNGAR_MAKE_QUANTITY(kinetic_energy); 39 | 40 | } 41 | 42 | UNGAR_MAKE_EVALUATOR(kinetic_energy, computeKineticEnergy, q, v); 43 | UNGAR_MAKE_GETTER(kinetic_energy, kinetic_energy); 44 | 45 | } // namespace RBD 46 | } // namespace Ungar 47 | 48 | #endif /* _UNGAR__RBD__QUANTITIES__KINETIC_ENERGY_HPP_ */ 49 | -------------------------------------------------------------------------------- /include/ungar/rbd/quantities/nonlinear_effects.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/rbd/quantities/nonlinear_effects.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__RBD__QUANTITIES__NONLINEAR_EFFECTS_HPP_ 28 | #define _UNGAR__RBD__QUANTITIES__NONLINEAR_EFFECTS_HPP_ 29 | 30 | #include "ungar/rbd/quantity.hpp" 31 | 32 | #include "pinocchio/algorithm/rnea.hpp" 33 | 34 | namespace Ungar { 35 | namespace RBD { 36 | namespace Quantities { 37 | 38 | UNGAR_MAKE_QUANTITY(nonlinear_effects); 39 | 40 | } 41 | 42 | UNGAR_MAKE_EVALUATOR(nonlinear_effects, nonLinearEffects, q, v); 43 | UNGAR_MAKE_GETTER(nonlinear_effects, nle); 44 | 45 | } // namespace RBD 46 | } // namespace Ungar 47 | 48 | #endif /* _UNGAR__RBD__QUANTITIES__NONLINEAR_EFFECTS_HPP_ */ 49 | -------------------------------------------------------------------------------- /include/ungar/rbd/quantities/potential_energy.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/rbd/quantities/potential_energy.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__RBD__QUANTITIES__POTENTIAL_ENERGY_HPP_ 28 | #define _UNGAR__RBD__QUANTITIES__POTENTIAL_ENERGY_HPP_ 29 | 30 | #include "ungar/rbd/quantity.hpp" 31 | 32 | #include "pinocchio/algorithm/energy.hpp" 33 | 34 | namespace Ungar { 35 | namespace RBD { 36 | namespace Quantities { 37 | 38 | UNGAR_MAKE_QUANTITY(potential_energy); 39 | 40 | } 41 | 42 | UNGAR_MAKE_EVALUATOR(potential_energy, computePotentialEnergy, q); 43 | UNGAR_MAKE_GETTER(potential_energy, potential_energy); 44 | 45 | } // namespace RBD 46 | } // namespace Ungar 47 | 48 | #endif /* _UNGAR__RBD__QUANTITIES__POTENTIAL_ENERGY_HPP_ */ 49 | -------------------------------------------------------------------------------- /include/ungar/rbd/quantities/quantities.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/rbd/quantities/quantities.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__RBD__QUANTITIES__QUANTITIES_HPP_ 28 | #define _UNGAR__RBD__QUANTITIES__QUANTITIES_HPP_ 29 | 30 | #include "ungar/rbd/quantities/centroidal_momentum.hpp" 31 | #include "ungar/rbd/quantities/centroidal_momentum_matrix.hpp" 32 | #include "ungar/rbd/quantities/com_acceleration.hpp" 33 | #include "ungar/rbd/quantities/com_position.hpp" 34 | #include "ungar/rbd/quantities/com_velocity.hpp" 35 | #include "ungar/rbd/quantities/composite_rigid_body_inertia.hpp" 36 | #include "ungar/rbd/quantities/frames.hpp" 37 | #include "ungar/rbd/quantities/generalized_accelerations.hpp" 38 | #include "ungar/rbd/quantities/generalized_gravity.hpp" 39 | #include "ungar/rbd/quantities/joint_space_inertia_matrix.hpp" 40 | #include "ungar/rbd/quantities/joint_space_inertia_matrix_inverse.hpp" 41 | #include "ungar/rbd/quantities/joint_torques.hpp" 42 | #include "ungar/rbd/quantities/kinetic_energy.hpp" 43 | #include "ungar/rbd/quantities/nonlinear_effects.hpp" 44 | 45 | #endif /* _UNGAR__RBD__QUANTITIES__QUANTITIES_HPP_ */ 46 | -------------------------------------------------------------------------------- /include/ungar/rbd/quantity.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/rbd/quantities.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__RBD__QUANTITY_HPP_ 28 | #define _UNGAR__RBD__QUANTITY_HPP_ 29 | 30 | #include "pinocchio/codegen/cppadcg.hpp" 31 | 32 | #include "ungar/rbd/evaluator.hpp" 33 | #include "ungar/rbd/getter.hpp" 34 | 35 | namespace Ungar { 36 | namespace RBD { 37 | namespace Quantities { 38 | 39 | /** 40 | * @brief Define a quantity computable using rigid body dynamics algorithms. 41 | */ 42 | #define UNGAR_MAKE_QUANTITY(name) \ 43 | inline constexpr struct name##_t { \ 44 | } name 45 | 46 | } // namespace Quantities 47 | } // namespace RBD 48 | } // namespace Ungar 49 | 50 | #endif /* _UNGAR__RBD__QUANTITY_HPP_ */ 51 | -------------------------------------------------------------------------------- /include/ungar/rbd/robot.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/rbd/robot.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__RBD__ROBOT_HPP_ 28 | #define _UNGAR__RBD__ROBOT_HPP_ 29 | 30 | #include "ungar/rbd/quantity.hpp" 31 | 32 | #include "pinocchio/algorithm/joint-configuration.hpp" 33 | #include "pinocchio/parsers/urdf.hpp" 34 | 35 | #include "ungar/utils/utils.hpp" 36 | 37 | namespace Ungar { 38 | 39 | template 40 | class Robot { 41 | public: 42 | template 43 | Robot(const std::string& urdfFilename, const _RootJoint& rootJoint = {}) : _model{}, _data{} { 44 | pinocchio::Model model; 45 | pinocchio::urdf::buildModel(urdfFilename, rootJoint, model); 46 | 47 | _model = model.cast<_Scalar>(); 48 | _data = std::make_unique>(_model); 49 | UNGAR_ASSERT(_model.check(*_data)); 50 | } 51 | 52 | Robot(const Robot& other) 53 | : _model{other._model}, _data{std::make_unique>(_model)} { 54 | UNGAR_ASSERT(_model.check(*_data)); 55 | } 56 | 57 | constexpr auto Compute(auto quantity) { 58 | return RBD::Evaluator{_model, *_data}; 59 | } 60 | 61 | decltype(auto) Get(auto quantity) const { 62 | const auto getter = RBD::Getter{_model, *_data}; 63 | return getter.Get(); 64 | } 65 | 66 | decltype(auto) Get(auto quantity, auto&&... args) const { 67 | const auto getter = RBD::Getter{_model, *_data}; 68 | return getter.Get(std::forward(args)...); 69 | } 70 | 71 | decltype(auto) Get(auto quantity) { 72 | auto getter = RBD::Getter{_model, *_data}; 73 | return getter.Get(); 74 | } 75 | 76 | decltype(auto) Get(auto quantity, auto&&... args) { 77 | auto getter = RBD::Getter{_model, *_data}; 78 | return getter.Get(std::forward(args)...); 79 | } 80 | 81 | const pinocchio::ModelTpl<_Scalar>& Model() const { 82 | return _model; 83 | } 84 | 85 | pinocchio::ModelTpl<_Scalar>& Model() { 86 | return _model; 87 | } 88 | 89 | const pinocchio::DataTpl<_Scalar>& Data() const { 90 | return *_data; 91 | } 92 | 93 | pinocchio::DataTpl<_Scalar>& Data() { 94 | return *_data; 95 | } 96 | 97 | VectorX<_Scalar> RandomConfiguration() const { 98 | return pinocchio::randomConfiguration(_model); 99 | } 100 | 101 | protected: 102 | pinocchio::ModelTpl<_Scalar> _model; 103 | std::unique_ptr> _data; 104 | }; 105 | 106 | } // namespace Ungar 107 | 108 | #endif /* _UNGAR__RBD__ROBOT_HPP_ */ 109 | -------------------------------------------------------------------------------- /include/ungar/utils/defaulted.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/utils/defaulted.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__UTILS__DEFAULTED_HPP_ 28 | #define _UNGAR__UTILS__DEFAULTED_HPP_ 29 | 30 | #include 31 | 32 | namespace Ungar { 33 | namespace Internal { 34 | 35 | template 36 | struct default_helper { 37 | using value_type = _T; 38 | 39 | constexpr default_helper(_T v) : value{std::move(v)} { 40 | } 41 | 42 | _T value; 43 | }; 44 | 45 | } // namespace Internal 46 | 47 | template 48 | class defaulted : private std::optional { 49 | public: 50 | using value_type = typename decltype(_DEFAULT)::value_type; 51 | using std::optional::optional; 52 | 53 | constexpr value_type value() const& { 54 | return value_or(_DEFAULT.value); 55 | } 56 | 57 | constexpr value_type value() && { 58 | return value_or(_DEFAULT.value); 59 | } 60 | 61 | constexpr operator value_type() const noexcept { 62 | return value(); 63 | } 64 | 65 | private: 66 | using std::optional::value_or; 67 | }; 68 | 69 | inline constexpr auto default_value = std::nullopt; 70 | 71 | } // namespace Ungar 72 | 73 | #endif /* _UNGAR__UTILS__DEFAULTED_HPP_ */ 74 | -------------------------------------------------------------------------------- /include/ungar/utils/integral_map.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/utils/integral_map.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__UTILS__INTEGRAL_MAP_HPP_ 28 | #define _UNGAR__UTILS__INTEGRAL_MAP_HPP_ 29 | 30 | #include "ungar/data_types.hpp" 31 | 32 | namespace Ungar { 33 | 34 | template 35 | class integral_map { 36 | public: 37 | /// @todo Consider using std::unique_ptr instead of std::optional to 38 | /// reduce the memory footprint. 39 | using array_type = std::array, _MAX_SIZE>; 40 | using value_type = _T; 41 | 42 | constexpr integral_map() : _array{} { 43 | } 44 | 45 | constexpr integral_map(const integral_map& other) : _array{other._array} { 46 | } 47 | 48 | constexpr integral_map(integral_map&& other) : _array{std::move(other._array)} { 49 | } 50 | 51 | constexpr integral_map& operator=(const integral_map& other) { 52 | _array = other._array; 53 | return *this; 54 | } 55 | 56 | constexpr integral_map& operator=(integral_map&& other) { 57 | _array = std::move(other._array); 58 | return *this; 59 | } 60 | 61 | constexpr size_t size() const { 62 | size_t sz = 0UL; 63 | for (const auto& el : _array) { 64 | sz += static_cast(el.has_value()); 65 | }; 66 | return sz; 67 | } 68 | 69 | constexpr const value_type& at(const std::integral auto key) const { 70 | return _array.at(static_cast(key)).value(); 71 | } 72 | 73 | constexpr const value_type& operator[](const std::integral auto key) const { 74 | return *_array[static_cast(key)]; 75 | } 76 | 77 | constexpr value_type& at(const std::integral auto key) { 78 | return _array.at(static_cast(key)).value(); 79 | } 80 | 81 | constexpr value_type& operator[](const std::integral auto key) { 82 | return *_array[static_cast(key)]; 83 | } 84 | 85 | template 86 | constexpr void emplace(const std::integral auto key, _Args&&... args) { 87 | _array[static_cast(key)].emplace(std::forward<_Args>(args)...); 88 | } 89 | 90 | template 91 | constexpr bool try_emplace(const std::integral auto key, _Args&&... args) { 92 | if (contains(key)) { 93 | return false; 94 | } else { 95 | _array[static_cast(key)].emplace(std::forward<_Args>(args)...); 96 | return true; 97 | } 98 | } 99 | 100 | template 101 | constexpr void insert_or_assign(const std::integral auto key, _Value&& value) { 102 | _array[static_cast(key)] = std::forward<_Value>(value); 103 | } 104 | 105 | template 106 | constexpr bool insert(const std::integral auto key, _Value&& value) { 107 | if (contains(key)) { 108 | return false; 109 | } else { 110 | _array[static_cast(key)].emplace(std::forward<_Value>(value)); 111 | return true; 112 | } 113 | } 114 | 115 | constexpr bool contains(const std::integral auto key) const { 116 | return _array[static_cast(key)].has_value(); 117 | } 118 | 119 | constexpr bool empty() const { 120 | return !size(); 121 | } 122 | 123 | private: 124 | array_type _array; 125 | }; 126 | 127 | } // namespace Ungar 128 | 129 | #endif /* _UNGAR__UTILS__INTEGRAL_MAP_HPP_ */ 130 | -------------------------------------------------------------------------------- /include/ungar/utils/macros/for_each.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/utils/macros/for_each.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__UTILS__MACROS__FOR_EACH_HPP_ 28 | #define _UNGAR__UTILS__MACROS__FOR_EACH_HPP_ 29 | 30 | #define _UNGAR_PARENS () 31 | 32 | #define _UNGAR_EXPAND(...) \ 33 | _UNGAR_EXPAND4(_UNGAR_EXPAND4(_UNGAR_EXPAND4(_UNGAR_EXPAND4(__VA_ARGS__)))) 34 | #define _UNGAR_EXPAND4(...) \ 35 | _UNGAR_EXPAND3(_UNGAR_EXPAND3(_UNGAR_EXPAND3(_UNGAR_EXPAND3(__VA_ARGS__)))) 36 | #define _UNGAR_EXPAND3(...) \ 37 | _UNGAR_EXPAND2(_UNGAR_EXPAND2(_UNGAR_EXPAND2(_UNGAR_EXPAND2(__VA_ARGS__)))) 38 | #define _UNGAR_EXPAND2(...) \ 39 | _UNGAR_EXPAND1(_UNGAR_EXPAND1(_UNGAR_EXPAND1(_UNGAR_EXPAND1(__VA_ARGS__)))) 40 | #define _UNGAR_EXPAND1(...) __VA_ARGS__ 41 | 42 | #define UNGAR_FOR_EACH(macro, ...) \ 43 | __VA_OPT__(_UNGAR_EXPAND(_UNGAR_FOR_EACH_IMPL_1(macro, __VA_ARGS__))) 44 | #define _UNGAR_FOR_EACH_IMPL_1(macro, first, ...) \ 45 | macro(first) __VA_OPT__(_UNGAR_FOR_EACH_IMPL_2 _UNGAR_PARENS(macro, __VA_ARGS__)) 46 | #define _UNGAR_FOR_EACH_IMPL_2() _UNGAR_FOR_EACH_IMPL_1 47 | 48 | #endif /* _UNGAR__UTILS__MACROS__FOR_EACH_HPP_ */ 49 | -------------------------------------------------------------------------------- /include/ungar/utils/passkey.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/utils/passkey.hpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #ifndef _UNGAR__UTILS__PASSKEY_HPP_ 28 | #define _UNGAR__UTILS__PASSKEY_HPP_ 29 | 30 | namespace Ungar { 31 | 32 | template 33 | struct Passkey { 34 | friend _Owner; 35 | 36 | private: 37 | Passkey() = default; 38 | Passkey(const Passkey&) = delete; 39 | Passkey(Passkey&&) = delete; 40 | Passkey& operator=(const Passkey&) = delete; 41 | Passkey& operator=(Passkey&&) = delete; 42 | }; 43 | 44 | } // namespace Ungar 45 | 46 | #define UNGAR_PASSKEY \ 47 | {} 48 | 49 | #endif /* _UNGAR__UTILS__PASSKEY_HPP_ */ 50 | -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include(CTest) 2 | 3 | add_executable(ungar.utils.test utils/utils.test.cpp) 4 | target_link_libraries(ungar.utils.test ungar::ungar GTest::gtest) 5 | add_test(NAME ungar.utils.test COMMAND ungar.utils.test) 6 | 7 | add_executable(ungar.variable.test variable.test.cpp) 8 | target_link_libraries(ungar.variable.test ungar::ungar GTest::gtest) 9 | add_test(NAME ungar.variable.test COMMAND ungar.variable.test) 10 | 11 | if(UNGAR_ENABLE_AUTODIFF) 12 | add_executable(ungar.function.test autodiff/function.test.cpp) 13 | target_link_libraries(ungar.function.test ungar::ungar GTest::gtest) 14 | add_test(NAME ungar.function.test COMMAND ungar.function.test) 15 | endif() 16 | 17 | if(UNGAR_ENABLE_OPTIMIZATION) 18 | add_executable(ungar.soft_sqp.test optimization/soft_sqp.test.cpp) 19 | target_link_libraries(ungar.soft_sqp.test ungar::ungar GTest::gtest) 20 | add_test(NAME ungar.soft_sqp.test COMMAND ungar.soft_sqp.test) 21 | endif() 22 | 23 | if(UNGAR_ENABLE_PINOCCHIO) 24 | add_executable(ungar.robot.test rbd/robot.test.cpp) 25 | target_link_libraries(ungar.robot.test ungar::ungar GTest::gtest) 26 | add_test(NAME ungar.robot.test COMMAND ungar.robot.test) 27 | endif() 28 | -------------------------------------------------------------------------------- /test/optimization/soft_sqp.test.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/test/soft_sqp.test.cpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #include 28 | 29 | #include "ungar/optimization/soft_sqp.hpp" 30 | 31 | namespace Ungar { 32 | namespace Test { 33 | namespace { 34 | 35 | TEST(OptimizationTest, SoftSQP) { 36 | using namespace Ungar; 37 | using namespace Ungar::Autodiff; 38 | 39 | const index_t xSize = 2_idx; 40 | const index_t pSize = 0_idx; 41 | 42 | const auto predicate = [](const auto& lhs, const auto& rhs) { return lhs.isApprox(rhs, 1e-1); }; 43 | 44 | const auto objBlueprint = 45 | Function::Blueprint{[](const VectorXad& xp, VectorXad& y) { 46 | y = VectorXad{{Utils::Pow(xp.x() - ad_scalar_t{3.0}, 2) + 47 | Utils::Pow(xp.y() - ad_scalar_t{2.0}, 2)}}; 48 | }, 49 | xSize, 50 | pSize, 51 | "obj_soft_sqp_test"sv}; 52 | const auto eqsBlueprint = Function::Blueprint{ 53 | [](const VectorXad& xp, VectorXad& y) { y = VectorXad{{xp.x() - xp.y()}}; }, 54 | xSize, 55 | pSize, 56 | "eqs_soft_sqp_test"sv, 57 | EnabledDerivatives::JACOBIAN}; 58 | 59 | const auto ineqsBlueprint1 = 60 | Function::Blueprint{[](const VectorXad& xp, VectorXad& y) { 61 | y = VectorXad{{xp.y() - ad_scalar_t{1.0}, -xp.x()}}; 62 | }, 63 | xSize, 64 | pSize, 65 | "ineqs_1_soft_sqp_test"sv, 66 | EnabledDerivatives::JACOBIAN}; 67 | auto nlpProblem1 = MakeNLPProblem( 68 | MakeFunction(objBlueprint, true), hana::nothing, MakeFunction(ineqsBlueprint1, true)); 69 | SoftSQPOptimizer optimizer1{false, 1.0, 100_idx, 100.0, 2e-8}; 70 | 71 | const VectorXr xOpt1 = optimizer1.Optimize(nlpProblem1, VectorXr::Zero(xSize + pSize)); 72 | const VectorXr xOptGroundTruth1{{3.0, 1.0}}; 73 | UNGAR_LOG(debug, Utils::DASH_LINE_SEPARATOR); 74 | UNGAR_LOG(debug, "PROBLEM 1"); 75 | UNGAR_LOG(debug, 76 | "The ground truth optimal solution is {}; SQP converged to {}.", 77 | xOptGroundTruth1, 78 | xOpt1); 79 | ASSERT_PRED2(predicate, xOpt1, xOptGroundTruth1); 80 | 81 | const auto ineqsBlueprint2 = 82 | Function::Blueprint{[](const VectorXad& xp, VectorXad& y) { 83 | y = VectorXad{{Utils::Pow(xp.x(), 2) - xp.y() - ad_scalar_t{3.0}, 84 | xp.y() - ad_scalar_t{1.0}, 85 | -xp.x()}}; 86 | }, 87 | xSize, 88 | pSize, 89 | "ineqs_2_soft_sqp_test"sv, 90 | EnabledDerivatives::JACOBIAN}; 91 | auto nlpProblem2 = MakeNLPProblem( 92 | MakeFunction(objBlueprint, false), hana::nothing, MakeFunction(ineqsBlueprint2, true)); 93 | SoftSQPOptimizer optimizer2{false, 1.0, 100_idx, 100.0, 2e-8}; 94 | 95 | const VectorXr xOpt2 = optimizer2.Optimize(nlpProblem2, VectorXr::Zero(xSize + pSize)); 96 | const VectorXr xOptGroundTruth2{{2.0, 1.0}}; 97 | UNGAR_LOG(debug, Utils::DASH_LINE_SEPARATOR); 98 | UNGAR_LOG(debug, "PROBLEM 2"); 99 | UNGAR_LOG(debug, 100 | "The ground truth optimal solution is {}; SQP converged to {}.", 101 | xOptGroundTruth2, 102 | xOpt2); 103 | ASSERT_PRED2(predicate, xOpt2, xOptGroundTruth2); 104 | 105 | auto nlpProblem3 = MakeNLPProblem(MakeFunction(objBlueprint, false), 106 | MakeFunction(eqsBlueprint, true), 107 | MakeFunction(ineqsBlueprint2, false)); 108 | SoftSQPOptimizer optimizer3{false, 1.0, 100_idx, 100.0, 2e-8}; 109 | 110 | const VectorXr xOpt3 = optimizer3.Optimize(nlpProblem3, VectorXr::Zero(xSize + pSize)); 111 | const VectorXr xOptGroundTruth3{{1.0, 1.0}}; 112 | UNGAR_LOG(debug, Utils::DASH_LINE_SEPARATOR); 113 | UNGAR_LOG(debug, "PROBLEM 3"); 114 | UNGAR_LOG(debug, 115 | "The ground truth optimal solution is {}; SQP converged to {}.", 116 | xOptGroundTruth3, 117 | xOpt3); 118 | ASSERT_PRED2(predicate, xOpt3, xOptGroundTruth3); 119 | } 120 | 121 | } // namespace 122 | } // namespace Test 123 | } // namespace Ungar 124 | 125 | int main() { 126 | ::testing::InitGoogleTest(); 127 | return RUN_ALL_TESTS(); 128 | } 129 | -------------------------------------------------------------------------------- /test/variable.test.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * @file ungar/test/variable.test.cpp 4 | * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) 5 | * 6 | * @section LICENSE 7 | * ----------------------------------------------------------------------- 8 | * 9 | * Copyright 2023 Flavio De Vincenti 10 | * 11 | * ----------------------------------------------------------------------- 12 | * 13 | * Licensed under the Apache License, Version 2.0 (the "License"); 14 | * you may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at 16 | * 17 | * http://www.apache.org/licenses/LICENSE-2.0 18 | * 19 | * Unless required by applicable law or agreed to in writing, 20 | * software distributed under the License is distributed on an "AS 21 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 22 | * express or implied. See the License for the specific language 23 | * governing permissions and limitations under the License. 24 | * 25 | ******************************************************************************/ 26 | 27 | #include 28 | 29 | #include "ungar/variable_map.hpp" 30 | 31 | namespace Ungar { 32 | namespace Test { 33 | namespace { 34 | 35 | constexpr auto N = 10_c; 36 | constexpr auto NUM_IPMS = 4_c; 37 | constexpr auto NUM_SRBDS = 2_c; 38 | 39 | constexpr auto position = var_c<"position", 3>; 40 | constexpr auto orientation = var_c<"orientation", Q>; 41 | constexpr auto linear_velocity = var_c<"linear_velocity", 3>; 42 | constexpr auto b_angular_velocity = var_c<"b_angular_velocity", 3>; 43 | 44 | constexpr auto srbd_state = var_c<"srbd_state"> <<= 45 | (position, orientation, linear_velocity, b_angular_velocity); 46 | constexpr auto ipm_state = var_c<"ipm_state"> <<= (position, linear_velocity); 47 | 48 | constexpr auto x = var_c<"x"> <<= (NUM_IPMS * ipm_state, NUM_SRBDS* srbd_state); 49 | constexpr auto X = var_c<"X"> <<= N * x; 50 | 51 | constexpr auto ipm_mass = var_c<"ipm_mass", 1>; 52 | constexpr auto srbd_mass = var_c<"srbd_mass", 1>; 53 | constexpr auto Rho = var_c<"Rho"> <<= (NUM_IPMS * ipm_mass, NUM_SRBDS* srbd_mass); 54 | 55 | constexpr auto variables = var_c<"variables"> <<= (X, Rho); 56 | 57 | TEST(RolloutVariablesTest, VariableMapAccess) { 58 | using namespace Ungar; 59 | 60 | auto map = MakeVariableMap(variables); 61 | auto lazyMap = VariableLazyMap{map.Get(), variables}; 62 | 63 | for (const auto i : enumerate(1024)) { 64 | map.Get().setRandom(); 65 | 66 | EXPECT_EQ(map.Get(variables), map.Get()); 67 | EXPECT_EQ(map.Get(variables), lazyMap.Get(variables)); 68 | EXPECT_EQ(map.Get(X), lazyMap.Get(X)); 69 | EXPECT_EQ(map.Get(Rho), lazyMap.Get(Rho)); 70 | for (const auto k : enumerate(N)) { 71 | EXPECT_EQ(map.Get(X, x, k), lazyMap.Get(X, x, k)); 72 | 73 | for (const auto ipmIndex : enumerate(NUM_IPMS)) { 74 | EXPECT_EQ(map.Get(ipm_state, k, ipmIndex), lazyMap.Get(ipm_state, k, ipmIndex)); 75 | EXPECT_EQ(map.Get(ipm_state, k, ipmIndex, position), 76 | lazyMap.Get(ipm_state, k, ipmIndex, position)); 77 | EXPECT_EQ(map.Get(ipm_state, k, ipmIndex, linear_velocity), 78 | lazyMap.Get(ipm_state, k, ipmIndex, linear_velocity)); 79 | } 80 | 81 | for (const auto srbdIndex : enumerate(NUM_SRBDS)) { 82 | const auto& var = variables(srbd_state, k, srbdIndex); 83 | EXPECT_EQ(map.Get1(var), lazyMap.Get1(var)); 84 | EXPECT_EQ(map.Get1(var(position)), lazyMap.Get1(var(position))); 85 | EXPECT_EQ(map.Get1(var(orientation)), lazyMap.Get1(var(orientation))); 86 | EXPECT_EQ(map.Get1(var(linear_velocity)), lazyMap.Get1(var(linear_velocity))); 87 | EXPECT_EQ(map.Get1(var(b_angular_velocity)), lazyMap.Get1(var(b_angular_velocity))); 88 | } 89 | } 90 | 91 | for (const auto ipmIndex : enumerate(NUM_IPMS)) { 92 | EXPECT_EQ(map.Get(ipm_mass, ipmIndex), lazyMap.Get(ipm_mass, ipmIndex)); 93 | } 94 | 95 | for (const auto srbdIndex : enumerate(NUM_SRBDS)) { 96 | EXPECT_EQ(map.Get(srbd_mass, srbdIndex), lazyMap.Get(srbd_mass, srbdIndex)); 97 | } 98 | } 99 | } 100 | 101 | TEST(RolloutVariablesTest, VariableMapAssignment) { 102 | using namespace Ungar; 103 | 104 | auto map = MakeVariableMap(variables); 105 | 106 | map.Get().setZero(); 107 | 108 | for (const auto k : enumerate(N)) { 109 | for (const auto ipmIndex : enumerate(NUM_IPMS)) { 110 | const auto& var = variables(ipm_state, k, ipmIndex); 111 | map.Get1(var).setOnes(); 112 | } 113 | 114 | for (const auto srbdIndex : enumerate(NUM_SRBDS)) { 115 | map.Get(srbd_state, k, srbdIndex).setOnes(); 116 | } 117 | 118 | EXPECT_EQ(map.Get(x, k), VectorXr::Ones(x.Size())); 119 | } 120 | 121 | for (const auto ipmIndex : enumerate(NUM_IPMS)) { 122 | const auto& var = variables(ipm_mass, ipmIndex); 123 | map.Get1(var) = 1.0; 124 | } 125 | 126 | for (const auto srbdIndex : enumerate(NUM_SRBDS)) { 127 | map.Get(srbd_mass, srbdIndex) = 1.0; 128 | } 129 | EXPECT_EQ(map.Get(Rho), VectorXr::Ones(Rho.Size())); 130 | } 131 | 132 | } // namespace 133 | } // namespace Test 134 | } // namespace Ungar 135 | 136 | int main() { 137 | ::testing::InitGoogleTest(); 138 | return RUN_ALL_TESTS(); 139 | } 140 | --------------------------------------------------------------------------------