Merge branch 'main' of https://github.com/DojoSim/Dojo.jl into main

Author: simon-lc

README.md

@@ -3,7 +3,7 @@
 [![](https://img.shields.io/badge/docs-dev-blue.svg)](https://dojo-sim.github.io/Dojo.jl/dev)
 
 # Dojo
-A differentiable simulator for robotics.
+A differentiable simulator for robotics
 
 # Examples (RSS 2022)
 

docs/make.jl

@@ -4,7 +4,7 @@ using Documenter, Dojo
 
 makedocs(
     modules = [Dojo],
-    format = Documenter.HTML(prettyurls = false),
+    format = Documenter.HTML(prettyurls=false),
     sitename = "Dojo",
     pages = [
         ##############################################

environments/atlas/methods/env.jl

@@ -21,7 +21,8 @@ function atlas(;
     name=:robot,
     infeasible_control=false,
     opts_step=SolverOptions(rtol=3.0e-4, btol=3.0e-4, undercut=1.5),
-    opts_grad=SolverOptions(rtol=3.0e-4, btol=3.0e-4, undercut=1.5)) where T
+    opts_grad=SolverOptions(rtol=3.0e-4, btol=3.0e-4, undercut=1.5),
+    T=Float64)
 
     mechanism = get_mechanism(:atlas, 
         timestep=timestep, 

environments/atlas/methods/initialize.jl

@@ -223,7 +223,7 @@ function initialize_atlas!(mechanism::Mechanism;
     return nothing
 end
 
-function initialize_atlasstance!(mechanism::Mechanism;
+function initialize_atlas_stance!(mechanism::Mechanism;
     body_position=[0.0, 0.0, 0.2],
     body_orientation=[0.0, 0.0, 0.0],
     link_linear_velocity=[zeros(3)  for i=1:length(mechanism.bodies)],

environments/box/methods/env.jl

@@ -19,7 +19,8 @@ function block(;
     name::Symbol=:robot,
     control_scaling=Diagonal(ones(3)),
     opts_step=SolverOptions(rtol=3.0e-4, btol=3.0e-4, undercut=1.5), 
-    opts_grad=SolverOptions(rtol=3.0e-4, btol=3.0e-4, undercut=1.5)) where T
+    opts_grad=SolverOptions(rtol=3.0e-4, btol=3.0e-4, undercut=1.5),
+    T=Float64)
 
     mechanism = get_box(
         timestep=timestep, 

environments/box2d/methods/env.jl renamed to environments/box2D/methods/env.jl

@@ -1,9 +1,9 @@
 ################################################################################
 # Box (2D)
 ################################################################################
-struct Box2d end
+struct Box2D end
 
-function box2d(; 
+function box2D(; 
     representation=:minimal, 
     timestep=0.05, 
     gravity=[0.0; 0.0; -9.81], 
@@ -18,12 +18,12 @@ function box2d(;
     opts_grad=SolverOptions(rtol=3.0e-4, btol=3.0e-4, undercut=1.5),
     T=Float64)
 
-    mechanism = get_mechanism(:box2d, 
+    mechanism = get_mechanism(:box2D, 
         timestep=timestep, 
         gravity=gravity,
         friction_coefficient=friction_coefficient)
 
-    initialize!(mechanism, :box2d)
+    initialize!(mechanism, :box2D)
 
     if representation == :minimal
         nx = minimal_dimension(mechanism)

environments/box2d/methods/initialize.jl renamed to environments/box2D/methods/initialize.jl

@@ -1,12 +1,12 @@
-function get_box2d(; 
+function get_box2D(; 
     timestep=0.01, 
     gravity=[0.0; 0.0; -9.81], 
     friction_coefficient=0.8, 
     radius=0.0, 
     side=0.5,
     contact=true,
     contact_type=:nonlinear,
-    mode=:box2d,
+    mode=:box2D,
     T=Float64) 
 
     # Parameters
@@ -23,15 +23,15 @@ function get_box2d(;
         # Corner vectors
         if mode == :particle
             corners = [[0.0, 0.0, 0.0]]
-        elseif mode == :box2d
+        elseif mode == :box2D
             corners = [
                 [[0.0,  side / 2.0,  side / 2.0]]
                 [[0.0,  side / 2.0, -side / 2.0]]
                 [[0.0, -side / 2.0,  side / 2.0]]
                 [[0.0, -side / 2.0, -side / 2.0]]
             ]
         else
-            @error "incorrect mode specified, try :particle or :box2d"
+            @error "incorrect mode specified, try :particle or :box2D"
         end
         n = length(corners)
         normal = [[0.0, 0.0, 1.0] for i = 1:n]
@@ -55,7 +55,7 @@ function get_box2d(;
     return mech
 end
 
-function initialize_box2d!(mechanism::Mechanism{T}; 
+function initialize_box2D!(mechanism::Mechanism{T}; 
     position=[0.0, 1.0],
     linear_velocity=[0.0, 0.0], 
     orientation=0.0, 

environments/cartpole/methods/env.jl

@@ -16,7 +16,8 @@ function cartpole(;
     name=:robot,
     control_scaling=Diagonal(ones(1)),
     opts_step=SolverOptions(), 
-    opts_grad=SolverOptions()) where T
+    opts_grad=SolverOptions(),
+    T=Float64)
 
     mechanism = get_cartpole(
         timestep=timestep, 

environments/cartpole/methods/initialize.jl

@@ -2,7 +2,8 @@ function get_cartpole(;
     timestep=0.1, 
     gravity=[0.0; 0.0; -9.81], 
     spring=0.0, 
-    damper=0.0) where T
+    damper=0.0,
+    T=Float64)
 
     # Parameters
     slider_axis = [0.0; 1.0; 0.0]

environments/dzhanibekov/methods/initialize.jl

@@ -1,7 +1,8 @@
 function get_dzhanibekov(;
     timestep=0.01,
     gravity=-9.81,
-    color=RGBA(0.9,0.9,0.9,1)) where T
+    color=RGBA(0.9,0.9,0.9,1),
+    T=Float64)
 
     radius = 0.1
     body_length = 1.0

environments/include.jl

@@ -4,7 +4,7 @@
 include("ant/methods/initialize.jl")
 include("atlas/methods/initialize.jl")
 include("box/methods/initialize.jl")
-include("box2d/methods/initialize.jl")
+include("box2D/methods/initialize.jl")
 include("cartpole/methods/initialize.jl")
 include("dzhanibekov/methods/initialize.jl")
 include("fourbar/methods/initialize.jl")

environments/raiberthopper/methods/initialize.jl

@@ -79,7 +79,7 @@ function initialize_raiberthopper!(mech::Mechanism{T,Nn,Ne,Nb};
     pbody = mech.bodies[1]
     cbody = mech.bodies[2]
     joint2 = mech.joints[2]
-    tra2 = joint2.constraints[1]
+    tra2 = joint2.translational
 
     # origin to body
     set_maximal_configurations!(mech.origin, pbody, 

environments/rexhopper/methods/env.jl

@@ -66,16 +66,17 @@ function rexhopper(;
     fu = zeros(nx, nu)
 
     u_prev = zeros(nu)
-    control_mask = infeasible_control ? I(nu) : cat(zeros(3,3), I(3), 1, 0, 1, zeros(5,5), dims=(1,2))
-    motor_gear = ones(nu)
-    control_scaling = Diagonal(motor_gear)
+    control_map = infeasible_control ? 1.0 * I(nu) : cat(zeros(3, 3), 1.0 * I(3), 1.0, 0.0, 1.0, zeros(5, 5), dims=(1,2))
 
     build_robot(mechanism, vis=vis, name=name)
 
     TYPES = [RexHopper, T, typeof(mechanism), typeof(aspace), typeof(ospace), typeof(info)]
-    env = Environment{TYPES...}(mechanism, representation, aspace, ospace,
+    env = Environment{TYPES...}(mechanism, 
+        representation, 
+        aspace, 
+        ospace,
         x, fx, fu,
-        u_prev, control_mask' * control_scaling,
+        u_prev, control_map,
         nx, nu, no,
         info,
         [rng], vis,

environments/tennisracket/methods/initialize.jl

@@ -3,10 +3,10 @@ function get_tennisracket(;
     gravity=[0.0; 0.0; -9.81],
     T=Float64) 
 
-    origin = Origin{T}(name="origin")
+    origin = Origin{T}(name=:origin)
     mass = 1.0
     h = 1.0
-    bodies = [Box(h / 25.0, h / 2, h, mass, color=RGBA(1.0, 0.0, 0.0), name=:box)]
+    bodies = [Box(h / 25.0, h / 2.0, h, mass, color=RGBA(1.0, 0.0, 0.0), name=:box)]
     joints = [JointConstraint(Floating(origin, bodies[1]), name=:floating_joint)]
     mechanism = Mechanism(origin, bodies, joints, 
         timestep=timestep, 

environments/walker/methods/env.jl

@@ -5,12 +5,32 @@
 """
 struct Walker end
 
-function walker(; mode::Symbol=:minimal, dt::T=0.05, gravity=[0.0; 0.0; -9.81],
-    friction_coefficient::T=1.9, spring=0.0, damper=0.1,
-    seed=1, contact::Bool=true, info=nothing, vis::Visualizer=Visualizer(), name::Symbol=:robot,
-    opts_step=SolverOptions(), opts_grad=SolverOptions()) where T
+function walker(; 
+    mode::Symbol=:minimal, 
+    timestep=0.05, 
+    gravity=[0.0; 0.0; -9.81],
+    friction_coefficient=1.9, 
+    spring=0.0, 
+    damper=0.1,
+    seed=1, 
+    contact_feet=true, 
+    contact_body=true,
+    info=nothing, 
+    vis=Visualizer(), 
+    name=:robot,
+    opts_step=SolverOptions(), 
+    opts_grad=SolverOptions(),
+    T=Float64)
+
+    mechanism = get_walker(
+        timestep=timestep, 
+        gravity=gravity, 
+        friction_coefficient=friction_coefficient, 
+        spring=spring, 
+        damper=damper, 
+        contact_feet=contact_feet,
+        contact_body=contact_body)
 
-    mechanism = get_walker(timestep=dt, gravity=gravity, friction_coefficient=friction_coefficient, spring=spring, damper=damper, contact=contact)
     initialize_walker!(mechanism)
 
     if mode == :minimal
@@ -40,9 +60,11 @@ function walker(; mode::Symbol=:minimal, dt::T=0.05, gravity=[0.0; 0.0; -9.81],
     u_prev = zeros(nu)
     control_mask = [zeros(nu, 3) I(nu)]
     motor_gear = [100, 100, 100, 100, 100, 100.]
-    control_scaling = Diagonal(dt * motor_gear)
+    control_scaling = Diagonal(timestep * motor_gear)
 
-    build_robot(mechanism, vis=vis, name=name)
+    build_robot(mechanism, 
+        vis=vis, 
+        name=name)
 
     TYPES = [Walker, T, typeof(mechanism), typeof(aspace), typeof(ospace), typeof(info)]
     env = Environment{TYPES...}(mechanism, mode, aspace, ospace,
@@ -61,7 +83,8 @@ function reset(env::Environment{Walker}; x=nothing, reset_noise_scale = 0.005)
         env.state .= x
     else
         # initialize above the ground to make sure that with random initialization we do not violate the ground constraint.
-        initialize!(env.mechanism, :walker, z = 0.25)
+        initialize!(env.mechanism, :walker, 
+            z=0.25)
         x0 = get_minimal_state(env.mechanism)
         nx = minimal_dimension(env.mechanism)
 
@@ -80,7 +103,8 @@ function reset(env::Environment{Walker}; x=nothing, reset_noise_scale = 0.005)
     return get_observation(env)
 end
 
-function get_observation(env::Environment{Walker}; full_state::Bool=false)
+function get_observation(env::Environment{Walker}; 
+    full_state=false)
     full_state && (return env.state)
     nx = minimal_dimension(env.mechanism)
     if env.representation == :minimal
@@ -124,19 +148,3 @@ function is_done(::Environment{Walker}, x)
     done = !((height > 0.8) && (height < 2.0) && (abs(ang) < 1.0))
     return done
 end
-
-
-#
-# env.state
-# i_torso = findfirst(body -> body.name == "torso", collect(env.mechanism.bodies))
-# z_torso = z[(i_torso-1)*13 .+ (1:13)]
-# x_velocity = z_torso[4]
-# z[3*13 + 4] = 324.0
-# z
-# set_maximal_state!(env.mechanism, z)
-#
-# initialize!(env.mechanism, :walker, x = 111.0, z = 1.0, θ=0.18)
-# x = get_minimal_state(env.mechanism)
-# z = get_maximal_state(env.mechanism)
-# is_done(env, x)
-#

examples/Project.toml

@@ -1,11 +1,13 @@
 [deps]
 IterativeLQR = "605048dd-e178-462b-beb9-98a09398ef27"
+JLD2 = "033835bb-8acc-5ee8-8aae-3f567f8a3819"
 Literate = "98b081ad-f1c9-55d3-8b20-4c87d4299306"
 PGFPlots = "3b7a836e-365b-5785-a47d-02c71176b4aa"
 Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80"
 
 [compat]
 IterativeLQR = "0.1.1"
 Literate = "2.13.0"
-Plots = "1.25.11"
 PGFPlots = "3.4.1"
+Plots = "1.25.11"
+JLD2 = "0.4.21"

examples/README.md

@@ -0,0 +1,20 @@
+# Dojo.jl examples
+
+This directory contains examples using Dojo.
+The `.jl` files in each subdirectory are meant to be processed using [Literate.jl](https://github.com/fredrikekre/Literate.jl).
+During the build process, the `.jl` files are converted to notebooks. 
+
+Installation of the package generates notebooks and they can be run locally by performing the following steps:
+
+1. [install Dojo.jl](https://github.com/dojo-sim/Dojo.jl)
+2. [install IJulia](https://github.com/JuliaLang/IJulia.jl) (`add` it to the default project)
+3. in the Julia REPL, run (do once)
+   ```
+   using Pkg
+   Pkg.build("Dojo")
+   ```
+4. interact with notebooks
+   ```
+   using IJulia, Dojo
+   notebook(dir=joinpath(dirname(pathof(Dojo)), "..", "examples"))
+   ````

examples/animations/generate/atlas_min.jl

@@ -0,0 +1,7 @@
+set_camera!(env.vis, 
+	zoom=5.0, 
+	cam_pos=[0.0, -5.0, 0.0])
+
+z = [minimal_to_maximal(env.mechanism, x) for x in x_sol]
+t = 1 #10, 20, 30, 41
+set_robot(env.vis, env.mechanism, z[t])

examples/animations/generate/cone_compare.jl

@@ -0,0 +1,31 @@
+# linear cone 
+line_mat_lc = LineBasicMaterial(color=color_lc, linewidth=10.0)
+points_lc = Vector{Point{3,Float64}}()
+for (i, xt) in enumerate(storage_lc.x[1])
+    k = xt
+    push!(points_lc, Point(k[1], k[2] + 0.04, k[3]))
+end
+
+setobject!(vis[:path_lc], Line(points_lc, line_mat_lc))
+
+# nonlinear cone 
+line_mat_nc = Dojo.LineBasicMaterial(color=color_nc, linewidth=25.0)
+points_nc = Vector{Point{3,Float64}}()
+for (i, xt) in enumerate(storage_nc.x[1])
+    k = xt
+    push!(points_nc, Point(k[1], k[2] - 0.04, k[3] + 0.0))
+end
+setobject!(vis[:path_nc], MeshCat.Line(points_nc, line_mat_nc))
+
+# MuJoCo 
+line_mat_mj = LineBasicMaterial(color=color_mj, linewidth=25.0)
+points_mj = Vector{Point{3,Float64}}()
+for (i, xt) in enumerate(storage_mj.x[1])
+    k = xt
+    push!(points_mj, Point(k[1], k[2], k[3]))
+end
+setobject!(vis[:path_mj], MeshCat.Line(points_mj, line_mat_mj))
+
+settransform!(vis[:lc], MeshCat.Translation(0,+0.04,0))
+settransform!(vis[:nc], MeshCat.Translation(0,-0.04,0))
+settransform!(vis[:mj], MeshCat.Translation(0,+0.00,0))