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

Author: thowell

README.md

@@ -7,18 +7,18 @@ A differentiable simulator for robotics.
 
 # Examples (RSS 2022)
 
-## Simulation 
+## Simulation
 
-### Atlas drop 
+### Atlas drop
 <img src="examples/animations/atlas_drop.gif" width="100"/>
 
 ### Astronaut
 <img src="examples/animations/astronaut.gif" width="200"/>
 
-### Friction-cone comparison 
+### Friction-cone comparison
 - blue = nonlinear friction cone
 - orange = linear friction cone
-- magenta = MuJoCo friction model 
+- magenta = MuJoCo friction model
 <img src="examples/animations/cone_compare_mujoco.gif" height="150"/>
 
 ### Dzhanibekov effect
@@ -27,28 +27,31 @@ A differentiable simulator for robotics.
 ### Tippe top
 <img src="examples/animations/tippetop.gif" width="150"/>
 
-## Trajectory Optimization 
+### Pendulum swing-up
+<img src="examples/animations/pendulum.gif" width="150"/>
 
-### Box 
+## Trajectory Optimization
+
+### Box
 <img src="examples/animations/box_right.gif" width="200"/>
 
 <img src="examples/animations/box_up.gif" width="95"/>
 
-### Hopper 
+### Hopper
 <img src="examples/animations/hopper_max.gif" width="100"/>
 
-### Quadruped 
+### Quadruped
 <img src="examples/animations/quadruped_min.gif" width="200"/>
 
 ### Atlas
 <img src="examples/animations/atlas_ilqr.gif" width="200"/>
 
-### Cart-pole 
+### Cart-pole
 <img src="examples/animations/cartpole_max.gif" width="200"/>
 
-## Reinforcement Learning 
+## Reinforcement Learning
 
-### Half Cheetah 
+### Half Cheetah
 <img src="examples/animations/halfcheetah_ars.gif" width="600"/>
 
 ### Ant
@@ -65,9 +68,9 @@ Learning friction coefficient:
 ### Toss
 <img src="examples/animations/box_toss.gif" width="300"/>
 
-## Installation 
+## Installation
 
-`Dojo` can be added via the Julia package manager (type `]`): 
+`Dojo` can be added via the Julia package manager (type `]`):
 ```julia
 pkg> add Dojo
 ```
@@ -88,6 +91,3 @@ pkg> add Dojo#main
 
 ## How To Contribute
 Please submit a pull request, open an issue, or reach out to: thowell@stanford.edu (Taylor) or simonlc@stanford.edu (Simon)
-
-
-

docs/src/assets/dzhanibekov_nasa.gif

@@ -1 +1,64 @@
-# Defining a Controller
+# Defining a Controller
+Here, we explain how to write a controller and simulate its effect on a dynamical system
+i.e. a [`Mechanism`](@ref).
+We focus on a simple pendulum swing-up.
+
+Load Dojo and use the pendulum mechanism with desired simulation time step, desired gravity and desired damping at the joint.
+
+```julia
+using Dojo
+
+mechanism = get_mechanism(:pendulum,
+    timestep=0.01,
+    gravity=-9.81,
+    damper=5.0)
+```
+
+Define the controller. This is a method that takes 2 input arguments:
+- a [`Mechanism`](@ref),
+- an integer `k` indicating the current simulation step.
+The controller computes the control inputs based on the current state `x`, the goal state `x_goal` and a proportional gain `K`.
+
+
+```julia
+function controller!(mechanism, k)
+    ## Target state
+    x_goal = [1.0 * π; 0.0]
+
+    ## Current state
+    x = get_minimal_state(mechanism)
+
+    ## Gains
+    K = [5.0 0.5] * 0.1
+
+    # Control inputs
+    u =  -K * (x - x_goal)
+    set_input!(mechanism, u)
+end
+```
+
+We initialize the pendulum in the lower position, and we will let the controller perform the swing-up movement.
+```julia
+initialize!(mechanism, :pendulum,
+    angle=0.0 * π,
+    angular_velocity=0.0);
+```
+
+We simulate the system for 2 seconds using the `controller!`.
+```julia
+storage = simulate!(mechanism, 2.0, controller!,
+    record=true,
+    verbose=true);
+```
+
+We visualize the results.
+```julia
+vis = Visualizer()
+open(vis)
+visualize(mechanism, storage, vis=vis);
+```
+
+You should get something like this,
+```@raw html
+<img src="./../../examples/animations/pendulum.gif" width="300"/>
+```

docs/src/assets/quadruped_min.gif

@@ -1 +1,50 @@
-# Defining a Simulation
+# Defining a Simulation
+Here, we explain how to simulate a dynamical system i.e. a [`Mechanism`](@ref) forward in time.
+The example that we are trying to replicate the Dzhanibekov effect shown below.
+
+![dzhanibekov](./assets/dzhanibekov_nasa.gif)
+
+
+Load the `Dojo` package.
+```julia
+using Dojo
+```
+
+Define the simulation time step, and other parameters like the gravity.
+```julia
+timestep=0.01
+gravity=0.0
+```
+
+We use the mechanism called `:dzhanibekov`.
+```julia
+mech = get_mechanism(:dzhanibekov,
+        timestep=timestep,
+        gravity=gravity);
+```
+
+We initialize the system with a given initial angular velocity.
+```julia
+initialize_dzhanibekov!(mech,
+    angular_velocity=[15.0; 0.01; 0.0])
+```
+
+We simulate this system for 4 seconds, we record the resulting trajectory in `storage`,
+```julia
+storage = simulate!(mech, 4.00,
+    record=true,
+    verbose=false)
+```
+
+We visualize the trajectory in the browser,
+```julia
+vis = Visualizer()
+open(vis)
+visualize(mech, storage, vis=vis)
+```
+
+And voila! You should see something like this;
+
+```@raw html
+<img src="./../../examples/animations/dzhanibekov.gif" width="300"/>
+```

