Merge branch 'main' of https://github.com/dojo-sim/Dojo.jl into main

Author: thowell

README.md

@@ -12,6 +12,9 @@ A differentiable simulator for robotics.
 ### Atlas drop
 <img src="examples/animations/atlas_drop.gif" width="100"/>
 
+### REx Hopper drop
+<img src="examples/animations/rexhopper.gif" width="200"/>
+
 ### Astronaut
 <img src="examples/animations/astronaut.gif" width="200"/>
 

docs/src/assets/rexhopper.png

@@ -81,10 +81,10 @@ The second method that we need to define is `initialize_...` where `...` is the
 
 ```julia
 function initialize_tippetop!(mechanism::Mechanism{T};
-    x=zeros(3),
-    θ=zeros(3),
-    v=zeros(3),
-    ω=zeros(3)) where T
+    body_position=zeros(3),
+    body_orientation=zeros(3),
+    body_linear_velocity=zeros(3),
+    body_angular_velocity=zeros(3)) where T
 
     # we retrieve the joints from the mechanism by their names
     floating_joint = get_joint(mechanism, :floating_joint)
@@ -94,7 +94,8 @@ function initialize_tippetop!(mechanism::Mechanism{T};
     # we set all the bodies' velocities to zeros
     zero_velocity!(mechanism)
     # we set to minimal coordinates of the floating joint
-    set_minimal_coordinates_velocities!(mechanism, floating_joint, xmin=[x; θ; v; ω])
+    set_minimal_coordinates_velocities!(mechanism, floating_joint,
+        xmin=[body_position; body_orientation; body_linear_velocity; body_angular_velocity])
     # we set to minimal coordinates of the fixed joint (this joint has zero minimal coordinate).
     set_minimal_coordinates_velocities!(mechanism, fixed_joint)
     return nothing

docs/src/assets/rexhopper_contact.png

@@ -1 +1,133 @@
-# Loading Mechanism via URDF
+# Loading Mechanism via URDF
+Another way to build a mechanism is to directly load it from a URDF file. We illustrate this with the RExLab hopper.
+
+```julia
+function get_rexhopper(;
+    timestep=0.01,
+    gravity=[0.0; 0.0; -9.81],
+    friction_coefficient=1.0,
+    contact_body=true,
+    limits=true,
+    model=:rexhopper,
+    floating=true,
+    contact_type=:nonlinear,
+    spring=0.0,
+    damper=1.0,
+    T=Float64)
+
+    # we define the path of the URDF file
+    path = joinpath(@__DIR__, "../deps/$(String(model)).urdf")
+    # we load the URDF file into a Mechanism object
+    mech = Mechanism(path, floating, T,
+        gravity=gravity,
+        timestep=timestep,
+        spring=spring,
+        damper=damper)
+    return mech
+end
+```
+Here, we have created a mechanism, that contains all the bodies and the joint and also the meshes loaded from the path contained in the URDF file. This is the resulting robot;
+![rexhopper](./assets/rexhopper.png)
+
+However, we are still missing the joint limits and the contact spheres. The following methods provides an example for adding joint limits and contact spheres. Our goal here was to best approximate the mesh shapes using as few spheres as possible.
+Parsing joint limits from the URDF to add them automatically to the mechanism should be easily feasible. That could be a great way to contribute and familiarize yourself with the code!
+
+```julia
+function get_rexhopper(;
+    timestep=0.01,
+    gravity=[0.0; 0.0; -9.81],
+    friction_coefficient=1.0,
+    contact_body=true,
+    limits=true,
+    model=:rexhopper,
+    floating=true,
+    contact_type=:nonlinear,
+    spring=0.0,
+    damper=1.0,
+    T=Float64)
+
+    # we define the path of the URDF file
+    path = joinpath(@__DIR__, "../deps/$(String(model)).urdf")
+    # we load the URDF file into a Mechanism object
+    mech = Mechanism(path, floating, T,
+        gravity=gravity,
+        timestep=timestep,
+        spring=spring,
+        damper=damper)
+
+    # we add joint limits to the mechanism
+    joints = deepcopy(mech.joints)
+    if limits
+        joint1 = get_joint(mech, :joint1)
+        joint3 = get_joint(mech, :joint3)
+        joints[joint1.id] = add_limits(mech, joint1,
+            rot_limits=[SVector{1}(-0.7597), SVector{1}(1.8295)])
+        joints[joint3.id] = add_limits(mech, joint3,
+            rot_limits=[SVector{1}(-1.8295), SVector{1}(0.7597)])
+        mech = Mechanism(Origin{T}(), [mech.bodies...], [joints...],
+            gravity=gravity,
+            timestep=timestep,
+            spring=spring,
+            damper=damper)
+    end
+
+    # we attach spherical collisions zones to certain bodies
+    if contact_body
+        origin = Origin{T}()
+        bodies = mech.bodies
+        joints = mech.joints
+
+        normal = [0.0; 0.0; 1.0]
+        contacts = []
+
+        link3 = get_body(mech, :link3)
+        link2 = get_body(mech, :link2)
+        foot_radius = 0.0203
+        ankle_radius = 0.025
+        base_radius = 0.125
+        p = [0.1685; 0.0025; -0.0055]
+        o = [0;0; foot_radius]
+        push!(contacts, contact_constraint(link3, normal,
+            friction_coefficient=friction_coefficient,
+            contact_point=p,
+            offset=o,
+            contact_type=contact_type,
+            name=:foot))
+        p = [-0.10; -0.002; 0.01]
+        o = [0;0; ankle_radius]
+        push!(contacts, contact_constraint(link3, normal,
+            friction_coefficient=friction_coefficient,
+            contact_point=p,
+            offset=o,
+            contact_type=contact_type,
+            name=:ankle3))
+        p = [0.24; 0.007; 0.005]
+        push!(contacts, contact_constraint(link2, normal,
+            friction_coefficient=friction_coefficient,
+            contact_point=p,
+            offset=o,
+            contact_type=contact_type,
+            name=:ankle2))
+        base_link = get_body(mech, :base_link)
+        p = [0.0; 0.0; 0.0]
+        o = [0;0; base_radius]
+        push!(contacts, contact_constraint(base_link, normal,
+            friction_coefficient=friction_coefficient,
+            contact_point=p,
+            offset=o,
+            contact_type=contact_type,
+            name=:torso))
+
+        set_minimal_coordinates!(mech, get_joint(mech, :auto_generated_floating_joint), [0,0,1.0, 0,0,0])
+        mech = Mechanism(origin, bodies, joints, [contacts...],
+            gravity=gravity,
+            timestep=timestep,
+            spring=spring,
+            damper=damper)
+    end
+    return mech
+end
+```
+
+This is the result, we added three spherical contacts to the robot. There is one coverinn the torso one covering the knee and one covering the foot, they are shown in red;
+![rexhopper_contact](./assets/rexhopper_contact.png)

docs/src/define_mechanism.md

@@ -1,6 +1,6 @@
-function get_rexhopper(; 
-    timestep=0.01, 
-    gravity=[0.0; 0.0; -9.81], 
+function get_rexhopper(;
+    timestep=0.01,
+    gravity=[0.0; 0.0; -9.81],
     friction_coefficient=1.0,
     contact_foot=true,
     contact_body=true,
@@ -13,10 +13,10 @@ function get_rexhopper(;
     T=Float64)
 
     path = joinpath(@__DIR__, "../deps/$(String(model)).urdf")
-    mech = Mechanism(path, floating, T, 
-        gravity=gravity, 
-        timestep=timestep, 
-        spring=spring, 
+    mech = Mechanism(path, floating, T,
+        gravity=gravity,
+        timestep=timestep,
+        spring=spring,
         damper=damper)
 
     # joint limits
@@ -25,14 +25,14 @@ function get_rexhopper(;
     if limits
         joint1 = get_joint(mech, :joint1)
         joint3 = get_joint(mech, :joint3)
-        joints[joint1.id] = add_limits(mech, joint1, 
+        joints[joint1.id] = add_limits(mech, joint1,
             rot_limits=[SVector{1}(-0.7597), SVector{1}(1.8295)])
-        joints[joint3.id] = add_limits(mech, joint3, 
+        joints[joint3.id] = add_limits(mech, joint3,
             rot_limits=[SVector{1}(-1.8295), SVector{1}(0.7597)])
-        mech = Mechanism(Origin{T}(), [mech.bodies...], [joints...], 
-            gravity=gravity, 
-            timestep=timestep, 
-            spring=spring, 
+        mech = Mechanism(Origin{T}(), [mech.bodies...], [joints...],
+            gravity=gravity,
+            timestep=timestep,
+            spring=spring,
             damper=damper)
     end
 
@@ -48,54 +48,61 @@ function get_rexhopper(;
         link2 = get_body(mech, :link2)
         foot_radius = 0.0203
         ankle_radius = 0.025
-        base_radius = 0.125
+        base_radius = 0.14
         p = [0.1685; 0.0025; -0.0055]
         o = [0;0; foot_radius]
-        push!(models, contact_constraint(link3, normal, 
+        push!(models, contact_constraint(link3, normal,
             friction_coefficient=friction_coefficient,
-            contact_point=p, 
-            offset=o, 
-            contact_type=contact_type, 
+            contact_point=p,
+            offset=o,
+            contact_type=contact_type,
             name=:foot))
         p = [-0.10; -0.002; 0.01]
         o = [0;0; ankle_radius]
-        push!(models, contact_constraint(link3, normal, 
+        push!(models, contact_constraint(link3, normal,
             friction_coefficient=friction_coefficient,
-            contact_point=p, 
-            offset=o, 
-            contact_type=contact_type, 
+            contact_point=p,
+            offset=o,
+            contact_type=contact_type,
             name=:ankle3))
         p = [0.24; 0.007; 0.005]
-        push!(models, contact_constraint(link2, normal, 
+        push!(models, contact_constraint(link2, normal,
             friction_coefficient=friction_coefficient,
-            contact_point=p, 
-            offset=o, 
-            contact_type=contact_type, 
+            contact_point=p,
+            offset=o,
+            contact_type=contact_type,
             name=:ankle2))
         base_link = get_body(mech, :base_link)
-        p = [0.0; 0.0; 0.0]
+        pl = [0.0; +0.075; 0.03]
+        pr = [0.0; -0.075; 0.03]
         o = [0;0; base_radius]
-        push!(models, contact_constraint(base_link, normal, 
+        push!(models, contact_constraint(base_link, normal,
             friction_coefficient=friction_coefficient,
-            contact_point=p, 
-            offset=o, 
-            contact_type=contact_type, 
-            name=:torso))
+            contact_point=pl,
+            offset=o,
+            contact_type=contact_type,
+            name=:torso_left))
+        push!(models, contact_constraint(base_link, normal,
+            friction_coefficient=friction_coefficient,
+            contact_point=pr,
+            offset=o,
+            contact_type=contact_type,
+            name=:torso_right))
 
         set_minimal_coordinates!(mech, get_joint(mech, :auto_generated_floating_joint), [0,0,1.0, 0,0,0])
-        mech = Mechanism(origin, bodies, joints, [models...], 
-            gravity=gravity, 
-            timestep=timestep, 
-            spring=spring, 
+        mech = Mechanism(origin, bodies, joints, [models...],
+            gravity=gravity,
+            timestep=timestep,
+            spring=spring,
             damper=damper)
     end
     return mech
 end
 
-function initialize_rexhopper!(mechanism::Mechanism{T}; 
-        body_position=zeros(3), 
-        body_orientation=zeros(3), 
-        body_linear_velocity=zeros(3), 
+function initialize_rexhopper!(mechanism::Mechanism{T};
+        body_position=zeros(3),
+        body_orientation=zeros(3),
+        body_linear_velocity=zeros(3),
         body_angular_velocity=zeros(3)) where T
 
     zero_velocity!(mechanism)
@@ -104,7 +111,7 @@ function initialize_rexhopper!(mechanism::Mechanism{T};
     for joint in mechanism.joints
         !(joint.name in (:loop_joint, :floating_joint)) && set_minimal_coordinates!(mechanism, joint, zeros(input_dimension(joint)))
     end
-    
+
     set_minimal_coordinates!(mechanism, get_joint(mechanism,
         :auto_generated_floating_joint), [body_position; body_orientation])
     set_minimal_velocities!(mechanism, get_joint(mechanism,

docs/src/load_mechanism.md

@@ -52,17 +52,18 @@ function get_tippetop(;
 end
 
 function initialize_tippetop!(mechanism::Mechanism{T};
-    x=zeros(3),
-    θ=zeros(3),
-    v=zeros(3),
-    ω=zeros(3)) where T
+    body_position=zeros(3),
+    body_orientation=zeros(3),
+    body_linear_velocity=zeros(3),
+    body_angular_velocity=zeros(3)) where T
 
     floating_joint = get_joint(mechanism, :floating_joint)
     fixed_joint = get_joint(mechanism, :fixed_joint)
     radius = fixed_joint.translational.vertices[1][3]
 
     zero_velocity!(mechanism)
-    set_minimal_coordinates_velocities!(mechanism, floating_joint, xmin=[x; θ; v; ω])
+    set_minimal_coordinates_velocities!(mechanism, floating_joint,
+        xmin=[body_position; body_orientation; body_linear_velocity; body_angular_velocity])
     set_minimal_coordinates_velocities!(mechanism, fixed_joint)
     return nothing
 end

environments/rexhopper/methods/initialize.jl

@@ -1,7 +1,7 @@
 using Dojo
 
 # Open visualizer
-vis=visualizer()
+vis = Visualizer()
 open(vis)
 # render(vis)
 
@@ -11,15 +11,15 @@ include(joinpath(module_dir(), "environments/rexhopper/methods/initialize.jl"))
 # Mechanism
 mechanism = get_rexhopper(model=:rexhopper_fixed, timestep=0.05, gravity=-9.81,
     contact_body=true, friction_coefficient=1.0)
-env = get_environment("rexhopper",
-    timestep=0.05, gravity=-9.81, contact_body=true, friction_coefficient=1.0)
+# env = get_environment("rexhopper",
+#     timestep=0.05, gravity=-9.81, contact_body=true, friction_coefficient=1.0)
 
 # Simulate
-initialize!(env.mechanism, :rexhopper, x=[0.0; 0.0; 0.4])
+initialize!(mechanism, :rexhopper, body_position=[0.0; 0.0; 0.1], body_orientation=[0.1, 0.9, 0.0])
 
 # Open visualizer
-storage = simulate!(mechanism, 2.5, record=true, opts=SolverOptions(undercut=10.0,
+storage = simulate!(mechanism, 1.5, record=true, opts=SolverOptions(undercut=10.0,
     btol=1.0e-4, rtol=1.0e-4, verbose=false));
 
 # Visualize
-visualize(mechanism, storage, vis=vis, show_contact=false);
+visualize(mechanism, storage, vis=vis, show_contact=true, build=true);