Copy disabled (too large)
Download .txt
Showing preview only (73,582K chars total). Download the full file to get everything.
Repository: dojo-sim/Dojo.jl
Branch: main
Commit: be7b5183965e
Files: 962
Total size: 110.2 MB
Directory structure:
gitextract_l11i1c07/
├── .github/
│ └── workflows/
│ ├── CI.yml
│ ├── CI_DojoEnv.yml
│ ├── Documenter.yml
│ ├── TagBot.yml
│ ├── benchmark.yml
│ └── dailyCI.yml
├── .gitignore
├── CITATION.bib
├── DojoEnvironments/
│ ├── LICENSE
│ ├── Project.toml
│ ├── README.md
│ ├── src/
│ │ ├── DojoEnvironments.jl
│ │ ├── environments/
│ │ │ ├── ant_ars.jl
│ │ │ ├── cartpole_dqn.jl
│ │ │ ├── include.jl
│ │ │ ├── pendulum.jl
│ │ │ ├── quadrotor_waypoint.jl
│ │ │ ├── quadruped_sampling.jl
│ │ │ ├── quadruped_waypoint.jl
│ │ │ ├── uuv_waypoint.jl
│ │ │ └── youbot_waypoint.jl
│ │ ├── environments.jl
│ │ ├── mechanisms/
│ │ │ ├── ant/
│ │ │ │ ├── dependencies/
│ │ │ │ │ └── ant.urdf
│ │ │ │ └── mechanism.jl
│ │ │ ├── atlas/
│ │ │ │ ├── dependencies/
│ │ │ │ │ ├── atlas.urdf
│ │ │ │ │ ├── atlas_armless.urdf
│ │ │ │ │ ├── atlas_complex.urdf
│ │ │ │ │ ├── atlas_fast.urdf
│ │ │ │ │ ├── atlas_ones.urdf
│ │ │ │ │ ├── atlas_simple.urdf
│ │ │ │ │ ├── atlas_v2.urdf
│ │ │ │ │ └── mesh/
│ │ │ │ │ ├── GRIPPER_OPEN_chull.obj
│ │ │ │ │ ├── head.obj
│ │ │ │ │ ├── head_camera.obj
│ │ │ │ │ ├── head_camera_chull.obj
│ │ │ │ │ ├── head_chull.obj
│ │ │ │ │ ├── l_foot.obj
│ │ │ │ │ ├── l_foot_chull.obj
│ │ │ │ │ ├── l_lglut.obj
│ │ │ │ │ ├── l_lglut_chull.obj
│ │ │ │ │ ├── l_lleg.obj
│ │ │ │ │ ├── l_lleg_chull.obj
│ │ │ │ │ ├── l_talus.obj
│ │ │ │ │ ├── l_uglut.obj
│ │ │ │ │ ├── l_uglut_chull.obj
│ │ │ │ │ ├── l_uleg.obj
│ │ │ │ │ ├── l_uleg_chull.obj
│ │ │ │ │ ├── ltorso.obj
│ │ │ │ │ ├── mtorso.obj
│ │ │ │ │ ├── pelvis.obj
│ │ │ │ │ ├── pelvis_chull.obj
│ │ │ │ │ ├── r_clav.obj
│ │ │ │ │ ├── r_clav_chull.obj
│ │ │ │ │ ├── r_farm.obj
│ │ │ │ │ ├── r_farm_chull.obj
│ │ │ │ │ ├── r_foot.obj
│ │ │ │ │ ├── r_foot_chull.obj
│ │ │ │ │ ├── r_hand.obj
│ │ │ │ │ ├── r_hand_chull.obj
│ │ │ │ │ ├── r_larm.obj
│ │ │ │ │ ├── r_larm_chull.obj
│ │ │ │ │ ├── r_lglut.obj
│ │ │ │ │ ├── r_lglut_chull.obj
│ │ │ │ │ ├── r_lleg.obj
│ │ │ │ │ ├── r_lleg_chull.obj
│ │ │ │ │ ├── r_scap.obj
│ │ │ │ │ ├── r_scap_chull.obj
│ │ │ │ │ ├── r_talus.obj
│ │ │ │ │ ├── r_uarm.obj
│ │ │ │ │ ├── r_uarm_chull.obj
│ │ │ │ │ ├── r_uglut.obj
│ │ │ │ │ ├── r_uglut_chull.obj
│ │ │ │ │ ├── r_uleg.obj
│ │ │ │ │ ├── r_uleg_chull.obj
│ │ │ │ │ ├── utorso.obj
│ │ │ │ │ └── utorso_chull.obj
│ │ │ │ └── mechanism.jl
│ │ │ ├── block/
│ │ │ │ └── mechanism.jl
│ │ │ ├── block2d/
│ │ │ │ └── mechanism.jl
│ │ │ ├── cartpole/
│ │ │ │ └── mechanism.jl
│ │ │ ├── dzhanibekov/
│ │ │ │ └── mechanism.jl
│ │ │ ├── exoskeleton/
│ │ │ │ ├── dependencies/
│ │ │ │ │ ├── mesh/
│ │ │ │ │ │ ├── eFE_simple.obj
│ │ │ │ │ │ ├── lowerarm.obj
│ │ │ │ │ │ ├── sAA_simple.obj
│ │ │ │ │ │ ├── sFE_simple.obj
│ │ │ │ │ │ ├── sIE_simple.obj
│ │ │ │ │ │ ├── torso.obj
│ │ │ │ │ │ └── upperarm.obj
│ │ │ │ │ └── model.urdf
│ │ │ │ └── mechanism.jl
│ │ │ ├── fourbar/
│ │ │ │ ├── dependencies/
│ │ │ │ │ ├── fourbar.urdf
│ │ │ │ │ └── fourbar_open.urdf
│ │ │ │ └── mechanism.jl
│ │ │ ├── halfcheetah/
│ │ │ │ ├── dependencies/
│ │ │ │ │ └── halfcheetah.urdf
│ │ │ │ └── mechanism.jl
│ │ │ ├── hopper/
│ │ │ │ ├── dependencies/
│ │ │ │ │ └── hopper.urdf
│ │ │ │ └── mechanism.jl
│ │ │ ├── humanoid/
│ │ │ │ ├── dependencies/
│ │ │ │ │ └── humanoid.urdf
│ │ │ │ └── mechanism.jl
│ │ │ ├── include.jl
│ │ │ ├── npendulum/
│ │ │ │ └── mechanism.jl
│ │ │ ├── nslider/
│ │ │ │ └── mechanism.jl
│ │ │ ├── panda/
│ │ │ │ ├── dependencies/
│ │ │ │ │ ├── mesh/
│ │ │ │ │ │ ├── collision/
│ │ │ │ │ │ │ ├── finger.obj
│ │ │ │ │ │ │ ├── hand.obj
│ │ │ │ │ │ │ ├── hand.stl
│ │ │ │ │ │ │ ├── link0.obj
│ │ │ │ │ │ │ ├── link0.stl
│ │ │ │ │ │ │ ├── link1.obj
│ │ │ │ │ │ │ ├── link1.stl
│ │ │ │ │ │ │ ├── link2.obj
│ │ │ │ │ │ │ ├── link2.stl
│ │ │ │ │ │ │ ├── link3.obj
│ │ │ │ │ │ │ ├── link3.stl
│ │ │ │ │ │ │ ├── link4.obj
│ │ │ │ │ │ │ ├── link4.stl
│ │ │ │ │ │ │ ├── link5.obj
│ │ │ │ │ │ │ ├── link5.stl
│ │ │ │ │ │ │ ├── link6.obj
│ │ │ │ │ │ │ ├── link6.stl
│ │ │ │ │ │ │ ├── link7.obj
│ │ │ │ │ │ │ └── link7.stl
│ │ │ │ │ │ └── visual/
│ │ │ │ │ │ ├── finger.dae
│ │ │ │ │ │ ├── hand.dae
│ │ │ │ │ │ ├── link0.dae
│ │ │ │ │ │ ├── link1.dae
│ │ │ │ │ │ ├── link2.dae
│ │ │ │ │ │ ├── link3.dae
│ │ │ │ │ │ ├── link4.dae
│ │ │ │ │ │ ├── link5.dae
│ │ │ │ │ │ ├── link6.dae
│ │ │ │ │ │ └── link7.dae
│ │ │ │ │ ├── panda_end_effector.urdf
│ │ │ │ │ ├── panda_no_end_effector.urdf
│ │ │ │ │ └── panda_original.urdf
│ │ │ │ └── mechanism.jl
│ │ │ ├── pendulum/
│ │ │ │ └── mechanism.jl
│ │ │ ├── quadrotor/
│ │ │ │ ├── dependencies/
│ │ │ │ │ ├── mesh/
│ │ │ │ │ │ ├── pelican.obj
│ │ │ │ │ │ ├── propeller_ccw.obj
│ │ │ │ │ │ └── propeller_cw.obj
│ │ │ │ │ ├── pelican.urdf
│ │ │ │ │ └── pelican_fixed_rotors.urdf
│ │ │ │ └── mechanism.jl
│ │ │ ├── quadruped/
│ │ │ │ ├── dependencies/
│ │ │ │ │ ├── gazebo_a1.urdf
│ │ │ │ │ ├── mesh/
│ │ │ │ │ │ ├── calf.dae
│ │ │ │ │ │ ├── hip.dae
│ │ │ │ │ │ ├── thigh.dae
│ │ │ │ │ │ ├── thigh_mirror.dae
│ │ │ │ │ │ └── trunk.dae
│ │ │ │ │ ├── quadruped.urdf
│ │ │ │ │ ├── quadruped_ones.urdf
│ │ │ │ │ └── quadruped_simple.urdf
│ │ │ │ └── mechanism.jl
│ │ │ ├── raiberthopper/
│ │ │ │ └── mechanism.jl
│ │ │ ├── slider/
│ │ │ │ └── mechanism.jl
│ │ │ ├── snake/
│ │ │ │ └── mechanism.jl
│ │ │ ├── sphere/
│ │ │ │ └── mechanism.jl
│ │ │ ├── tippetop/
│ │ │ │ └── mechanism.jl
│ │ │ ├── twister/
│ │ │ │ └── mechanism.jl
│ │ │ ├── uuv/
│ │ │ │ ├── dependencies/
│ │ │ │ │ ├── mesh/
│ │ │ │ │ │ ├── material.lib
│ │ │ │ │ │ ├── obsrov.mtl
│ │ │ │ │ │ ├── obsrov.obj
│ │ │ │ │ │ ├── red_propcw_minitortuga.obj
│ │ │ │ │ │ └── silver_propcw_minitortuga.obj
│ │ │ │ │ ├── mini_tortuga.urdf
│ │ │ │ │ └── mini_tortuga_fixed_rotors.urdf
│ │ │ │ └── mechanism.jl
│ │ │ ├── walker/
│ │ │ │ ├── dependencies/
│ │ │ │ │ └── walker.urdf
│ │ │ │ └── mechanism.jl
│ │ │ └── youbot/
│ │ │ ├── dependencies/
│ │ │ │ ├── meshes/
│ │ │ │ │ ├── sensors/
│ │ │ │ │ │ ├── asus_xtion_pro_live_base.dae
│ │ │ │ │ │ ├── asus_xtion_pro_live_camera.dae
│ │ │ │ │ │ ├── hokuyo.dae
│ │ │ │ │ │ ├── hokuyo_convex.dae
│ │ │ │ │ │ ├── kinect.dae
│ │ │ │ │ │ ├── kinect.tga
│ │ │ │ │ │ └── microsoft_lifecam.dae
│ │ │ │ │ ├── youbot_arm/
│ │ │ │ │ │ ├── arm0.dae
│ │ │ │ │ │ ├── arm0_convex.dae
│ │ │ │ │ │ ├── arm1.dae
│ │ │ │ │ │ ├── arm1_convex.dae
│ │ │ │ │ │ ├── arm2.dae
│ │ │ │ │ │ ├── arm2_convex.dae
│ │ │ │ │ │ ├── arm3.dae
│ │ │ │ │ │ ├── arm3_convex.dae
│ │ │ │ │ │ ├── arm4.dae
│ │ │ │ │ │ ├── arm4_convex.dae
│ │ │ │ │ │ ├── arm5.dae
│ │ │ │ │ │ └── arm5_convex.dae
│ │ │ │ │ ├── youbot_base/
│ │ │ │ │ │ ├── back_left_wheel.dae
│ │ │ │ │ │ ├── back_right_wheel.dae
│ │ │ │ │ │ ├── base.dae
│ │ │ │ │ │ ├── base_convex.dae
│ │ │ │ │ │ ├── front_left_wheel.dae
│ │ │ │ │ │ └── front_right_wheel.dae
│ │ │ │ │ ├── youbot_gripper/
│ │ │ │ │ │ ├── finger.dae
│ │ │ │ │ │ ├── finger_convex.dae
│ │ │ │ │ │ ├── palm.dae
│ │ │ │ │ │ └── palm_convex.dae
│ │ │ │ │ └── youbot_plate/
│ │ │ │ │ ├── plate.dae
│ │ │ │ │ └── plate_convex.dae
│ │ │ │ ├── youbot.urdf
│ │ │ │ ├── youbot_arm_only.urdf
│ │ │ │ ├── youbot_arm_only_fixed_gripper.urdf
│ │ │ │ ├── youbot_base_only.urdf
│ │ │ │ ├── youbot_base_only_no_wheels.urdf
│ │ │ │ ├── youbot_dual_arm.urdf
│ │ │ │ ├── youbot_fixed_gripper.urdf
│ │ │ │ └── youbot_with_cam3d.urdf
│ │ │ └── mechanism.jl
│ │ ├── mechanisms.jl
│ │ ├── precompile.jl
│ │ └── utilities.jl
│ └── test/
│ ├── Project.toml
│ ├── environments.jl
│ ├── mechanisms.jl
│ └── runtests.jl
├── LICENSE
├── Project.toml
├── README.md
├── activate.jl
├── benchmark/
│ ├── Project.toml
│ ├── benchmarks.jl
│ └── mechanisms_benchmark.jl
├── deps/
│ └── build.jl
├── docs/
│ ├── Project.toml
│ ├── make.jl
│ └── src/
│ ├── api.md
│ ├── background_contact/
│ │ ├── collisions.md
│ │ ├── contact_models.md
│ │ ├── impact.md
│ │ ├── linearized_friction.md
│ │ └── nonlinear_friction.md
│ ├── background_representations/
│ │ ├── gradients.md
│ │ ├── maximal_representation.md
│ │ └── minimal_representation.md
│ ├── background_solver/
│ │ ├── interior_point.md
│ │ └── solver_options.md
│ ├── citing.md
│ ├── contributing.md
│ ├── creating_mechanism/
│ │ ├── environment_existing.md
│ │ ├── mechanism_directly.md
│ │ ├── mechanism_existing.md
│ │ ├── overview.md
│ │ ├── quadruped.md
│ │ └── tippetop.md
│ ├── creating_simulation/
│ │ ├── define_controller.md
│ │ ├── define_simulation.md
│ │ └── simulation_with_gradients.md
│ ├── environments/
│ │ └── overview.md
│ ├── examples/
│ │ ├── reinforcement_learning.md
│ │ ├── simulation.md
│ │ ├── system_identification.md
│ │ └── trajectory_optimization.md
│ └── index.md
├── examples/
│ ├── Project.toml
│ ├── README.md
│ ├── control/
│ │ ├── cartpole_lqr.jl
│ │ ├── pendulum_pid.jl
│ │ └── quadrotor_cascade.jl
│ ├── generate_notebooks.jl
│ ├── learning/
│ │ ├── ant_ars.jl
│ │ ├── cartpole_dqn.jl
│ │ └── quadruped_sampling.jl
│ ├── simulation/
│ │ ├── all_mechanisms.jl
│ │ ├── mechanics/
│ │ │ ├── contact_gradients.jl
│ │ │ ├── energy_momentum_conservation.jl
│ │ │ ├── friction_cone_comparison.jl
│ │ │ └── hard_contact.jl
│ │ ├── object_collision_development/
│ │ │ ├── collision_sphere_box.jl
│ │ │ ├── collision_sphere_capsule.jl
│ │ │ ├── collision_sphere_sphere.jl
│ │ │ └── two_finger_box_control.jl
│ │ ├── pendulum_direct.jl
│ │ ├── pendulum_environment.jl
│ │ └── pendulum_mechanism.jl
│ └── system_identification/
│ ├── data/
│ │ ├── datasets/
│ │ │ ├── real_block.jld2
│ │ │ └── synthetic_sphere.jld2
│ │ └── experiment/
│ │ ├── conversion.jl
│ │ └── tosses_csv/
│ │ ├── 0.csv
│ │ ├── 1.csv
│ │ ├── 10.csv
│ │ ├── 100.csv
│ │ ├── 101.csv
│ │ ├── 102.csv
│ │ ├── 103.csv
│ │ ├── 104.csv
│ │ ├── 105.csv
│ │ ├── 106.csv
│ │ ├── 107.csv
│ │ ├── 108.csv
│ │ ├── 109.csv
│ │ ├── 11.csv
│ │ ├── 110.csv
│ │ ├── 111.csv
│ │ ├── 112.csv
│ │ ├── 113.csv
│ │ ├── 114.csv
│ │ ├── 115.csv
│ │ ├── 116.csv
│ │ ├── 117.csv
│ │ ├── 118.csv
│ │ ├── 119.csv
│ │ ├── 12.csv
│ │ ├── 120.csv
│ │ ├── 121.csv
│ │ ├── 122.csv
│ │ ├── 123.csv
│ │ ├── 124.csv
│ │ ├── 125.csv
│ │ ├── 126.csv
│ │ ├── 127.csv
│ │ ├── 128.csv
│ │ ├── 129.csv
│ │ ├── 13.csv
│ │ ├── 130.csv
│ │ ├── 131.csv
│ │ ├── 132.csv
│ │ ├── 133.csv
│ │ ├── 134.csv
│ │ ├── 135.csv
│ │ ├── 136.csv
│ │ ├── 137.csv
│ │ ├── 138.csv
│ │ ├── 139.csv
│ │ ├── 14.csv
│ │ ├── 140.csv
│ │ ├── 141.csv
│ │ ├── 142.csv
│ │ ├── 143.csv
│ │ ├── 144.csv
│ │ ├── 145.csv
│ │ ├── 146.csv
│ │ ├── 147.csv
│ │ ├── 148.csv
│ │ ├── 149.csv
│ │ ├── 15.csv
│ │ ├── 150.csv
│ │ ├── 151.csv
│ │ ├── 152.csv
│ │ ├── 153.csv
│ │ ├── 154.csv
│ │ ├── 155.csv
│ │ ├── 156.csv
│ │ ├── 157.csv
│ │ ├── 158.csv
│ │ ├── 159.csv
│ │ ├── 16.csv
│ │ ├── 160.csv
│ │ ├── 161.csv
│ │ ├── 162.csv
│ │ ├── 163.csv
│ │ ├── 164.csv
│ │ ├── 165.csv
│ │ ├── 166.csv
│ │ ├── 167.csv
│ │ ├── 168.csv
│ │ ├── 169.csv
│ │ ├── 17.csv
│ │ ├── 170.csv
│ │ ├── 171.csv
│ │ ├── 172.csv
│ │ ├── 173.csv
│ │ ├── 174.csv
│ │ ├── 175.csv
│ │ ├── 176.csv
│ │ ├── 177.csv
│ │ ├── 178.csv
│ │ ├── 179.csv
│ │ ├── 18.csv
│ │ ├── 180.csv
│ │ ├── 181.csv
│ │ ├── 182.csv
│ │ ├── 183.csv
│ │ ├── 184.csv
│ │ ├── 185.csv
│ │ ├── 186.csv
│ │ ├── 187.csv
│ │ ├── 188.csv
│ │ ├── 189.csv
│ │ ├── 19.csv
│ │ ├── 190.csv
│ │ ├── 191.csv
│ │ ├── 192.csv
│ │ ├── 193.csv
│ │ ├── 194.csv
│ │ ├── 195.csv
│ │ ├── 196.csv
│ │ ├── 197.csv
│ │ ├── 198.csv
│ │ ├── 199.csv
│ │ ├── 2.csv
│ │ ├── 20.csv
│ │ ├── 200.csv
│ │ ├── 201.csv
│ │ ├── 202.csv
│ │ ├── 203.csv
│ │ ├── 204.csv
│ │ ├── 205.csv
│ │ ├── 206.csv
│ │ ├── 207.csv
│ │ ├── 208.csv
│ │ ├── 209.csv
│ │ ├── 21.csv
│ │ ├── 210.csv
│ │ ├── 211.csv
│ │ ├── 212.csv
│ │ ├── 213.csv
│ │ ├── 214.csv
│ │ ├── 215.csv
│ │ ├── 216.csv
│ │ ├── 217.csv
│ │ ├── 218.csv
│ │ ├── 219.csv
│ │ ├── 22.csv
│ │ ├── 220.csv
│ │ ├── 221.csv
│ │ ├── 222.csv
│ │ ├── 223.csv
│ │ ├── 224.csv
│ │ ├── 225.csv
│ │ ├── 226.csv
│ │ ├── 227.csv
│ │ ├── 228.csv
│ │ ├── 229.csv
│ │ ├── 23.csv
│ │ ├── 230.csv
│ │ ├── 231.csv
│ │ ├── 232.csv
│ │ ├── 233.csv
│ │ ├── 234.csv
│ │ ├── 235.csv
│ │ ├── 236.csv
│ │ ├── 237.csv
│ │ ├── 238.csv
│ │ ├── 239.csv
│ │ ├── 24.csv
│ │ ├── 240.csv
│ │ ├── 241.csv
│ │ ├── 242.csv
│ │ ├── 243.csv
│ │ ├── 244.csv
│ │ ├── 245.csv
│ │ ├── 246.csv
│ │ ├── 247.csv
│ │ ├── 248.csv
│ │ ├── 249.csv
│ │ ├── 25.csv
│ │ ├── 250.csv
│ │ ├── 251.csv
│ │ ├── 252.csv
│ │ ├── 253.csv
│ │ ├── 254.csv
│ │ ├── 255.csv
│ │ ├── 256.csv
│ │ ├── 257.csv
│ │ ├── 258.csv
│ │ ├── 259.csv
│ │ ├── 26.csv
│ │ ├── 260.csv
│ │ ├── 261.csv
│ │ ├── 262.csv
│ │ ├── 263.csv
│ │ ├── 264.csv
│ │ ├── 265.csv
│ │ ├── 266.csv
│ │ ├── 267.csv
│ │ ├── 268.csv
│ │ ├── 269.csv
│ │ ├── 27.csv
│ │ ├── 270.csv
│ │ ├── 271.csv
│ │ ├── 272.csv
│ │ ├── 273.csv
│ │ ├── 274.csv
│ │ ├── 275.csv
│ │ ├── 276.csv
│ │ ├── 277.csv
│ │ ├── 278.csv
│ │ ├── 279.csv
│ │ ├── 28.csv
│ │ ├── 280.csv
│ │ ├── 281.csv
│ │ ├── 282.csv
│ │ ├── 283.csv
│ │ ├── 284.csv
│ │ ├── 285.csv
│ │ ├── 286.csv
│ │ ├── 287.csv
│ │ ├── 288.csv
│ │ ├── 289.csv
│ │ ├── 29.csv
│ │ ├── 290.csv
│ │ ├── 291.csv
│ │ ├── 292.csv
│ │ ├── 293.csv
│ │ ├── 294.csv
│ │ ├── 295.csv
│ │ ├── 296.csv
│ │ ├── 297.csv
│ │ ├── 298.csv
│ │ ├── 299.csv
│ │ ├── 3.csv
│ │ ├── 30.csv
│ │ ├── 300.csv
│ │ ├── 301.csv
│ │ ├── 302.csv
│ │ ├── 303.csv
│ │ ├── 304.csv
│ │ ├── 305.csv
│ │ ├── 306.csv
│ │ ├── 307.csv
│ │ ├── 308.csv
│ │ ├── 309.csv
│ │ ├── 31.csv
│ │ ├── 310.csv
│ │ ├── 311.csv
│ │ ├── 312.csv
│ │ ├── 313.csv
│ │ ├── 314.csv
│ │ ├── 315.csv
│ │ ├── 316.csv
│ │ ├── 317.csv
│ │ ├── 318.csv
│ │ ├── 319.csv
│ │ ├── 32.csv
│ │ ├── 320.csv
│ │ ├── 321.csv
│ │ ├── 322.csv
│ │ ├── 323.csv
│ │ ├── 324.csv
│ │ ├── 325.csv
│ │ ├── 326.csv
│ │ ├── 327.csv
│ │ ├── 328.csv
│ │ ├── 329.csv
│ │ ├── 33.csv
│ │ ├── 330.csv
│ │ ├── 331.csv
│ │ ├── 332.csv
│ │ ├── 333.csv
│ │ ├── 334.csv
│ │ ├── 335.csv
│ │ ├── 336.csv
│ │ ├── 337.csv
│ │ ├── 338.csv
│ │ ├── 339.csv
│ │ ├── 34.csv
│ │ ├── 340.csv
│ │ ├── 341.csv
│ │ ├── 342.csv
│ │ ├── 343.csv
│ │ ├── 344.csv
│ │ ├── 345.csv
│ │ ├── 346.csv
│ │ ├── 347.csv
│ │ ├── 348.csv
│ │ ├── 349.csv
│ │ ├── 35.csv
│ │ ├── 350.csv
│ │ ├── 351.csv
│ │ ├── 352.csv
│ │ ├── 353.csv
│ │ ├── 354.csv
│ │ ├── 355.csv
│ │ ├── 356.csv
│ │ ├── 357.csv
│ │ ├── 358.csv
│ │ ├── 359.csv
│ │ ├── 36.csv
│ │ ├── 360.csv
│ │ ├── 361.csv
│ │ ├── 362.csv
│ │ ├── 363.csv
│ │ ├── 364.csv
│ │ ├── 365.csv
│ │ ├── 366.csv
│ │ ├── 367.csv
│ │ ├── 368.csv
│ │ ├── 369.csv
│ │ ├── 37.csv
│ │ ├── 370.csv
│ │ ├── 371.csv
│ │ ├── 372.csv
│ │ ├── 373.csv
│ │ ├── 374.csv
│ │ ├── 375.csv
│ │ ├── 376.csv
│ │ ├── 377.csv
│ │ ├── 378.csv
│ │ ├── 379.csv
│ │ ├── 38.csv
│ │ ├── 380.csv
│ │ ├── 381.csv
│ │ ├── 382.csv
│ │ ├── 383.csv
│ │ ├── 384.csv
│ │ ├── 385.csv
│ │ ├── 386.csv
│ │ ├── 387.csv
│ │ ├── 388.csv
│ │ ├── 389.csv
│ │ ├── 39.csv
│ │ ├── 390.csv
│ │ ├── 391.csv
│ │ ├── 392.csv
│ │ ├── 393.csv
│ │ ├── 394.csv
│ │ ├── 395.csv
│ │ ├── 396.csv
│ │ ├── 397.csv
│ │ ├── 398.csv
│ │ ├── 399.csv
│ │ ├── 4.csv
│ │ ├── 40.csv
│ │ ├── 400.csv
│ │ ├── 401.csv
│ │ ├── 402.csv
│ │ ├── 403.csv
│ │ ├── 404.csv
│ │ ├── 405.csv
│ │ ├── 406.csv
│ │ ├── 407.csv
│ │ ├── 408.csv
│ │ ├── 409.csv
│ │ ├── 41.csv
│ │ ├── 410.csv
│ │ ├── 411.csv
│ │ ├── 412.csv
│ │ ├── 413.csv
│ │ ├── 414.csv
│ │ ├── 415.csv
│ │ ├── 416.csv
│ │ ├── 417.csv
│ │ ├── 418.csv
│ │ ├── 419.csv
│ │ ├── 42.csv
│ │ ├── 420.csv
│ │ ├── 421.csv
│ │ ├── 422.csv
│ │ ├── 423.csv
│ │ ├── 424.csv
│ │ ├── 425.csv
│ │ ├── 426.csv
│ │ ├── 427.csv
│ │ ├── 428.csv
│ │ ├── 429.csv
│ │ ├── 43.csv
│ │ ├── 430.csv
│ │ ├── 431.csv
│ │ ├── 432.csv
│ │ ├── 433.csv
│ │ ├── 434.csv
│ │ ├── 435.csv
│ │ ├── 436.csv
│ │ ├── 437.csv
│ │ ├── 438.csv
│ │ ├── 439.csv
│ │ ├── 44.csv
│ │ ├── 440.csv
│ │ ├── 441.csv
│ │ ├── 442.csv
│ │ ├── 443.csv
│ │ ├── 444.csv
│ │ ├── 445.csv
│ │ ├── 446.csv
│ │ ├── 447.csv
│ │ ├── 448.csv
│ │ ├── 449.csv
│ │ ├── 45.csv
│ │ ├── 450.csv
│ │ ├── 451.csv
│ │ ├── 452.csv
│ │ ├── 453.csv
│ │ ├── 454.csv
│ │ ├── 455.csv
│ │ ├── 456.csv
│ │ ├── 457.csv
│ │ ├── 458.csv
│ │ ├── 459.csv
│ │ ├── 46.csv
│ │ ├── 460.csv
│ │ ├── 461.csv
│ │ ├── 462.csv
│ │ ├── 463.csv
│ │ ├── 464.csv
│ │ ├── 465.csv
│ │ ├── 466.csv
│ │ ├── 467.csv
│ │ ├── 468.csv
│ │ ├── 469.csv
│ │ ├── 47.csv
│ │ ├── 470.csv
│ │ ├── 471.csv
│ │ ├── 472.csv
│ │ ├── 473.csv
│ │ ├── 474.csv
│ │ ├── 475.csv
│ │ ├── 476.csv
│ │ ├── 477.csv
│ │ ├── 478.csv
│ │ ├── 479.csv
│ │ ├── 48.csv
│ │ ├── 480.csv
│ │ ├── 481.csv
│ │ ├── 482.csv
│ │ ├── 483.csv
│ │ ├── 484.csv
│ │ ├── 485.csv
│ │ ├── 486.csv
│ │ ├── 487.csv
│ │ ├── 488.csv
│ │ ├── 489.csv
│ │ ├── 49.csv
│ │ ├── 490.csv
│ │ ├── 491.csv
│ │ ├── 492.csv
│ │ ├── 493.csv
│ │ ├── 494.csv
│ │ ├── 495.csv
│ │ ├── 496.csv
│ │ ├── 497.csv
│ │ ├── 498.csv
│ │ ├── 499.csv
│ │ ├── 5.csv
│ │ ├── 50.csv
│ │ ├── 500.csv
│ │ ├── 501.csv
│ │ ├── 502.csv
│ │ ├── 503.csv
│ │ ├── 504.csv
│ │ ├── 505.csv
│ │ ├── 506.csv
│ │ ├── 507.csv
│ │ ├── 508.csv
│ │ ├── 509.csv
│ │ ├── 51.csv
│ │ ├── 510.csv
│ │ ├── 511.csv
│ │ ├── 512.csv
│ │ ├── 513.csv
│ │ ├── 514.csv
│ │ ├── 515.csv
│ │ ├── 516.csv
│ │ ├── 517.csv
│ │ ├── 518.csv
│ │ ├── 519.csv
│ │ ├── 52.csv
│ │ ├── 520.csv
│ │ ├── 521.csv
│ │ ├── 522.csv
│ │ ├── 523.csv
│ │ ├── 524.csv
│ │ ├── 525.csv
│ │ ├── 526.csv
│ │ ├── 527.csv
│ │ ├── 528.csv
│ │ ├── 529.csv
│ │ ├── 53.csv
│ │ ├── 530.csv
│ │ ├── 531.csv
│ │ ├── 532.csv
│ │ ├── 533.csv
│ │ ├── 534.csv
│ │ ├── 535.csv
│ │ ├── 536.csv
│ │ ├── 537.csv
│ │ ├── 538.csv
│ │ ├── 539.csv
│ │ ├── 54.csv
│ │ ├── 540.csv
│ │ ├── 541.csv
│ │ ├── 542.csv
│ │ ├── 543.csv
│ │ ├── 544.csv
│ │ ├── 545.csv
│ │ ├── 546.csv
│ │ ├── 547.csv
│ │ ├── 548.csv
│ │ ├── 549.csv
│ │ ├── 55.csv
│ │ ├── 550.csv
│ │ ├── 551.csv
│ │ ├── 552.csv
│ │ ├── 553.csv
│ │ ├── 554.csv
│ │ ├── 555.csv
│ │ ├── 556.csv
│ │ ├── 557.csv
│ │ ├── 558.csv
│ │ ├── 559.csv
│ │ ├── 56.csv
│ │ ├── 560.csv
│ │ ├── 561.csv
│ │ ├── 562.csv
│ │ ├── 563.csv
│ │ ├── 564.csv
│ │ ├── 565.csv
│ │ ├── 566.csv
│ │ ├── 567.csv
│ │ ├── 568.csv
│ │ ├── 569.csv
│ │ ├── 57.csv
│ │ ├── 58.csv
│ │ ├── 59.csv
│ │ ├── 6.csv
│ │ ├── 60.csv
│ │ ├── 61.csv
│ │ ├── 62.csv
│ │ ├── 63.csv
│ │ ├── 64.csv
│ │ ├── 65.csv
│ │ ├── 66.csv
│ │ ├── 67.csv
│ │ ├── 68.csv
│ │ ├── 69.csv
│ │ ├── 7.csv
│ │ ├── 70.csv
│ │ ├── 71.csv
│ │ ├── 72.csv
│ │ ├── 73.csv
│ │ ├── 74.csv
│ │ ├── 75.csv
│ │ ├── 76.csv
│ │ ├── 77.csv
│ │ ├── 78.csv
│ │ ├── 79.csv
│ │ ├── 8.csv
│ │ ├── 80.csv
│ │ ├── 81.csv
│ │ ├── 82.csv
│ │ ├── 83.csv
│ │ ├── 84.csv
│ │ ├── 85.csv
│ │ ├── 86.csv
│ │ ├── 87.csv
│ │ ├── 88.csv
│ │ ├── 89.csv
│ │ ├── 9.csv
│ │ ├── 90.csv
│ │ ├── 91.csv
│ │ ├── 92.csv
│ │ ├── 93.csv
│ │ ├── 94.csv
│ │ ├── 95.csv
│ │ ├── 96.csv
│ │ ├── 97.csv
│ │ ├── 98.csv
│ │ └── 99.csv
│ ├── real_block.jl
│ ├── synthetic_sphere.jl
│ └── utilities.jl
├── src/
│ ├── Dojo.jl
│ ├── bodies/
│ │ ├── constructor.jl
│ │ ├── origin.jl
│ │ ├── set.jl
│ │ ├── shapes.jl
│ │ └── state.jl
│ ├── contacts/
│ │ ├── collisions/
│ │ │ ├── capsule_capsule.jl
│ │ │ ├── collision.jl
│ │ │ ├── point_to_box.jl
│ │ │ ├── point_to_box_v2.jl
│ │ │ ├── point_to_segment.jl
│ │ │ ├── sphere_box.jl
│ │ │ ├── sphere_capsule.jl
│ │ │ ├── sphere_halfspace.jl
│ │ │ ├── sphere_sphere.jl
│ │ │ └── string.jl
│ │ ├── cone.jl
│ │ ├── constraints.jl
│ │ ├── constructor.jl
│ │ ├── contact.jl
│ │ ├── impact.jl
│ │ ├── linear.jl
│ │ ├── nonlinear.jl
│ │ ├── utilities.jl
│ │ └── velocity.jl
│ ├── gradients/
│ │ ├── contact.jl
│ │ ├── data.jl
│ │ ├── finite_difference.jl
│ │ ├── state.jl
│ │ └── utilities.jl
│ ├── integrators/
│ │ ├── constraint.jl
│ │ └── integrator.jl
│ ├── joints/
│ │ ├── constraints.jl
│ │ ├── impulses.jl
│ │ ├── joint.jl
│ │ ├── limits.jl
│ │ ├── minimal.jl
│ │ ├── orthogonal.jl
│ │ ├── prototypes.jl
│ │ ├── rotational/
│ │ │ ├── constructor.jl
│ │ │ ├── dampers.jl
│ │ │ ├── impulses.jl
│ │ │ ├── input.jl
│ │ │ ├── minimal.jl
│ │ │ └── springs.jl
│ │ └── translational/
│ │ ├── constructor.jl
│ │ ├── dampers.jl
│ │ ├── impulses.jl
│ │ ├── input.jl
│ │ ├── minimal.jl
│ │ └── springs.jl
│ ├── mechanics/
│ │ ├── energy.jl
│ │ └── momentum.jl
│ ├── mechanism/
│ │ ├── constructor.jl
│ │ ├── data.jl
│ │ ├── edge.jl
│ │ ├── get.jl
│ │ ├── gravity.jl
│ │ ├── id.jl
│ │ ├── methods.jl
│ │ ├── node.jl
│ │ ├── set.jl
│ │ ├── state.jl
│ │ ├── system.jl
│ │ ├── traversal.jl
│ │ └── urdf.jl
│ ├── orientation/
│ │ ├── axis_angle.jl
│ │ ├── mapping.jl
│ │ ├── mrp.jl
│ │ ├── quaternion.jl
│ │ └── rotate.jl
│ ├── precompile.jl
│ ├── simulation/
│ │ ├── simulate.jl
│ │ ├── step.jl
│ │ └── storage.jl
│ ├── solver/
│ │ ├── centering.jl
│ │ ├── complementarity.jl
│ │ ├── correction.jl
│ │ ├── initialization.jl
│ │ ├── line_search.jl
│ │ ├── linear_system.jl
│ │ ├── mehrotra.jl
│ │ ├── options.jl
│ │ └── violations.jl
│ ├── utilities/
│ │ ├── custom_static.jl
│ │ ├── methods.jl
│ │ └── normalize.jl
│ └── visuals/
│ ├── colors.jl
│ ├── convert.jl
│ ├── rope.jl
│ ├── set.jl
│ └── visualizer.jl
└── test/
├── Project.toml
├── behaviors.jl
├── bodies.jl
├── collisions.jl
├── damper.jl
├── data.jl
├── energy.jl
├── impulse_map.jl
├── integrator.jl
├── jacobian.jl
├── joint_limits.jl
├── mechanism.jl
├── minimal.jl
├── momentum.jl
├── mrp.jl
├── runtests.jl
├── simulate.jl
├── state.jl
├── utilities.jl
└── visuals.jl
================================================
FILE CONTENTS
================================================
================================================
FILE: .github/workflows/CI.yml
================================================
name: CI
on:
push:
branches:
- main
tags: '*'
pull_request:
jobs:
test:
name: Julia ${{ matrix.version }} - ${{ matrix.os }} - ${{ matrix.arch }}
runs-on: ${{ matrix.os }}
strategy:
matrix:
version:
- '1.6' # LTS
- '1' # Current Stable
os:
- ubuntu-latest
- macOS-latest
- windows-latest
arch:
- x64
steps:
- uses: actions/checkout@v2
- uses: julia-actions/setup-julia@latest
with:
version: ${{ matrix.version }}
arch: ${{ matrix.arch }}
# - uses: julia-actions/julia-buildpkg@latest
- uses: julia-actions/cache@v1
- name: Install Julia dependencies and Test
shell: julia --project=monorepo {0}
run: |
using Pkg;
# dev mono repo versions
pkg"dev . ./DojoEnvironments"
Pkg.test("Dojo"; coverage=true)
- uses: julia-actions/julia-uploadcodecov@latest
env:
CODECOV_TOKEN: ${{ secrets.CODECOV_TOKEN }}
================================================
FILE: .github/workflows/CI_DojoEnv.yml
================================================
name: CI DojoEnvironments
on:
push:
branches:
- main
tags: '*'
pull_request:
jobs:
test:
name: Julia ${{ matrix.version }} - ${{ matrix.os }} - ${{ matrix.arch }}
runs-on: ${{ matrix.os }}
strategy:
matrix:
version:
- '1.6' # LTS
- '1' # Current Stable
os:
- ubuntu-latest
- macOS-latest
- windows-latest
arch:
- x64
steps:
- uses: actions/checkout@v2
- uses: julia-actions/setup-julia@latest
with:
version: ${{ matrix.version }}
arch: ${{ matrix.arch }}
# - uses: julia-actions/julia-buildpkg@latest
- uses: julia-actions/cache@v1
- name: Install Julia dependencies and Test
shell: julia --project=monorepo {0}
run: |
using Pkg;
# dev mono repo versions
pkg"dev . ./DojoEnvironments"
Pkg.test("DojoEnvironments"; coverage=true)
- uses: julia-actions/julia-uploadcodecov@latest
env:
CODECOV_TOKEN: ${{ secrets.CODECOV_TOKEN }}
================================================
FILE: .github/workflows/Documenter.yml
================================================
name: Documentation
on:
push:
branches:
- main
tags: '*'
pull_request:
jobs:
build:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v2
- uses: julia-actions/setup-julia@latest
with:
version: '1'
- name: Install dependencies
run: julia --project=docs/ -e 'using Pkg; Pkg.develop(PackageSpec(path=pwd())); Pkg.instantiate()'
- name: Build and deploy
env:
GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} # If authenticating with GitHub Actions token
DOCUMENTER_KEY: ${{ secrets.DOCUMENTER_KEY }} # If authenticating with SSH deploy key
run: julia --project=docs/ docs/make.jl
================================================
FILE: .github/workflows/TagBot.yml
================================================
name: TagBot
on:
issue_comment:
types:
- created
workflow_dispatch:
jobs:
TagBot:
if: github.event_name == 'workflow_dispatch' || github.actor == 'JuliaTagBot'
runs-on: ubuntu-latest
steps:
- uses: JuliaRegistries/TagBot@v1
with:
token: ${{ secrets.GITHUB_TOKEN }}
ssh: ${{ secrets.DOCUMENTER_KEY }}
================================================
FILE: .github/workflows/benchmark.yml
================================================
name: Run benchmarks
on:
pull_request:
jobs:
Benchmark:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v2
- uses: julia-actions/setup-julia@latest
with:
version: 1
# - uses: julia-actions/julia-buildpkg@latest
- name: Install dependencies
run: julia -e 'using Pkg; pkg"add PkgBenchmark BenchmarkCI@0.1"'
- name: Run benchmarks
run: julia -e 'using BenchmarkCI; BenchmarkCI.judge(retune=true,baseline="origin/main")' # retune sometimes necessary to avoid counting compile-time
- name: Print judgement
run: julia -e 'using BenchmarkCI; BenchmarkCI.displayjudgement()'
================================================
FILE: .github/workflows/dailyCI.yml
================================================
name: dailyCI
on:
schedule:
# Every day at 10AM and 10PM UTC
# - cron: '00 10,22 * * *'
# Every Monday at 1PM UTC (6AM PST)
- cron: '0 13 * * 1'
jobs:
test:
name: Julia ${{ matrix.version }} - ${{ matrix.os }} - ${{ matrix.arch }}
runs-on: ${{ matrix.os }}
continue-on-error: ${{ matrix.version == 'nightly'}}
strategy:
matrix:
version:
- '1.6' # LTS
- '1' # Current Stable
- 'nightly' # Upcoming
os:
- ubuntu-latest
- macOS-latest
- windows-latest
arch:
- x64
steps:
- uses: actions/checkout@v1
- uses: julia-actions/setup-julia@latest
with:
version: ${{ matrix.version }}
arch: ${{ matrix.arch }}
# - uses: julia-actions/julia-buildpkg@latest
- uses: julia-actions/julia-runtest@latest
- uses: julia-actions/julia-uploadcodecov@latest
env:
CODECOV_TOKEN: ${{ secrets.CODECOV_TOKEN }}
================================================
FILE: .gitignore
================================================
# Files from local benchmarking
benchmark/tune.json
# Files generated by invoking Julia with --code-coverage
*.jl.cov
*.jl.*.cov
# Files generated by invoking Julia with --track-allocation
*.jl.mem
# Notebooks
*.ipynb
# System-specific files and directories generated by the BinaryProvider and BinDeps packages
# They contain absolute paths specific to the host computer, and so should not be committed
deps/deps.jl
deps/build.log
deps/downloads/
deps/usr/
deps/src/
# Build artifacts for creating documentation generated by the Documenter package
docs/build/
docs/site/
# File generated by Pkg, the package manager, based on a corresponding Project.toml
# It records a fixed state of all packages used by the project. As such, it should not be
# committed for packages, but should be committed for applications that require a static
# environment.
Manifest.toml
# Outputs
*.txt
================================================
FILE: CITATION.bib
================================================
@article{howell_lecleach2022,
title={Dojo: A Differentiable Simulator for Robotics},
author={Howell, Taylor and Le Cleac'h, Simon and Bruedigam, Jan and Kolter, Zico and Schwager, Mac and Manchester, Zachary},
journal={arXiv preprint arXiv:2203.00806},
url={https://arxiv.org/abs/2203.00806},
year={2022}
}
================================================
FILE: DojoEnvironments/LICENSE
================================================
Copyright (c) 2023 Simon Le Cleac'h & Taylor Howell & Jan Bruedigam, and Zachary Manchester
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
================================================
FILE: DojoEnvironments/Project.toml
================================================
name = "DojoEnvironments"
uuid = "8728bab7-e8ae-46fd-a1b6-a8c4d00f608e"
authors = ["simonlc <simonlc@stanford.edu>", "thowell <thowell@stanford.edu>", "Jan Bruedigam <jan.bruedigam@tum.de>", "rejuvyesh <mail@rejuvyesh.com>"]
version = "0.6.1"
[deps]
Dojo = "ac60b53e-8d92-4c83-b960-e78698fa1916"
LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
PrecompileTools = "aea7be01-6a6a-4083-8856-8a6e6704d82a"
Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
[compat]
Dojo = "0.3.0, 0.4.0, 0.5.0, 0.6.0, 0.7.0"
PrecompileTools = "1.0, 1.2"
julia = "1.6, 1.10"
================================================
FILE: DojoEnvironments/README.md
================================================
# DojoEnvironments
This package contains convenience mechanisms and environments for the [Dojo.jl](https://github.com/dojo-sim/Dojo.jl) package.
For explanations, docs, and examples, see the main package.
================================================
FILE: DojoEnvironments/src/DojoEnvironments.jl
================================================
module DojoEnvironments
using LinearAlgebra
using Random
using Dojo
import Dojo: string_to_symbol, add_limits, RotX, RotY, RotZ, rotation_vector, SVector
using PrecompileTools
include("mechanisms.jl")
include("environments.jl")
include("utilities.jl")
include("mechanisms/include.jl")
include("environments/include.jl")
# Precompilation
include("precompile.jl")
# Mechanism
export
get_mechanism,
set_springs!,
set_dampers!,
set_limits
# Environment
export
Environment,
get_environment,
step!,
get_state,
visualize
end # module
================================================
FILE: DojoEnvironments/src/environments/ant_ars.jl
================================================
mutable struct AntARS{T,N} <: Environment{T,N}
mechanism::Mechanism{T}
storage::Storage{T,N}
end
function ant_ars(;
horizon=100,
timestep=0.05,
input_scaling=timestep,
gravity=-9.81,
urdf=:ant,
springs=0,
dampers=0,
parse_springs=true,
parse_dampers=true,
joint_limits=Dict([
(:hip_1, [-30,30] * π / 180),
(:ankle_1, [30,70] * π / 180),
(:hip_2, [-30,30] * π / 180),
(:ankle_2, [-70,-30] * π / 180),
(:hip_3, [-30,30] * π / 180),
(:ankle_3, [-70,-30] * π / 180),
(:hip_4, [-30,30] * π / 180),
(:ankle_4, [30,70] * π / 180)]),
keep_fixed_joints=true,
friction_coefficient=0.5,
contact_feet=true,
contact_body=true,
T=Float64)
mechanism = get_ant(;
timestep,
input_scaling,
gravity,
urdf,
springs,
dampers,
parse_springs,
parse_dampers,
joint_limits,
keep_fixed_joints,
friction_coefficient,
contact_feet,
contact_body,
T
)
storage = Storage(horizon, length(mechanism.bodies))
return AntARS{T,horizon}(mechanism, storage)
end
function DojoEnvironments.state_map(::AntARS, state)
state = state[1:28]
return state
end
function DojoEnvironments.input_map(::AntARS, input::AbstractVector)
input = [zeros(6);input] # floating base not actuated
return input
end
function Dojo.step!(environment::AntARS, state, input=nothing; k=1, record=false, opts=SolverOptions())
state = state_map(environment, state)
input = input_map(environment, input)
Dojo.step_minimal_coordinates!(environment.mechanism, state, input; opts)
record && Dojo.save_to_storage!(environment.mechanism, environment.storage, k)
return
end
function DojoEnvironments.get_state(environment::AntARS{T}) where T
contact_force = T[]
for contact in environment.mechanism.contacts
push!(contact_force, max(-1, min(1, contact.impulses[2][1])))
end
state = [get_minimal_state(environment.mechanism); contact_force]
return state
end
================================================
FILE: DojoEnvironments/src/environments/cartpole_dqn.jl
================================================
mutable struct CartpoleDQN{T,N} <: Environment{T,N}
mechanism::Mechanism{T}
storage::Storage{T,N}
end
function cartpole_dqn(;
horizon=100,
timestep=0.01,
input_scaling=timestep,
gravity=-9.81,
slider_mass=1,
pendulum_mass=1,
link_length=1,
radius=0.075,
color=RGBA(0.7, 0.7, 0.7, 1),
springs=0,
dampers=0,
joint_limits=Dict(),
keep_fixed_joints=true,
T=Float64)
mechanism = get_cartpole(;
timestep,
input_scaling,
gravity,
slider_mass,
pendulum_mass,
link_length,
radius,
color,
springs,
dampers,
joint_limits,
keep_fixed_joints,
T
)
storage = Storage(horizon, length(mechanism.bodies))
return CartpoleDQN{T,horizon}(mechanism, storage)
end
function DojoEnvironments.state_map(::CartpoleDQN, state)
return state
end
function DojoEnvironments.input_map(::CartpoleDQN, input::Real)
input = [input;0] # only the cart is actuated
return input
end
function Dojo.step!(environment::CartpoleDQN, state, input=nothing; k=1, record=false, opts=SolverOptions())
state = state_map(environment, state)
input = input_map(environment, input)
Dojo.step_minimal_coordinates!(environment.mechanism, state, input; opts)
record && Dojo.save_to_storage!(environment.mechanism, environment.storage, k)
return
end
function DojoEnvironments.get_state(environment::CartpoleDQN)
state = get_minimal_state(environment.mechanism)
return state
end
================================================
FILE: DojoEnvironments/src/environments/include.jl
================================================
include("ant_ars.jl")
include("cartpole_dqn.jl")
include("pendulum.jl")
include("quadruped_waypoint.jl")
include("quadruped_sampling.jl")
include("quadrotor_waypoint.jl")
include("uuv_waypoint.jl")
include("youbot_waypoint.jl")
================================================
FILE: DojoEnvironments/src/environments/pendulum.jl
================================================
mutable struct Pendulum{T,N} <: Environment{T,N}
mechanism::Mechanism{T}
storage::Storage{T,N}
end
function pendulum(;
horizon=100,
timestep=0.01,
input_scaling=timestep,
gravity=-9.81,
mass=1,
link_length=1,
color=RGBA(1, 0, 0),
springs=0,
dampers=0,
joint_limits=Dict(),
spring_offset=szeros(1),
orientation_offset=one(Quaternion),
T=Float64)
mechanism = get_pendulum(;
timestep,
input_scaling,
gravity,
mass,
link_length,
color,
springs,
dampers,
joint_limits,
spring_offset,
orientation_offset,
T
)
storage = Storage(horizon, length(mechanism.bodies))
return Pendulum{T,horizon}(mechanism, storage)
end
function DojoEnvironments.state_map(::Pendulum, state)
return state
end
function DojoEnvironments.input_map(::Pendulum, input::AbstractVector)
return input
end
function Dojo.step!(environment::Pendulum, state, input=nothing; k=1, record=false, opts=SolverOptions())
state = state_map(environment, state)
input = input_map(environment, input)
Dojo.step_minimal_coordinates!(environment.mechanism, state, input; opts)
record && Dojo.save_to_storage!(environment.mechanism, environment.storage, k)
return
end
function DojoEnvironments.get_state(environment::Pendulum)
state = get_minimal_state(environment.mechanism)
return state
end
================================================
FILE: DojoEnvironments/src/environments/quadrotor_waypoint.jl
================================================
mutable struct QuadrotorWaypoint{T,N} <: Environment{T,N}
mechanism::Mechanism{T}
storage::Storage{T,N}
rpms::AbstractVector
end
function quadrotor_waypoint(;
horizon=100,
timestep=0.01,
input_scaling=timestep,
gravity=-9.81,
urdf=:pelican,
springs=0,
dampers=0,
parse_springs=true,
parse_dampers=true,
joint_limits=Dict(),
keep_fixed_joints=false,
friction_coefficient=0.5,
contact_rotors=true,
contact_body=true,
T=Float64)
mechanism = get_quadrotor(;
timestep,
input_scaling,
gravity,
urdf,
springs,
dampers,
parse_springs,
parse_dampers,
joint_limits,
keep_fixed_joints,
friction_coefficient,
contact_rotors,
contact_body,
T
)
storage = Storage(horizon, length(mechanism.bodies))
return QuadrotorWaypoint{T,horizon}(mechanism, storage, zeros(4))
end
function DojoEnvironments.state_map(::QuadrotorWaypoint, state)
state = [state;zeros(8)]
return state
end
function DojoEnvironments.input_map(environment::QuadrotorWaypoint, input::AbstractVector)
# Input is rotor rpm directly
# Rotors are only visualized, dynamics are mapped here
environment.rpms = input
body = get_body(environment.mechanism, :base_link)
q = body.state.q2
force_torque = rpm_to_force_torque(environment, input, q)
input = [force_torque;zeros(4)]
return input
end
function Dojo.step!(environment::QuadrotorWaypoint, state, input=nothing; k=1, record=false, opts=SolverOptions())
state = state_map(environment, state)
input = input_map(environment, input)
Dojo.step_minimal_coordinates!(environment.mechanism, state, input; opts)
record && Dojo.save_to_storage!(environment.mechanism, environment.storage, k)
return
end
function Dojo.simulate!(environment::QuadrotorWaypoint{T,N}, controller! = (environment, k) -> nothing; kwargs...) where {T,N}
mechanism = environment.mechanism
joint_rotor_0 = get_joint(mechanism, :rotor_0_joint)
joint_rotor_1 = get_joint(mechanism, :rotor_1_joint)
joint_rotor_2 = get_joint(mechanism, :rotor_2_joint)
joint_rotor_3 = get_joint(mechanism, :rotor_3_joint)
function controller_wrapper!(mechanism, k)
rpms = environment.rpms
set_minimal_velocities!(mechanism, joint_rotor_0, [rpms[1]])
set_minimal_velocities!(mechanism, joint_rotor_1, [-rpms[2]])
set_minimal_velocities!(mechanism, joint_rotor_2, [rpms[3]])
set_minimal_velocities!(mechanism, joint_rotor_3, [-rpms[4]])
controller!(environment, k)
end
simulate!(environment.mechanism, 1:N, environment.storage, controller_wrapper!; kwargs...)
end
function DojoEnvironments.get_state(environment::QuadrotorWaypoint)
state = get_minimal_state(environment.mechanism)[1:12]
return state
end
function Dojo.visualize(environment::QuadrotorWaypoint;
waypoints=[
[1;1;0.3],
[2;0;0.3],
[1;-1;0.3],
[0;0;0.3],
],
return_animation=false,
kwargs...)
vis, animation = visualize(environment.mechanism, environment.storage; return_animation=true, kwargs...)
for (i,waypoint) in enumerate(waypoints)
waypoint_shape = Sphere(0.2;color=RGBA(0,0.25*i,0,0.3))
visshape = Dojo.convert_shape(waypoint_shape)
subvisshape = vis["waypoints"]["waypoint$i"]
Dojo.setobject!(subvisshape, visshape, waypoint_shape)
Dojo.atframe(animation, 1) do
Dojo.set_node!(waypoint, one(Quaternion), waypoint_shape, subvisshape, true)
end
end
Dojo.setanimation!(vis,animation)
return_animation ? (return vis, animation) : (return vis)
end
# ## physics functions
function rpm_to_force_torque(::QuadrotorWaypoint, rpm::Real, rotor_sign::Int64)
force_factor = 0.001
torque_factor = 0.0001
force = sign(rpm)*force_factor*rpm^2
torque = sign(rpm)*rotor_sign*torque_factor*rpm^2
return [force;0;0], [torque;0;0]
end
function rpm_to_force_torque(environment::QuadrotorWaypoint, rpms::AbstractVector, q::Quaternion)
qympi2 = Dojo.RotY(-pi/2)
orientations = [qympi2;qympi2;qympi2;qympi2]
directions = [1;-1;1;-1]
force_vertices = [
[0.21; 0; 0.05],
[0; 0.21; 0.05],
[-0.21; 0; 0.05],
[0; -0.21; 0.05],
]
forces_torques = [rpm_to_force_torque(environment, rpms[i], directions[i]) for i=1:4]
forces = getindex.(forces_torques,1)
torques = getindex.(forces_torques,2)
forces = Dojo.vector_rotate.(forces, orientations) # in local frame
torques = Dojo.vector_rotate.(torques, orientations) # in local frame
torques_from_forces = Dojo.cross.(force_vertices, forces)
force = Dojo.vector_rotate(sum(forces), q) # in minimal frame
torque = Dojo.vector_rotate(sum(torques .+ torques_from_forces), q) # in minimal frame
return [force; torque]
end
================================================
FILE: DojoEnvironments/src/environments/quadruped_sampling.jl
================================================
mutable struct QuadrupedSampling{T,N} <: Environment{T,N}
mechanism::Mechanism{T}
storage::Storage{T,N}
end
function quadruped_sampling(;
horizon=100,
timestep=0.01,
input_scaling=timestep,
gravity=-9.81,
urdf=:gazebo_a1,
springs=0,
dampers=0,
parse_springs=true,
parse_dampers=true,
spring_offset=true,
joint_limits=Dict(vcat([[
(Symbol(group,:_hip_joint), [-0.5,0.5]),
(Symbol(group,:_thigh_joint), [-0.5,1.5]),
(Symbol(group,:_calf_joint), [-2.5,-1])]
for group in [:FR, :FL, :RR, :RL]]...)),
keep_fixed_joints=false,
friction_coefficient=0.8,
contact_feet=true,
contact_body=true,
T=Float64)
mechanism = get_quadruped(;
timestep,
input_scaling,
gravity,
urdf,
springs,
dampers,
parse_springs,
parse_dampers,
spring_offset,
joint_limits,
keep_fixed_joints,
friction_coefficient,
contact_feet,
contact_body,
T
)
storage = Storage(horizon, length(mechanism.bodies))
return QuadrupedSampling{T,horizon}(mechanism, storage)
end
function DojoEnvironments.state_map(::QuadrupedSampling, state)
return state
end
function DojoEnvironments.input_map(::QuadrupedSampling, input::AbstractVector)
input = [zeros(6);input] # trunk not actuated
return input
end
function Dojo.step!(environment::QuadrupedSampling, state, input=nothing; k=1, record=false, opts=SolverOptions())
state = state_map(environment, state)
input = input_map(environment, input)
Dojo.step_minimal_coordinates!(environment.mechanism, state, input; opts)
record && Dojo.save_to_storage!(environment.mechanism, environment.storage, k)
return
end
function DojoEnvironments.get_state(environment::QuadrupedSampling)
state = get_minimal_state(environment.mechanism)
# x: floating base, FR (hip, thigh, calf), FL, RR, RL
return state
end
================================================
FILE: DojoEnvironments/src/environments/quadruped_waypoint.jl
================================================
mutable struct QuadrupedWaypoint{T,N} <: Environment{T,N}
mechanism::Mechanism{T}
storage::Storage{T,N}
end
function quadruped_waypoint(;
horizon=100,
timestep=0.001,
input_scaling=timestep,
gravity=-9.81,
urdf=:gazebo_a1,
springs=0,
dampers=0,
parse_springs=true,
parse_dampers=true,
spring_offset=true,
joint_limits=Dict(vcat([[
(Symbol(group,:_hip_joint), [-0.5,0.5]),
(Symbol(group,:_thigh_joint), [-0.5,1.5]),
(Symbol(group,:_calf_joint), [-2.5,-1])]
for group in [:FR, :FL, :RR, :RL]]...)),
keep_fixed_joints=false,
friction_coefficient=0.8,
contact_feet=true,
contact_body=false,
T=Float64)
mechanism = get_quadruped(;
timestep,
input_scaling,
gravity,
urdf,
springs,
dampers,
parse_springs,
parse_dampers,
spring_offset,
joint_limits,
keep_fixed_joints,
friction_coefficient,
contact_feet,
contact_body,
T
)
storage = Storage(horizon, length(mechanism.bodies))
return QuadrupedWaypoint{T,horizon}(mechanism, storage)
end
function DojoEnvironments.state_map(::QuadrupedWaypoint, state)
return state
end
function DojoEnvironments.input_map(::QuadrupedWaypoint, input::AbstractVector)
input = [zeros(6);input] # trunk not actuated
return input
end
function Dojo.step!(environment::QuadrupedWaypoint, state, input=nothing; k=1, record=false, opts=SolverOptions())
state = state_map(environment, state)
input = input_map(environment, input)
Dojo.step_minimal_coordinates!(environment.mechanism, state, input; opts)
record && Dojo.save_to_storage!(environment.mechanism, environment.storage, k)
return
end
function DojoEnvironments.get_state(environment::QuadrupedWaypoint)
state = get_minimal_state(environment.mechanism)
# x: floating base, FR (hip, thigh, calf), FL, RR, RL
return state
end
function Dojo.visualize(environment::QuadrupedWaypoint;
waypoints=[
[0.5;0.5;0.3],
[1;0;0.3],
[0.5;-0.5;0.3],
[0;0;0.3],
],
return_animation=false,
kwargs...)
vis, animation = visualize(environment.mechanism, environment.storage; return_animation=true, kwargs...)
for (i,waypoint) in enumerate(waypoints)
waypoint_shape = Sphere(0.2;color=RGBA(0,0.25*i,0,0.3))
visshape = Dojo.convert_shape(waypoint_shape)
subvisshape = vis["waypoints"]["waypoint$i"]
Dojo.setobject!(subvisshape, visshape, waypoint_shape)
Dojo.atframe(animation, 1) do
Dojo.set_node!(waypoint, one(Quaternion), waypoint_shape, subvisshape, true)
end
end
Dojo.setanimation!(vis,animation)
return_animation ? (return vis, animation) : (return vis)
end
================================================
FILE: DojoEnvironments/src/environments/uuv_waypoint.jl
================================================
mutable struct UUVWaypoint{T,N} <: Environment{T,N}
mechanism::Mechanism{T}
storage::Storage{T,N}
rpms::AbstractVector
end
function uuv_waypoint(;
horizon=100,
timestep=0.01,
input_scaling=timestep,
gravity=-9.81,
urdf=:mini_tortuga,
springs=0,
dampers=0,
parse_springs=true,
parse_dampers=true,
joint_limits=Dict(),
keep_fixed_joints=false,
friction_coefficient=0.5,
contact_body=true,
T=Float64)
mechanism = get_uuv(;
timestep,
input_scaling,
gravity,
urdf,
springs,
dampers,
parse_springs,
parse_dampers,
joint_limits,
keep_fixed_joints,
friction_coefficient,
contact_body,
T
)
storage = Storage(horizon, length(mechanism.bodies))
return UUVWaypoint{T,horizon}(mechanism, storage, zeros(6))
end
function DojoEnvironments.state_map(::UUVWaypoint, state)
state = [state;zeros(12)]
return state
end
function DojoEnvironments.input_map(environment::UUVWaypoint, input::AbstractVector)
# Input is rotor rpm directly
# Rotors are only visualized, dynamics are mapped here
environment.rpms = input
body = get_body(environment.mechanism, :base_link)
q = body.state.q2
force_torque = rpm_to_force_torque(environment, input, q)
input = [force_torque;zeros(6)]
return input
end
function Dojo.step!(environment::UUVWaypoint, state, input=nothing; k=1, record=false, opts=SolverOptions())
state = state_map(environment, state)
input = input_map(environment, input)
Dojo.step_minimal_coordinates!(environment.mechanism, state, input; opts)
record && Dojo.save_to_storage!(environment.mechanism, environment.storage, k)
return
end
function Dojo.simulate!(environment::UUVWaypoint{T,N}, controller! = (environment, k) -> nothing; kwargs...) where {T,N}
mechanism = environment.mechanism
rotor_1_joint = get_joint(mechanism, :rotor_1_joint)
rotor_2_joint = get_joint(mechanism, :rotor_2_joint)
rotor_3_joint = get_joint(mechanism, :rotor_3_joint)
rotor_4_joint = get_joint(mechanism, :rotor_4_joint)
rotor_5_joint = get_joint(mechanism, :rotor_5_joint)
rotor_6_joint = get_joint(mechanism, :rotor_6_joint)
function controller_wrapper!(mechanism, k)
rpms = environment.rpms
set_minimal_velocities!(mechanism, rotor_1_joint, [rpms[1]])
set_minimal_velocities!(mechanism, rotor_2_joint, [rpms[2]])
set_minimal_velocities!(mechanism, rotor_3_joint, [-rpms[3]])
set_minimal_velocities!(mechanism, rotor_4_joint, [-rpms[4]])
set_minimal_velocities!(mechanism, rotor_5_joint, [rpms[5]])
set_minimal_velocities!(mechanism, rotor_6_joint, [-rpms[6]])
buoyancy!(environment, mechanism)
controller!(environment, k)
end
simulate!(environment.mechanism, 1:N, environment.storage, controller_wrapper!; kwargs...)
end
function DojoEnvironments.get_state(environment::UUVWaypoint)
state = get_minimal_state(environment.mechanism)[1:12]
return state
end
function Dojo.visualize(environment::UUVWaypoint;
waypoints=[
[1;1;0.3],
[2;0;0.3],
[1;-1;0.3],
[0;0;0.3],
],
return_animation=false,
kwargs...)
vis, animation = visualize(environment.mechanism, environment.storage; return_animation=true, kwargs...)
for (i,waypoint) in enumerate(waypoints)
waypoint_shape = Sphere(0.2;color=RGBA(0,0.25*i,0,0.3))
visshape = Dojo.convert_shape(waypoint_shape)
subvisshape = vis["waypoints"]["waypoint$i"]
Dojo.setobject!(subvisshape, visshape, waypoint_shape)
Dojo.atframe(animation, 1) do
Dojo.set_node!(waypoint, one(Quaternion), waypoint_shape, subvisshape, true)
end
end
Dojo.setanimation!(vis,animation)
return_animation ? (return vis, animation) : (return vis)
end
# ## physics functions
function rpm_to_force_torque(::UUVWaypoint, rpm::Real, rotor_sign::Int64)
force_factor = 0.01
torque_factor = 0.001
force = sign(rpm)*force_factor*rpm^2
torque = sign(rpm)*rotor_sign*torque_factor*rpm^2
return [force;0;0], [torque;0;0]
end
function rpm_to_force_torque(environment::UUVWaypoint, rpms::AbstractVector, q::Quaternion)
qzpi4 = Dojo.RotZ(pi/4)
qzmpi4 = Dojo.RotZ(-pi/4)
qympi2 = Dojo.RotY(-pi/2)
orientations = [qzpi4;qzmpi4;qzmpi4;qzpi4;qympi2;qympi2]
directions = [1;1;-1;-1;1;-1]
force_vertices = [
[0.14; -0.09; 0.059],
[0.14; 0.09; 0.059],
[-0.14; -0.09; 0.059],
[-0.14; 0.09; 0.059],
[0; -0.0855; 0.165],
[0; 0.0855; 0.165],
]
forces_torques = [rpm_to_force_torque(environment, rpms[i], directions[i]) for i=1:6]
forces = getindex.(forces_torques,1)
torques = getindex.(forces_torques,2)
forces = Dojo.vector_rotate.(forces, orientations) # in local frame
torques = Dojo.vector_rotate.(torques, orientations) # in local frame
torques_from_forces = Dojo.cross.(force_vertices, forces)
force = Dojo.vector_rotate(sum(forces), q) # in minimal frame
torque = Dojo.vector_rotate(sum(torques .+ torques_from_forces), q) # in minimal frame
return [force; torque]
end
function buoyancy!(::UUVWaypoint, mechanism)
body = get_body(mechanism, :base_link)
q = body.state.q2
force = Dojo.vector_rotate([0;0;19.5*9.81], inv(q)) # slightly positive
torque = Dojo.cross([0;0;0.2], force)
add_external_force!(body; force, torque)
return
end
================================================
FILE: DojoEnvironments/src/environments/youbot_waypoint.jl
================================================
mutable struct YoubotWaypoint{T,N} <: Environment{T,N}
mechanism::Mechanism{T}
storage::Storage{T,N}
end
function youbot_waypoint(;
horizon=100,
timestep=0.01,
input_scaling=timestep,
gravity=-9.81,
urdf=:youbot,
springs=0,
dampers=0,
parse_springs=true,
parse_dampers=true,
joint_limits=Dict([
(:arm_joint_1, [-2.95,2.95]),
(:arm_joint_2, [-1.57,1.13]),
(:arm_joint_3, [-2.55,2.55]),
(:arm_joint_4, [-1.78,1.78]),
(:arm_joint_5, [-2.92,2.92]),
# (:gripper_finger_joint_l, [0,0.03]),
# (:gripper_finger_joint_r, [-0.03,0]),
]),
keep_fixed_joints=false,
T=Float64)
mechanism = get_youbot(;
timestep,
input_scaling,
gravity,
urdf,
springs,
dampers,
parse_springs,
parse_dampers,
joint_limits,
keep_fixed_joints,
T
)
storage = Storage(horizon, length(mechanism.bodies))
return YoubotWaypoint{T,horizon}(mechanism, storage)
end
function DojoEnvironments.state_map(::YoubotWaypoint, state)
xy = state[1:2] # minimal coordinates are rotated for youbot
vxy = state[4:5] # minimal coordinates are rotated for youbot
xy_minimal = Dojo.vector_rotate([xy;0],Dojo.RotZ(-pi/2))[1:2]
vxy_minimal = Dojo.vector_rotate([vxy;0],Dojo.RotZ(-pi/2))[1:2]
state[1:2] = xy_minimal
state[4:5] = vxy_minimal
state = [state[1:6]; zeros(8); state[7:end]]
return state
end
function DojoEnvironments.input_map(environment::YoubotWaypoint, input::AbstractVector)
# Wheels are only for visualization and not actuated
# so the wheel input must be mapped to the base
l = 0.456
w = 0.316
H = [
1 -1 -l-w
1 1 l+w
1 1 -l-w
1 -1 l+w
]
θz = get_state(environment)[3]
wheel_input = input[1:4] # fl, fr, bl, br
base_input = H\wheel_input/10 # incorrect but roughly ok mapping
xy = base_input[1:2]
xy_minimal = Dojo.vector_rotate([xy;0],Dojo.RotZ(θz-pi/2))[1:2]
base_input[1:2] = xy_minimal
wheel_input = zeros(4)
arm_input = input[5:end] # joint1 to joint5 to fingerl to fingerr
input = [base_input;wheel_input;arm_input]
return input
end
function Dojo.step!(environment::YoubotWaypoint, state, input=nothing; k=1, record=false, opts=SolverOptions())
state = state_map(environment, state)
input = input_map(environment, input)
Dojo.step_minimal_coordinates!(environment.mechanism, state, input; opts)
record && Dojo.save_to_storage!(environment.mechanism, environment.storage, k)
return
end
function Dojo.simulate!(environment::YoubotWaypoint{T,N}, controller! = (environment, k) -> nothing; kwargs...) where {T,N}
mechanism = environment.mechanism
joint_fl = get_joint(mechanism, :wheel_joint_fl)
joint_fr = get_joint(mechanism, :wheel_joint_fr)
joint_bl = get_joint(mechanism, :wheel_joint_bl)
joint_br = get_joint(mechanism, :wheel_joint_br)
function controller_wrapper!(mechanism, k)
l = 0.456; w = 0.316; r = 0.0475
H = [
1 -1 -l-w
1 1 l+w
1 1 -l-w
1 -1 l+w
]
v = get_state(environment)[4:6]
wheel_speeds = H*v/r
set_minimal_velocities!(mechanism, joint_fl, [wheel_speeds[1]])
set_minimal_velocities!(mechanism, joint_fr, [wheel_speeds[2]])
set_minimal_velocities!(mechanism, joint_bl, [wheel_speeds[3]])
set_minimal_velocities!(mechanism, joint_br, [wheel_speeds[4]])
controller!(environment, k)
end
simulate!(environment.mechanism, 1:N, environment.storage, controller_wrapper!; kwargs...)
end
function DojoEnvironments.get_state(environment::YoubotWaypoint)
state = get_minimal_state(environment.mechanism)
xy_minimal = state[1:2] # minimal coordinates are rotated for youbot
vxy_minimal = state[4:5] # minimal coordinates are rotated for youbot
xy = Dojo.vector_rotate([xy_minimal;0],Dojo.RotZ(pi/2))[1:2]
vxy = Dojo.vector_rotate([vxy_minimal;0],Dojo.RotZ(pi/2))[1:2]
state[1:2] = xy
state[4:5] = vxy
state = [state[1:6]; state[15:end]]
return state
end
function Dojo.visualize(environment::YoubotWaypoint;
waypoints=[
[1;1;0.3],
[2;0;0.3],
[1;-1;0.3],
[0;0;0.3],
],
return_animation=false,
kwargs...)
vis, animation = visualize(environment.mechanism, environment.storage; return_animation=true, kwargs...)
for (i,waypoint) in enumerate(waypoints)
waypoint_shape = Sphere(0.2;color=RGBA(0,0.25*i,0,0.3))
visshape = Dojo.convert_shape(waypoint_shape)
subvisshape = vis["waypoints"]["waypoint$i"]
Dojo.setobject!(subvisshape, visshape, waypoint_shape)
Dojo.atframe(animation, 1) do
Dojo.set_node!(waypoint, one(Quaternion), waypoint_shape, subvisshape, true)
end
end
Dojo.setanimation!(vis,animation)
return_animation ? (return vis, animation) : (return vis)
end
================================================
FILE: DojoEnvironments/src/environments.jl
================================================
"""
Environment{T,N}
abstract type for an environment consisting of a mechanism and a storage
"""
abstract type Environment{T,N} end
function Base.show(io::IO, mime::MIME{Symbol("text/plain")}, environment::Environment)
summary(io, environment)
end
"""
get_environment(model; kwargs...)
construct existing environment
model: name of of environment
kwargs: environment specific parameters
"""
function get_environment(model; kwargs...)
return eval(string_to_symbol(model))(; string_to_symbol(kwargs)...)
end
"""
state_map(environment, state)
maps the provided state to the environments internal state
environment: environment
state: provided state
"""
function state_map(::Environment, state)
return state
end
"""
input_map(environment, input)
maps the provided input to the environment's internal input
environment: environment
input: provided input
"""
function input_map(::Environment, input)
return input
end
function input_map(environment::Environment, ::Nothing)
return zeros(input_dimension(environment.mechanism))
end
"""
set_input!(environment, input)
sets the provided input to the environment's mechanism
environment: environment
input: provided input
"""
function Dojo.set_input!(environment::Environment, input)
set_input!(environment.mechanism, input_map(environment, input))
return
end
"""
step(environment, x, u; k, record, opts)
simulates environment one time step
environment: Environment
x: state
u: input
k: current timestep
record: record step in storage
opts: SolverOptions
"""
function Dojo.step!(environment::Environment, state, input=nothing; k=1, record=false, opts=SolverOptions())
state = state_map(environment, state)
input = input_map(environment, input)
Dojo.step_minimal_coordinates!(environment.mechanism, state, input; opts)
record && Dojo.save_to_storage!(environment.mechanism, environment.storage, k)
return
end
"""
simulate!(environment, controller! = (mechanism, k) -> nothing; kwargs...)
simulates the mechanism of the environment
environment: Environment
controller!: Control function
kwargs: same as for Dojo.simulate
"""
function Dojo.simulate!(environment::Environment{T,N}, controller! = (environment, k) -> nothing; kwargs...) where {T,N}
controller_wrapper!(mechanism, k) = controller!(environment, k)
simulate!(environment.mechanism, 1:N, environment.storage, controller_wrapper!; kwargs...)
end
"""
get_state(environment)
returns the state of the environment
environment: Environment
"""
function get_state(environment::Environment)
return get_minimal_state(environment.mechanism)
end
"""
initialize!(environment; kwargs...)
initializes the environment's mechanism
environment: Environment
kwargs: same as for DojoEnvironments' mechanisms
"""
function Dojo.initialize!(environment::Environment, model; kwargs...)
eval(Symbol(:initialize, :_, string_to_symbol(model), :!))(environment.mechanism; kwargs...)
end
"""
visualize(environment; kwargs...)
visualizes the environment with its internal storage
environment: Environment
kwargs: same as for Dojo.visualize
"""
function Dojo.visualize(environment::Environment; kwargs...)
visualize(environment.mechanism, environment.storage; kwargs...)
end
================================================
FILE: DojoEnvironments/src/mechanisms/ant/dependencies/ant.urdf
================================================
<?xml version="1.0" ?>
<robot name="ant">
<link name="torso">
<inertial>
<origin rpy="0.00000 -0.00000 0.00000" xyz="0.00000 0.00000 0.00000"/>
<mass value="0.32724923474893675"/>
<inertia ixx="0.00818123" ixy="0" ixz="0" iyy="0.00818123" iyz="0" izz="0.00818123"/>
</inertial>
<collision>
<origin rpy="0.00000 -0.00000 0.00000" xyz="0.00000 0.00000 0.00000"/>
<geometry>
<sphere radius="0.25000"/>
</geometry>
</collision>
<visual>
<origin rpy="0.00000 -0.00000 0.00000" xyz="0.00000 0.00000 0.00000"/>
<geometry>
<sphere radius="0.25000"/>
</geometry>
</visual>
</link>
<link name="front_left_leg">
<inertial>
<origin rpy="0.00000 -0.00000 0.00000" xyz="0.10000 0.10000 0.00000"/>
<mass value="0.03647692799740343"/>
<inertia ixx="0.00045856" ixy="0" ixz="0" iyy="0.00045856" iyz="0" izz="0.000116726"/>
</inertial>
<collision>
<origin rpy="-1.57080 0.78540 -0.78540" xyz="0.10000 0.10000 0.00000"/>
<geometry>
<capsule length="0.28284" radius="0.08000"/>
</geometry>
</collision>
<visual>
<origin rpy="-1.57080 0.78540 -0.78540" xyz="0.10000 0.10000 0.00000"/>
<geometry>
<capsule length="0.28284" radius="0.08000"/>
</geometry>
</visual>
</link>
<link name="aux_1">
<inertial>
<origin rpy="0.00000 -0.00000 0.00000" xyz="0.10000 0.10000 0.00000"/>
<mass value="0.03647692799740343"/>
<inertia ixx="0.00045856" ixy="0" ixz="0" iyy="0.00045856" iyz="0" izz="0.000116726"/>
</inertial>
<collision>
<origin rpy="-1.57080 0.78540 -0.78540" xyz="0.10000 0.10000 0.00000"/>
<geometry>
<capsule length="0.28284" radius="0.08000"/>
</geometry>
</collision>
<visual>
<origin rpy="-1.57080 0.78540 -0.78540" xyz="0.10000 0.10000 0.00000"/>
<geometry>
<capsule length="0.28284" radius="0.08000"/>
</geometry>
</visual>
</link>
<link name="front_left_foot">
<inertial>
<origin rpy="0.00000 -0.00000 0.00000" xyz="0.20000 0.20000 0.00000"/>
<mass value="0.06491137880161697"/>
<inertia ixx="0.00235904" ixy="0" ixz="0" iyy="0.00235904" iyz="0" izz="0.000207716"/>
</inertial>
<collision>
<origin rpy="-1.57080 0.78540 -0.78540" xyz="0.20000 0.20000 0.00000"/>
<geometry>
<capsule length="0.56569" radius="0.08000"/>
</geometry>
</collision>
<visual>
<origin rpy="-1.57080 0.78540 -0.78540" xyz="0.20000 0.20000 0.00000"/>
<geometry>
<capsule length="0.56569" radius="0.08000"/>
</geometry>
</visual>
</link>
<link name="front_right_leg">
<inertial>
<origin rpy="0.00000 -0.00000 0.00000" xyz="-0.10000 0.10000 0.00000"/>
<mass value="0.03647692799740343"/>
<inertia ixx="0.00045856" ixy="0" ixz="0" iyy="0.00045856" iyz="0" izz="0.000116726"/>
</inertial>
<collision>
<origin rpy="-1.57080 -0.78540 0.78540" xyz="-0.10000 0.10000 0.00000"/>
<geometry>
<capsule length="0.28284" radius="0.08000"/>
</geometry>
</collision>
<visual>
<origin rpy="-1.57080 -0.78540 0.78540" xyz="-0.10000 0.10000 0.00000"/>
<geometry>
<capsule length="0.28284" radius="0.08000"/>
</geometry>
</visual>
</link>
<link name="aux_2">
<inertial>
<origin rpy="0.00000 -0.00000 0.00000" xyz="-0.10000 0.10000 0.00000"/>
<mass value="0.03647692799740343"/>
<inertia ixx="0.00045856" ixy="0" ixz="0" iyy="0.00045856" iyz="0" izz="0.000116726"/>
</inertial>
<collision>
<origin rpy="-1.57080 -0.78540 0.78540" xyz="-0.10000 0.10000 0.00000"/>
<geometry>
<capsule length="0.28284" radius="0.08000"/>
</geometry>
</collision>
<visual>
<origin rpy="-1.57080 -0.78540 0.78540" xyz="-0.10000 0.10000 0.00000"/>
<geometry>
<capsule length="0.28284" radius="0.08000"/>
</geometry>
</visual>
</link>
<link name="front_right_foot">
<inertial>
<origin rpy="0.00000 -0.00000 0.00000" xyz="-0.20000 0.20000 0.00000"/>
<mass value="0.06491137880161697"/>
<inertia ixx="0.00235904" ixy="0" ixz="0" iyy="0.00235904" iyz="0" izz="0.000207716"/>
</inertial>
<collision>
<origin rpy="-1.57080 -0.78540 0.78540" xyz="-0.20000 0.20000 0.00000"/>
<geometry>
<capsule length="0.56569" radius="0.08000"/>
</geometry>
</collision>
<visual>
<origin rpy="-1.57080 -0.78540 0.78540" xyz="-0.20000 0.20000 0.00000"/>
<geometry>
<capsule length="0.56569" radius="0.08000"/>
</geometry>
</visual>
</link>
<link name="left_back_leg">
<inertial>
<origin rpy="0.00000 -0.00000 0.00000" xyz="-0.10000 -0.10000 0.00000"/>
<mass value="0.03647692799740343"/>
<inertia ixx="0.00045856" ixy="0" ixz="0" iyy="0.00045856" iyz="0" izz="0.000116726"/>
</inertial>
<collision>
<origin rpy="1.57080 -0.78540 -0.78540" xyz="-0.10000 -0.10000 0.00000"/>
<geometry>
<capsule length="0.28284" radius="0.08000"/>
</geometry>
</collision>
<visual>
<origin rpy="1.57080 -0.78540 -0.78540" xyz="-0.10000 -0.10000 0.00000"/>
<geometry>
<capsule length="0.28284" radius="0.08000"/>
</geometry>
</visual>
</link>
<link name="aux_3">
<inertial>
<origin rpy="0.00000 -0.00000 0.00000" xyz="-0.10000 -0.10000 0.00000"/>
<mass value="0.03647692799740343"/>
<inertia ixx="0.00045856" ixy="0" ixz="0" iyy="0.00045856" iyz="0" izz="0.000116726"/>
</inertial>
<collision>
<origin rpy="1.57080 -0.78540 -0.78540" xyz="-0.10000 -0.10000 0.00000"/>
<geometry>
<capsule length="0.28284" radius="0.08000"/>
</geometry>
</collision>
<visual>
<origin rpy="1.57080 -0.78540 -0.78540" xyz="-0.10000 -0.10000 0.00000"/>
<geometry>
<capsule length="0.28284" radius="0.08000"/>
</geometry>
</visual>
</link>
<link name="left_back_foot">
<inertial>
<origin rpy="0.00000 -0.00000 0.00000" xyz="-0.20000 -0.20000 0.00000"/>
<mass value="0.06491137880161697"/>
<inertia ixx="0.00235904" ixy="0" ixz="0" iyy="0.00235904" iyz="0" izz="0.000207716"/>
</inertial>
<collision>
<origin rpy="1.57080 -0.78540 -0.78540" xyz="-0.20000 -0.20000 0.00000"/>
<geometry>
<capsule length="0.56569" radius="0.08000"/>
</geometry>
</collision>
<visual>
<origin rpy="1.57080 -0.78540 -0.78540" xyz="-0.20000 -0.20000 0.00000"/>
<geometry>
<capsule length="0.56569" radius="0.08000"/>
</geometry>
</visual>
</link>
<link name="right_back_leg">
<inertial>
<origin rpy="0.00000 -0.00000 0.00000" xyz="0.10000 -0.10000 0.00000"/>
<mass value="0.03647692799740343"/>
<inertia ixx="0.00045856" ixy="0" ixz="0" iyy="0.00045856" iyz="0" izz="0.000116726"/>
</inertial>
<collision>
<origin rpy="1.57080 0.78540 0.78540" xyz="0.10000 -0.10000 0.00000"/>
<geometry>
<capsule length="0.28284" radius="0.08000"/>
</geometry>
</collision>
<visual>
<origin rpy="1.57080 0.78540 0.78540" xyz="0.10000 -0.10000 0.00000"/>
<geometry>
<capsule length="0.28284" radius="0.08000"/>
</geometry>
</visual>
</link>
<link name="aux_4">
<inertial>
<origin rpy="0.00000 -0.00000 0.00000" xyz="0.10000 -0.10000 0.00000"/>
<mass value="0.03647692799740343"/>
<inertia ixx="0.00045856" ixy="0" ixz="0" iyy="0.00045856" iyz="0" izz="0.000116726"/>
</inertial>
<collision>
<origin rpy="1.57080 0.78540 0.78540" xyz="0.10000 -0.10000 0.00000"/>
<geometry>
<capsule length="0.28284" radius="0.08000"/>
</geometry>
</collision>
<visual>
<origin rpy="1.57080 0.78540 0.78540" xyz="0.10000 -0.10000 0.00000"/>
<geometry>
<capsule length="0.28284" radius="0.08000"/>
</geometry>
</visual>
</link>
<link name="right_back_foot">
<inertial>
<origin rpy="0.00000 -0.00000 0.00000" xyz="0.20000 -0.20000 0.00000"/>
<mass value="0.06491137880161697"/>
<inertia ixx="0.00235904" ixy="0" ixz="0" iyy="0.00235904" iyz="0" izz="0.000207716"/>
</inertial>
<collision>
<origin rpy="1.57080 0.78540 0.78540" xyz="0.20000 -0.20000 0.00000"/>
<geometry>
<capsule length="0.56569" radius="0.08000"/>
</geometry>
</collision>
<visual>
<origin rpy="1.57080 0.78540 0.78540" xyz="0.20000 -0.20000 0.00000"/>
<geometry>
<capsule length="0.56569" radius="0.08000"/>
</geometry>
</visual>
</link>
<joint name="hip_1" type="revolute">
<parent link="torso"/>
<child link="front_left_leg"/>
<dynamics damping="1.0" friction="0.0001"/>
<origin rpy="0.00000 -0.00000 0.00000" xyz="0.20000 0.20000 0.00000"/>
<axis xyz="0.00000 0.00000 1.00000"/>
</joint>
<joint name="ankle_1" type="revolute">
<parent link="front_left_leg"/>
<child link="aux_1"/>
<dynamics damping="1.0" friction="0.0001"/>
<origin rpy="0.00000 -0.00000 0.00000" xyz="0.20000 0.20000 0.00000"/>
<axis xyz="-1.00000 1.00000 0.00000"/>
</joint>
<joint name="jointfix_0_5" type="fixed">
<parent link="aux_1"/>
<child link="front_left_foot"/>
<dynamics damping="1.0" friction="0.0001"/>
<origin rpy="0.00000 -0.00000 0.00000" xyz="0.00000 0.00000 0.00000"/>
<axis xyz="0.00000 0.00000 0.00000"/>
</joint>
<joint name="hip_2" type="revolute">
<parent link="torso"/>
<child link="front_right_leg"/>
<dynamics damping="1.0" friction="0.0001"/>
<origin rpy="0.00000 -0.00000 0.00000" xyz="-0.20000 0.20000 0.00000"/>
<axis xyz="0.00000 0.00000 1.00000"/>
</joint>
<joint name="ankle_2" type="revolute">
<parent link="front_right_leg"/>
<child link="aux_2"/>
<dynamics damping="1.0" friction="0.0001"/>
<origin rpy="0.00000 -0.00000 0.00000" xyz="-0.20000 0.20000 0.00000"/>
<axis xyz="1.00000 1.00000 0.00000"/>
</joint>
<joint name="jointfix_3_10" type="fixed">
<parent link="aux_2"/>
<child link="front_right_foot"/>
<dynamics damping="1.0" friction="0.0001"/>
<origin rpy="0.00000 -0.00000 0.00000" xyz="0.00000 0.00000 0.00000"/>
<axis xyz="0.00000 0.00000 0.00000"/>
</joint>
<joint name="hip_3" type="revolute">
<parent link="torso"/>
<child link="left_back_leg"/>
<dynamics damping="1.0" friction="0.0001"/>
<origin rpy="0.00000 -0.00000 0.00000" xyz="-0.20000 -0.20000 0.00000"/>
<axis xyz="0.00000 0.00000 1.00000"/>
</joint>
<joint name="ankle_3" type="revolute">
<parent link="left_back_leg"/>
<child link="aux_3"/>
<dynamics damping="1.0" friction="0.0001"/>
<origin rpy="0.00000 -0.00000 0.00000" xyz="-0.20000 -0.20000 0.00000"/>
<axis xyz="-1.00000 1.00000 0.00000"/>
</joint>
<joint name="jointfix_6_15" type="fixed">
<parent link="aux_3"/>
<child link="left_back_foot"/>
<dynamics damping="1.0" friction="0.0001"/>
<origin rpy="0.00000 -0.00000 0.00000" xyz="0.00000 0.00000 0.00000"/>
<axis xyz="0.00000 0.00000 0.00000"/>
</joint>
<joint name="hip_4" type="revolute">
<parent link="torso"/>
<child link="right_back_leg"/>
<dynamics damping="1.0" friction="0.0001"/>
<origin rpy="0.00000 -0.00000 0.00000" xyz="0.20000 -0.20000 0.00000"/>
<axis xyz="0.00000 0.00000 1.00000"/>
</joint>
<joint name="ankle_4" type="revolute">
<parent link="right_back_leg"/>
<child link="aux_4"/>
<dynamics damping="1.0" friction="0.0001"/>
<origin rpy="0.00000 -0.00000 0.00000" xyz="0.20000 -0.20000 0.00000"/>
<axis xyz="1.00000 1.00000 0.00000"/>
</joint>
<joint name="jointfix_9_20" type="fixed">
<parent link="aux_4"/>
<child link="right_back_foot"/>
<dynamics damping="1.0" friction="0.0001"/>
<origin rpy="0.00000 -0.00000 0.00000" xyz="0.00000 0.00000 0.00000"/>
<axis xyz="0.00000 0.00000 0.00000"/>
</joint>
</robot>
================================================
FILE: DojoEnvironments/src/mechanisms/ant/mechanism.jl
================================================
function get_ant(;
timestep=0.05,
input_scaling=timestep,
gravity=-9.81,
urdf=:ant,
springs=0,
dampers=0,
parse_springs=true,
parse_dampers=true,
joint_limits=Dict([
(:hip_1, [-30,30] * π / 180),
(:ankle_1, [30,70] * π / 180),
(:hip_2, [-30,30] * π / 180),
(:ankle_2, [-70,-30] * π / 180),
(:hip_3, [-30,30] * π / 180),
(:ankle_3, [-70,-30] * π / 180),
(:hip_4, [-30,30] * π / 180),
(:ankle_4, [30,70] * π / 180)]),
keep_fixed_joints=true,
friction_coefficient=0.5,
contact_feet=true,
contact_body=true,
T=Float64)
# mechanism
path = joinpath(@__DIR__, "dependencies/$(string(urdf)).urdf")
mechanism = Mechanism(path; floating=true, T,
gravity, timestep, input_scaling,
parse_dampers, keep_fixed_joints)
# springs and dampers
!parse_springs && set_springs!(mechanism.joints, springs)
!parse_dampers && set_dampers!(mechanism.joints, dampers)
# joint limits
joints = set_limits(mechanism, joint_limits)
mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints;
gravity, timestep, input_scaling)
# contacts
contacts = ContactConstraint{T}[]
if contact_feet
# feet contacts
body_names = [:front_left_foot, :front_right_foot, :left_back_foot, :right_back_foot]
contact_bodies = [get_body(mechanism, name) for name in body_names]
n = length(contact_bodies)
normals = fill(Z_AXIS,n)
friction_coefficients = fill(friction_coefficient,n)
contact_origins = [
[0.2; 0.2; 0],
[-0.2; 0.2; 0],
[-0.2; -0.2; 0],
[0.2; -0.2; 0]
]
contact_radii = [body.shape.shapes[1].rh[1] for body in contact_bodies]
contacts = [contacts;contact_constraint(contact_bodies, normals; friction_coefficients, contact_origins, contact_radii)]
end
if contact_body
# torso contact
torso = get_body(mechanism, :torso)
contact_radius = torso.shape.r
contacts = [contacts;contact_constraint(torso, Z_AXIS; friction_coefficient, contact_radius)]
# elbow contact
body_names = [:aux_1, :aux_2, :aux_3, :aux_4]
contact_bodies = [get_body(mechanism, name) for name in body_names]
n = length(contact_bodies)
normals = fill(Z_AXIS,n)
friction_coefficients = fill(friction_coefficient,n)
contact_origins = [
-[0.1; 0.1; 0],
-[-0.1; 0.1; 0],
-[-0.1;-0.1; 0],
-[0.1; -0.1; 0]
]
contact_radii = [body.shape.shapes[1].rh[1] for body in contact_bodies]
contacts = [contacts;contact_constraint(contact_bodies, normals; friction_coefficients, contact_origins, contact_radii)]
end
mechanism = Mechanism(mechanism.origin, mechanism.bodies, mechanism.joints, contacts;
gravity, timestep, input_scaling)
# zero configuration
initialize_ant!(mechanism)
# construction finished
return mechanism
end
function initialize_ant!(mechanism::Mechanism;
body_position=0.5*Z_AXIS, body_orientation=one(Quaternion), ankle_angle=0.25)
zero_velocities!(mechanism)
zero_coordinates!(mechanism)
set_minimal_coordinates!(mechanism, get_joint(mechanism, :floating_base), [body_position; Dojo.rotation_vector(body_orientation)])
for i in [1, 4]
set_minimal_coordinates!(mechanism, get_joint(mechanism, Symbol("hip_$i")), [0])
set_minimal_coordinates!(mechanism, get_joint(mechanism, Symbol("ankle_$i")), [ankle_angle * π])
end
for i in [2, 3]
set_minimal_coordinates!(mechanism, get_joint(mechanism, Symbol("hip_$i")), [0])
set_minimal_coordinates!(mechanism, get_joint(mechanism, Symbol("ankle_$i")), [-ankle_angle * π])
end
return
end
================================================
FILE: DojoEnvironments/src/mechanisms/atlas/dependencies/atlas.urdf
================================================
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from atlas_v5_convex_hull_open_hands.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="atlas" xmlns:xacro="http://www.ros.org/wiki/xacro">
<link name="l_clav">
<inertial>
<mass value="4.466"/>
<origin rpy="0 0 0" xyz="0 0 -0.084"/>
<inertia ixx="0.011" ixy="0" ixz="0" iyy="0.009" iyz="0.004" izz="0.004"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_clav.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_clav_chull.obj"/>
</geometry>
</collision>
</link>
<link name="l_foot">
<inertial>
<mass value="2.410"/>
<origin rpy="0 0 0" xyz="0.027 0 -0.067"/>
<inertia ixx="0.002" ixy="0" ixz="0" iyy="0.007" iyz="0" izz="0.008"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/l_foot.obj"/>
</geometry>
</visual>
<collision group="heel">
<geometry>
<mesh filename="package://Atlas/urdf/meshes/l_foot_chull.obj"/>
</geometry>
</collision>
<collision group="heel">
<origin rpy="0 0 0" xyz="-0.0876 0.066 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="heel">
<origin rpy="0 0 0" xyz="-0.0876 0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision group="heel">
<origin rpy="0 0 0" xyz="-0.0876 -0.0626 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="heel">
<origin rpy="0 0 0" xyz="-0.0876 -0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision group="toe">
<origin rpy="0 0 0" xyz="0.1728 0.066 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="toe">
<origin rpy="0 0 0" xyz="0.1728 0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision group="toe">
<origin rpy="0 0 0" xyz="0.1728 -0.0626 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="toe">
<origin rpy="0 0 0" xyz="0.1728 -0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision group="midfoot">
<origin rpy="0 0 0" xyz="-0.0426 0.066 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="midfoot">
<origin rpy="0 0 0" xyz="-0.0426 0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision group="midfoot">
<origin rpy="0 0 0" xyz="-0.0426 -0.0626 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="midfoot">
<origin rpy="0 0 0" xyz="-0.0426 -0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
</link>
<link name="l_hand">
<inertial>
<mass value="0.5839"/>
<origin rpy="0 0 0" xyz="0.00016 -0.08159 0.00002"/>
<inertia ixx="0.000388" ixy="0" ixz="0" iyy="0.000477" iyz="0" izz="0.000379"/>
</inertial>
</link>
<link name="l_larm">
<inertial>
<mass value="3.248"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.00265" ixy="0" ixz="0" iyy="0.00446" iyz="0" izz="0.00446"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_larm.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_larm_chull.obj"/>
</geometry>
</collision>
</link>
<link name="l_lfarm">
<inertial>
<mass value="0.648"/>
<origin rpy="0 0 0" xyz="0.00017 -0.02515 0.00163"/>
<inertia ixx="0.000764" ixy="0" ixz="0" iyy="0.000429" iyz="0" izz="0.000825"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_hand.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_hand_chull.obj"/>
</geometry>
</collision>
</link>
<link name="l_lglut">
<inertial>
<mass value="0.898"/>
<origin rpy="0 0 0" xyz="0.0133341 0.0170484 -0.0312052"/>
<inertia ixx="0.000691326" ixy="-2.24344e-05" ixz="2.50508e-06" iyy="0.00126856" iyz="0.000137862" izz="0.00106487"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/l_lglut.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/l_lglut_chull.obj"/>
</geometry>
</collision>
</link>
<link name="l_lleg">
<inertial>
<mass value="4.515"/>
<origin rpy="0 0 0" xyz="0.001 0 -0.187"/>
<inertia ixx="0.077" ixy="0" ixz="-0.003" iyy="0.076" iyz="0" izz="0.01"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/l_lleg.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/l_lleg_chull.obj"/>
</geometry>
</collision>
</link>
<link name="l_scap">
<inertial>
<mass value="3.899"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.00319" ixy="0" ixz="0" iyy="0.00583" iyz="0" izz="0.00583"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_scap.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_scap_chull.obj"/>
</geometry>
</collision>
</link>
<link name="l_talus">
<inertial>
<mass value="0.125"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="1.01674e-05" ixy="0" ixz="0" iyy="8.42775e-06" iyz="0" izz="1.30101e-05"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/l_talus.obj"/>
</geometry>
</visual>
</link>
<link name="l_uarm">
<inertial>
<mass value="4.386"/>
<origin rpy="0 0 0" xyz="0 0.065 0"/>
<inertia ixx="0.00656" ixy="0" ixz="0" iyy="0.00358" iyz="0" izz="0.00656"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_uarm.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_uarm_chull.obj"/>
</geometry>
</collision>
</link>
<link name="l_ufarm">
<inertial>
<mass value="2.4798"/>
<origin rpy="0 0 0" xyz="0.00015 0.08296 0.00037"/>
<inertia ixx="0.012731" ixy="0" ixz="0" iyy="0.002857" iyz="0" izz="0.011948"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_farm.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_farm_chull.obj"/>
</geometry>
</collision>
</link>
<link name="l_uglut">
<inertial>
<mass value="1.959"/>
<origin rpy="0 0 0" xyz="0.00529262 -0.00344732 0.00313046"/>
<inertia ixx="0.00074276" ixy="-3.79607e-08" ixz="-2.79549e-05" iyy="0.000688179" iyz="-3.2735e-08" izz="0.00041242"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/l_uglut.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/l_uglut_chull.obj"/>
</geometry>
</collision>
</link>
<link name="l_uleg">
<inertial>
<mass value="8.204"/>
<origin rpy="0 0 0" xyz="0 0 -0.21"/>
<inertia ixx="0.09" ixy="0" ixz="0" iyy="0.09" iyz="0" izz="0.02"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/l_uleg.obj"/>
</geometry>
</visual>
<!-- to cover the logo shield -->
<!-- inside strut -->
<!-- outside strut -->
<collision>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/l_uleg_chull.obj"/>
</geometry>
</collision>
</link>
<link name="ltorso">
<inertial>
<mass value="2.270"/>
<origin rpy="0 0 0" xyz="-0.0112984 -3.15366e-06 0.0746835"/>
<inertia ixx="0.0039092" ixy="-5.04491e-08" ixz="-0.000342157" iyy="0.00341694" iyz="4.87119e-07" izz="0.00174492"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/ltorso.obj"/>
</geometry>
</visual>
</link>
<link name="mtorso">
<inertial>
<mass value="0.799"/>
<origin rpy="0 0 0" xyz="-0.00816266 -0.0131245 0.0305974"/>
<inertia ixx="0.000454181" ixy="-6.10764e-05" ixz="3.94009e-05" iyy="0.000483282" iyz="5.27463e-05" izz="0.000444215"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/mtorso.obj"/>
</geometry>
</visual>
</link>
<link name="pelvis">
<inertial>
<mass value="9.509"/>
<origin rpy="0 0 0" xyz="0.0111 0 0.0271"/>
<inertia ixx="0.1244" ixy="0.0008" ixz="-0.0007" iyy="0.0958" iyz="-0.0005" izz="0.1167"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/pelvis.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/pelvis_chull.obj"/>
</geometry>
</collision>
</link>
<link name="r_clav">
<inertial>
<mass value="4.466"/>
<origin rpy="0 0 0" xyz="0 0 -0.084"/>
<inertia ixx="0.011" ixy="0" ixz="0" iyy="0.009" iyz="0.004" izz="0.004"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_clav.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_clav_chull.obj"/>
</geometry>
</collision>
</link>
<link name="r_foot">
<inertial>
<mass value="2.41"/>
<origin rpy="0 0 0" xyz="0.027 0 -0.067"/>
<inertia ixx="0.002" ixy="0" ixz="0" iyy="0.007" iyz="0" izz="0.008"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_foot.obj"/>
</geometry>
</visual>
<collision group="heel">
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_foot_chull.obj"/>
</geometry>
</collision>
<collision group="heel">
<origin rpy="0 0 0" xyz="-0.0876 0.0626 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="heel">
<origin rpy="0 0 0" xyz="-0.0876 0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision group="heel">
<origin rpy="0 0 0" xyz="-0.0876 -0.066 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="heel">
<origin rpy="0 0 0" xyz="-0.0876 -0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision group="toe">
<origin rpy="0 0 0" xyz="0.1728 0.0626 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="toe">
<origin rpy="0 0 0" xyz="0.1728 0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision group="toe">
<origin rpy="0 0 0" xyz="0.1728 -0.066 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="toe">
<origin rpy="0 0 0" xyz="0.1728 -0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision group="midfoot">
<origin rpy="0 0 0" xyz="0.0426 0.0626 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="midfoot">
<origin rpy="0 0 0" xyz="0.0426 0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision group="midfoot">
<origin rpy="0 0 0" xyz="0.0426 -0.066 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="midfoot">
<origin rpy="0 0 0" xyz="0.0426 -0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
</link>
<link name="r_hand">
<inertial>
<mass value="0.5839"/>
<origin rpy="0 0 0" xyz="0.00016 -0.08159 0.00002"/>
<inertia ixx="0.000388" ixy="0" ixz="0" iyy="0.000477" iyz="0" izz="0.000379"/>
</inertial>
</link>
<link name="r_larm">
<inertial>
<mass value="3.248"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.00265" ixy="0" ixz="0" iyy="0.00446" iyz="0" izz="0.00446"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_larm.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_larm_chull.obj"/>
</geometry>
</collision>
</link>
<link name="r_lfarm">
<inertial>
<mass value="0.648"/>
<origin rpy="0 0 0" xyz="0.00017 -0.02515 0.00163"/>
<inertia ixx="0.000764" ixy="0" ixz="0" iyy="0.000429" iyz="0" izz="0.000825"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_hand.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_hand_chull.obj"/>
</geometry>
</collision>
</link>
<link name="r_lglut">
<inertial>
<mass value="0.898"/>
<origin rpy="0 0 0" xyz="0.0133341 -0.0170484 -0.0312052"/>
<inertia ixx="0.000691326" ixy="2.24344e-05" ixz="2.50508e-06" iyy="0.00126856" iyz="-0.000137862" izz="0.00106487"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_lglut.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_lglut_chull.obj"/>
</geometry>
</collision>
</link>
<link name="r_lleg">
<inertial>
<mass value="4.515"/>
<origin rpy="0 0 0" xyz="0.001 0 -0.187"/>
<inertia ixx="0.077" ixy="-0" ixz="-0.003" iyy="0.076" iyz="-0" izz="0.01"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_lleg.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_lleg_chull.obj"/>
</geometry>
</collision>
</link>
<link name="r_scap">
<inertial>
<mass value="3.899"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.00319" ixy="0" ixz="0" iyy="0.00583" iyz="0" izz="0.00583"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_scap.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_scap_chull.obj"/>
</geometry>
</collision>
</link>
<link name="r_talus">
<inertial>
<mass value="0.125"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="1.01674e-05" ixy="0" ixz="0" iyy="8.42775e-06" iyz="0" izz="1.30101e-05"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_talus.obj"/>
</geometry>
</visual>
</link>
<link name="r_uarm">
<inertial>
<mass value="4.386"/>
<origin rpy="0 0 0" xyz="0 0.065 0"/>
<inertia ixx="0.00656" ixy="0" ixz="0" iyy="0.00358" iyz="0" izz="0.00656"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_uarm.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_uarm_chull.obj"/>
</geometry>
</collision>
</link>
<link name="r_ufarm">
<inertial>
<mass value="2.4798"/>
<origin rpy="0 0 0" xyz="0.00015 0.08296 0.00037"/>
<inertia ixx="0.012731" ixy="0" ixz="0" iyy="0.002857" iyz="0" izz="0.011948"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_farm.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_farm_chull.obj"/>
</geometry>
</collision>
</link>
<link name="r_uglut">
<inertial>
<mass value="1.959"/>
<origin rpy="0 0 0" xyz="0.00529262 0.00344732 0.00313046"/>
<inertia ixx="0.00074276" ixy="3.79607e-08" ixz="-2.79549e-05" iyy="0.000688179" iyz="3.2735e-08" izz="0.00041242"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_uglut.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_uglut_chull.obj"/>
</geometry>
</collision>
</link>
<link name="r_uleg">
<inertial>
<mass value="8.204"/>
<origin rpy="0 0 0" xyz="0 0 -0.21"/>
<inertia ixx="0.09" ixy="0" ixz="0" iyy="0.09" iyz="0" izz="0.02"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_uleg.obj"/>
</geometry>
</visual>
<!-- to cover the logo shield -->
<!-- inside strut -->
<!-- outside strut -->
<collision>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/r_uleg_chull.obj"/>
</geometry>
</collision>
</link>
<link name="utorso">
<inertial>
<mass value="84.409"/>
<origin rpy="0 0 0" xyz="-0.0622 0.0023 0.3157"/>
<inertia ixx="1.577" ixy="-0.032" ixz="0.102" iyy="1.602" iyz="0.047" izz="0.565"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/utorso.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/utorso_chull.obj"/>
</geometry>
</collision>
</link>
<joint name="back_bkx" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.05"/>
<axis xyz="1 0 0"/>
<parent link="mtorso"/>
<child link="utorso"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="300" lower="-0.523599" upper="0.523599" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.5236" soft_upper_limit="10.5236"/>
</joint>
<joint name="back_bky" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.162"/>
<axis xyz="0 1 0"/>
<parent link="ltorso"/>
<child link="mtorso"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="445" lower="-0.219388" upper="0.538783" velocity="9"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.2194" soft_upper_limit="10.5388"/>
</joint>
<joint name="back_bkz" type="revolute">
<origin rpy="0 0 0" xyz="-0.0125 0 0"/>
<axis xyz="0 0 1"/>
<parent link="pelvis"/>
<child link="ltorso"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="106" lower="-0.663225" upper="0.663225" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.6632" soft_upper_limit="10.6632"/>
</joint>
<joint name="l_arm_elx" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.119 0.0092"/>
<axis xyz="-1.0 -0.0 -0.0"/>
<parent link="l_uarm"/>
<child link="l_larm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="112" lower="0" upper="2.35619" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.3562"/>
</joint>
<joint name="l_arm_ely" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.187 -0.016"/>
<axis xyz="-0.0 -1.0 -0.0"/>
<parent link="l_scap"/>
<child link="l_uarm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="63" lower="0" upper="3.14159" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="13.1416"/>
</joint>
<joint name="l_arm_shx" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.11 -0.245"/>
<axis xyz="-1.0 -0.0 -0.0"/>
<parent link="l_clav"/>
<child link="l_scap"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="99" lower="-1.5708" upper="1.5708" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.5708" soft_upper_limit="11.5708"/>
</joint>
<joint name="l_arm_shz" type="revolute">
<origin rpy="0 0 3.14159265359" xyz="0.1406 0.2256 0.4776"/>
<axis xyz="0 0 1"/>
<parent link="utorso"/>
<child link="l_clav"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="87" lower="-1.5708" upper="0.785398" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.5708" soft_upper_limit="10.7854"/>
</joint>
<joint name="l_arm_mwx" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
<parent link="l_ufarm"/>
<child link="l_lfarm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="25" lower="-1.7628" upper="1.7628" velocity="10"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.7628" soft_upper_limit="11.7628"/>
</joint>
<joint name="l_arm_uwy" type="revolute">
<origin rpy="0 3.14159265359 0" xyz="0 -0.29955 -0.00921"/>
<axis xyz="-0.0 -1.0 -0.0"/>
<parent link="l_larm"/>
<child link="l_ufarm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="25" lower="-3.011" upper="3.011" velocity="10"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-13.011" soft_upper_limit="13.011"/>
</joint>
<joint name="l_arm_lwy" type="revolute">
<origin rpy="0 3.14159265359 0" xyz="0 0 0"/>
<axis xyz="-0.0 -1.0 -0.0"/>
<parent link="l_lfarm"/>
<child link="l_hand"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="25" lower="-2.9671" upper="2.9671" velocity="10"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-12.9671" soft_upper_limit="12.9671"/>
</joint>
<joint name="l_leg_akx" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
<parent link="l_talus"/>
<child link="l_foot"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="360" lower="-0.8" upper="0.8" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.8" soft_upper_limit="10.8"/>
</joint>
<joint name="l_leg_aky" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.422"/>
<axis xyz="0 1 0"/>
<parent link="l_lleg"/>
<child link="l_talus"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="740" lower="-1" upper="0.7" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11" soft_upper_limit="10.7"/>
</joint>
<joint name="l_leg_hpx" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
<parent link="l_uglut"/>
<child link="l_lglut"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="530" lower="-0.523599" upper="0.523599" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.5236" soft_upper_limit="10.5236"/>
</joint>
<joint name="l_leg_hpy" type="revolute">
<origin rpy="0 0 0" xyz="0.05 0.0225 -0.066"/>
<axis xyz="0 1 0"/>
<parent link="l_lglut"/>
<child link="l_uleg"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="840" lower="-1.61234" upper="0.65764" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.6123" soft_upper_limit="10.6576"/>
</joint>
<joint name="l_leg_hpz" type="revolute">
<origin rpy="0 0 0" xyz="0 0.089 0"/>
<axis xyz="0 0 1"/>
<parent link="pelvis"/>
<child link="l_uglut"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="275" lower="-0.174358" upper="0.786794" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.1744" soft_upper_limit="10.7868"/>
</joint>
<joint name="l_leg_kny" type="revolute">
<origin rpy="0 0 0" xyz="-0.05 0 -0.374"/>
<axis xyz="0 1 0"/>
<parent link="l_uleg"/>
<child link="l_lleg"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="890" lower="0" upper="2.35637" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.3564"/>
</joint>
<joint name="neck_ay" type="revolute">
<origin rpy="0 0 0" xyz="0.2546 0 0.6215"/>
<axis xyz="0 1 0"/>
<parent link="utorso"/>
<child link="head"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="25" lower="-0.602139" upper="1.14319" velocity="6.28"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.6021" soft_upper_limit="11.1432"/>
</joint>
<joint name="r_arm_elx" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.119 0.0092"/>
<axis xyz="1 0 0"/>
<parent link="r_uarm"/>
<child link="r_larm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="112" lower="-2.35619" upper="0" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-12.3562" soft_upper_limit="10"/>
</joint>
<joint name="r_arm_ely" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.187 -0.016"/>
<axis xyz="0 1 0"/>
<parent link="r_scap"/>
<child link="r_uarm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="63" lower="0" upper="3.14159" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="13.1416"/>
</joint>
<joint name="r_arm_shx" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.11 -0.245"/>
<axis xyz="1 0 0"/>
<parent link="r_clav"/>
<child link="r_scap"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="99" lower="-1.5708" upper="1.5708" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.5708" soft_upper_limit="11.5708"/>
</joint>
<joint name="r_arm_shz" type="revolute">
<origin rpy="0 0 0" xyz="0.1406 -0.2256 0.4776"/>
<axis xyz="0 0 1"/>
<parent link="utorso"/>
<child link="r_clav"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="87" lower="-0.785398" upper="1.5708" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.7854" soft_upper_limit="11.5708"/>
</joint>
<joint name="r_arm_mwx" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
<parent link="r_ufarm"/>
<child link="r_lfarm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="25" lower="-1.7628" upper="1.7628" velocity="10"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.7628" soft_upper_limit="11.7628"/>
</joint>
<joint name="r_arm_uwy" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.29955 -0.00921"/>
<axis xyz="0 1 0"/>
<parent link="r_larm"/>
<child link="r_ufarm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="25" lower="-3.011" upper="3.011" velocity="10"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-13.011" soft_upper_limit="13.011"/>
</joint>
<joint name="r_arm_lwy" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 1 0"/>
<parent link="r_lfarm"/>
<child link="r_hand"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="25" lower="-2.9671" upper="2.9671" velocity="10"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-12.9671" soft_upper_limit="12.9671"/>
</joint>
<joint name="r_leg_akx" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
<parent link="r_talus"/>
<child link="r_foot"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="360" lower="-0.8" upper="0.8" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.8" soft_upper_limit="10.8"/>
</joint>
<joint name="r_leg_aky" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.422"/>
<axis xyz="0 1 0"/>
<parent link="r_lleg"/>
<child link="r_talus"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="740" lower="-1" upper="0.7" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11" soft_upper_limit="10.7"/>
</joint>
<joint name="r_leg_hpx" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
<parent link="r_uglut"/>
<child link="r_lglut"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="530" lower="-0.523599" upper="0.523599" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.5236" soft_upper_limit="10.5236"/>
</joint>
<joint name="r_leg_hpy" type="revolute">
<origin rpy="0 0 0" xyz="0.05 -0.0225 -0.066"/>
<axis xyz="0 1 0"/>
<parent link="r_lglut"/>
<child link="r_uleg"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="840" lower="-1.61234" upper="0.65764" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.6123" soft_upper_limit="10.6576"/>
</joint>
<joint name="r_leg_hpz" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.089 0"/>
<axis xyz="0 0 1"/>
<parent link="pelvis"/>
<child link="r_uglut"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="275" lower="-0.786794" upper="0.174358" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.7868" soft_upper_limit="10.1744"/>
</joint>
<joint name="r_leg_kny" type="revolute">
<origin rpy="0 0 0" xyz="-0.05 0 -0.374"/>
<axis xyz="0 1 0"/>
<parent link="r_uleg"/>
<child link="r_lleg"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="890" lower="0" upper="2.35637" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.3564"/>
</joint>
<link name="r_situational_awareness_camera_link">
<inertial>
<mass value="0.1"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</collision>
</link>
<joint name="r_situational_awareness_camera_joint" type="fixed">
<parent link="utorso"/>
<child link="r_situational_awareness_camera_link"/>
<!-- SA camera mounted above the multisense head -->
<origin rpy="0 0 -1.309" xyz="0.155 -0.121 0.785"/>
</joint>
<joint name="r_situational_awareness_camera_optical_frame_joint" type="fixed">
<origin rpy="-1.5708 0.0 -1.5708" xyz="0 0 0"/>
<parent link="r_situational_awareness_camera_link"/>
<child link="r_situational_awareness_camera_optical_frame"/>
</joint>
<link name="r_situational_awareness_camera_optical_frame"/>
<link name="l_situational_awareness_camera_link">
<inertial>
<mass value="0.1"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</collision>
</link>
<joint name="l_situational_awareness_camera_joint" type="fixed">
<parent link="utorso"/>
<child link="l_situational_awareness_camera_link"/>
<!-- SA camera mounted above the multisense head -->
<origin rpy="0 0 1.309" xyz="0.155 0.121 0.785"/>
</joint>
<joint name="l_situational_awareness_camera_optical_frame_joint" type="fixed">
<origin rpy="-1.5708 0.0 -1.5708" xyz="0 0 0"/>
<parent link="l_situational_awareness_camera_link"/>
<child link="l_situational_awareness_camera_optical_frame"/>
</joint>
<link name="l_situational_awareness_camera_optical_frame"/>
<frame link="l_foot" name="l_foot_sole" rpy="0 0 0" xyz="0.0426 0.0017 -0.07645"/>
<frame link="r_foot" name="r_foot_sole" rpy="0 0 0" xyz="0.0426 -0.0017 -0.07645"/>
<frame link="l_foot" name="l_foot_toe" rpy="0 0 0" xyz="0.1728 0.0017 -0.07645"/>
<frame link="r_foot" name="r_foot_toe" rpy="0 0 0" xyz="0.1728 -0.0017 -0.07645"/>
<collision_filter_group name="feet">
<member link="l_foot"/>
<member link="r_foot"/>
<ignored_collision_filter_group collision_filter_group="feet"/>
</collision_filter_group>
<collision_filter_group name="core">
<member link="utorso"/>
<member link="pelvis"/>
<ignored_collision_filter_group collision_filter_group="core"/>
</collision_filter_group>
<collision_filter_group name="ignore_core">
<member link="r_scap"/>
<member link="l_scap"/>
<member link="r_clav"/>
<member link="l_clav"/>
<ignored_collision_filter_group collision_filter_group="core"/>
</collision_filter_group>
<collision_filter_group name="r_uleg">
<member link="r_uglut"/>
<member link="r_lglut"/>
<member link="r_uleg"/>
<ignored_collision_filter_group collision_filter_group="core"/>
<ignored_collision_filter_group collision_filter_group="r_uleg"/>
<ignored_collision_filter_group collision_filter_group="l_uleg"/>
</collision_filter_group>
<collision_filter_group name="l_uleg">
<member link="l_uglut"/>
<member link="l_lglut"/>
<member link="l_uleg"/>
<ignored_collision_filter_group collision_filter_group="core"/>
<ignored_collision_filter_group collision_filter_group="l_uleg"/>
<ignored_collision_filter_group collision_filter_group="l_uleg"/>
</collision_filter_group>
<collision_filter_group name="r_leg">
<member link="r_lleg"/>
<member link="r_talus"/>
<member link="r_foot"/>
<ignored_collision_filter_group collision_filter_group="r_leg"/>
<ignored_collision_filter_group collision_filter_group="r_uleg"/>
</collision_filter_group>
<collision_filter_group name="l_leg">
<member link="l_lleg"/>
<member link="l_talus"/>
<member link="l_foot"/>
<ignored_collision_filter_group collision_filter_group="l_leg"/>
<ignored_collision_filter_group collision_filter_group="l_uleg"/>
</collision_filter_group>
<collision_filter_group name="r_arm">
<member link="r_clav"/>
<member link="r_scap"/>
<member link="r_uarm"/>
<member link="r_larm"/>
<member link="r_ufarm"/>
<member link="r_lfarm"/>
<member link="r_hand"/>
<ignored_collision_filter_group collision_filter_group="r_arm"/>
</collision_filter_group>
<collision_filter_group name="l_arm">
<member link="l_clav"/>
<member link="l_scap"/>
<member link="l_uarm"/>
<member link="l_larm"/>
<member link="l_ufarm"/>
<member link="l_lfarm"/>
<member link="l_hand"/>
<ignored_collision_filter_group collision_filter_group="l_arm"/>
</collision_filter_group>
<link name="head">
<inertial>
<origin rpy="0 0 0" xyz="-0.075493 3.3383E-05 0.02774"/>
<mass value="1.4199"/>
<inertia ixx="0.0039688" ixy="-1.5797E-06" ixz="-0.00089293" iyy="0.0041178" iyz="-6.8415E-07" izz="0.0035243"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/head_chull.obj"/>
</geometry>
<material name="">
<color rgba="0.9098 0.44314 0.031373 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/head_chull.obj"/>
</geometry>
<material name="">
<color rgba="0.9098 0.44314 0.031373 1"/>
</material>
</collision>
</link>
<link name="pre_spindle"/>
<joint name="pre_spindle_joint" type="fixed">
<axis xyz="0 1 0"/>
<origin rpy="0 0 0" xyz="-0.0446 0 0.0880"/>
<parent link="head"/>
<child link="pre_spindle"/>
</joint>
<link name="pre_spindle_cal_x"/>
<link name="pre_spindle_cal_y"/>
<link name="pre_spindle_cal_z"/>
<link name="pre_spindle_cal_roll"/>
<link name="pre_spindle_cal_pitch"/>
<link name="pre_spindle_cal_yaw"/>
<joint name="pre_spindle_cal_x_joint" type="fixed">
<parent link="pre_spindle"/>
<child link="pre_spindle_cal_x"/>
<axis xyz="1 0 0"/>
<limit effort="0.0" lower="-0.5" upper="0.5" velocity="0.0"/>
</joint>
<joint name="pre_spindle_cal_y_joint" type="fixed">
<parent link="pre_spindle_cal_x"/>
<child link="pre_spindle_cal_y"/>
<axis xyz="0 1 0"/>
<limit effort="0.0" lower="-0.5" upper="0.5" velocity="0.0"/>
</joint>
<joint name="pre_spindle_cal_z_joint" type="fixed">
<parent link="pre_spindle_cal_y"/>
<child link="pre_spindle_cal_z"/>
<axis xyz="0 0 1"/>
<limit effort="0.0" lower="-0.5" upper="0.5" velocity="0.0"/>
</joint>
<joint name="pre_spindle_cal_roll_joint" type="fixed">
<parent link="pre_spindle_cal_z"/>
<child link="pre_spindle_cal_roll"/>
<axis xyz="1 0 0"/>
</joint>
<joint name="pre_spindle_cal_pitch_joint" type="fixed">
<parent link="pre_spindle_cal_roll"/>
<child link="pre_spindle_cal_pitch"/>
<axis xyz="0 1 0"/>
</joint>
<joint name="pre_spindle_cal_yaw_joint" type="fixed">
<parent link="pre_spindle_cal_pitch"/>
<child link="pre_spindle_cal_yaw"/>
<axis xyz="0 0 1"/>
</joint>
<joint name="hokuyo_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="pre_spindle_cal_yaw"/>
<child link="post_spindle"/>
<axis xyz="1 0 0"/>
</joint>
<link name="post_spindle"/>
<link name="post_spindle_cal_x"/>
<link name="post_spindle_cal_y"/>
<link name="post_spindle_cal_z"/>
<link name="post_spindle_cal_roll"/>
<link name="post_spindle_cal_pitch"/>
<joint name="post_spindle_cal_x_joint" type="fixed">
<parent link="post_spindle"/>
<child link="post_spindle_cal_x"/>
<axis xyz="1 0 0"/>
<limit effort="0.0" lower="-0.5" upper="0.5" velocity="0.0"/>
</joint>
<joint name="post_spindle_cal_y_joint" type="fixed">
<parent link="post_spindle_cal_x"/>
<child link="post_spindle_cal_y"/>
<axis xyz="0 1 0"/>
<limit effort="0.0" lower="-0.5" upper="0.5" velocity="0.0"/>
</joint>
<joint name="post_spindle_cal_z_joint" type="fixed">
<parent link="post_spindle_cal_y"/>
<child link="post_spindle_cal_z"/>
<axis xyz="0 0 1"/>
<limit effort="0.0" lower="-0.5" upper="0.5" velocity="0.0"/>
</joint>
<joint name="post_spindle_cal_roll_joint" type="fixed">
<parent link="post_spindle_cal_z"/>
<child link="post_spindle_cal_roll"/>
<axis xyz="1 0 0"/>
</joint>
<joint name="post_spindle_cal_pitch_joint" type="fixed">
<parent link="post_spindle_cal_roll"/>
<child link="post_spindle_cal_pitch"/>
<axis xyz="0 1 0"/>
</joint>
<joint name="post_spindle_cal_yaw_joint" type="fixed">
<parent link="post_spindle_cal_pitch"/>
<child link="hokuyo_link"/>
<axis xyz="0 0 1"/>
</joint>
<link name="hokuyo_link">
<inertial>
<origin rpy="0 0 0" xyz="0.012428 0.0004084 -0.0041835"/>
<mass value="0.057664"/>
<inertia ixx="3.7174E-05" ixy="4.9927E-08" ixz="1.1015E-05" iyy="4.2412E-05" iyz="-9.8165E-09" izz="4.167E-05"/>
</inertial>
<visual>
<origin rpy="-0.314 0 0" xyz="0.045 -0.0261018277 -0.08342369"/>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/head_camera_chull.obj"/>
</geometry>
<material name="">
<color rgba="0.72941 0.35686 0.023529 1"/>
</material>
</visual>
<collision>
<origin rpy="-0.314 0 0" xyz="0.045 -0.0261018277 -0.08342369"/>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/head_camera_chull.obj"/>
</geometry>
<material name="">
<color rgba="0.72941 0.35686 0.023529 1"/>
</material>
</collision>
</link>
<joint name="head_hokuyo_fixed_joint" type="fixed">
<axis xyz="0 1 0"/>
<origin rpy="0 0 0" xyz="0.03 0 0.015"/>
<parent link="hokuyo_link"/>
<child link="head_hokuyo_frame"/>
</joint>
<link name="head_hokuyo_frame">
<inertial>
<mass value="0.01"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
</inertial>
</link>
<gazebo reference="head_hokuyo_frame">
<sensor name="head_hokuyo_sensor" type="ray">
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>40</update_rate>
<ray>
<scan>
<horizontal>
<samples>720</samples>
<resolution>1</resolution>
<min_angle>-1.570796</min_angle>
<max_angle>1.570796</max_angle>
</horizontal>
</scan>
<range>
<min>0.10</min>
<max>30.0</max>
<resolution>0.01</resolution>
</range>
</ray>
<plugin filename="libgazebo_ros_laser.so" name="gazebo_ros_head_hokuyo_controller">
<gaussianNoise>0.005</gaussianNoise>
<alwaysOn>true</alwaysOn>
<updateRate>40</updateRate>
<topicName>/scan</topicName>
<frameName>head_hokuyo_frame</frameName>
</plugin>
</sensor>
</gazebo>
<joint name="left_camera_frame_joint" type="fixed">
<!-- optical frame collocated with tilting DOF -->
<origin xyz="0.0 0.035 -0.002"/>
<parent link="head"/>
<child link="left_camera_frame"/>
</joint>
<link name="left_camera_frame">
<inertial>
<mass value="0.1"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
</link>
<joint name="left_camera_optical_frame_joint" type="fixed">
<origin rpy="-1.5708 0.0 -1.5708" xyz="0 0 0"/>
<parent link="left_camera_frame"/>
<child link="left_camera_optical_frame"/>
</joint>
<link name="left_camera_optical_frame"/>
<gazebo reference="left_camera_frame">
<sensor name="left_camera_sensor" type="camera">
<!-- update rate based on spec sheet for 2mp 15fps -->
<update_rate>15.0</update_rate>
<camera>
<!-- hfov based on spec sheet, 80 degrees X 45 degrees -->
<!-- for resolution of 2048 x 1088, we have non-unity aspect ratio -->
<!-- but gazebo will violate the vertical fov for now to keep aspect ratio of 1 -->
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>2048</width>
<height>1088</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>100</far>
</clip>
</camera>
<plugin filename="libgazebo_ros_camera.so" name="left_camera_controller">
<alwaysOn>true</alwaysOn>
<!-- update rate based on spec sheet for 2mp 15fps -->
<updateRate>15.0</updateRate>
<cameraName>multisense/left</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>left_camera_optical_frame</frameName>
<hackBaseline>0.0</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
<joint name="right_camera_frame_joint" type="fixed">
<origin xyz="0.0 -0.035 -0.002"/>
<parent link="head"/>
<child link="right_camera_frame"/>
</joint>
<link name="right_camera_frame">
<inertial>
<mass value="0.1"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
</link>
<joint name="right_camera_optical_frame_joint" type="fixed">
<origin rpy="-1.5708 0.0 -1.5708" xyz="0 0 0"/>
<parent link="right_camera_frame"/>
<child link="right_camera_optical_frame"/>
</joint>
<link name="right_camera_optical_frame"/>
<gazebo reference="right_camera_frame">
<sensor name="right_camera_sensor" type="camera">
<!-- update rate based on spec sheet for 2mp 15fps -->
<update_rate>15.0</update_rate>
<camera>
<!-- hfov based on spec sheet, 80 degrees X 45 degrees -->
<!-- for resolution of 2048 x 1088, we have non-unity aspect ratio -->
<!-- but gazebo will violate the vertical fov for now to keep aspect ratio of 1 -->
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>2048</width>
<height>1088</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>100</far>
</clip>
</camera>
<plugin filename="libgazebo_ros_camera.so" name="right_camera_controller">
<alwaysOn>true</alwaysOn>
<!-- update rate based on spec sheet for 2mp 15fps -->
<updateRate>15.0</updateRate>
<cameraName>multisense/right</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>left_camera_optical_frame</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
<joint name="center_top_led_frame_joint" type="fixed">
<origin xyz="0.01125 0.0 0.0105"/>
<parent link="head"/>
<child link="center_top_led_frame"/>
</joint>
<link name="center_top_led_frame">
<inertial>
<mass value="0.1"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
</link>
<joint name="center_bottom_led_frame_joint" type="fixed">
<origin xyz="0.01125 0.0 -0.0155"/>
<parent link="head"/>
<child link="center_bottom_led_frame"/>
</joint>
<link name="center_bottom_led_frame">
<inertial>
<mass value="0.1"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
</link>
<joint name="left_led_frame_joint" type="fixed">
<origin rpy="0 -0.15 0.53" xyz="-0.01443 0.07452 0.050346"/>
<parent link="head"/>
<child link="left_led_frame"/>
</joint>
<link name="left_led_frame">
<inertial>
<mass value="0.1"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
</link>
<joint name="right_led_frame_joint" type="fixed">
<origin rpy="0 -0.15 -0.53" xyz="-0.01443 -0.07452 0.050346"/>
<parent link="head"/>
<child link="right_led_frame"/>
</joint>
<link name="right_led_frame">
<inertial>
<mass value="0.1"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
</link>
<gazebo reference="r_foot">
<!-- contact sensor -->
<sensor name="r_foot_contact_sensor" type="contact">
<update_rate>1000.0</update_rate>
<always_on>1</always_on>
<contact>
<topic>/r_foot_contact</topic>
</contact>
</sensor>
</gazebo>
<gazebo reference="l_foot">
<!-- contact sensor -->
<sensor name="l_foot_contact_sensor" type="contact">
<update_rate>1000.0</update_rate>
<always_on>1</always_on>
<contact>
<topic>/l_foot_contact</topic>
</contact>
</sensor>
</gazebo>
<transmission name="head_hokuyo_trans" type="pr2_mechanism_model/SimpleTransmission">
<joint name="hokuyo_joint"/>
<actuator name="hokuyo_motor"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="back_bkz_trans" type="pr2_mechanism_model/SimpleTransmission">
<joint name="back_bkz"/>
<actuator name="back_bkz_motor"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="back_bky_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="back_bky_motor"/>
<joint name="back_bky"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="back_bkx_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="back_bkx_motor"/>
<joint name="back_bkx"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="l_arm_elx_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="l_arm_elx_motor"/>
<joint name="l_arm_elx"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="l_arm_ely_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="l_arm_ely_motor"/>
<joint name="l_arm_ely"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="l_arm_mwx_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="l_arm_mwx_motor"/>
<joint name="l_arm_mwx"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="l_arm_shx_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="l_arm_shx_motor"/>
<joint name="l_arm_shx"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="l_arm_shz_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="l_arm_shz_motor"/>
<joint name="l_arm_shz"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="l_arm_uwy_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="l_arm_uwy_motor"/>
<joint name="l_arm_uwy"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="l_arm_lwy_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="l_arm_lwy_motor"/>
<joint name="l_arm_lwy"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="l_leg_kny_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="l_leg_kny_motor"/>
<joint name="l_leg_kny"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="l_leg_akx_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="l_leg_akx_motor"/>
<joint name="l_leg_akx"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="l_leg_hpy_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="l_leg_hpy_motor"/>
<joint name="l_leg_hpy"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="l_leg_hpx_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="l_leg_hpx_motor"/>
<joint name="l_leg_hpx"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="l_leg_aky_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="l_leg_aky_motor"/>
<joint name="l_leg_aky"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="l_leg_hpz_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="l_leg_hpz_motor"/>
<joint name="l_leg_hpz"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="neck_ay_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="neck_ay_motor"/>
<joint name="neck_ay"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="r_arm_elx_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="r_arm_elx_motor"/>
<joint name="r_arm_elx"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="r_arm_ely_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="r_arm_ely_motor"/>
<joint name="r_arm_ely"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="r_arm_mwx_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="r_arm_mwx_motor"/>
<joint name="r_arm_mwx"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="r_arm_shx_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="r_arm_shx_motor"/>
<joint name="r_arm_shx"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="r_arm_shz_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="r_arm_shz_motor"/>
<joint name="r_arm_shz"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="r_arm_uwy_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="r_arm_uwy_motor"/>
<joint name="r_arm_uwy"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="r_arm_lwy_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="r_arm_lwy_motor"/>
<joint name="r_arm_lwy"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="r_leg_kny_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="r_leg_kny_motor"/>
<joint name="r_leg_kny"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="r_leg_akx_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="r_leg_akx_motor"/>
<joint name="r_leg_akx"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="r_leg_hpy_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="r_leg_hpy_motor"/>
<joint name="r_leg_hpy"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="r_leg_hpx_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="r_leg_hpx_motor"/>
<joint name="r_leg_hpx"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="r_leg_aky_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="r_leg_aky_motor"/>
<joint name="r_leg_aky"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<transmission name="r_leg_hpz_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="r_leg_hpz_motor"/>
<joint name="r_leg_hpz"/>
<mechanicalReduction>1</mechanicalReduction>
</transmission>
<link name="l_hand_force_torque">
<inertial>
<mass value="0.1"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</visual>
</link>
<joint name="l_hand_force_torque_joint" type="fixed">
<origin rpy="0 0 3.141592" xyz="0 -0.1245 0"/>
<parent link="l_hand"/>
<child link="l_hand_force_torque"/>
</joint>
<link name="r_hand_force_torque">
<inertial>
<mass value="0.1"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</visual>
</link>
<joint name="r_hand_force_torque_joint" type="fixed">
<origin rpy="0 0 3.141592" xyz="0 -0.1245 0"/>
<parent link="r_hand"/>
<child link="r_hand_force_torque"/>
</joint>
<!-- Set the initial hand position and orientation by changing 'rpy' and 'xyz'
to mach the values in your actual robot. -->
<link name="left_palm">
<visual>
<origin rpy="-1.57079 -1.57079 0" xyz="0 0.1725 0"/>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/GRIPPER_OPEN_chull.obj" scale="1 1 1"/>
</geometry>
</visual>
<collision group="left_hand_robotiq">
<origin rpy="-1.57079 -1.57079 0" xyz="0 0.1725 0"/>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/GRIPPER_OPEN_chull.obj" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="left_palm_joint" type="fixed">
<parent link="l_hand_force_torque"/>
<child link="left_palm"/>
</joint>
<link name="right_palm">
<visual>
<origin rpy="-1.57079 -1.57079 0" xyz="0 0.1725 0"/>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/GRIPPER_OPEN_chull.obj" scale="1 1 1"/>
</geometry>
</visual>
<collision group="right_hand_robotiq">
<origin rpy="-1.57079 -1.57079 0" xyz="0 0.1725 0"/>
<geometry>
<mesh filename="package://Atlas/urdf/meshes/GRIPPER_OPEN_chull.obj" scale="1 1 1"/>
</geometry>
</collision>
</link>
<joint name="right_palm_joint" type="fixed">
<parent link="r_hand_force_torque"/>
<child link="right_palm"/>
</joint>
<joint name="l_hand_face_joint" type="fixed">
<origin rpy="0.0 0.0 0.0" xyz="0 0.12 0"/>
<parent link="l_hand_force_torque"/>
<child link="l_hand_face"/>
</joint>
<link name="l_hand_face"/>
<joint name="l_hand_gripper_joint" type="fixed">
<origin rpy="0.0 0.0 0.0" xyz="0 0.18 0"/>
<parent link="l_hand_force_torque"/>
<child link="l_hand_gripper"/>
</joint>
<link name="l_hand_gripper"/>
<link name="l_hand_camera_link"/>
<joint name="l_hand_camera_joint" type="fixed">
<parent link="l_hand_force_torque"/>
<child link="l_hand_camera_link"/>
<!-- new camera has no inward angle, placeholder is still here in case it is later tilted-->
<origin rpy=" -1.57079 0 1.5708" xyz="-0.09892 0.01431 0.0"/>
</joint>
<joint name="l_hand_camera_optical_frame_joint" type="fixed">
<origin rpy="-1.57079 0.0 -1.57079" xyz="0 0 0"/>
<parent link="l_hand_camera_link"/>
<child link="l_hand_camera_optical_frame"/>
</joint>
<link name="l_hand_camera_optical_frame"/>
<gazebo reference="l_hand_camera_link">
<sensor name="l_hand_camera_sensor" type="camera">
<update_rate>10.0</update_rate>
<camera name="l_hand_camera">
<pose>0 0 0 0 0 0</pose>
<!-- m.antone estimate of 2.8mm tamron lense fov -->
<horizontal_fov>1.7281</horizontal_fov>
<image>
<width>1280</width>
<height>1024</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>100</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame.
That pixel's noise value is added to each of its color
channels, which at that point lie in the range [0,1].
The stddev value of 0.007 is based on experimental data
from a camera in a Sandia hand pointed at a static scene
in a couple of different lighting conditions. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin filename="libgazebo_ros_camera.so" name="l_hand_camera_controller">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>robotiq_hands/l_hand_camera</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>l_hand_camera_optical_frame</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
<joint name="r_hand_face_joint" type="fixed">
<origin rpy="0.0 0.0 0.0" xyz="0 0.12 0"/>
<parent link="r_hand_force_torque"/>
<child link="r_hand_face"/>
</joint>
<link name="r_hand_face"/>
<joint name="r_hand_gripper_joint" type="fixed">
<origin rpy="0.0 0.0 0.0" xyz="0 0.18 0"/>
<parent link="r_hand_force_torque"/>
<child link="r_hand_gripper"/>
</joint>
<link name="r_hand_gripper"/>
<link name="r_hand_camera_link"/>
<joint name="r_hand_camera_joint" type="fixed">
<parent link="r_hand_force_torque"/>
<child link="r_hand_camera_link"/>
<!-- new camera has no inward angle, placeholder is still here in case it is later tilted-->
<origin rpy=" 1.57079 0 1.5708" xyz="0.09892 0.01431 0.0"/>
</joint>
<joint name="r_hand_camera_optical_frame_joint" type="fixed">
<origin rpy="-1.57079 0.0 -1.57079" xyz="0 0 0"/>
<parent link="r_hand_camera_link"/>
<child link="r_hand_camera_optical_frame"/>
</joint>
<link name="r_hand_camera_optical_frame"/>
<gazebo reference="r_hand_camera_link">
<sensor name="r_hand_camera_sensor" type="camera">
<update_rate>10.0</update_rate>
<camera name="r_hand_camera">
<pose>0 0 0 0 0 0</pose>
<!-- m.antone estimate of 2.8mm tamron lense fov -->
<horizontal_fov>1.7281</horizontal_fov>
<image>
<width>1280</width>
<height>1024</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>100</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame.
That pixel's noise value is added to each of its color
channels, which at that point lie in the range [0,1].
The stddev value of 0.007 is based on experimental data
from a camera in a Sandia hand pointed at a static scene
in a couple of different lighting conditions. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin filename="libgazebo_ros_camera.so" name="r_hand_camera_controller">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>robotiq_hands/r_hand_camera</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>r_hand_camera_optical_frame</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
</robot>
================================================
FILE: DojoEnvironments/src/mechanisms/atlas/dependencies/atlas_armless.urdf
================================================
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from atlas_v5_convex_hull_open_hands.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="atlas" xmlns:xacro="http://www.ros.org/wiki/xacro">
<material name="dark_gray">
<color rgba="0.35 0.35 0.40 1"/>
</material>
<material name="white">
<color rgba="0.95 0.95 0.95 1.0"/>
</material>
<material name="orange">
<color rgba="0.9098 0.44314 0.031373 1"/>
</material>
<material name="metal">
<color rgba="0.8784 0.8745 0.8588 1"/>
</material>
<link name="base">
<inertial>
<origin rpy="0.00000 -0.00000 0.00000" xyz="0.00000 0.00000 0.00000"/>
<mass value="1.00000"/>
<inertia ixx="1.00000" ixy="0" ixz="0" iyy="1.00000" iyz="0" izz="1.00000"/>
</inertial>
</link>
<link name="utorso">
<inertial>
<mass value="84.409"/>
<!-- <origin rpy="0 0 0" xyz="-0.0622 0.0023 0.3157"/> -->
<origin rpy="0 0 0" xyz="0.0000 0.0000 0.0000"/>
<inertia ixx="1.577" ixy="-0.032" ixz="0.102" iyy="1.602" iyz="0.047" izz="0.565"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/utorso.obj"/>
</geometry>
<material name="white"/>
</visual>
</link>
<link name="pelvis">
<inertial>
<mass value="9.509"/>
<origin rpy="0 0 0" xyz="0.0111 0 0.0271"/>
<inertia ixx="0.1244" ixy="0.0008" ixz="-0.0007" iyy="0.0958" iyz="-0.0005" izz="0.1167"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/pelvis.obj"/>
</geometry>
<material name="metal"/>
</visual>
</link>
<!-- <link name="head">
<inertial>
<origin rpy="0 0 0" xyz="-0.075493 3.3383E-05 0.02774"/>
<mass value="1.4199"/>
<inertia ixx="0.0039688" ixy="-1.5797E-06" ixz="-0.00089293" iyy="0.0041178" iyz="-6.8415E-07" izz="0.0035243"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="mesh/head_chull.obj"/>
</geometry>
<material name="dark_gray"/>
</visual>
</link> -->
<link name="l_uleg">
<inertial>
<mass value="8.204"/>
<origin rpy="0 0 0" xyz="0 0 -0.21"/>
<inertia ixx="0.09" ixy="0" ixz="0" iyy="0.09" iyz="0" izz="0.02"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/l_uleg.obj"/>
</geometry>
<material name="white"/>
</visual>
</link>
<link name="r_uleg">
<inertial>
<mass value="8.204"/>
<origin rpy="0 0 0" xyz="0 0 -0.21"/>
<inertia ixx="0.09" ixy="0" ixz="0" iyy="0.09" iyz="0" izz="0.02"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_uleg.obj"/>
</geometry>
<material name="white"/>
</visual>
</link>
<link name="l_lleg">
<inertial>
<mass value="4.515"/>
<origin rpy="0 0 0" xyz="0.001 0 -0.187"/>
<inertia ixx="0.077" ixy="0" ixz="-0.003" iyy="0.076" iyz="0" izz="0.01"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/l_lleg.obj"/>
</geometry>
<material name="dark_gray"/>
</visual>
</link>
<link name="r_lleg">
<inertial>
<mass value="4.515"/>
<origin rpy="0 0 0" xyz="0.001 0 -0.187"/>
<inertia ixx="0.077" ixy="-0" ixz="-0.003" iyy="0.076" iyz="-0" izz="0.01"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_lleg.obj"/>
</geometry>
<material name="dark_gray"/>
</visual>
</link>
<link name="l_foot">
<inertial>
<mass value="2.410"/>
<origin rpy="0 0 0" xyz="0.027 0 -0.067"/>
<inertia ixx="0.002" ixy="0" ixz="0" iyy="0.007" iyz="0" izz="0.008"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/l_foot.obj"/>
</geometry>
<material name="dark_gray"/>
</visual>
<visual group="heel">
<origin rpy="0 0 0" xyz="-0.0876 0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<visual group="heel">
<origin rpy="0 0 0" xyz="-0.0876 -0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<visual group="toe">
<origin rpy="0 0 0" xyz="0.1728 0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<visual group="toe">
<origin rpy="0 0 0" xyz="0.1728 -0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<visual group="midfoot">
<origin rpy="0 0 0" xyz="-0.0426 0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<visual group="midfoot">
<origin rpy="0 0 0" xyz="-0.0426 -0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
</link>
<link name="r_foot">
<inertial>
<mass value="2.41"/>
<origin rpy="0 0 0" xyz="0.027 0 -0.067"/>
<inertia ixx="0.002" ixy="0" ixz="0" iyy="0.007" iyz="0" izz="0.008"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_foot.obj"/>
</geometry>
<material name="dark_gray"/>
</visual>
<visual group="heel">
<origin rpy="0 0 0" xyz="-0.0876 0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<visual group="heel">
<origin rpy="0 0 0" xyz="-0.0876 -0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<visual group="toe">
<origin rpy="0 0 0" xyz="0.1728 0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<visual group="toe">
<origin rpy="0 0 0" xyz="0.1728 -0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<visual group="midfoot">
<origin rpy="0 0 0" xyz="0.0426 0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<visual group="midfoot">
<origin rpy="0 0 0" xyz="0.0426 -0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
</link>
<joint name="floating_base" type="floating">
<parent link="base"/>
<child link="utorso"/>
<origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.00000"/>
<axis xyz="0.00000 1.00000 0.00000"/>
</joint>
<joint name="back_bkxyz" type="ball">
<!-- <origin rpy="0 0 0" xyz="-0.0125 0 0.212"/> -->
<origin rpy="0 0 0" xyz="0.0125 0 -0.212"/>
<axis xyz="0 0 1"/>
<parent link="utorso"/>
<child link="pelvis"/>
<dynamics damping="0.1" friction="0"/>
</joint>
<!-- <joint name="neck_ay" type="revolute">
<origin rpy="0 0 0" xyz="0.2546 0 0.6215"/>
<axis xyz="0 1 0"/>
<parent link="utorso"/>
<child link="head"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="25" lower="-0.602139" upper="1.14319" velocity="6.28"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.6021" soft_upper_limit="11.1432"/>
</joint> -->
<joint name="l_leg_hpxyz" type="ball">
<origin rpy="0 0 0" xyz="0.05 0.1145 -0.066"/>
<axis xyz="0 0 1"/>
<parent link="pelvis"/>
<child link="l_uleg"/>
<dynamics damping="0.1" friction="0"/>
</joint>
<joint name="r_leg_hpxyz" type="ball">
<origin rpy="0 0 0" xyz="0.05 -0.1145 -0.066"/>
<axis xyz="0 0 1"/>
<parent link="pelvis"/>
<child link="r_uleg"/>
<dynamics damping="0.1" friction="0"/>
</joint>
<joint name="l_leg_kny" type="revolute">
<origin rpy="0 0 0" xyz="-0.05 0 -0.374"/>
<axis xyz="0 1 0"/>
<parent link="l_uleg"/>
<child link="l_lleg"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="890" lower="0" upper="2.35637" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.3564"/>
</joint>
<joint name="r_leg_kny" type="revolute">
<origin rpy="0 0 0" xyz="-0.05 0 -0.374"/>
<axis xyz="0 1 0"/>
<parent link="r_uleg"/>
<child link="r_lleg"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="890" lower="0" upper="2.35637" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.3564"/>
</joint>
<joint name="l_leg_akxy" type="orbital">
<origin rpy="0 0 0" xyz="0 0 -0.422"/>
<axis xyz="0 0 1"/>
<parent link="l_lleg"/>
<child link="l_foot"/>
<dynamics damping="0.1" friction="0"/>
</joint>
<joint name="r_leg_akxy" type="orbital">
<origin rpy="0 0 0" xyz="0 0 -0.422"/>
<axis xyz="0 0 1"/>
<parent link="r_lleg"/>
<child link="r_foot"/>
<dynamics damping="0.1" friction="0"/>
</joint>
</robot>
<!-- <link name="l_clav">
<inertial>
<mass value="4.466"/>
<origin rpy="0 0 0" xyz="0 0 -0.084"/>
<inertia ixx="0.011" ixy="0" ixz="0" iyy="0.009" iyz="0.004" izz="0.004"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_clav.obj"/>
</geometry>
</visual>
</link>
<link name="l_foot">
<inertial>
<mass value="2.410"/>
<origin rpy="0 0 0" xyz="0.027 0 -0.067"/>
<inertia ixx="0.002" ixy="0" ixz="0" iyy="0.007" iyz="0" izz="0.008"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/l_foot.obj"/>
</geometry>
</visual>
<visual group="heel">
<origin rpy="0 0 0" xyz="-0.0876 0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<visual group="heel">
<origin rpy="0 0 0" xyz="-0.0876 -0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<visual group="toe">
<origin rpy="0 0 0" xyz="0.1728 0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<visual group="toe">
<origin rpy="0 0 0" xyz="0.1728 -0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<visual group="midfoot">
<origin rpy="0 0 0" xyz="-0.0426 0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<visual group="midfoot">
<origin rpy="0 0 0" xyz="-0.0426 -0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
</link>
<link name="l_hand">
<inertial>
<mass value="0.5839"/>
<origin rpy="0 0 0" xyz="0.00016 -0.08159 0.00002"/>
<inertia ixx="0.000388" ixy="0" ixz="0" iyy="0.000477" iyz="0" izz="0.000379"/>
</inertial>
</link>
<link name="l_larm">
<inertial>
<mass value="3.248"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.00265" ixy="0" ixz="0" iyy="0.00446" iyz="0" izz="0.00446"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_larm.obj"/>
</geometry>
</visual>
</link>
<link name="l_lfarm">
<inertial>
<mass value="0.648"/>
<origin rpy="0 0 0" xyz="0.00017 -0.02515 0.00163"/>
<inertia ixx="0.000764" ixy="0" ixz="0" iyy="0.000429" iyz="0" izz="0.000825"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_hand.obj"/>
</geometry>
</visual>
</link>
<link name="l_lglut">
<inertial>
<mass value="0.898"/>
<origin rpy="0 0 0" xyz="0.0133341 0.0170484 -0.0312052"/>
<inertia ixx="0.000691326" ixy="-2.24344e-05" ixz="2.50508e-06" iyy="0.00126856" iyz="0.000137862" izz="0.00106487"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/l_lglut.obj"/>
</geometry>
</visual>
</link>
<link name="l_lleg">
<inertial>
<mass value="4.515"/>
<origin rpy="0 0 0" xyz="0.001 0 -0.187"/>
<inertia ixx="0.077" ixy="0" ixz="-0.003" iyy="0.076" iyz="0" izz="0.01"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/l_lleg.obj"/>
</geometry>
</visual>
</link>
<link name="l_scap">
<inertial>
<mass value="3.899"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.00319" ixy="0" ixz="0" iyy="0.00583" iyz="0" izz="0.00583"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_scap.obj"/>
</geometry>
</visual>
</link>
<link name="l_talus">
<inertial>
<mass value="0.125"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="1.01674e-05" ixy="0" ixz="0" iyy="8.42775e-06" iyz="0" izz="1.30101e-05"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/l_talus.obj"/>
</geometry>
</visual>
</link>
<link name="l_uarm">
<inertial>
<mass value="4.386"/>
<origin rpy="0 0 0" xyz="0 0.065 0"/>
<inertia ixx="0.00656" ixy="0" ixz="0" iyy="0.00358" iyz="0" izz="0.00656"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_uarm.obj"/>
</geometry>
</visual>
</link>
<link name="l_ufarm">
<inertial>
<mass value="2.4798"/>
<origin rpy="0 0 0" xyz="0.00015 0.08296 0.00037"/>
<inertia ixx="0.012731" ixy="0" ixz="0" iyy="0.002857" iyz="0" izz="0.011948"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_farm.obj"/>
</geometry>
</visual>
</link>
<link name="l_uglut">
<inertial>
<mass value="1.959"/>
<origin rpy="0 0 0" xyz="0.00529262 -0.00344732 0.00313046"/>
<inertia ixx="0.00074276" ixy="-3.79607e-08" ixz="-2.79549e-05" iyy="0.000688179" iyz="-3.2735e-08" izz="0.00041242"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/l_uglut.obj"/>
</geometry>
</visual>
</link>
<link name="l_uleg">
<inertial>
<mass value="8.204"/>
<origin rpy="0 0 0" xyz="0 0 -0.21"/>
<inertia ixx="0.09" ixy="0" ixz="0" iyy="0.09" iyz="0" izz="0.02"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/l_uleg.obj"/>
</geometry>
</visual>
</link>
<link name="ltorso">
<inertial>
<mass value="2.270"/>
<origin rpy="0 0 0" xyz="-0.0112984 -3.15366e-06 0.0746835"/>
<inertia ixx="0.0039092" ixy="-5.04491e-08" ixz="-0.000342157" iyy="0.00341694" iyz="4.87119e-07" izz="0.00174492"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/ltorso.obj"/>
</geometry>
</visual>
</link>
<link name="mtorso">
<inertial>
<mass value="0.799"/>
<origin rpy="0 0 0" xyz="-0.00816266 -0.0131245 0.0305974"/>
<inertia ixx="0.000454181" ixy="-6.10764e-05" ixz="3.94009e-05" iyy="0.000483282" iyz="5.27463e-05" izz="0.000444215"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/mtorso.obj"/>
</geometry>
</visual>
</link>
<link name="pelvis">
<inertial>
<mass value="9.509"/>
<origin rpy="0 0 0" xyz="0.0111 0 0.0271"/>
<inertia ixx="0.1244" ixy="0.0008" ixz="-0.0007" iyy="0.0958" iyz="-0.0005" izz="0.1167"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/pelvis.obj"/>
</geometry>
</visual>
</link>
<link name="r_clav">
<inertial>
<mass value="4.466"/>
<origin rpy="0 0 0" xyz="0 0 -0.084"/>
<inertia ixx="0.011" ixy="0" ixz="0" iyy="0.009" iyz="0.004" izz="0.004"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_clav.obj"/>
</geometry>
</visual>
</link>
<link name="r_foot">
<inertial>
<mass value="2.41"/>
<origin rpy="0 0 0" xyz="0.027 0 -0.067"/>
<inertia ixx="0.002" ixy="0" ixz="0" iyy="0.007" iyz="0" izz="0.008"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_foot.obj"/>
</geometry>
</visual>
<visual group="heel">
<origin rpy="0 0 0" xyz="-0.0876 0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<visual group="heel">
<origin rpy="0 0 0" xyz="-0.0876 -0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<visual group="toe">
<origin rpy="0 0 0" xyz="0.1728 0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<visual group="toe">
<origin rpy="0 0 0" xyz="0.1728 -0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<visual group="midfoot">
<origin rpy="0 0 0" xyz="0.0426 0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<visual group="midfoot">
<origin rpy="0 0 0" xyz="0.0426 -0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
</link>
<link name="r_hand">
<inertial>
<mass value="0.5839"/>
<origin rpy="0 0 0" xyz="0.00016 -0.08159 0.00002"/>
<inertia ixx="0.000388" ixy="0" ixz="0" iyy="0.000477" iyz="0" izz="0.000379"/>
</inertial>
</link>
<link name="r_larm">
<inertial>
<mass value="3.248"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.00265" ixy="0" ixz="0" iyy="0.00446" iyz="0" izz="0.00446"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_larm.obj"/>
</geometry>
</visual>
</link>
<link name="r_lfarm">
<inertial>
<mass value="0.648"/>
<origin rpy="0 0 0" xyz="0.00017 -0.02515 0.00163"/>
<inertia ixx="0.000764" ixy="0" ixz="0" iyy="0.000429" iyz="0" izz="0.000825"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_hand.obj"/>
</geometry>
</visual>
</link>
<link name="r_lglut">
<inertial>
<mass value="0.898"/>
<origin rpy="0 0 0" xyz="0.0133341 -0.0170484 -0.0312052"/>
<inertia ixx="0.000691326" ixy="2.24344e-05" ixz="2.50508e-06" iyy="0.00126856" iyz="-0.000137862" izz="0.00106487"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_lglut.obj"/>
</geometry>
</visual>
</link>
<link name="r_lleg">
<inertial>
<mass value="4.515"/>
<origin rpy="0 0 0" xyz="0.001 0 -0.187"/>
<inertia ixx="0.077" ixy="-0" ixz="-0.003" iyy="0.076" iyz="-0" izz="0.01"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_lleg.obj"/>
</geometry>
</visual>
</link>
<link name="r_scap">
<inertial>
<mass value="3.899"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.00319" ixy="0" ixz="0" iyy="0.00583" iyz="0" izz="0.00583"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_scap.obj"/>
</geometry>
</visual>
</link>
<link name="r_talus">
<inertial>
<mass value="0.125"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="1.01674e-05" ixy="0" ixz="0" iyy="8.42775e-06" iyz="0" izz="1.30101e-05"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_talus.obj"/>
</geometry>
</visual>
</link>
<link name="r_uarm">
<inertial>
<mass value="4.386"/>
<origin rpy="0 0 0" xyz="0 0.065 0"/>
<inertia ixx="0.00656" ixy="0" ixz="0" iyy="0.00358" iyz="0" izz="0.00656"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_uarm.obj"/>
</geometry>
</visual>
</link>
<link name="r_ufarm">
<inertial>
<mass value="2.4798"/>
<origin rpy="0 0 0" xyz="0.00015 0.08296 0.00037"/>
<inertia ixx="0.012731" ixy="0" ixz="0" iyy="0.002857" iyz="0" izz="0.011948"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_farm.obj"/>
</geometry>
</visual>
</link>
<link name="r_uglut">
<inertial>
<mass value="1.959"/>
<origin rpy="0 0 0" xyz="0.00529262 0.00344732 0.00313046"/>
<inertia ixx="0.00074276" ixy="3.79607e-08" ixz="-2.79549e-05" iyy="0.000688179" iyz="3.2735e-08" izz="0.00041242"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_uglut.obj"/>
</geometry>
</visual>
</link>
<link name="r_uleg">
<inertial>
<mass value="8.204"/>
<origin rpy="0 0 0" xyz="0 0 -0.21"/>
<inertia ixx="0.09" ixy="0" ixz="0" iyy="0.09" iyz="0" izz="0.02"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_uleg.obj"/>
</geometry>
</visual>
</link>
<link name="utorso">
<inertial>
<mass value="84.409"/>
<origin rpy="0 0 0" xyz="-0.0622 0.0023 0.3157"/>
<inertia ixx="1.577" ixy="-0.032" ixz="0.102" iyy="1.602" iyz="0.047" izz="0.565"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/utorso.obj"/>
</geometry>
</visual>
</link>
<joint name="back_bkx" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.05"/>
<axis xyz="1 0 0"/>
<parent link="mtorso"/>
<child link="utorso"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="300" lower="-0.523599" upper="0.523599" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.5236" soft_upper_limit="10.5236"/>
</joint>
<joint name="back_bky" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.162"/>
<axis xyz="0 1 0"/>
<parent link="ltorso"/>
<child link="mtorso"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="445" lower="-0.219388" upper="0.538783" velocity="9"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.2194" soft_upper_limit="10.5388"/>
</joint>
<joint name="back_bkz" type="revolute">
<origin rpy="0 0 0" xyz="-0.0125 0 0"/>
<axis xyz="0 0 1"/>
<parent link="pelvis"/>
<child link="ltorso"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="106" lower="-0.663225" upper="0.663225" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.6632" soft_upper_limit="10.6632"/>
</joint>
<joint name="l_arm_elx" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.119 0.0092"/>
<axis xyz="-1.0 -0.0 -0.0"/>
<parent link="l_uarm"/>
<child link="l_larm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="112" lower="0" upper="2.35619" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.3562"/>
</joint>
<joint name="l_arm_ely" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.187 -0.016"/>
<axis xyz="-0.0 -1.0 -0.0"/>
<parent link="l_scap"/>
<child link="l_uarm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="63" lower="0" upper="3.14159" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="13.1416"/>
</joint>
<joint name="l_arm_shx" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.11 -0.245"/>
<axis xyz="-1.0 -0.0 -0.0"/>
<parent link="l_clav"/>
<child link="l_scap"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="99" lower="-1.5708" upper="1.5708" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.5708" soft_upper_limit="11.5708"/>
</joint>
<joint name="l_arm_shz" type="revolute">
<origin rpy="0 0 3.14159265359" xyz="0.1406 0.2256 0.4776"/>
<axis xyz="0 0 1"/>
<parent link="utorso"/>
<child link="l_clav"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="87" lower="-1.5708" upper="0.785398" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.5708" soft_upper_limit="10.7854"/>
</joint>
<joint name="l_arm_mwx" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
<parent link="l_ufarm"/>
<child link="l_lfarm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="25" lower="-1.7628" upper="1.7628" velocity="10"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.7628" soft_upper_limit="11.7628"/>
</joint>
<joint name="l_arm_uwy" type="revolute">
<origin rpy="0 3.14159265359 0" xyz="0 -0.29955 -0.00921"/>
<axis xyz="-0.0 -1.0 -0.0"/>
<parent link="l_larm"/>
<child link="l_ufarm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="25" lower="-3.011" upper="3.011" velocity="10"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-13.011" soft_upper_limit="13.011"/>
</joint>
<joint name="l_arm_lwy" type="revolute">
<origin rpy="0 3.14159265359 0" xyz="0 0 0"/>
<axis xyz="-0.0 -1.0 -0.0"/>
<parent link="l_lfarm"/>
<child link="l_hand"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="25" lower="-2.9671" upper="2.9671" velocity="10"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-12.9671" soft_upper_limit="12.9671"/>
</joint>
<joint name="l_leg_akx" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
<parent link="l_talus"/>
<child link="l_foot"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="360" lower="-0.8" upper="0.8" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.8" soft_upper_limit="10.8"/>
</joint>
<joint name="l_leg_aky" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.422"/>
<axis xyz="0 1 0"/>
<parent link="l_lleg"/>
<child link="l_talus"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="740" lower="-1" upper="0.7" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11" soft_upper_limit="10.7"/>
</joint>
<joint name="l_leg_hpx" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
<parent link="l_uglut"/>
<child link="l_lglut"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="530" lower="-0.523599" upper="0.523599" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.5236" soft_upper_limit="10.5236"/>
</joint>
<joint name="l_leg_hpy" type="revolute">
<origin rpy="0 0 0" xyz="0.05 0.0225 -0.066"/>
<axis xyz="0 1 0"/>
<parent link="l_lglut"/>
<child link="l_uleg"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="840" lower="-1.61234" upper="0.65764" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.6123" soft_upper_limit="10.6576"/>
</joint>
<joint name="l_leg_hpz" type="revolute">
<origin rpy="0 0 0" xyz="0 0.089 0"/>
<axis xyz="0 0 1"/>
<parent link="pelvis"/>
<child link="l_uglut"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="275" lower="-0.174358" upper="0.786794" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.1744" soft_upper_limit="10.7868"/>
</joint>
<joint name="l_leg_kny" type="revolute">
<origin rpy="0 0 0" xyz="-0.05 0 -0.374"/>
<axis xyz="0 1 0"/>
<parent link="l_uleg"/>
<child link="l_lleg"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="890" lower="0" upper="2.35637" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.3564"/>
</joint>
<joint name="neck_ay" type="revolute">
<origin rpy="0 0 0" xyz="0.2546 0 0.6215"/>
<axis xyz="0 1 0"/>
<parent link="utorso"/>
<child link="head"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="25" lower="-0.602139" upper="1.14319" velocity="6.28"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.6021" soft_upper_limit="11.1432"/>
</joint>
<joint name="r_arm_elx" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.119 0.0092"/>
<axis xyz="1 0 0"/>
<parent link="r_uarm"/>
<child link="r_larm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="112" lower="-2.35619" upper="0" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-12.3562" soft_upper_limit="10"/>
</joint>
<joint name="r_arm_ely" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.187 -0.016"/>
<axis xyz="0 1 0"/>
<parent link="r_scap"/>
<child link="r_uarm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="63" lower="0" upper="3.14159" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="13.1416"/>
</joint>
<joint name="r_arm_shx" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.11 -0.245"/>
<axis xyz="1 0 0"/>
<parent link="r_clav"/>
<child link="r_scap"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="99" lower="-1.5708" upper="1.5708" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.5708" soft_upper_limit="11.5708"/>
</joint>
<joint name="r_arm_shz" type="revolute">
<origin rpy="0 0 0" xyz="0.1406 -0.2256 0.4776"/>
<axis xyz="0 0 1"/>
<parent link="utorso"/>
<child link="r_clav"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="87" lower="-0.785398" upper="1.5708" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.7854" soft_upper_limit="11.5708"/>
</joint>
<joint name="r_arm_mwx" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
<parent link="r_ufarm"/>
<child link="r_lfarm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="25" lower="-1.7628" upper="1.7628" velocity="10"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.7628" soft_upper_limit="11.7628"/>
</joint>
<joint name="r_arm_uwy" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.29955 -0.00921"/>
<axis xyz="0 1 0"/>
<parent link="r_larm"/>
<child link="r_ufarm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="25" lower="-3.011" upper="3.011" velocity="10"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-13.011" soft_upper_limit="13.011"/>
</joint>
<joint name="r_arm_lwy" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 1 0"/>
<parent link="r_lfarm"/>
<child link="r_hand"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="25" lower="-2.9671" upper="2.9671" velocity="10"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-12.9671" soft_upper_limit="12.9671"/>
</joint>
<joint name="r_leg_akx" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
<parent link="r_talus"/>
<child link="r_foot"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="360" lower="-0.8" upper="0.8" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.8" soft_upper_limit="10.8"/>
</joint>
<joint name="r_leg_aky" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.422"/>
<axis xyz="0 1 0"/>
<parent link="r_lleg"/>
<child link="r_talus"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="740" lower="-1" upper="0.7" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11" soft_upper_limit="10.7"/>
</joint>
<joint name="r_leg_hpx" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
<parent link="r_uglut"/>
<child link="r_lglut"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="530" lower="-0.523599" upper="0.523599" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.5236" soft_upper_limit="10.5236"/>
</joint>
<joint name="r_leg_hpy" type="revolute">
<origin rpy="0 0 0" xyz="0.05 -0.0225 -0.066"/>
<axis xyz="0 1 0"/>
<parent link="r_lglut"/>
<child link="r_uleg"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="840" lower="-1.61234" upper="0.65764" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.6123" soft_upper_limit="10.6576"/>
</joint>
<joint name="r_leg_hpz" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.089 0"/>
<axis xyz="0 0 1"/>
<parent link="pelvis"/>
<child link="r_uglut"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="275" lower="-0.786794" upper="0.174358" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.7868" soft_upper_limit="10.1744"/>
</joint>
<joint name="r_leg_kny" type="revolute">
<origin rpy="0 0 0" xyz="-0.05 0 -0.374"/>
<axis xyz="0 1 0"/>
<parent link="r_uleg"/>
<child link="r_lleg"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="890" lower="0" upper="2.35637" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.3564"/>
</joint>
<link name="head">
<inertial>
<origin rpy="0 0 0" xyz="-0.075493 3.3383E-05 0.02774"/>
<mass value="1.4199"/>
<inertia ixx="0.0039688" ixy="-1.5797E-06" ixz="-0.00089293" iyy="0.0041178" iyz="-6.8415E-07" izz="0.0035243"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="mesh/head_chull.obj"/>
</geometry>
<material name="">
<color rgba="0.9098 0.44314 0.031373 1"/>
</material>
</visual>
</link> -->
================================================
FILE: DojoEnvironments/src/mechanisms/atlas/dependencies/atlas_complex.urdf
================================================
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from atlas_v5_convex_hull_open_hands.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="atlas" xmlns:xacro="http://www.ros.org/wiki/xacro">
<link name="l_clav">
<inertial>
<mass value="4.466"/>
<origin rpy="0 0 0" xyz="0 0 -0.084"/>
<inertia ixx="0.011" ixy="0" ixz="0" iyy="0.009" iyz="0.004" izz="0.004"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_clav.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="mesh/r_clav_chull.obj"/>
</geometry>
</collision>
</link>
<link name="l_foot">
<inertial>
<mass value="2.410"/>
<origin rpy="0 0 0" xyz="0.027 0 -0.067"/>
<inertia ixx="0.002" ixy="0" ixz="0" iyy="0.007" iyz="0" izz="0.008"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/l_foot.obj"/>
</geometry>
</visual>
<collision group="heel">
<geometry>
<mesh filename="mesh/l_foot_chull.obj"/>
</geometry>
</collision>
<collision group="heel">
<origin rpy="0 0 0" xyz="-0.0876 0.066 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="heel">
<origin rpy="0 0 0" xyz="-0.0876 0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision group="heel">
<origin rpy="0 0 0" xyz="-0.0876 -0.0626 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="heel">
<origin rpy="0 0 0" xyz="-0.0876 -0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision group="toe">
<origin rpy="0 0 0" xyz="0.1728 0.066 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="toe">
<origin rpy="0 0 0" xyz="0.1728 0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision group="toe">
<origin rpy="0 0 0" xyz="0.1728 -0.0626 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="toe">
<origin rpy="0 0 0" xyz="0.1728 -0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision group="midfoot">
<origin rpy="0 0 0" xyz="-0.0426 0.066 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="midfoot">
<origin rpy="0 0 0" xyz="-0.0426 0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision group="midfoot">
<origin rpy="0 0 0" xyz="-0.0426 -0.0626 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="midfoot">
<origin rpy="0 0 0" xyz="-0.0426 -0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
</link>
<link name="l_hand">
<inertial>
<mass value="0.5839"/>
<origin rpy="0 0 0" xyz="0.00016 -0.08159 0.00002"/>
<inertia ixx="0.000388" ixy="0" ixz="0" iyy="0.000477" iyz="0" izz="0.000379"/>
</inertial>
</link>
<link name="l_larm">
<inertial>
<mass value="3.248"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.00265" ixy="0" ixz="0" iyy="0.00446" iyz="0" izz="0.00446"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_larm.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="mesh/r_larm_chull.obj"/>
</geometry>
</collision>
</link>
<link name="l_lfarm">
<inertial>
<mass value="0.648"/>
<origin rpy="0 0 0" xyz="0.00017 -0.02515 0.00163"/>
<inertia ixx="0.000764" ixy="0" ixz="0" iyy="0.000429" iyz="0" izz="0.000825"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_hand.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="mesh/r_hand_chull.obj"/>
</geometry>
</collision>
</link>
<link name="l_lglut">
<inertial>
<mass value="0.898"/>
<origin rpy="0 0 0" xyz="0.0133341 0.0170484 -0.0312052"/>
<inertia ixx="0.000691326" ixy="-2.24344e-05" ixz="2.50508e-06" iyy="0.00126856" iyz="0.000137862" izz="0.00106487"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/l_lglut.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="mesh/l_lglut_chull.obj"/>
</geometry>
</collision>
</link>
<link name="l_lleg">
<inertial>
<mass value="4.515"/>
<origin rpy="0 0 0" xyz="0.001 0 -0.187"/>
<inertia ixx="0.077" ixy="0" ixz="-0.003" iyy="0.076" iyz="0" izz="0.01"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/l_lleg.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="mesh/l_lleg_chull.obj"/>
</geometry>
</collision>
</link>
<link name="l_scap">
<inertial>
<mass value="3.899"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.00319" ixy="0" ixz="0" iyy="0.00583" iyz="0" izz="0.00583"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_scap.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="mesh/r_scap_chull.obj"/>
</geometry>
</collision>
</link>
<link name="l_talus">
<inertial>
<mass value="0.125"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="1.01674e-05" ixy="0" ixz="0" iyy="8.42775e-06" iyz="0" izz="1.30101e-05"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/l_talus.obj"/>
</geometry>
</visual>
</link>
<link name="l_uarm">
<inertial>
<mass value="4.386"/>
<origin rpy="0 0 0" xyz="0 0.065 0"/>
<inertia ixx="0.00656" ixy="0" ixz="0" iyy="0.00358" iyz="0" izz="0.00656"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_uarm.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="mesh/r_uarm_chull.obj"/>
</geometry>
</collision>
</link>
<link name="l_ufarm">
<inertial>
<mass value="2.4798"/>
<origin rpy="0 0 0" xyz="0.00015 0.08296 0.00037"/>
<inertia ixx="0.012731" ixy="0" ixz="0" iyy="0.002857" iyz="0" izz="0.011948"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_farm.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="mesh/r_farm_chull.obj"/>
</geometry>
</collision>
</link>
<link name="l_uglut">
<inertial>
<mass value="1.959"/>
<origin rpy="0 0 0" xyz="0.00529262 -0.00344732 0.00313046"/>
<inertia ixx="0.00074276" ixy="-3.79607e-08" ixz="-2.79549e-05" iyy="0.000688179" iyz="-3.2735e-08" izz="0.00041242"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/l_uglut.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="mesh/l_uglut_chull.obj"/>
</geometry>
</collision>
</link>
<link name="l_uleg">
<inertial>
<mass value="8.204"/>
<origin rpy="0 0 0" xyz="0 0 -0.21"/>
<inertia ixx="0.09" ixy="0" ixz="0" iyy="0.09" iyz="0" izz="0.02"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/l_uleg.obj"/>
</geometry>
</visual>
<!-- to cover the logo shield -->
<!-- inside strut -->
<!-- outside strut -->
<collision>
<geometry>
<mesh filename="mesh/l_uleg_chull.obj"/>
</geometry>
</collision>
</link>
<link name="ltorso">
<inertial>
<mass value="2.270"/>
<origin rpy="0 0 0" xyz="-0.0112984 -3.15366e-06 0.0746835"/>
<inertia ixx="0.0039092" ixy="-5.04491e-08" ixz="-0.000342157" iyy="0.00341694" iyz="4.87119e-07" izz="0.00174492"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/ltorso.obj"/>
</geometry>
</visual>
</link>
<link name="mtorso">
<inertial>
<mass value="0.799"/>
<origin rpy="0 0 0" xyz="-0.00816266 -0.0131245 0.0305974"/>
<inertia ixx="0.000454181" ixy="-6.10764e-05" ixz="3.94009e-05" iyy="0.000483282" iyz="5.27463e-05" izz="0.000444215"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/mtorso.obj"/>
</geometry>
</visual>
</link>
<link name="pelvis">
<inertial>
<mass value="9.509"/>
<origin rpy="0 0 0" xyz="0.0111 0 0.0271"/>
<inertia ixx="0.1244" ixy="0.0008" ixz="-0.0007" iyy="0.0958" iyz="-0.0005" izz="0.1167"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/pelvis.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="mesh/pelvis_chull.obj"/>
</geometry>
</collision>
</link>
<link name="r_clav">
<inertial>
<mass value="4.466"/>
<origin rpy="0 0 0" xyz="0 0 -0.084"/>
<inertia ixx="0.011" ixy="0" ixz="0" iyy="0.009" iyz="0.004" izz="0.004"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_clav.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="mesh/r_clav_chull.obj"/>
</geometry>
</collision>
</link>
<link name="r_foot">
<inertial>
<mass value="2.41"/>
<origin rpy="0 0 0" xyz="0.027 0 -0.067"/>
<inertia ixx="0.002" ixy="0" ixz="0" iyy="0.007" iyz="0" izz="0.008"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_foot.obj"/>
</geometry>
</visual>
<collision group="heel">
<geometry>
<mesh filename="mesh/r_foot_chull.obj"/>
</geometry>
</collision>
<collision group="heel">
<origin rpy="0 0 0" xyz="-0.0876 0.0626 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="heel">
<origin rpy="0 0 0" xyz="-0.0876 0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision group="heel">
<origin rpy="0 0 0" xyz="-0.0876 -0.066 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="heel">
<origin rpy="0 0 0" xyz="-0.0876 -0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision group="toe">
<origin rpy="0 0 0" xyz="0.1728 0.0626 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="toe">
<origin rpy="0 0 0" xyz="0.1728 0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision group="toe">
<origin rpy="0 0 0" xyz="0.1728 -0.066 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="toe">
<origin rpy="0 0 0" xyz="0.1728 -0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision group="midfoot">
<origin rpy="0 0 0" xyz="0.0426 0.0626 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="midfoot">
<origin rpy="0 0 0" xyz="0.0426 0.0626 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision group="midfoot">
<origin rpy="0 0 0" xyz="0.0426 -0.066 -0.07645"/>
<geometry>
<sphere radius="0.0"/>
</geometry>
</collision>
<visual group="midfoot">
<origin rpy="0 0 0" xyz="0.0426 -0.066 -0.07645"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
</link>
<link name="r_hand">
<inertial>
<mass value="0.5839"/>
<origin rpy="0 0 0" xyz="0.00016 -0.08159 0.00002"/>
<inertia ixx="0.000388" ixy="0" ixz="0" iyy="0.000477" iyz="0" izz="0.000379"/>
</inertial>
</link>
<link name="r_larm">
<inertial>
<mass value="3.248"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.00265" ixy="0" ixz="0" iyy="0.00446" iyz="0" izz="0.00446"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_larm.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="mesh/r_larm_chull.obj"/>
</geometry>
</collision>
</link>
<link name="r_lfarm">
<inertial>
<mass value="0.648"/>
<origin rpy="0 0 0" xyz="0.00017 -0.02515 0.00163"/>
<inertia ixx="0.000764" ixy="0" ixz="0" iyy="0.000429" iyz="0" izz="0.000825"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_hand.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="mesh/r_hand_chull.obj"/>
</geometry>
</collision>
</link>
<link name="r_lglut">
<inertial>
<mass value="0.898"/>
<origin rpy="0 0 0" xyz="0.0133341 -0.0170484 -0.0312052"/>
<inertia ixx="0.000691326" ixy="2.24344e-05" ixz="2.50508e-06" iyy="0.00126856" iyz="-0.000137862" izz="0.00106487"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_lglut.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="mesh/r_lglut_chull.obj"/>
</geometry>
</collision>
</link>
<link name="r_lleg">
<inertial>
<mass value="4.515"/>
<origin rpy="0 0 0" xyz="0.001 0 -0.187"/>
<inertia ixx="0.077" ixy="-0" ixz="-0.003" iyy="0.076" iyz="-0" izz="0.01"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_lleg.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="mesh/r_lleg_chull.obj"/>
</geometry>
</collision>
</link>
<link name="r_scap">
<inertial>
<mass value="3.899"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.00319" ixy="0" ixz="0" iyy="0.00583" iyz="0" izz="0.00583"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_scap.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="mesh/r_scap_chull.obj"/>
</geometry>
</collision>
</link>
<link name="r_talus">
<inertial>
<mass value="0.125"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="1.01674e-05" ixy="0" ixz="0" iyy="8.42775e-06" iyz="0" izz="1.30101e-05"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_talus.obj"/>
</geometry>
</visual>
</link>
<link name="r_uarm">
<inertial>
<mass value="4.386"/>
<origin rpy="0 0 0" xyz="0 0.065 0"/>
<inertia ixx="0.00656" ixy="0" ixz="0" iyy="0.00358" iyz="0" izz="0.00656"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_uarm.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="mesh/r_uarm_chull.obj"/>
</geometry>
</collision>
</link>
<link name="r_ufarm">
<inertial>
<mass value="2.4798"/>
<origin rpy="0 0 0" xyz="0.00015 0.08296 0.00037"/>
<inertia ixx="0.012731" ixy="0" ixz="0" iyy="0.002857" iyz="0" izz="0.011948"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_farm.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="mesh/r_farm_chull.obj"/>
</geometry>
</collision>
</link>
<link name="r_uglut">
<inertial>
<mass value="1.959"/>
<origin rpy="0 0 0" xyz="0.00529262 0.00344732 0.00313046"/>
<inertia ixx="0.00074276" ixy="3.79607e-08" ixz="-2.79549e-05" iyy="0.000688179" iyz="3.2735e-08" izz="0.00041242"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_uglut.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="mesh/r_uglut_chull.obj"/>
</geometry>
</collision>
</link>
<link name="r_uleg">
<inertial>
<mass value="8.204"/>
<origin rpy="0 0 0" xyz="0 0 -0.21"/>
<inertia ixx="0.09" ixy="0" ixz="0" iyy="0.09" iyz="0" izz="0.02"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/r_uleg.obj"/>
</geometry>
</visual>
<!-- to cover the logo shield -->
<!-- inside strut -->
<!-- outside strut -->
<collision>
<geometry>
<mesh filename="mesh/r_uleg_chull.obj"/>
</geometry>
</collision>
</link>
<link name="utorso">
<inertial>
<mass value="84.409"/>
<origin rpy="0 0 0" xyz="-0.0622 0.0023 0.3157"/>
<inertia ixx="1.577" ixy="-0.032" ixz="0.102" iyy="1.602" iyz="0.047" izz="0.565"/>
</inertial>
<visual>
<geometry>
<mesh filename="mesh/utorso.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="mesh/utorso_chull.obj"/>
</geometry>
</collision>
</link>
<joint name="back_bkx" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.05"/>
<axis xyz="1 0 0"/>
<parent link="mtorso"/>
<child link="utorso"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="300" lower="-0.523599" upper="0.523599" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.5236" soft_upper_limit="10.5236"/>
</joint>
<joint name="back_bky" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.162"/>
<axis xyz="0 1 0"/>
<parent link="ltorso"/>
<child link="mtorso"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="445" lower="-0.219388" upper="0.538783" velocity="9"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.2194" soft_upper_limit="10.5388"/>
</joint>
<joint name="back_bkz" type="revolute">
<origin rpy="0 0 0" xyz="-0.0125 0 0"/>
<axis xyz="0 0 1"/>
<parent link="pelvis"/>
<child link="ltorso"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="106" lower="-0.663225" upper="0.663225" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.6632" soft_upper_limit="10.6632"/>
</joint>
<joint name="l_arm_elx" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.119 0.0092"/>
<axis xyz="-1.0 -0.0 -0.0"/>
<parent link="l_uarm"/>
<child link="l_larm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="112" lower="0" upper="2.35619" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.3562"/>
</joint>
<joint name="l_arm_ely" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.187 -0.016"/>
<axis xyz="-0.0 -1.0 -0.0"/>
<parent link="l_scap"/>
<child link="l_uarm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="63" lower="0" upper="3.14159" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="13.1416"/>
</joint>
<joint name="l_arm_shx" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.11 -0.245"/>
<axis xyz="-1.0 -0.0 -0.0"/>
<parent link="l_clav"/>
<child link="l_scap"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="99" lower="-1.5708" upper="1.5708" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.5708" soft_upper_limit="11.5708"/>
</joint>
<joint name="l_arm_shz" type="revolute">
<origin rpy="0 0 3.14159265359" xyz="0.1406 0.2256 0.4776"/>
<axis xyz="0 0 1"/>
<parent link="utorso"/>
<child link="l_clav"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="87" lower="-1.5708" upper="0.785398" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.5708" soft_upper_limit="10.7854"/>
</joint>
<joint name="l_arm_mwx" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
<parent link="l_ufarm"/>
<child link="l_lfarm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="25" lower="-1.7628" upper="1.7628" velocity="10"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.7628" soft_upper_limit="11.7628"/>
</joint>
<joint name="l_arm_uwy" type="revolute">
<origin rpy="0 3.14159265359 0" xyz="0 -0.29955 -0.00921"/>
<axis xyz="-0.0 -1.0 -0.0"/>
<parent link="l_larm"/>
<child link="l_ufarm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="25" lower="-3.011" upper="3.011" velocity="10"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-13.011" soft_upper_limit="13.011"/>
</joint>
<joint name="l_arm_lwy" type="revolute">
<origin rpy="0 3.14159265359 0" xyz="0 0 0"/>
<axis xyz="-0.0 -1.0 -0.0"/>
<parent link="l_lfarm"/>
<child link="l_hand"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="25" lower="-2.9671" upper="2.9671" velocity="10"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-12.9671" soft_upper_limit="12.9671"/>
</joint>
<joint name="l_leg_akx" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
<parent link="l_talus"/>
<child link="l_foot"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="360" lower="-0.8" upper="0.8" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.8" soft_upper_limit="10.8"/>
</joint>
<joint name="l_leg_aky" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.422"/>
<axis xyz="0 1 0"/>
<parent link="l_lleg"/>
<child link="l_talus"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="740" lower="-1" upper="0.7" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11" soft_upper_limit="10.7"/>
</joint>
<joint name="l_leg_hpx" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
<parent link="l_uglut"/>
<child link="l_lglut"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="530" lower="-0.523599" upper="0.523599" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.5236" soft_upper_limit="10.5236"/>
</joint>
<joint name="l_leg_hpy" type="revolute">
<origin rpy="0 0 0" xyz="0.05 0.0225 -0.066"/>
<axis xyz="0 1 0"/>
<parent link="l_lglut"/>
<child link="l_uleg"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="840" lower="-1.61234" upper="0.65764" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.6123" soft_upper_limit="10.6576"/>
</joint>
<joint name="l_leg_hpz" type="revolute">
<origin rpy="0 0 0" xyz="0 0.089 0"/>
<axis xyz="0 0 1"/>
<parent link="pelvis"/>
<child link="l_uglut"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="275" lower="-0.174358" upper="0.786794" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.1744" soft_upper_limit="10.7868"/>
</joint>
<joint name="l_leg_kny" type="revolute">
<origin rpy="0 0 0" xyz="-0.05 0 -0.374"/>
<axis xyz="0 1 0"/>
<parent link="l_uleg"/>
<child link="l_lleg"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="890" lower="0" upper="2.35637" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.3564"/>
</joint>
<joint name="neck_ay" type="revolute">
<origin rpy="0 0 0" xyz="0.2546 0 0.6215"/>
<axis xyz="0 1 0"/>
<parent link="utorso"/>
<child link="head"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="25" lower="-0.602139" upper="1.14319" velocity="6.28"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.6021" soft_upper_limit="11.1432"/>
</joint>
<joint name="r_arm_elx" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.119 0.0092"/>
<axis xyz="1 0 0"/>
<parent link="r_uarm"/>
<child link="r_larm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="112" lower="-2.35619" upper="0" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-12.3562" soft_upper_limit="10"/>
</joint>
<joint name="r_arm_ely" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.187 -0.016"/>
<axis xyz="0 1 0"/>
<parent link="r_scap"/>
<child link="r_uarm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="63" lower="0" upper="3.14159" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="13.1416"/>
</joint>
<joint name="r_arm_shx" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.11 -0.245"/>
<axis xyz="1 0 0"/>
<parent link="r_clav"/>
<child link="r_scap"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="99" lower="-1.5708" upper="1.5708" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.5708" soft_upper_limit="11.5708"/>
</joint>
<joint name="r_arm_shz" type="revolute">
<origin rpy="0 0 0" xyz="0.1406 -0.2256 0.4776"/>
<axis xyz="0 0 1"/>
<parent link="utorso"/>
<child link="r_clav"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="87" lower="-0.785398" upper="1.5708" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.7854" soft_upper_limit="11.5708"/>
</joint>
<joint name="r_arm_mwx" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
<parent link="r_ufarm"/>
<child link="r_lfarm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="25" lower="-1.7628" upper="1.7628" velocity="10"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.7628" soft_upper_limit="11.7628"/>
</joint>
<joint name="r_arm_uwy" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.29955 -0.00921"/>
<axis xyz="0 1 0"/>
<parent link="r_larm"/>
<child link="r_ufarm"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="25" lower="-3.011" upper="3.011" velocity="10"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-13.011" soft_upper_limit="13.011"/>
</joint>
<joint name="r_arm_lwy" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 1 0"/>
<parent link="r_lfarm"/>
<child link="r_hand"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="25" lower="-2.9671" upper="2.9671" velocity="10"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-12.9671" soft_upper_limit="12.9671"/>
</joint>
<joint name="r_leg_akx" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
<parent link="r_talus"/>
<child link="r_foot"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="360" lower="-0.8" upper="0.8" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.8" soft_upper_limit="10.8"/>
</joint>
<joint name="r_leg_aky" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.422"/>
<axis xyz="0 1 0"/>
<parent link="r_lleg"/>
<child link="r_talus"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="740" lower="-1" upper="0.7" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11" soft_upper_limit="10.7"/>
</joint>
<joint name="r_leg_hpx" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
<parent link="r_uglut"/>
<child link="r_lglut"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="530" lower="-0.523599" upper="0.523599" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.5236" soft_upper_limit="10.5236"/>
</joint>
<joint name="r_leg_hpy" type="revolute">
<origin rpy="0 0 0" xyz="0.05 -0.0225 -0.066"/>
<axis xyz="0 1 0"/>
<parent link="r_lglut"/>
<child link="r_uleg"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="840" lower="-1.61234" upper="0.65764" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.6123" soft_upper_limit="10.6576"/>
</joint>
<joint name="r_leg_hpz" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.089 0"/>
<axis xyz="0 0 1"/>
<parent link="pelvis"/>
<child link="r_uglut"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="275" lower="-0.786794" upper="0.174358" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.7868" soft_upper_limit="10.1744"/>
</joint>
<joint name="r_leg_kny" type="revolute">
<origin rpy="0 0 0" xyz="-0.05 0 -0.374"/>
<axis xyz="0 1 0"/>
<parent link="r_uleg"/>
<child link="r_lleg"/>
<dynamics damping="0.1" friction="0"/>
<limit effort="890" lower="0" upper="2.35637" velocity="12"/>
<safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.3564"/>
</joint>
<link name="r_situational_awareness_camera_link">
<inertial>
<mass value="0.1"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</collision>
</link>
<joint name="r_situational_awareness_camera_joint" type="fixed">
<parent link="utorso"/>
<child link="r_situational_awareness_camera_link"/>
<!-- SA camera mounted above the multisense head -->
<origin rpy="0 0 -1.309" xyz="0.155 -0.121 0.785"/>
</joint>
<joint name="r_situational_awareness_camera_optical_frame_joint" type="fixed">
<origin rpy="-1.5708 0.0 -1.5708" xyz="0 0 0"/>
<parent link="r_situational_awareness_camera_link"/>
<child link="r_situational_awareness_camera_optical_frame"/>
</joint>
<link name="r_situational_awareness_camera_optical_frame"/>
<link name="l_situational_awareness_camera_link">
<inertial>
<mass value="0.1"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</collision>
</link>
<joint name="l_situational_awareness_camera_joint" type="fixed">
<parent link="utorso"/>
<child link="l_situational_awareness_camera_link"/>
<!-- SA camera mounted above the multisense head -->
<origin rpy="0 0 1.309" xyz="0.155 0.121 0.785"/>
</joint>
<joint name="l_situational_awareness_camera_optical_frame_joint" type="fixed">
<origin rpy="-1.5708 0.0 -1.5708" xyz="0 0 0"/>
<parent link="l_situational_awareness_camera_link"/>
<child link="l_situational_awareness_camera_optical_frame"/>
</joint>
<link name="l_situational_awareness_camera_optical_frame"/>
<frame link="l_foot" name="l_foot_sole" rpy="0 0 0" xyz="0.0426 0.0017 -0.07645"/>
<frame link="r_foot" name="r_foot_sole" rpy="0 0 0" xyz="0.0426 -0.0017 -0.07645"/>
<frame link="l_foot" name="l_foot_toe" rpy="0 0 0" xyz="0.1728 0.0017 -0.07645"/>
<frame link="r_foot" name="r_foot_toe" rpy="0 0 0" xyz="0.1728 -0.0017 -0.07645"/>
<collision_filter_group name="feet">
<member link="l_foot"/>
<member link="r_foot"/>
<ignored_collision_filter_group collision_filter_group="feet"/>
</collision_filter_group>
<collision_filter_group name="core">
<member link="utorso"/>
<member link="pelvis"/>
<ignored_collision_filter_group collision_filter_group="core"/>
</collision_filter_group>
<collision_filter_group name="ignore_core">
<member link="r_scap"/>
<member link="l_scap"/>
<member link="r_clav"/>
<member link="l_clav"/>
<ignored_collision_filter_group collision_filter_group="core"/>
</collision_filter_group>
<collision_filter_group name="r_uleg">
<member link="r_uglut"/>
<member link="r_lglut"/>
<member link="r_uleg"/>
<ignored_collision_filter_group collision_filter_group="core"/>
<ignored_collision_filter_group collision_filter_group="r_uleg"/>
<ignored_collision_filter_group collision_filter_group="l_uleg"/>
</collision_filter_group>
<collision_filter_group name="l_uleg">
<member link="l_uglut"/>
<member link="l_lglut"/>
<member link="l_uleg"/>
<ignored_collision_filter_group collision_filter_group="core"/>
<ignored_collision_filter_group collision_filter_group="l_uleg"/>
<ignored_collision_filter_group collision_filter_group="l_uleg"/>
</collision_filter_group>
<collision_filter_group name="r_leg">
<member link="r_lleg"/>
<member link="r_talus"/>
<member link="r_foot"/>
<ignored_collision_filter_group collision_filter_group="r_leg"/>
<ignored_collision_filter_group collision_filter_group="r_uleg"/>
</collision_filter_group>
<collision_filter_group name="l_leg">
<member link="l_lleg"/>
<member link="l_talus"/>
<member link="l_foot"/>
<ignored_collision_filter_group collision_filter_group="l_leg"/>
<ignored_collision_filter_group collision_filter_group="l_uleg"/>
</collision_filter_group>
<collision_filter_group name="r_arm">
<member link="r_clav"/>
<member link="r_scap"/>
<member link="r_uarm"/>
<member link="r_larm"/>
<member link="r_ufarm"/>
<member link="r_lfarm"/>
<member link="r_hand"/>
<ignored_collision_filter_group collision_filter_group="r_arm"/>
</collision_filter_group>
<collision_filter_group name="l_arm">
<member link="l_clav"/>
<member link="l_scap"/>
<member link="l_uarm"/>
<member link="l_larm"/>
<member link="l_ufarm"/>
<member link="l_lfarm"/>
<member link="l_hand"/>
<ignored_collision_filter_group collision_filter_group="l_arm"/>
</collision_filter_group>
<link name="head">
<inertial>
<origin rpy="0 0 0" xyz="-0.075493 3.3383E-05 0.02774"/>
<mass value="1.4199"/>
<inertia ixx="0.0039688" ixy="-1.5797E-06" ixz="-0.00089293" iyy="0.0041178" iyz="-6.8415E-07" izz="0.0035243"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="mesh/head_chull.obj"/>
</geometry>
<material name="">
<color rgba="0.9098 0.44314 0.031373 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="mesh/head_chull.obj"/>
</geometry>
<material name="">
<color rgba="0.9098 0.44314 0.031373 1"/>
</material>
</collision>
</link>
<link name="pre_spindle"/>
<joint name="pre_spindle_joint" type="fixed">
<axis xyz="0 1 0"/>
<origin rpy="0 0 0" xyz="-0.0446 0 0.0880"/>
<parent link="head"/>
<child link="pre_spindle"/>
</joint>
<link name="pre_spindle_cal_x"/>
<link name="pre_spindle_cal_y"/>
<link name="pre_spindle_cal_z"/>
<link name="pre_spindle_cal_roll"/>
<link name="pre_spindle_cal_pitch"/>
<link name="pre_spindle_cal_yaw"/>
<joint name="pre_spindle_cal_x_joint" type="fixed">
<parent link="pre_spindle"/>
<child link="pre_spindle_cal_x"/>
<axis xyz="1 0 0"/>
<limit effort="0.0" lower="-0.5" upper="0.5" velocity="0.0"/>
</joint>
<joint name="pre_spindle_cal_y_joint" type="fixed">
<parent link="pre_spindle_cal_x"/>
<child link="pre_spindle_cal_y"/>
<axis xyz="0 1 0"/>
<limit effort="0.0" lower="-0.5" upper="0.5" velocity="0.0"/>
</joint>
<joint name="pre_spindle_cal_z_joint" type="fixed">
<parent link="pre_spindle_cal_y"/>
<child link="pre_spindle_cal_z"/>
<axis xyz="0 0 1"/>
<limit effort="0.0" lower="-0.5" upper="0.5" velocity="0.0"/>
</joint>
<joint name="pre_spindle_cal_roll_joint" type="fixed">
<parent link="pre_spindle_cal_z"/>
<child link="pre_spindle_cal_roll"/>
<axis xyz="1 0 0"/>
</joint>
<joint name="pre_spindle_cal_pitch_joint" type="fixed">
<parent link="pre_spindle_cal_roll"/>
<child link="pre_spindle_cal_pitch"/>
<axis xyz="0 1 0"/>
</joint>
<joint name="pre_spindle_cal_yaw_joint" type="fixed">
<parent link="pre_spindle_cal_pitch"/>
<child link="pre_spindle_cal_yaw"/>
<axis xyz="0 0 1"/>
</joint>
<joint name="hokuyo_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="pre_spindle_cal_yaw"/>
<child link="post_spindle"/>
<axis xyz="1 0 0"/>
</joint>
<link name="post_spindle"/>
<link name="post_spindle_cal_x"/>
<link name="post_spindle_cal_y"/>
<link name="post_spindle_cal_z"/>
<link name="post_spindle_cal_roll"/>
<link name="post_spindle_cal_pitch"/>
<joint name="post_spindle_cal_x_joint" type="fixed">
<parent link="post_spindle"/>
<child link="post_spindle_cal_x"/>
<axis xyz="1 0 0"/>
<limit effort="0.0" lower="-0.5" upper="0.5" velocity="0.0"/>
</joint>
<joint name="post_spindle_cal_y_joint" type="fixed">
<parent link="post_spindle_cal_x"/>
<child link="post_spindle_cal_y"/>
<axis xyz="0 1 0"/>
<limit effort="0.0" lower="-0.5" upper="0.5" velocity="0.0"/>
</joint>
<joint name="post_spindle_cal_z_joint" type="fixed">
<parent link="post_spindle_cal_y"/>
<child link="post_spindle_cal_z"/>
<axis xyz="0 0 1"/>
<limit effort="0.0" lower="-0.5" upper="0.5" velocity="0.0"/>
</joint>
<joint name="post_spindle_cal_roll_joint" type="fixed">
<parent link="post_spindle_cal_z"/>
<child link="post_spindle_cal_roll"/>
<axis xyz="1 0 0"/>
</joint>
<joint name="post_spindle_cal_pitch_joint" type="fixed">
<parent link="post_spindle_cal_roll"/>
<child link="post_spindle_cal_pitch"/>
<axis xyz="0 1 0"/>
</joint>
<joint name="post_spindle_cal_yaw_joint" type="fixed">
<parent link="post_spindle_cal_pitch"/>
<child link="hokuyo_link"/>
<axis xyz="0 0 1"/>
</joint>
<link name="hokuyo_link">
<inertial>
<origin rpy="0 0 0" xyz="0.012428 0.0004084 -0.0041835"/>
<mass value="0.057664"/>
<inertia ixx="3.7174E-05" ixy="4.9927E-08" ixz="1.1015E-05" iyy="4.2412E-05" iyz="-9.8165E-09" izz="4.167E-05"/>
</inertial>
<visual>
<origin rpy="-0.314 0 0" xyz="0.045 -0.0261018277 -0.08342369"/>
<geometry>
<mesh filename="mesh/head_camera_chull.obj"/>
</geometry>
<material name="">
<color rgba="0.72941 0.35686 0.023529 1"/>
</material>
</visual>
<collision>
<origin rpy="-0.314 0 0" xyz="0.045 -0.0261018277 -0.08342369"/>
<geometry>
<mesh filename="mesh/head_camera_chull.obj"/>
</geometry>
<material name="">
<color rgba="0.72941 0.35686 0.023529 1"/>
</material>
</collision>
</link>
<joint name="head_hokuyo_fixed_joint" type="fixed">
<axis xyz="0 1 0"/>
<origin rpy="0 0 0" xyz="0.03 0 0.015"/>
<parent link="hokuyo_link"/>
<child link="head_hokuyo_frame"/>
</joint>
<link name="head_hokuyo_frame">
<inertial>
<mass value="0.01"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
</inertial>
</link>
<gazebo reference="head_hokuyo_frame">
<sensor name="head_hokuyo_sensor" type="ray">
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>40</update_rate>
<ray>
<scan>
<horizontal>
<samples>720</samples>
<resolution>1</resolution>
<min_angle>-1.570796</min_angle>
<max_angle>1.570796</max_angle>
</horizontal>
</scan>
<range>
<min>0.10</min>
<max>30.0</max>
<resolution>0.01</resolution>
</range>
</ray>
<plugin filename="libgazebo_ros_laser.so" name="gazebo_ros_head_hokuyo_controller">
<gaussianNoise>0.005</gaussianNoise>
<alwaysOn>true</alwaysOn>
<updateRate>40</updateRate>
<topicName>/scan</topicName>
<frameName>head_hokuyo_frame</frameName>
</plugin>
</sensor>
</gazebo>
<joint name="left_camera_frame_joint" type="fixed">
<!-- optical frame collocated with tilting DOF -->
<origin xyz="0.0 0.035 -0.002"/>
<parent link="head"/>
<child link="left_camera_frame"/>
</joint>
<link name="left_camera_frame">
<inertial>
<mass value="0.1"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
</link>
<joint name="left_camera_optical_frame_joint" type="fixed">
<origin rpy="-1.5708 0.0 -1.5708" xyz="0 0 0"/>
<parent link="left_camera_frame"/>
<child link="left_camera_optical_frame"/>
</joint>
<link name="left_camera_optical_frame"/>
<gazebo reference="left_camera_frame">
<sensor name="left_camera_sensor" type="camera">
<!-- update rate based on spec sheet for 2mp 15fps -->
<update_rate>15.0</update_rate>
<camera>
<!-- hfov based on spec sheet, 80 degrees X 45 degrees -->
<!-- for resolution of 2048 x 1088, we have non-unity aspect ratio -->
<!-- but gazebo will violate the vertical fov for now to keep aspect ratio of 1 -->
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>2048</width>
<height>1088</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>100</far>
</clip>
</camera>
<plugin filename="libgazebo_ros_camera.so" name="left_camera_controller">
<alwaysOn>true</alwaysOn>
<!-- update rate based on spec sheet for 2mp 15fps -->
<updateRate>15.0</updateRate>
<cameraName>multisense/left</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>left_camera_optical_frame</frameName>
<hackBaseline>0.0</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
<joint name="right_camera_frame_joint" type="fixed">
<origin xyz="0.0 -0.035 -0.002"/>
<parent link="head"/>
<child link="right_camera_frame"/>
</joint>
<link name="right_camera_frame">
<inertial>
<mass value="0.1"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
</link>
<joint name="right_camera_optical_frame_joint" type="fixed">
<origin rpy="-1.5708 0.0 -1.5708" xyz="0 0 0"/>
<parent link="right_camera_frame"/>
<child link="right_camera_optical_frame"/>
</joint>
<link name="right_camera_optical_frame"/>
<gazebo reference="right_camera_frame">
<sensor name="right_camera_sensor" type="camera">
<!-- update rate based on spec sheet for 2mp 15fps -->
<update_rate>15.0</update_rate>
<camera>
<!-- hfov based on spec sheet, 80 degrees X 45 degrees -->
<!-- for resolution of 2048 x 1088, we have non-unity aspect ratio -->
<!-- but gazebo will violate the vertical fov for now to keep aspect ratio of 1 -->
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>2048</width>
<height>1088</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>100</far>
</clip>
</camera>
<plugin filename="libgazebo_ros_camera.so" name="right_camera_controller">
<alwaysOn>true</alwaysOn>
<!-- update rate based on spec sheet for 2mp 15fps -->
<updateRate>15.0</updateRate>
<cameraName>multisense/right</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>left_camera_optical_frame</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
<joint n
gitextract_l11i1c07/
├── .github/
│ └── workflows/
│ ├── CI.yml
│ ├── CI_DojoEnv.yml
│ ├── Documenter.yml
│ ├── TagBot.yml
│ ├── benchmark.yml
│ └── dailyCI.yml
├── .gitignore
├── CITATION.bib
├── DojoEnvironments/
│ ├── LICENSE
│ ├── Project.toml
│ ├── README.md
│ ├── src/
│ │ ├── DojoEnvironments.jl
│ │ ├── environments/
│ │ │ ├── ant_ars.jl
│ │ │ ├── cartpole_dqn.jl
│ │ │ ├── include.jl
│ │ │ ├── pendulum.jl
│ │ │ ├── quadrotor_waypoint.jl
│ │ │ ├── quadruped_sampling.jl
│ │ │ ├── quadruped_waypoint.jl
│ │ │ ├── uuv_waypoint.jl
│ │ │ └── youbot_waypoint.jl
│ │ ├── environments.jl
│ │ ├── mechanisms/
│ │ │ ├── ant/
│ │ │ │ ├── dependencies/
│ │ │ │ │ └── ant.urdf
│ │ │ │ └── mechanism.jl
│ │ │ ├── atlas/
│ │ │ │ ├── dependencies/
│ │ │ │ │ ├── atlas.urdf
│ │ │ │ │ ├── atlas_armless.urdf
│ │ │ │ │ ├── atlas_complex.urdf
│ │ │ │ │ ├── atlas_fast.urdf
│ │ │ │ │ ├── atlas_ones.urdf
│ │ │ │ │ ├── atlas_simple.urdf
│ │ │ │ │ ├── atlas_v2.urdf
│ │ │ │ │ └── mesh/
│ │ │ │ │ ├── GRIPPER_OPEN_chull.obj
│ │ │ │ │ ├── head.obj
│ │ │ │ │ ├── head_camera.obj
│ │ │ │ │ ├── head_camera_chull.obj
│ │ │ │ │ ├── head_chull.obj
│ │ │ │ │ ├── l_foot.obj
│ │ │ │ │ ├── l_foot_chull.obj
│ │ │ │ │ ├── l_lglut.obj
│ │ │ │ │ ├── l_lglut_chull.obj
│ │ │ │ │ ├── l_lleg.obj
│ │ │ │ │ ├── l_lleg_chull.obj
│ │ │ │ │ ├── l_talus.obj
│ │ │ │ │ ├── l_uglut.obj
│ │ │ │ │ ├── l_uglut_chull.obj
│ │ │ │ │ ├── l_uleg.obj
│ │ │ │ │ ├── l_uleg_chull.obj
│ │ │ │ │ ├── ltorso.obj
│ │ │ │ │ ├── mtorso.obj
│ │ │ │ │ ├── pelvis.obj
│ │ │ │ │ ├── pelvis_chull.obj
│ │ │ │ │ ├── r_clav.obj
│ │ │ │ │ ├── r_clav_chull.obj
│ │ │ │ │ ├── r_farm.obj
│ │ │ │ │ ├── r_farm_chull.obj
│ │ │ │ │ ├── r_foot.obj
│ │ │ │ │ ├── r_foot_chull.obj
│ │ │ │ │ ├── r_hand.obj
│ │ │ │ │ ├── r_hand_chull.obj
│ │ │ │ │ ├── r_larm.obj
│ │ │ │ │ ├── r_larm_chull.obj
│ │ │ │ │ ├── r_lglut.obj
│ │ │ │ │ ├── r_lglut_chull.obj
│ │ │ │ │ ├── r_lleg.obj
│ │ │ │ │ ├── r_lleg_chull.obj
│ │ │ │ │ ├── r_scap.obj
│ │ │ │ │ ├── r_scap_chull.obj
│ │ │ │ │ ├── r_talus.obj
│ │ │ │ │ ├── r_uarm.obj
│ │ │ │ │ ├── r_uarm_chull.obj
│ │ │ │ │ ├── r_uglut.obj
│ │ │ │ │ ├── r_uglut_chull.obj
│ │ │ │ │ ├── r_uleg.obj
│ │ │ │ │ ├── r_uleg_chull.obj
│ │ │ │ │ ├── utorso.obj
│ │ │ │ │ └── utorso_chull.obj
│ │ │ │ └── mechanism.jl
│ │ │ ├── block/
│ │ │ │ └── mechanism.jl
│ │ │ ├── block2d/
│ │ │ │ └── mechanism.jl
│ │ │ ├── cartpole/
│ │ │ │ └── mechanism.jl
│ │ │ ├── dzhanibekov/
│ │ │ │ └── mechanism.jl
│ │ │ ├── exoskeleton/
│ │ │ │ ├── dependencies/
│ │ │ │ │ ├── mesh/
│ │ │ │ │ │ ├── eFE_simple.obj
│ │ │ │ │ │ ├── lowerarm.obj
│ │ │ │ │ │ ├── sAA_simple.obj
│ │ │ │ │ │ ├── sFE_simple.obj
│ │ │ │ │ │ ├── sIE_simple.obj
│ │ │ │ │ │ ├── torso.obj
│ │ │ │ │ │ └── upperarm.obj
│ │ │ │ │ └── model.urdf
│ │ │ │ └── mechanism.jl
│ │ │ ├── fourbar/
│ │ │ │ ├── dependencies/
│ │ │ │ │ ├── fourbar.urdf
│ │ │ │ │ └── fourbar_open.urdf
│ │ │ │ └── mechanism.jl
│ │ │ ├── halfcheetah/
│ │ │ │ ├── dependencies/
│ │ │ │ │ └── halfcheetah.urdf
│ │ │ │ └── mechanism.jl
│ │ │ ├── hopper/
│ │ │ │ ├── dependencies/
│ │ │ │ │ └── hopper.urdf
│ │ │ │ └── mechanism.jl
│ │ │ ├── humanoid/
│ │ │ │ ├── dependencies/
│ │ │ │ │ └── humanoid.urdf
│ │ │ │ └── mechanism.jl
│ │ │ ├── include.jl
│ │ │ ├── npendulum/
│ │ │ │ └── mechanism.jl
│ │ │ ├── nslider/
│ │ │ │ └── mechanism.jl
│ │ │ ├── panda/
│ │ │ │ ├── dependencies/
│ │ │ │ │ ├── mesh/
│ │ │ │ │ │ ├── collision/
│ │ │ │ │ │ │ ├── finger.obj
│ │ │ │ │ │ │ ├── hand.obj
│ │ │ │ │ │ │ ├── hand.stl
│ │ │ │ │ │ │ ├── link0.obj
│ │ │ │ │ │ │ ├── link0.stl
│ │ │ │ │ │ │ ├── link1.obj
│ │ │ │ │ │ │ ├── link1.stl
│ │ │ │ │ │ │ ├── link2.obj
│ │ │ │ │ │ │ ├── link2.stl
│ │ │ │ │ │ │ ├── link3.obj
│ │ │ │ │ │ │ ├── link3.stl
│ │ │ │ │ │ │ ├── link4.obj
│ │ │ │ │ │ │ ├── link4.stl
│ │ │ │ │ │ │ ├── link5.obj
│ │ │ │ │ │ │ ├── link5.stl
│ │ │ │ │ │ │ ├── link6.obj
│ │ │ │ │ │ │ ├── link6.stl
│ │ │ │ │ │ │ ├── link7.obj
│ │ │ │ │ │ │ └── link7.stl
│ │ │ │ │ │ └── visual/
│ │ │ │ │ │ ├── finger.dae
│ │ │ │ │ │ ├── hand.dae
│ │ │ │ │ │ ├── link0.dae
│ │ │ │ │ │ ├── link1.dae
│ │ │ │ │ │ ├── link2.dae
│ │ │ │ │ │ ├── link3.dae
│ │ │ │ │ │ ├── link4.dae
│ │ │ │ │ │ ├── link5.dae
│ │ │ │ │ │ ├── link6.dae
│ │ │ │ │ │ └── link7.dae
│ │ │ │ │ ├── panda_end_effector.urdf
│ │ │ │ │ ├── panda_no_end_effector.urdf
│ │ │ │ │ └── panda_original.urdf
│ │ │ │ └── mechanism.jl
│ │ │ ├── pendulum/
│ │ │ │ └── mechanism.jl
│ │ │ ├── quadrotor/
│ │ │ │ ├── dependencies/
│ │ │ │ │ ├── mesh/
│ │ │ │ │ │ ├── pelican.obj
│ │ │ │ │ │ ├── propeller_ccw.obj
│ │ │ │ │ │ └── propeller_cw.obj
│ │ │ │ │ ├── pelican.urdf
│ │ │ │ │ └── pelican_fixed_rotors.urdf
│ │ │ │ └── mechanism.jl
│ │ │ ├── quadruped/
│ │ │ │ ├── dependencies/
│ │ │ │ │ ├── gazebo_a1.urdf
│ │ │ │ │ ├── mesh/
│ │ │ │ │ │ ├── calf.dae
│ │ │ │ │ │ ├── hip.dae
│ │ │ │ │ │ ├── thigh.dae
│ │ │ │ │ │ ├── thigh_mirror.dae
│ │ │ │ │ │ └── trunk.dae
│ │ │ │ │ ├── quadruped.urdf
│ │ │ │ │ ├── quadruped_ones.urdf
│ │ │ │ │ └── quadruped_simple.urdf
│ │ │ │ └── mechanism.jl
│ │ │ ├── raiberthopper/
│ │ │ │ └── mechanism.jl
│ │ │ ├── slider/
│ │ │ │ └── mechanism.jl
│ │ │ ├── snake/
│ │ │ │ └── mechanism.jl
│ │ │ ├── sphere/
│ │ │ │ └── mechanism.jl
│ │ │ ├── tippetop/
│ │ │ │ └── mechanism.jl
│ │ │ ├── twister/
│ │ │ │ └── mechanism.jl
│ │ │ ├── uuv/
│ │ │ │ ├── dependencies/
│ │ │ │ │ ├── mesh/
│ │ │ │ │ │ ├── material.lib
│ │ │ │ │ │ ├── obsrov.mtl
│ │ │ │ │ │ ├── obsrov.obj
│ │ │ │ │ │ ├── red_propcw_minitortuga.obj
│ │ │ │ │ │ └── silver_propcw_minitortuga.obj
│ │ │ │ │ ├── mini_tortuga.urdf
│ │ │ │ │ └── mini_tortuga_fixed_rotors.urdf
│ │ │ │ └── mechanism.jl
│ │ │ ├── walker/
│ │ │ │ ├── dependencies/
│ │ │ │ │ └── walker.urdf
│ │ │ │ └── mechanism.jl
│ │ │ └── youbot/
│ │ │ ├── dependencies/
│ │ │ │ ├── meshes/
│ │ │ │ │ ├── sensors/
│ │ │ │ │ │ ├── asus_xtion_pro_live_base.dae
│ │ │ │ │ │ ├── asus_xtion_pro_live_camera.dae
│ │ │ │ │ │ ├── hokuyo.dae
│ │ │ │ │ │ ├── hokuyo_convex.dae
│ │ │ │ │ │ ├── kinect.dae
│ │ │ │ │ │ ├── kinect.tga
│ │ │ │ │ │ └── microsoft_lifecam.dae
│ │ │ │ │ ├── youbot_arm/
│ │ │ │ │ │ ├── arm0.dae
│ │ │ │ │ │ ├── arm0_convex.dae
│ │ │ │ │ │ ├── arm1.dae
│ │ │ │ │ │ ├── arm1_convex.dae
│ │ │ │ │ │ ├── arm2.dae
│ │ │ │ │ │ ├── arm2_convex.dae
│ │ │ │ │ │ ├── arm3.dae
│ │ │ │ │ │ ├── arm3_convex.dae
│ │ │ │ │ │ ├── arm4.dae
│ │ │ │ │ │ ├── arm4_convex.dae
│ │ │ │ │ │ ├── arm5.dae
│ │ │ │ │ │ └── arm5_convex.dae
│ │ │ │ │ ├── youbot_base/
│ │ │ │ │ │ ├── back_left_wheel.dae
│ │ │ │ │ │ ├── back_right_wheel.dae
│ │ │ │ │ │ ├── base.dae
│ │ │ │ │ │ ├── base_convex.dae
│ │ │ │ │ │ ├── front_left_wheel.dae
│ │ │ │ │ │ └── front_right_wheel.dae
│ │ │ │ │ ├── youbot_gripper/
│ │ │ │ │ │ ├── finger.dae
│ │ │ │ │ │ ├── finger_convex.dae
│ │ │ │ │ │ ├── palm.dae
│ │ │ │ │ │ └── palm_convex.dae
│ │ │ │ │ └── youbot_plate/
│ │ │ │ │ ├── plate.dae
│ │ │ │ │ └── plate_convex.dae
│ │ │ │ ├── youbot.urdf
│ │ │ │ ├── youbot_arm_only.urdf
│ │ │ │ ├── youbot_arm_only_fixed_gripper.urdf
│ │ │ │ ├── youbot_base_only.urdf
│ │ │ │ ├── youbot_base_only_no_wheels.urdf
│ │ │ │ ├── youbot_dual_arm.urdf
│ │ │ │ ├── youbot_fixed_gripper.urdf
│ │ │ │ └── youbot_with_cam3d.urdf
│ │ │ └── mechanism.jl
│ │ ├── mechanisms.jl
│ │ ├── precompile.jl
│ │ └── utilities.jl
│ └── test/
│ ├── Project.toml
│ ├── environments.jl
│ ├── mechanisms.jl
│ └── runtests.jl
├── LICENSE
├── Project.toml
├── README.md
├── activate.jl
├── benchmark/
│ ├── Project.toml
│ ├── benchmarks.jl
│ └── mechanisms_benchmark.jl
├── deps/
│ └── build.jl
├── docs/
│ ├── Project.toml
│ ├── make.jl
│ └── src/
│ ├── api.md
│ ├── background_contact/
│ │ ├── collisions.md
│ │ ├── contact_models.md
│ │ ├── impact.md
│ │ ├── linearized_friction.md
│ │ └── nonlinear_friction.md
│ ├── background_representations/
│ │ ├── gradients.md
│ │ ├── maximal_representation.md
│ │ └── minimal_representation.md
│ ├── background_solver/
│ │ ├── interior_point.md
│ │ └── solver_options.md
│ ├── citing.md
│ ├── contributing.md
│ ├── creating_mechanism/
│ │ ├── environment_existing.md
│ │ ├── mechanism_directly.md
│ │ ├── mechanism_existing.md
│ │ ├── overview.md
│ │ ├── quadruped.md
│ │ └── tippetop.md
│ ├── creating_simulation/
│ │ ├── define_controller.md
│ │ ├── define_simulation.md
│ │ └── simulation_with_gradients.md
│ ├── environments/
│ │ └── overview.md
│ ├── examples/
│ │ ├── reinforcement_learning.md
│ │ ├── simulation.md
│ │ ├── system_identification.md
│ │ └── trajectory_optimization.md
│ └── index.md
├── examples/
│ ├── Project.toml
│ ├── README.md
│ ├── control/
│ │ ├── cartpole_lqr.jl
│ │ ├── pendulum_pid.jl
│ │ └── quadrotor_cascade.jl
│ ├── generate_notebooks.jl
│ ├── learning/
│ │ ├── ant_ars.jl
│ │ ├── cartpole_dqn.jl
│ │ └── quadruped_sampling.jl
│ ├── simulation/
│ │ ├── all_mechanisms.jl
│ │ ├── mechanics/
│ │ │ ├── contact_gradients.jl
│ │ │ ├── energy_momentum_conservation.jl
│ │ │ ├── friction_cone_comparison.jl
│ │ │ └── hard_contact.jl
│ │ ├── object_collision_development/
│ │ │ ├── collision_sphere_box.jl
│ │ │ ├── collision_sphere_capsule.jl
│ │ │ ├── collision_sphere_sphere.jl
│ │ │ └── two_finger_box_control.jl
│ │ ├── pendulum_direct.jl
│ │ ├── pendulum_environment.jl
│ │ └── pendulum_mechanism.jl
│ └── system_identification/
│ ├── data/
│ │ ├── datasets/
│ │ │ ├── real_block.jld2
│ │ │ └── synthetic_sphere.jld2
│ │ └── experiment/
│ │ ├── conversion.jl
│ │ └── tosses_csv/
│ │ ├── 0.csv
│ │ ├── 1.csv
│ │ ├── 10.csv
│ │ ├── 100.csv
│ │ ├── 101.csv
│ │ ├── 102.csv
│ │ ├── 103.csv
│ │ ├── 104.csv
│ │ ├── 105.csv
│ │ ├── 106.csv
│ │ ├── 107.csv
│ │ ├── 108.csv
│ │ ├── 109.csv
│ │ ├── 11.csv
│ │ ├── 110.csv
│ │ ├── 111.csv
│ │ ├── 112.csv
│ │ ├── 113.csv
│ │ ├── 114.csv
│ │ ├── 115.csv
│ │ ├── 116.csv
│ │ ├── 117.csv
│ │ ├── 118.csv
│ │ ├── 119.csv
│ │ ├── 12.csv
│ │ ├── 120.csv
│ │ ├── 121.csv
│ │ ├── 122.csv
│ │ ├── 123.csv
│ │ ├── 124.csv
│ │ ├── 125.csv
│ │ ├── 126.csv
│ │ ├── 127.csv
│ │ ├── 128.csv
│ │ ├── 129.csv
│ │ ├── 13.csv
│ │ ├── 130.csv
│ │ ├── 131.csv
│ │ ├── 132.csv
│ │ ├── 133.csv
│ │ ├── 134.csv
│ │ ├── 135.csv
│ │ ├── 136.csv
│ │ ├── 137.csv
│ │ ├── 138.csv
│ │ ├── 139.csv
│ │ ├── 14.csv
│ │ ├── 140.csv
│ │ ├── 141.csv
│ │ ├── 142.csv
│ │ ├── 143.csv
│ │ ├── 144.csv
│ │ ├── 145.csv
│ │ ├── 146.csv
│ │ ├── 147.csv
│ │ ├── 148.csv
│ │ ├── 149.csv
│ │ ├── 15.csv
│ │ ├── 150.csv
│ │ ├── 151.csv
│ │ ├── 152.csv
│ │ ├── 153.csv
│ │ ├── 154.csv
│ │ ├── 155.csv
│ │ ├── 156.csv
│ │ ├── 157.csv
│ │ ├── 158.csv
│ │ ├── 159.csv
│ │ ├── 16.csv
│ │ ├── 160.csv
│ │ ├── 161.csv
│ │ ├── 162.csv
│ │ ├── 163.csv
│ │ ├── 164.csv
│ │ ├── 165.csv
│ │ ├── 166.csv
│ │ ├── 167.csv
│ │ ├── 168.csv
│ │ ├── 169.csv
│ │ ├── 17.csv
│ │ ├── 170.csv
│ │ ├── 171.csv
│ │ ├── 172.csv
│ │ ├── 173.csv
│ │ ├── 174.csv
│ │ ├── 175.csv
│ │ ├── 176.csv
│ │ ├── 177.csv
│ │ ├── 178.csv
│ │ ├── 179.csv
│ │ ├── 18.csv
│ │ ├── 180.csv
│ │ ├── 181.csv
│ │ ├── 182.csv
│ │ ├── 183.csv
│ │ ├── 184.csv
│ │ ├── 185.csv
│ │ ├── 186.csv
│ │ ├── 187.csv
│ │ ├── 188.csv
│ │ ├── 189.csv
│ │ ├── 19.csv
│ │ ├── 190.csv
│ │ ├── 191.csv
│ │ ├── 192.csv
│ │ ├── 193.csv
│ │ ├── 194.csv
│ │ ├── 195.csv
│ │ ├── 196.csv
│ │ ├── 197.csv
│ │ ├── 198.csv
│ │ ├── 199.csv
│ │ ├── 2.csv
│ │ ├── 20.csv
│ │ ├── 200.csv
│ │ ├── 201.csv
│ │ ├── 202.csv
│ │ ├── 203.csv
│ │ ├── 204.csv
│ │ ├── 205.csv
│ │ ├── 206.csv
│ │ ├── 207.csv
│ │ ├── 208.csv
│ │ ├── 209.csv
│ │ ├── 21.csv
│ │ ├── 210.csv
│ │ ├── 211.csv
│ │ ├── 212.csv
│ │ ├── 213.csv
│ │ ├── 214.csv
│ │ ├── 215.csv
│ │ ├── 216.csv
│ │ ├── 217.csv
│ │ ├── 218.csv
│ │ ├── 219.csv
│ │ ├── 22.csv
│ │ ├── 220.csv
│ │ ├── 221.csv
│ │ ├── 222.csv
│ │ ├── 223.csv
│ │ ├── 224.csv
│ │ ├── 225.csv
│ │ ├── 226.csv
│ │ ├── 227.csv
│ │ ├── 228.csv
│ │ ├── 229.csv
│ │ ├── 23.csv
│ │ ├── 230.csv
│ │ ├── 231.csv
│ │ ├── 232.csv
│ │ ├── 233.csv
│ │ ├── 234.csv
│ │ ├── 235.csv
│ │ ├── 236.csv
│ │ ├── 237.csv
│ │ ├── 238.csv
│ │ ├── 239.csv
│ │ ├── 24.csv
│ │ ├── 240.csv
│ │ ├── 241.csv
│ │ ├── 242.csv
│ │ ├── 243.csv
│ │ ├── 244.csv
│ │ ├── 245.csv
│ │ ├── 246.csv
│ │ ├── 247.csv
│ │ ├── 248.csv
│ │ ├── 249.csv
│ │ ├── 25.csv
│ │ ├── 250.csv
│ │ ├── 251.csv
│ │ ├── 252.csv
│ │ ├── 253.csv
│ │ ├── 254.csv
│ │ ├── 255.csv
│ │ ├── 256.csv
│ │ ├── 257.csv
│ │ ├── 258.csv
│ │ ├── 259.csv
│ │ ├── 26.csv
│ │ ├── 260.csv
│ │ ├── 261.csv
│ │ ├── 262.csv
│ │ ├── 263.csv
│ │ ├── 264.csv
│ │ ├── 265.csv
│ │ ├── 266.csv
│ │ ├── 267.csv
│ │ ├── 268.csv
│ │ ├── 269.csv
│ │ ├── 27.csv
│ │ ├── 270.csv
│ │ ├── 271.csv
│ │ ├── 272.csv
│ │ ├── 273.csv
│ │ ├── 274.csv
│ │ ├── 275.csv
│ │ ├── 276.csv
│ │ ├── 277.csv
│ │ ├── 278.csv
│ │ ├── 279.csv
│ │ ├── 28.csv
│ │ ├── 280.csv
│ │ ├── 281.csv
│ │ ├── 282.csv
│ │ ├── 283.csv
│ │ ├── 284.csv
│ │ ├── 285.csv
│ │ ├── 286.csv
│ │ ├── 287.csv
│ │ ├── 288.csv
│ │ ├── 289.csv
│ │ ├── 29.csv
│ │ ├── 290.csv
│ │ ├── 291.csv
│ │ ├── 292.csv
│ │ ├── 293.csv
│ │ ├── 294.csv
│ │ ├── 295.csv
│ │ ├── 296.csv
│ │ ├── 297.csv
│ │ ├── 298.csv
│ │ ├── 299.csv
│ │ ├── 3.csv
│ │ ├── 30.csv
│ │ ├── 300.csv
│ │ ├── 301.csv
│ │ ├── 302.csv
│ │ ├── 303.csv
│ │ ├── 304.csv
│ │ ├── 305.csv
│ │ ├── 306.csv
│ │ ├── 307.csv
│ │ ├── 308.csv
│ │ ├── 309.csv
│ │ ├── 31.csv
│ │ ├── 310.csv
│ │ ├── 311.csv
│ │ ├── 312.csv
│ │ ├── 313.csv
│ │ ├── 314.csv
│ │ ├── 315.csv
│ │ ├── 316.csv
│ │ ├── 317.csv
│ │ ├── 318.csv
│ │ ├── 319.csv
│ │ ├── 32.csv
│ │ ├── 320.csv
│ │ ├── 321.csv
│ │ ├── 322.csv
│ │ ├── 323.csv
│ │ ├── 324.csv
│ │ ├── 325.csv
│ │ ├── 326.csv
│ │ ├── 327.csv
│ │ ├── 328.csv
│ │ ├── 329.csv
│ │ ├── 33.csv
│ │ ├── 330.csv
│ │ ├── 331.csv
│ │ ├── 332.csv
│ │ ├── 333.csv
│ │ ├── 334.csv
│ │ ├── 335.csv
│ │ ├── 336.csv
│ │ ├── 337.csv
│ │ ├── 338.csv
│ │ ├── 339.csv
│ │ ├── 34.csv
│ │ ├── 340.csv
│ │ ├── 341.csv
│ │ ├── 342.csv
│ │ ├── 343.csv
│ │ ├── 344.csv
│ │ ├── 345.csv
│ │ ├── 346.csv
│ │ ├── 347.csv
│ │ ├── 348.csv
│ │ ├── 349.csv
│ │ ├── 35.csv
│ │ ├── 350.csv
│ │ ├── 351.csv
│ │ ├── 352.csv
│ │ ├── 353.csv
│ │ ├── 354.csv
│ │ ├── 355.csv
│ │ ├── 356.csv
│ │ ├── 357.csv
│ │ ├── 358.csv
│ │ ├── 359.csv
│ │ ├── 36.csv
│ │ ├── 360.csv
│ │ ├── 361.csv
│ │ ├── 362.csv
│ │ ├── 363.csv
│ │ ├── 364.csv
│ │ ├── 365.csv
│ │ ├── 366.csv
│ │ ├── 367.csv
│ │ ├── 368.csv
│ │ ├── 369.csv
│ │ ├── 37.csv
│ │ ├── 370.csv
│ │ ├── 371.csv
│ │ ├── 372.csv
│ │ ├── 373.csv
│ │ ├── 374.csv
│ │ ├── 375.csv
│ │ ├── 376.csv
│ │ ├── 377.csv
│ │ ├── 378.csv
│ │ ├── 379.csv
│ │ ├── 38.csv
│ │ ├── 380.csv
│ │ ├── 381.csv
│ │ ├── 382.csv
│ │ ├── 383.csv
│ │ ├── 384.csv
│ │ ├── 385.csv
│ │ ├── 386.csv
│ │ ├── 387.csv
│ │ ├── 388.csv
│ │ ├── 389.csv
│ │ ├── 39.csv
│ │ ├── 390.csv
│ │ ├── 391.csv
│ │ ├── 392.csv
│ │ ├── 393.csv
│ │ ├── 394.csv
│ │ ├── 395.csv
│ │ ├── 396.csv
│ │ ├── 397.csv
│ │ ├── 398.csv
│ │ ├── 399.csv
│ │ ├── 4.csv
│ │ ├── 40.csv
│ │ ├── 400.csv
│ │ ├── 401.csv
│ │ ├── 402.csv
│ │ ├── 403.csv
│ │ ├── 404.csv
│ │ ├── 405.csv
│ │ ├── 406.csv
│ │ ├── 407.csv
│ │ ├── 408.csv
│ │ ├── 409.csv
│ │ ├── 41.csv
│ │ ├── 410.csv
│ │ ├── 411.csv
│ │ ├── 412.csv
│ │ ├── 413.csv
│ │ ├── 414.csv
│ │ ├── 415.csv
│ │ ├── 416.csv
│ │ ├── 417.csv
│ │ ├── 418.csv
│ │ ├── 419.csv
│ │ ├── 42.csv
│ │ ├── 420.csv
│ │ ├── 421.csv
│ │ ├── 422.csv
│ │ ├── 423.csv
│ │ ├── 424.csv
│ │ ├── 425.csv
│ │ ├── 426.csv
│ │ ├── 427.csv
│ │ ├── 428.csv
│ │ ├── 429.csv
│ │ ├── 43.csv
│ │ ├── 430.csv
│ │ ├── 431.csv
│ │ ├── 432.csv
│ │ ├── 433.csv
│ │ ├── 434.csv
│ │ ├── 435.csv
│ │ ├── 436.csv
│ │ ├── 437.csv
│ │ ├── 438.csv
│ │ ├── 439.csv
│ │ ├── 44.csv
│ │ ├── 440.csv
│ │ ├── 441.csv
│ │ ├── 442.csv
│ │ ├── 443.csv
│ │ ├── 444.csv
│ │ ├── 445.csv
│ │ ├── 446.csv
│ │ ├── 447.csv
│ │ ├── 448.csv
│ │ ├── 449.csv
│ │ ├── 45.csv
│ │ ├── 450.csv
│ │ ├── 451.csv
│ │ ├── 452.csv
│ │ ├── 453.csv
│ │ ├── 454.csv
│ │ ├── 455.csv
│ │ ├── 456.csv
│ │ ├── 457.csv
│ │ ├── 458.csv
│ │ ├── 459.csv
│ │ ├── 46.csv
│ │ ├── 460.csv
│ │ ├── 461.csv
│ │ ├── 462.csv
│ │ ├── 463.csv
│ │ ├── 464.csv
│ │ ├── 465.csv
│ │ ├── 466.csv
│ │ ├── 467.csv
│ │ ├── 468.csv
│ │ ├── 469.csv
│ │ ├── 47.csv
│ │ ├── 470.csv
│ │ ├── 471.csv
│ │ ├── 472.csv
│ │ ├── 473.csv
│ │ ├── 474.csv
│ │ ├── 475.csv
│ │ ├── 476.csv
│ │ ├── 477.csv
│ │ ├── 478.csv
│ │ ├── 479.csv
│ │ ├── 48.csv
│ │ ├── 480.csv
│ │ ├── 481.csv
│ │ ├── 482.csv
│ │ ├── 483.csv
│ │ ├── 484.csv
│ │ ├── 485.csv
│ │ ├── 486.csv
│ │ ├── 487.csv
│ │ ├── 488.csv
│ │ ├── 489.csv
│ │ ├── 49.csv
│ │ ├── 490.csv
│ │ ├── 491.csv
│ │ ├── 492.csv
│ │ ├── 493.csv
│ │ ├── 494.csv
│ │ ├── 495.csv
│ │ ├── 496.csv
│ │ ├── 497.csv
│ │ ├── 498.csv
│ │ ├── 499.csv
│ │ ├── 5.csv
│ │ ├── 50.csv
│ │ ├── 500.csv
│ │ ├── 501.csv
│ │ ├── 502.csv
│ │ ├── 503.csv
│ │ ├── 504.csv
│ │ ├── 505.csv
│ │ ├── 506.csv
│ │ ├── 507.csv
│ │ ├── 508.csv
│ │ ├── 509.csv
│ │ ├── 51.csv
│ │ ├── 510.csv
│ │ ├── 511.csv
│ │ ├── 512.csv
│ │ ├── 513.csv
│ │ ├── 514.csv
│ │ ├── 515.csv
│ │ ├── 516.csv
│ │ ├── 517.csv
│ │ ├── 518.csv
│ │ ├── 519.csv
│ │ ├── 52.csv
│ │ ├── 520.csv
│ │ ├── 521.csv
│ │ ├── 522.csv
│ │ ├── 523.csv
│ │ ├── 524.csv
│ │ ├── 525.csv
│ │ ├── 526.csv
│ │ ├── 527.csv
│ │ ├── 528.csv
│ │ ├── 529.csv
│ │ ├── 53.csv
│ │ ├── 530.csv
│ │ ├── 531.csv
│ │ ├── 532.csv
│ │ ├── 533.csv
│ │ ├── 534.csv
│ │ ├── 535.csv
│ │ ├── 536.csv
│ │ ├── 537.csv
│ │ ├── 538.csv
│ │ ├── 539.csv
│ │ ├── 54.csv
│ │ ├── 540.csv
│ │ ├── 541.csv
│ │ ├── 542.csv
│ │ ├── 543.csv
│ │ ├── 544.csv
│ │ ├── 545.csv
│ │ ├── 546.csv
│ │ ├── 547.csv
│ │ ├── 548.csv
│ │ ├── 549.csv
│ │ ├── 55.csv
│ │ ├── 550.csv
│ │ ├── 551.csv
│ │ ├── 552.csv
│ │ ├── 553.csv
│ │ ├── 554.csv
│ │ ├── 555.csv
│ │ ├── 556.csv
│ │ ├── 557.csv
│ │ ├── 558.csv
│ │ ├── 559.csv
│ │ ├── 56.csv
│ │ ├── 560.csv
│ │ ├── 561.csv
│ │ ├── 562.csv
│ │ ├── 563.csv
│ │ ├── 564.csv
│ │ ├── 565.csv
│ │ ├── 566.csv
│ │ ├── 567.csv
│ │ ├── 568.csv
│ │ ├── 569.csv
│ │ ├── 57.csv
│ │ ├── 58.csv
│ │ ├── 59.csv
│ │ ├── 6.csv
│ │ ├── 60.csv
│ │ ├── 61.csv
│ │ ├── 62.csv
│ │ ├── 63.csv
│ │ ├── 64.csv
│ │ ├── 65.csv
│ │ ├── 66.csv
│ │ ├── 67.csv
│ │ ├── 68.csv
│ │ ├── 69.csv
│ │ ├── 7.csv
│ │ ├── 70.csv
│ │ ├── 71.csv
│ │ ├── 72.csv
│ │ ├── 73.csv
│ │ ├── 74.csv
│ │ ├── 75.csv
│ │ ├── 76.csv
│ │ ├── 77.csv
│ │ ├── 78.csv
│ │ ├── 79.csv
│ │ ├── 8.csv
│ │ ├── 80.csv
│ │ ├── 81.csv
│ │ ├── 82.csv
│ │ ├── 83.csv
│ │ ├── 84.csv
│ │ ├── 85.csv
│ │ ├── 86.csv
│ │ ├── 87.csv
│ │ ├── 88.csv
│ │ ├── 89.csv
│ │ ├── 9.csv
│ │ ├── 90.csv
│ │ ├── 91.csv
│ │ ├── 92.csv
│ │ ├── 93.csv
│ │ ├── 94.csv
│ │ ├── 95.csv
│ │ ├── 96.csv
│ │ ├── 97.csv
│ │ ├── 98.csv
│ │ └── 99.csv
│ ├── real_block.jl
│ ├── synthetic_sphere.jl
│ └── utilities.jl
├── src/
│ ├── Dojo.jl
│ ├── bodies/
│ │ ├── constructor.jl
│ │ ├── origin.jl
│ │ ├── set.jl
│ │ ├── shapes.jl
│ │ └── state.jl
│ ├── contacts/
│ │ ├── collisions/
│ │ │ ├── capsule_capsule.jl
│ │ │ ├── collision.jl
│ │ │ ├── point_to_box.jl
│ │ │ ├── point_to_box_v2.jl
│ │ │ ├── point_to_segment.jl
│ │ │ ├── sphere_box.jl
│ │ │ ├── sphere_capsule.jl
│ │ │ ├── sphere_halfspace.jl
│ │ │ ├── sphere_sphere.jl
│ │ │ └── string.jl
│ │ ├── cone.jl
│ │ ├── constraints.jl
│ │ ├── constructor.jl
│ │ ├── contact.jl
│ │ ├── impact.jl
│ │ ├── linear.jl
│ │ ├── nonlinear.jl
│ │ ├── utilities.jl
│ │ └── velocity.jl
│ ├── gradients/
│ │ ├── contact.jl
│ │ ├── data.jl
│ │ ├── finite_difference.jl
│ │ ├── state.jl
│ │ └── utilities.jl
│ ├── integrators/
│ │ ├── constraint.jl
│ │ └── integrator.jl
│ ├── joints/
│ │ ├── constraints.jl
│ │ ├── impulses.jl
│ │ ├── joint.jl
│ │ ├── limits.jl
│ │ ├── minimal.jl
│ │ ├── orthogonal.jl
│ │ ├── prototypes.jl
│ │ ├── rotational/
│ │ │ ├── constructor.jl
│ │ │ ├── dampers.jl
│ │ │ ├── impulses.jl
│ │ │ ├── input.jl
│ │ │ ├── minimal.jl
│ │ │ └── springs.jl
│ │ └── translational/
│ │ ├── constructor.jl
│ │ ├── dampers.jl
│ │ ├── impulses.jl
│ │ ├── input.jl
│ │ ├── minimal.jl
│ │ └── springs.jl
│ ├── mechanics/
│ │ ├── energy.jl
│ │ └── momentum.jl
│ ├── mechanism/
│ │ ├── constructor.jl
│ │ ├── data.jl
│ │ ├── edge.jl
│ │ ├── get.jl
│ │ ├── gravity.jl
│ │ ├── id.jl
│ │ ├── methods.jl
│ │ ├── node.jl
│ │ ├── set.jl
│ │ ├── state.jl
│ │ ├── system.jl
│ │ ├── traversal.jl
│ │ └── urdf.jl
│ ├── orientation/
│ │ ├── axis_angle.jl
│ │ ├── mapping.jl
│ │ ├── mrp.jl
│ │ ├── quaternion.jl
│ │ └── rotate.jl
│ ├── precompile.jl
│ ├── simulation/
│ │ ├── simulate.jl
│ │ ├── step.jl
│ │ └── storage.jl
│ ├── solver/
│ │ ├── centering.jl
│ │ ├── complementarity.jl
│ │ ├── correction.jl
│ │ ├── initialization.jl
│ │ ├── line_search.jl
│ │ ├── linear_system.jl
│ │ ├── mehrotra.jl
│ │ ├── options.jl
│ │ └── violations.jl
│ ├── utilities/
│ │ ├── custom_static.jl
│ │ ├── methods.jl
│ │ └── normalize.jl
│ └── visuals/
│ ├── colors.jl
│ ├── convert.jl
│ ├── rope.jl
│ ├── set.jl
│ └── visualizer.jl
└── test/
├── Project.toml
├── behaviors.jl
├── bodies.jl
├── collisions.jl
├── damper.jl
├── data.jl
├── energy.jl
├── impulse_map.jl
├── integrator.jl
├── jacobian.jl
├── joint_limits.jl
├── mechanism.jl
├── minimal.jl
├── momentum.jl
├── mrp.jl
├── runtests.jl
├── simulate.jl
├── state.jl
├── utilities.jl
└── visuals.jl
Copy disabled (too large)
Download .json
Condensed preview — 962 files, each showing path, character count, and a content snippet. Download the .json file for the full structured content (74,297K chars).
[
{
"path": ".github/workflows/CI.yml",
"chars": 1068,
"preview": "name: CI\non:\n push:\n branches:\n - main\n tags: '*'\n pull_request:\njobs:\n test:\n name: Julia ${{ matrix.v"
},
{
"path": ".github/workflows/CI_DojoEnv.yml",
"chars": 1097,
"preview": "name: CI DojoEnvironments\non:\n push:\n branches:\n - main\n tags: '*'\n pull_request:\njobs:\n test:\n name: J"
},
{
"path": ".github/workflows/Documenter.yml",
"chars": 689,
"preview": "name: Documentation\n\non:\n push:\n branches:\n - main\n tags: '*'\n pull_request:\n\njobs:\n build:\n runs-on: u"
},
{
"path": ".github/workflows/TagBot.yml",
"chars": 362,
"preview": "name: TagBot\non:\n issue_comment:\n types:\n - created\n workflow_dispatch:\njobs:\n TagBot:\n if: github.event_n"
},
{
"path": ".github/workflows/benchmark.yml",
"chars": 666,
"preview": "name: Run benchmarks\n\non:\n pull_request:\n\njobs:\n Benchmark:\n runs-on: ubuntu-latest\n steps:\n - uses: action"
},
{
"path": ".github/workflows/dailyCI.yml",
"chars": 1005,
"preview": "name: dailyCI\non:\n schedule:\n# Every day at 10AM and 10PM UTC\n# - cron: '00 10,22 * * *'\n# Every Monday a"
},
{
"path": ".gitignore",
"chars": 888,
"preview": "# Files from local benchmarking\nbenchmark/tune.json\n\n# Files generated by invoking Julia with --code-coverage\n*.jl.cov\n*"
},
{
"path": "CITATION.bib",
"chars": 316,
"preview": "\n@article{howell_lecleach2022,\n\ttitle={Dojo: A Differentiable Simulator for Robotics},\n\tauthor={Howell, Taylor and Le Cl"
},
{
"path": "DojoEnvironments/LICENSE",
"chars": 1116,
"preview": "Copyright (c) 2023 Simon Le Cleac'h & Taylor Howell & Jan Bruedigam, and Zachary Manchester\n\nPermission is hereby grante"
},
{
"path": "DojoEnvironments/Project.toml",
"chars": 559,
"preview": "name = \"DojoEnvironments\"\nuuid = \"8728bab7-e8ae-46fd-a1b6-a8c4d00f608e\"\nauthors = [\"simonlc <simonlc@stanford.edu>\", \"th"
},
{
"path": "DojoEnvironments/README.md",
"chars": 206,
"preview": "# DojoEnvironments\nThis package contains convenience mechanisms and environments for the [Dojo.jl](https://github.com/do"
},
{
"path": "DojoEnvironments/src/DojoEnvironments.jl",
"chars": 571,
"preview": "module DojoEnvironments\n\nusing LinearAlgebra\nusing Random\n\nusing Dojo\nimport Dojo: string_to_symbol, add_limits, RotX, R"
},
{
"path": "DojoEnvironments/src/environments/ant_ars.jl",
"chars": 2137,
"preview": "mutable struct AntARS{T,N} <: Environment{T,N}\n mechanism::Mechanism{T}\n storage::Storage{T,N}\nend\n\nfunction ant_a"
},
{
"path": "DojoEnvironments/src/environments/cartpole_dqn.jl",
"chars": 1562,
"preview": "mutable struct CartpoleDQN{T,N} <: Environment{T,N}\n mechanism::Mechanism{T}\n storage::Storage{T,N}\nend\n\nfunction "
},
{
"path": "DojoEnvironments/src/environments/include.jl",
"chars": 227,
"preview": "include(\"ant_ars.jl\")\ninclude(\"cartpole_dqn.jl\")\ninclude(\"pendulum.jl\")\ninclude(\"quadruped_waypoint.jl\")\ninclude(\"quadru"
},
{
"path": "DojoEnvironments/src/environments/pendulum.jl",
"chars": 1456,
"preview": "mutable struct Pendulum{T,N} <: Environment{T,N}\n mechanism::Mechanism{T}\n storage::Storage{T,N}\nend\n\nfunction pen"
},
{
"path": "DojoEnvironments/src/environments/quadrotor_waypoint.jl",
"chars": 4971,
"preview": "mutable struct QuadrotorWaypoint{T,N} <: Environment{T,N}\n mechanism::Mechanism{T}\n storage::Storage{T,N}\n\n rpm"
},
{
"path": "DojoEnvironments/src/environments/quadruped_sampling.jl",
"chars": 1957,
"preview": "mutable struct QuadrupedSampling{T,N} <: Environment{T,N}\n mechanism::Mechanism{T}\n storage::Storage{T,N}\nend\n\nfun"
},
{
"path": "DojoEnvironments/src/environments/quadruped_waypoint.jl",
"chars": 2814,
"preview": "mutable struct QuadrupedWaypoint{T,N} <: Environment{T,N}\n mechanism::Mechanism{T}\n storage::Storage{T,N}\nend\n\nfun"
},
{
"path": "DojoEnvironments/src/environments/uuv_waypoint.jl",
"chars": 5589,
"preview": "mutable struct UUVWaypoint{T,N} <: Environment{T,N}\n mechanism::Mechanism{T}\n storage::Storage{T,N}\n\n rpms::Abs"
},
{
"path": "DojoEnvironments/src/environments/youbot_waypoint.jl",
"chars": 5082,
"preview": "mutable struct YoubotWaypoint{T,N} <: Environment{T,N}\n mechanism::Mechanism{T}\n storage::Storage{T,N}\nend\n\nfuncti"
},
{
"path": "DojoEnvironments/src/environments.jl",
"chars": 3499,
"preview": "\"\"\"\n Environment{T,N} \n\n abstract type for an environment consisting of a mechanism and a storage\n\"\"\"\nabstract typ"
},
{
"path": "DojoEnvironments/src/mechanisms/ant/dependencies/ant.urdf",
"chars": 11368,
"preview": "<?xml version=\"1.0\" ?>\n<robot name=\"ant\">\n\t<link name=\"torso\">\n\t\t<inertial>\n\t\t\t<origin rpy=\"0.00000 -0.00000 0.00000\" xy"
},
{
"path": "DojoEnvironments/src/mechanisms/ant/mechanism.jl",
"chars": 3912,
"preview": "function get_ant(; \n timestep=0.05, \n input_scaling=timestep,\n gravity=-9.81, \n urdf=:ant,\n springs=0, \n "
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/atlas.urdf",
"chars": 63713,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/atlas_armless.urdf",
"chars": 33782,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/atlas_complex.urdf",
"chars": 62333,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/atlas_fast.urdf",
"chars": 19031,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/atlas_ones.urdf",
"chars": 24849,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/atlas_simple.urdf",
"chars": 26642,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/atlas_v2.urdf",
"chars": 24439,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/GRIPPER_OPEN_chull.obj",
"chars": 1308,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object GRIPPER_OPEN_chull.obj\n#\n# Vertices: 12\n# Faces: 20\n#\n####\nvn 0.7"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/head.obj",
"chars": 141123,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object head.obj\n#\n# Vertices: 1737\n# Faces: 1099\n#\n####\nvn 0.000000 0.74"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/head_camera.obj",
"chars": 36890,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object head_camera.obj\n#\n# Vertices: 480\n# Faces: 256\n#\n####\nvn 0.000000"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/head_camera_chull.obj",
"chars": 1299,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object head_camera_chull.obj\n#\n# Vertices: 12\n# Faces: 20\n#\n####\nvn 0.57"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/head_chull.obj",
"chars": 1306,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object head_chull.obj\n#\n# Vertices: 12\n# Faces: 20\n#\n####\nvn 0.601828 -0"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/l_foot.obj",
"chars": 16955,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object l_foot.obj\n#\n# Vertices: 319\n# Faces: 536\n#\n####\nv 0.006721 0.023"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/l_foot_chull.obj",
"chars": 1521,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object l_foot_chull.obj\n#\n# Vertices: 27\n# Faces: 50\n#\n####\nv 0.032478 -"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/l_lglut.obj",
"chars": 24945,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object l_lglut.obj\n#\n# Vertices: 446\n# Faces: 847\n#\n####\nv 0.049216 0.01"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/l_lglut_chull.obj",
"chars": 1509,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object l_lglut_chull.obj\n#\n# Vertices: 27\n# Faces: 50\n#\n####\nv -0.047214"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/l_lleg.obj",
"chars": 45248,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object l_lleg.obj\n#\n# Vertices: 809\n# Faces: 1494\n#\n####\nv -0.014057 0.0"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/l_lleg_chull.obj",
"chars": 1517,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object l_lleg_chull.obj\n#\n# Vertices: 27\n# Faces: 50\n#\n####\nv 0.005717 -"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/l_talus.obj",
"chars": 1519,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object l_talus.obj\n#\n# Vertices: 28\n# Faces: 48\n#\n####\nv 0.006590 -0.015"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/l_uglut.obj",
"chars": 13549,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object l_uglut.obj\n#\n# Vertices: 238\n# Faces: 486\n#\n####\nv 0.033190 0.01"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/l_uglut_chull.obj",
"chars": 1510,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object l_uglut_chull.obj\n#\n# Vertices: 27\n# Faces: 50\n#\n####\nv -0.010540"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/l_uleg.obj",
"chars": 74230,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object l_uleg.obj\n#\n# Vertices: 1246\n# Faces: 2464\n#\n####\nv -0.051197 0."
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/l_uleg_chull.obj",
"chars": 1523,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object l_uleg_chull.obj\n#\n# Vertices: 27\n# Faces: 50\n#\n####\nv -0.128024 "
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/ltorso.obj",
"chars": 6975,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object ltorso.obj\n#\n# Vertices: 132\n# Faces: 244\n#\n####\nv 0.021511 0.019"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/mtorso.obj",
"chars": 2381,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object mtorso.obj\n#\n# Vertices: 44\n# Faces: 84\n#\n####\nv 0.016322 0.01685"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/pelvis.obj",
"chars": 63660,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object pelvis.obj\n#\n# Vertices: 1174\n# Faces: 1978\n#\n####\nv -0.004260 0."
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/pelvis_chull.obj",
"chars": 1506,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object pelvis_chull.obj\n#\n# Vertices: 27\n# Faces: 50\n#\n####\nv 0.147139 0"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/r_clav.obj",
"chars": 17567,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object r_clav.obj\n#\n# Vertices: 326\n# Faces: 556\n#\n####\nv -0.054377 -0.1"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/r_clav_chull.obj",
"chars": 1532,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object r_clav_chull.obj\n#\n# Vertices: 27\n# Faces: 50\n#\n####\nv 0.062902 -"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/r_farm.obj",
"chars": 5335,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object r_farm.obj\n#\n# Vertices: 100\n# Faces: 196\n#\n####\nv 0.062026 0.013"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/r_farm_chull.obj",
"chars": 1509,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object r_farm_chull.obj\n#\n# Vertices: 27\n# Faces: 50\n#\n####\nv 0.061657 0"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/r_foot.obj",
"chars": 17020,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object r_foot.obj\n#\n# Vertices: 319\n# Faces: 540\n#\n####\nv 0.006721 -0.02"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/r_foot_chull.obj",
"chars": 1518,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object r_foot_chull.obj\n#\n# Vertices: 27\n# Faces: 50\n#\n####\nv 0.032478 0"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/r_hand.obj",
"chars": 8224,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object r_hand.obj\n#\n# Vertices: 148\n# Faces: 292\n#\n####\nv -0.009549 -0.1"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/r_hand_chull.obj",
"chars": 1515,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object r_hand_chull.obj\n#\n# Vertices: 27\n# Faces: 50\n#\n####\nv 0.044882 -"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/r_larm.obj",
"chars": 12056,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object r_larm.obj\n#\n# Vertices: 217\n# Faces: 408\n#\n####\nv 0.048129 -0.09"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/r_larm_chull.obj",
"chars": 1521,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object r_larm_chull.obj\n#\n# Vertices: 27\n# Faces: 50\n#\n####\nv -0.067883 "
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/r_lglut.obj",
"chars": 25243,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object r_lglut.obj\n#\n# Vertices: 446\n# Faces: 847\n#\n####\nv 0.049216 -0.0"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/r_lglut_chull.obj",
"chars": 1518,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object r_lglut_chull.obj\n#\n# Vertices: 27\n# Faces: 50\n#\n####\nv -0.047223"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/r_lleg.obj",
"chars": 45325,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object r_lleg.obj\n#\n# Vertices: 809\n# Faces: 1494\n#\n####\nv -0.014057 -0."
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/r_lleg_chull.obj",
"chars": 1520,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object r_lleg_chull.obj\n#\n# Vertices: 27\n# Faces: 50\n#\n####\nv 0.005717 0"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/r_scap.obj",
"chars": 20748,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object r_scap.obj\n#\n# Vertices: 380\n# Faces: 671\n#\n####\nv 0.049425 -0.00"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/r_scap_chull.obj",
"chars": 1516,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object r_scap_chull.obj\n#\n# Vertices: 27\n# Faces: 50\n#\n####\nv 0.072767 -"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/r_talus.obj",
"chars": 1521,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object r_talus.obj\n#\n# Vertices: 28\n# Faces: 48\n#\n####\nv 0.006590 0.0159"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/r_uarm.obj",
"chars": 16129,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object r_uarm.obj\n#\n# Vertices: 288\n# Faces: 546\n#\n####\nv -0.084211 -0.0"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/r_uarm_chull.obj",
"chars": 1516,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object r_uarm_chull.obj\n#\n# Vertices: 27\n# Faces: 50\n#\n####\nv 0.050910 -"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/r_uglut.obj",
"chars": 13627,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object r_uglut.obj\n#\n# Vertices: 238\n# Faces: 488\n#\n####\nv 0.033190 -0.0"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/r_uglut_chull.obj",
"chars": 1513,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object r_uglut_chull.obj\n#\n# Vertices: 27\n# Faces: 50\n#\n####\nv -0.010540"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/r_uleg.obj",
"chars": 74288,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object r_uleg.obj\n#\n# Vertices: 1246\n# Faces: 2464\n#\n####\nv -0.051197 -0"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/r_uleg_chull.obj",
"chars": 1523,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object r_uleg_chull.obj\n#\n# Vertices: 27\n# Faces: 50\n#\n####\nv -0.128024 "
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/utorso.obj",
"chars": 228933,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object utorso.obj\n#\n# Vertices: 3894\n# Faces: 6964\n#\n####\nv -0.024319 0."
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/dependencies/mesh/utorso_chull.obj",
"chars": 1494,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object utorso_chull.obj\n#\n# Vertices: 27\n# Faces: 50\n#\n####\nv 0.032539 -"
},
{
"path": "DojoEnvironments/src/mechanisms/atlas/mechanism.jl",
"chars": 4130,
"preview": "function get_atlas(; \n timestep=0.01, \n input_scaling=timestep, \n gravity=-9.81, \n urdf=:atlas_simple,\n s"
},
{
"path": "DojoEnvironments/src/mechanisms/block/mechanism.jl",
"chars": 2907,
"preview": "function get_block(;\n timestep=0.01,\n input_scaling=timestep, \n gravity=-9.81,\n mass=1,\n edge_length=0.5,"
},
{
"path": "DojoEnvironments/src/mechanisms/block2d/mechanism.jl",
"chars": 2606,
"preview": "function get_block2d(;\n timestep=0.01,\n input_scaling=timestep, \n gravity=-9.81,\n mass=1,\n edge_length=0."
},
{
"path": "DojoEnvironments/src/mechanisms/cartpole/mechanism.jl",
"chars": 1716,
"preview": "function get_cartpole(; \n timestep=0.01, \n input_scaling=timestep, \n gravity=-9.81, \n slider_mass=1,\n pen"
},
{
"path": "DojoEnvironments/src/mechanisms/dzhanibekov/mechanism.jl",
"chars": 1714,
"preview": "function get_dzhanibekov(;\n timestep=0.01,\n input_scaling=timestep, \n gravity=0,\n springs=0,\n dampers=0, "
},
{
"path": "DojoEnvironments/src/mechanisms/exoskeleton/dependencies/mesh/eFE_simple.obj",
"chars": 45761,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object eFE_very_simple.obj\n#\n# Vertices: 383\n# Faces: 762\n#\n####\n\nvn -0."
},
{
"path": "DojoEnvironments/src/mechanisms/exoskeleton/dependencies/mesh/lowerarm.obj",
"chars": 1391873,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object lowerarm.obj\n#\n# Vertices: 10539\n# Faces: 21074\n#\n####\n\nvn 3.1022"
},
{
"path": "DojoEnvironments/src/mechanisms/exoskeleton/dependencies/mesh/sAA_simple.obj",
"chars": 350238,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object sAA_very_simple.obj\n#\n# Vertices: 2683\n# Faces: 5366\n#\n####\n\nvn 0"
},
{
"path": "DojoEnvironments/src/mechanisms/exoskeleton/dependencies/mesh/sFE_simple.obj",
"chars": 56673,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object sFE_very_simple.obj\n#\n# Vertices: 465\n# Faces: 926\n#\n####\n\nvn 1.5"
},
{
"path": "DojoEnvironments/src/mechanisms/exoskeleton/dependencies/mesh/sIE_simple.obj",
"chars": 70636,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object sIE_very_simple.obj\n#\n# Vertices: 577\n# Faces: 1150\n#\n####\n\nvn 3."
},
{
"path": "DojoEnvironments/src/mechanisms/exoskeleton/dependencies/mesh/torso.obj",
"chars": 7460037,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object torso.obj\n#\n# Vertices: 52475\n# Faces: 104884\n#\n####\nvn 0.356032 "
},
{
"path": "DojoEnvironments/src/mechanisms/exoskeleton/dependencies/mesh/upperarm.obj",
"chars": 238392,
"preview": "####\n#\n# OBJ File Generated by Meshlab\n#\n####\n# Object upperarm.obj\n#\n# Vertices: 1897\n# Faces: 3790\n#\n####\n\nvn -6.13662"
},
{
"path": "DojoEnvironments/src/mechanisms/exoskeleton/dependencies/model.urdf",
"chars": 5240,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/exoskeleton/mechanism.jl",
"chars": 1592,
"preview": "function get_exoskeleton(;\n timestep=0.01,\n input_scaling=timestep, \n gravity=-9.81,\n urdf=:model,\n sprin"
},
{
"path": "DojoEnvironments/src/mechanisms/fourbar/dependencies/fourbar.urdf",
"chars": 3327,
"preview": "<?xml version=\"1.0\"?>\n\n<robot name=\"Fourbar\">\n\t<link name=\"base_link\">\n\t</link>\n\n <link name=\"link1\">\n <inertial>\n "
},
{
"path": "DojoEnvironments/src/mechanisms/fourbar/dependencies/fourbar_open.urdf",
"chars": 3032,
"preview": "<?xml version=\"1.0\"?>\n\n<robot name=\"Fourbar\">\n\t<link name=\"base_link\">\n\t</link>\n\n <link name=\"link1\">\n <inertial>\n "
},
{
"path": "DojoEnvironments/src/mechanisms/fourbar/mechanism.jl",
"chars": 1662,
"preview": "function get_fourbar(; \n timestep=0.01, \n input_scaling=timestep, \n gravity=-9.81,\n urdf=:fourbar,\n sprin"
},
{
"path": "DojoEnvironments/src/mechanisms/halfcheetah/dependencies/halfcheetah.urdf",
"chars": 7787,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/halfcheetah/mechanism.jl",
"chars": 3718,
"preview": "function get_halfcheetah(; \n timestep=0.01, \n input_scaling=timestep, \n gravity=-9.81,\n urdf=:halfcheetah,\n "
},
{
"path": "DojoEnvironments/src/mechanisms/hopper/dependencies/hopper.urdf",
"chars": 4163,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/hopper/mechanism.jl",
"chars": 3025,
"preview": "function get_hopper(; \n timestep=0.01, \n input_scaling=timestep, \n gravity=-9.81, \n urdf=:hopper,\n spring"
},
{
"path": "DojoEnvironments/src/mechanisms/humanoid/dependencies/humanoid.urdf",
"chars": 13758,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/humanoid/mechanism.jl",
"chars": 2626,
"preview": "function get_humanoid(; \n timestep=0.01, \n input_scaling=timestep, \n gravity=-9.81, \n urdf=:humanoid, \n s"
},
{
"path": "DojoEnvironments/src/mechanisms/include.jl",
"chars": 838,
"preview": "include(\"ant/mechanism.jl\")\ninclude(\"atlas/mechanism.jl\")\ninclude(\"block/mechanism.jl\")\ninclude(\"block2d/mechanism.jl\")\n"
},
{
"path": "DojoEnvironments/src/mechanisms/npendulum/mechanism.jl",
"chars": 1659,
"preview": "function get_npendulum(;\n timestep=0.01,\n input_scaling=timestep,\n gravity=-9.81,\n num_bodies=5,\n mass=1,"
},
{
"path": "DojoEnvironments/src/mechanisms/nslider/mechanism.jl",
"chars": 1485,
"preview": "function get_nslider(; \n timestep=0.01, \n input_scaling=timestep, \n gravity=-9.81, \n num_bodies=5,\n color"
},
{
"path": "DojoEnvironments/src/mechanisms/panda/dependencies/mesh/collision/finger.obj",
"chars": 7603,
"preview": "# File produced by Open Asset Import Library (http://www.assimp.sf.net)\n# (assimp v3.0.1262)\n\nmtllib finger.stl.obj.mtl\n"
},
{
"path": "DojoEnvironments/src/mechanisms/panda/dependencies/mesh/collision/hand.obj",
"chars": 15247,
"preview": "# File produced by Open Asset Import Library (http://www.assimp.sf.net)\n# (assimp v3.1.187496374)\n\nmtllib hand.obj.mtl\n\n"
},
{
"path": "DojoEnvironments/src/mechanisms/panda/dependencies/mesh/collision/link0.obj",
"chars": 14925,
"preview": "# File produced by Open Asset Import Library (http://www.assimp.sf.net)\n# (assimp v3.1.187496374)\n\nmtllib link0.obj.mtl\n"
},
{
"path": "DojoEnvironments/src/mechanisms/panda/dependencies/mesh/collision/link1.obj",
"chars": 22976,
"preview": "# File produced by Open Asset Import Library (http://www.assimp.sf.net)\n# (assimp v3.1.187496374)\n\nmtllib link1.obj.mtl\n"
},
{
"path": "DojoEnvironments/src/mechanisms/panda/dependencies/mesh/collision/link2.obj",
"chars": 22688,
"preview": "# File produced by Open Asset Import Library (http://www.assimp.sf.net)\n# (assimp v3.1.187496374)\n\nmtllib link2.obj.mtl\n"
},
{
"path": "DojoEnvironments/src/mechanisms/panda/dependencies/mesh/collision/link3.obj",
"chars": 22870,
"preview": "# File produced by Open Asset Import Library (http://www.assimp.sf.net)\n# (assimp v3.1.187496374)\n\nmtllib link3.obj.mtl\n"
},
{
"path": "DojoEnvironments/src/mechanisms/panda/dependencies/mesh/collision/link4.obj",
"chars": 68016,
"preview": "# File produced by Open Asset Import Library (http://www.assimp.sf.net)\n# (assimp v3.0.1262)\n\nmtllib link4.stl.obj.mtl\n\n"
},
{
"path": "DojoEnvironments/src/mechanisms/panda/dependencies/mesh/collision/link5.obj",
"chars": 67745,
"preview": "# File produced by Open Asset Import Library (http://www.assimp.sf.net)\n# (assimp v3.0.1262)\n\nmtllib link5.stl.obj.mtl\n\n"
},
{
"path": "DojoEnvironments/src/mechanisms/panda/dependencies/mesh/collision/link6.obj",
"chars": 5100145,
"preview": "# File produced by Open Asset Import Library (http://www.assimp.sf.net)\n# (assimp v3.0.1262)\n\nmtllib link6.dae.obj.mtl\n\n"
},
{
"path": "DojoEnvironments/src/mechanisms/panda/dependencies/mesh/collision/link7.obj",
"chars": 45132,
"preview": "# File produced by Open Asset Import Library (http://www.assimp.sf.net)\n# (assimp v3.0.1262)\n\nmtllib link7.stl.obj.mtl\n\n"
},
{
"path": "DojoEnvironments/src/mechanisms/panda/dependencies/mesh/visual/finger.dae",
"chars": 51239,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/panda/dependencies/mesh/visual/hand.dae",
"chars": 549239,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/panda/dependencies/mesh/visual/link0.dae",
"chars": 1591592,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/panda/dependencies/mesh/visual/link1.dae",
"chars": 978473,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/panda/dependencies/mesh/visual/link2.dae",
"chars": 998544,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/panda/dependencies/mesh/visual/link3.dae",
"chars": 1099883,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/panda/dependencies/mesh/visual/link4.dae",
"chars": 1145723,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/panda/dependencies/mesh/visual/link5.dae",
"chars": 1438343,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/panda/dependencies/mesh/visual/link6.dae",
"chars": 1728753,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/panda/dependencies/mesh/visual/link7.dae",
"chars": 936416,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/panda/dependencies/panda_end_effector.urdf",
"chars": 12562,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<robot name=\"panda\">\n <!-- ====================================================="
},
{
"path": "DojoEnvironments/src/mechanisms/panda/dependencies/panda_no_end_effector.urdf",
"chars": 10611,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<robot name=\"panda\">\n <!-- ====================================================="
},
{
"path": "DojoEnvironments/src/mechanisms/panda/dependencies/panda_original.urdf",
"chars": 18380,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<robot name=\"panda\">\n <!-- ====================================================="
},
{
"path": "DojoEnvironments/src/mechanisms/panda/mechanism.jl",
"chars": 1683,
"preview": "function get_panda(;\n timestep=0.01,\n input_scaling=timestep, \n gravity=-9.81,\n urdf=:panda_end_effector,\n "
},
{
"path": "DojoEnvironments/src/mechanisms/pendulum/mechanism.jl",
"chars": 1505,
"preview": "function get_pendulum(;\n timestep=0.01,\n input_scaling=timestep, \n gravity=-9.81,\n mass=1,\n link_length=1"
},
{
"path": "DojoEnvironments/src/mechanisms/quadrotor/dependencies/mesh/pelican.obj",
"chars": 1107354,
"preview": "#\n# object node\n#\n\nv -0.00274999999999999 0.0601 0.01\nv 0.00274999999999999 0.0601 0.01\nv 0.00274999999999999 0.0519 0.0"
},
{
"path": "DojoEnvironments/src/mechanisms/quadrotor/dependencies/mesh/propeller_ccw.obj",
"chars": 24031,
"preview": "#\n# object node\n#\n\nv -0.0835714 -0.14003099999999998 -0.07364560000000001\nv -0.0498426 -0.421246 -0.0546421\nv -0.113511 "
},
{
"path": "DojoEnvironments/src/mechanisms/quadrotor/dependencies/mesh/propeller_cw.obj",
"chars": 15560,
"preview": "#\n# object node\n#\n\nv -0.0835714 0.14003099999999998 -0.07364560000000001\nv -0.0400765 0.163115 -0.0483281\nv -0.117913 0."
},
{
"path": "DojoEnvironments/src/mechanisms/quadrotor/dependencies/pelican.urdf",
"chars": 5950,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<!-- ============================================================================"
},
{
"path": "DojoEnvironments/src/mechanisms/quadrotor/dependencies/pelican_fixed_rotors.urdf",
"chars": 5834,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<!-- ============================================================================"
},
{
"path": "DojoEnvironments/src/mechanisms/quadrotor/mechanism.jl",
"chars": 2682,
"preview": "function get_quadrotor(; \n timestep=0.01, \n input_scaling=timestep,\n gravity=-9.81, \n urdf=:pelican_fixed_ro"
},
{
"path": "DojoEnvironments/src/mechanisms/quadruped/dependencies/gazebo_a1.urdf",
"chars": 13170,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/quadruped/dependencies/mesh/calf.dae",
"chars": 1759371,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\" xml"
},
{
"path": "DojoEnvironments/src/mechanisms/quadruped/dependencies/mesh/hip.dae",
"chars": 668970,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\" xml"
},
{
"path": "DojoEnvironments/src/mechanisms/quadruped/dependencies/mesh/thigh.dae",
"chars": 1151632,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\" xml"
},
{
"path": "DojoEnvironments/src/mechanisms/quadruped/dependencies/mesh/thigh_mirror.dae",
"chars": 1187618,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\" xml"
},
{
"path": "DojoEnvironments/src/mechanisms/quadruped/dependencies/mesh/trunk.dae",
"chars": 4246100,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\" xml"
},
{
"path": "DojoEnvironments/src/mechanisms/quadruped/dependencies/quadruped.urdf",
"chars": 13140,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/quadruped/dependencies/quadruped_ones.urdf",
"chars": 12923,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/quadruped/dependencies/quadruped_simple.urdf",
"chars": 13454,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/quadruped/mechanism.jl",
"chars": 5322,
"preview": "function get_quadruped(;\n timestep=0.01,\n input_scaling=timestep, \n gravity=-9.81,\n urdf=:gazebo_a1, \n sp"
},
{
"path": "DojoEnvironments/src/mechanisms/raiberthopper/mechanism.jl",
"chars": 2333,
"preview": "function get_raiberthopper(; \n timestep=0.05, \n input_scaling=timestep, \n gravity=-9.81, \n body_mass=4.18,\n "
},
{
"path": "DojoEnvironments/src/mechanisms/slider/mechanism.jl",
"chars": 1415,
"preview": "function get_slider(; \n timestep=0.01, \n input_scaling=timestep, \n gravity=-9.81, \n color=RGBA(1, 0, 0),\n "
},
{
"path": "DojoEnvironments/src/mechanisms/snake/mechanism.jl",
"chars": 2559,
"preview": "function get_snake(;\n timestep=0.01,\n input_scaling=timestep, \n gravity=-9.81,\n num_bodies=2,\n link_lengt"
},
{
"path": "DojoEnvironments/src/mechanisms/sphere/mechanism.jl",
"chars": 1891,
"preview": "function get_sphere(;\n timestep=0.01,\n input_scaling=timestep, \n gravity=-9.81,\n mass=1,\n radius=0.5,\n "
},
{
"path": "DojoEnvironments/src/mechanisms/tippetop/mechanism.jl",
"chars": 2369,
"preview": "function get_tippetop(;\n timestep=0.01,\n input_scaling=timestep, \n gravity=-9.81,\n mass=1,\n radius=0.5,\n "
},
{
"path": "DojoEnvironments/src/mechanisms/twister/mechanism.jl",
"chars": 2488,
"preview": "function get_twister(; \n timestep=0.01, \n input_scaling=timestep, \n gravity=-9.81, \n num_bodies=5,\n heigh"
},
{
"path": "DojoEnvironments/src/mechanisms/uuv/dependencies/mesh/material.lib",
"chars": 161,
"preview": "newmtl Material__211\n\tKa 0.2 0.2 0.2\n\tKd 0.752941 0 0\n\td 1\n\tTr 0\n\tillum 2\n\nnewmtl Material__52\n\tKa 0.2 0.2 0.2\n\tKd 0.792"
},
{
"path": "DojoEnvironments/src/mechanisms/uuv/dependencies/mesh/obsrov.mtl",
"chars": 17432,
"preview": "# Blender 3.5.0 MTL File: 'untitled.blend'\n# www.blender.org\n\nnewmtl Empty_material\nNs 899.999939\nKa 1.000000 1.000000 1"
},
{
"path": "DojoEnvironments/src/mechanisms/uuv/dependencies/mesh/red_propcw_minitortuga.obj",
"chars": 2688578,
"preview": "mtllib material.lib\n\n#\n# object PropMinitortuga\n#\n\nv -0.0002401895795642545 -0.00217168008055605 -0.00168643874250995\nv "
},
{
"path": "DojoEnvironments/src/mechanisms/uuv/dependencies/mesh/silver_propcw_minitortuga.obj",
"chars": 2688687,
"preview": "# Aspose.3D Wavefront OBJ Exporter\n# Copyright 2004-2023 Aspose Pty Ltd.\n# File created: 04/14/2023 08:07:56\n\nmtllib mat"
},
{
"path": "DojoEnvironments/src/mechanisms/uuv/dependencies/mini_tortuga.urdf",
"chars": 4302,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n\n<robot name=\"mini_tortuga\">\n <link name=\"origin\">\n <inertial>\n <mass va"
},
{
"path": "DojoEnvironments/src/mechanisms/uuv/dependencies/mini_tortuga_fixed_rotors.urdf",
"chars": 4128,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n\n<robot name=\"mini_tortuga\">\n <link name=\"origin\">\n <inertial>\n <mass va"
},
{
"path": "DojoEnvironments/src/mechanisms/uuv/mechanism.jl",
"chars": 2004,
"preview": "function get_uuv(; \n timestep=0.01, \n input_scaling=timestep,\n gravity=0, \n urdf=:mini_tortuga_fixed_rotors,"
},
{
"path": "DojoEnvironments/src/mechanisms/walker/dependencies/walker.urdf",
"chars": 6625,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/walker/mechanism.jl",
"chars": 3433,
"preview": "function get_walker(; \n timestep=0.01, \n input_scaling=timestep, \n gravity=-9.81, \n urdf=:walker, \n sprin"
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/sensors/asus_xtion_pro_live_base.dae",
"chars": 95170,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/sensors/asus_xtion_pro_live_camera.dae",
"chars": 381591,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/sensors/hokuyo.dae",
"chars": 246327,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA version=\"1.4.0\" xmlns=\"http://www.collada.org/2005/11/COLLADASchema\">\n\t<"
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/sensors/hokuyo_convex.dae",
"chars": 10617,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/sensors/kinect.dae",
"chars": 900240,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA version=\"1.4.0\" xmlns=\"http://www.collada.org/2005/11/COLLADASchema\">\n\t<"
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/sensors/microsoft_lifecam.dae",
"chars": 385290,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_arm/arm0.dae",
"chars": 673595,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA version=\"1.4.0\" xmlns=\"http://www.collada.org/2005/11/COLLADASchema\">\n\t<"
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_arm/arm0_convex.dae",
"chars": 39733,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_arm/arm1.dae",
"chars": 1951063,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA version=\"1.4.0\" xmlns=\"http://www.collada.org/2005/11/COLLADASchema\">\n\t<"
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_arm/arm1_convex.dae",
"chars": 83207,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_arm/arm2.dae",
"chars": 586300,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA version=\"1.4.0\" xmlns=\"http://www.collada.org/2005/11/COLLADASchema\">\n\t<"
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_arm/arm2_convex.dae",
"chars": 193130,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_arm/arm3.dae",
"chars": 1022391,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA version=\"1.4.0\" xmlns=\"http://www.collada.org/2005/11/COLLADASchema\">\n\t<"
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_arm/arm3_convex.dae",
"chars": 111869,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_arm/arm4.dae",
"chars": 452364,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA version=\"1.4.0\" xmlns=\"http://www.collada.org/2005/11/COLLADASchema\">\n\t<"
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_arm/arm4_convex.dae",
"chars": 225950,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_arm/arm5.dae",
"chars": 137980,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA version=\"1.4.0\" xmlns=\"http://www.collada.org/2005/11/COLLADASchema\">\n\t<"
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_arm/arm5_convex.dae",
"chars": 68840,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_base/back_left_wheel.dae",
"chars": 829182,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_base/back_right_wheel.dae",
"chars": 829201,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_base/base.dae",
"chars": 1438302,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA version=\"1.4.0\" xmlns=\"http://www.collada.org/2005/11/COLLADASchema\">\n\t<"
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_base/base_convex.dae",
"chars": 26334,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_base/front_left_wheel.dae",
"chars": 829172,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_base/front_right_wheel.dae",
"chars": 830477,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_gripper/finger.dae",
"chars": 70128,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_gripper/finger_convex.dae",
"chars": 6305,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_gripper/palm.dae",
"chars": 470543,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_gripper/palm_convex.dae",
"chars": 169605,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_plate/plate.dae",
"chars": 133078,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA version=\"1.4.0\" xmlns=\"http://www.collada.org/2005/11/COLLADASchema\">\n\t<"
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/meshes/youbot_plate/plate_convex.dae",
"chars": 15260,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/youbot.urdf",
"chars": 24053,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/youbot_arm_only.urdf",
"chars": 14700,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/youbot_arm_only_fixed_gripper.urdf",
"chars": 14692,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/youbot_base_only.urdf",
"chars": 9913,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/youbot_base_only_no_wheels.urdf",
"chars": 3986,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/youbot_dual_arm.urdf",
"chars": 41343,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/youbot_fixed_gripper.urdf",
"chars": 24023,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/dependencies/youbot_with_cam3d.urdf",
"chars": 36798,
"preview": "<?xml version=\"1.0\" ?>\n<!-- =================================================================================== -->\n<!--"
},
{
"path": "DojoEnvironments/src/mechanisms/youbot/mechanism.jl",
"chars": 2456,
"preview": "function get_youbot(;\n timestep=0.01,\n input_scaling=timestep, \n gravity=-9.81,\n urdf=:youbot,\n springs=0"
},
{
"path": "DojoEnvironments/src/mechanisms.jl",
"chars": 702,
"preview": "\"\"\"\n get_mechanism(model; kwargs...)\n \n constructs mechanism\n\n model: name of mechanism \n kwargs: mechani"
},
{
"path": "DojoEnvironments/src/precompile.jl",
"chars": 1328,
"preview": "@setup_workload begin\n mechanisms = [\n :ant,\n :atlas,\n :block,\n :block2d,\n :cartpo"
},
{
"path": "DojoEnvironments/src/utilities.jl",
"chars": 2105,
"preview": "function set_springs!(joints, value::Real)\n for joint in joints\n (value==0) && break\n typeof(joint) <: "
}
]
// ... and 762 more files (download for full content)
About this extraction
This page contains the full source code of the dojo-sim/Dojo.jl GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 962 files (110.2 MB), approximately 18.4M tokens. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.