docs/src/define_controller.md

@@ -21,6 +21,4 @@ where $\circ$ is an element-wise product operator, enforces zero force if the bo
 
 
 ### Constructor
-```@docs
-ImpactContact
-```
+[`ImpactContact`](@ref)

docs/src/define_simulation.md

@@ -29,6 +29,4 @@ $$[v^T  -v^T]^T + \psi \mathbf{1} - \eta = 0, \\
 where $\psi \in \mathbf{R}$ and $\eta \in \mathbf{R}^{4}$ are the dual variables associated with the friction cone and positivity constraints, respectively, and $\textbf{1}$ is a vector of ones.
 
 ### Constructor
-```@docs
-LinearContact
-```
+[`LinearContact`](@ref)

docs/src/impact.md

@@ -45,6 +45,4 @@ v^{(i)}(z, z_{+}) - \eta_{(2:3)}^{(i)} = 0, \quad i = 1, \dots, P, \\
 where $u \in \mathbf{R}^m$ is the control input at the current time step, $\lambda = (\beta^{(1)}_{(2:3)}, \gamma^{(1)}, \dots, \beta^{(P)}_{(2:3)}, \gamma^{(P)}) \in \mathbf{\Lambda}$ is the concatenation of impact and friction impulses, $B : \mathbf{Z} \rightarrow \mathbf{R}^{6N \times m}$ is the input Jacobian mapping control inputs into maximal coordinates, $C : \mathbf{Z} \rightarrow \mathbf{R}^{\text{dim}(\mathbf{\Lambda}) \times 6N}$ is a contact Jacobian mapping between maximal coordinates and contact surfaces, $s \in \mathbf{R}^P$ is a slack variable introduced for convenience, and $v^{(i)} : \mathbf{Z} \times \mathbf{Z} \rightarrow \mathbf{R}^2$ is the tangential velocity at contact point $i$. Joint limits and internal friction are readily incorporated into this problem formulation.
 
 ### Constructor
-```@docs
-NonlinearContact
-```
+[`NonlinearContact`](@ref)

docs/src/linearized_friction.md

@@ -1,44 +1,39 @@
-function get_dzhanibekov(; 
-    timestep=0.01, 
-    gravity=-9.81, 
-    color=magenta) where T
+function get_dzhanibekov(;
+    timestep=0.01,
+    gravity=-9.81,
+    color=RGBA(0.9,0.9,0.9,1)) where T
 
     radius = 0.1
     body_length = 1.0
     body_mass = 1.0
     origin = Origin{Float64}()
-    main_body = Capsule(radius, body_length, body_mass, 
-        color=color)
-    side_body = Capsule(0.5 * radius, 0.35 * body_length, 0.1 * body_mass, 
-        axis_offset=UnitQuaternion(RotY(0.5 * π)), 
-        color=color)
+    main_body = Capsule(radius, body_length, body_mass,
+        color=color, name=:main)
+    side_body = Capsule(0.5 * radius, 0.35 * body_length, 0.5 * body_mass,
+        axis_offset=UnitQuaternion(RotY(0.5 * π)),
+        color=color, name=:side)
     links = [main_body, side_body]
 
     # Joint Constraints
-    joint_float = JointConstraint(Floating(origin, links[1]))
-    joint_attach = JointConstraint(Fixed(links[1], links[2]; 
-        parent_vertex=szeros(3), 
-        child_vertex=[-0.25 * body_length; 0.0; 0.0]))
-    joints = [joint_float, joint_attach]
+    joint_float = JointConstraint(Floating(origin, links[1]), name=:floating)
+    joint_fixed = JointConstraint(Fixed(links[1], links[2];
+        parent_vertex=szeros(3),
+        child_vertex=[-0.25 * body_length; 0.0; 0.0]), name=:fixed)
+    joints = [joint_float, joint_fixed]
 
+    links[1].inertia = Diagonal([3e-2, 1e-3, 1e-1])
     # Mechanism
-    return Mechanism(origin, links, joints, 
-        gravity=gravity, 
+    return Mechanism(origin, links, joints,
+        gravity=gravity,
         timestep=timestep)
 end
 
-function initialize_dzhanibekov!(mech::Mechanism{T,Nn,Ne,Nb}; 
-    linear_velocity=zeros(3), 
+function initialize_dzhanibekov!(mechanism::Mechanism{T,Nn,Ne,Nb};
+    linear_velocity=zeros(3),
     angular_velocity=zeros(3)) where {T,Nn,Ne,Nb}
 
-    set_maximal_configurations!(mech.origin, mech.bodies[3])
-    set_maximal_velocities!(mech.bodies[3], 
-        v=linear_velocity, 
-        ω=angular_velocity)
-
-    set_maximal_configurations!(mech.bodies[3], mech.bodies[4], 
-        Δx=[0.25; 0.0; 0.0])
-    set_maximal_velocities!(mech.bodies[4], 
-        v=zeros(3), 
-        ω=zeros(3))
-end
+    zero_velocity!(mechanism)
+    set_minimal_coordinates_velocities!(mechanism, get_joint(mechanism, :floating);
+        xmin=[0;0;1;0;0;0; linear_velocity; angular_velocity])
+    set_minimal_coordinates_velocities!(mechanism, get_joint(mechanism, :fixed))
+end