addin rexhopper animation

Author: simon-lc

docs/make.jl

@@ -20,7 +20,6 @@ makedocs(
         "Creating a Mechanism" => [
             "define_mechanism.md",
             "load_mechanism.md",
-            "mechanism_interfaces.md",
            ],
 
         "Creating a Simulation" => [

docs/src/define_mechanism.md

@@ -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/load_mechanism.md

@@ -1 +1,98 @@
-# Loading Mechanism via URDF
+# Loading Mechanism via URDF
+Another way to build a mechanism is to directly load it from a URDF file. We can do this is the mechanism builder function. 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_foot=true,
+    contact_body=true,
+    limits=true,
+    model=:rexhopper,
+    floating=true,
+    contact_type=:nonlinear,
+    spring=0.0,
+    damper=1.0,
+    T=Float64)
+
+    path = joinpath(@__DIR__, "../deps/$(String(model)).urdf")
+    mech = Mechanism(path, floating, T,
+        gravity=gravity,
+        timestep=timestep,
+        spring=spring,
+        damper=damper)
+
+    # joint limits
+    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
+
+    if contact_foot
+        origin = Origin{T}()
+        bodies = mech.bodies
+        joints = mech.joints
+
+        normal = [0.0; 0.0; 1.0]
+        models = []
+
+        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!(models, 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!(models, 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!(models, 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!(models, 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, [models...],
+            gravity=gravity,
+            timestep=timestep,
+            spring=spring,
+            damper=damper)
+    end
+    return mech
+end
+```

docs/src/mechanism_interfaces.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,

environments/rexhopper/methods/initialize.jl

@@ -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/tippetop/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=false);