fixing Dzhanibekov

Author: simon-lc

docs/src/assets/dzhanibekov_nasa.gif

@@ -1 +1,41 @@
-# 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 timestep, and other parameters like the gravity.
+```julia
+timestep=0.01
+gravity=0.0
+```
+
+We want to simulate the
+```julia
+mech = get_mechanism(:dzhanibekov,
+        timestep=timestep,
+        gravity=gravity);
+
+# ## Simulate
+initialize_dzhanibekov!(mech,
+    angular_velocity=[15.0; 0.01; 0.0])
+storage = simulate!(mech, 4.65,
+    record=true,
+    verbose=false)
+
+# ## Visualizers
+vis=visualizer()
+render(vis)
+visualize(mech, storage,
+    vis=vis)
+```
+
+And voila! You should see something like this;
+
+![dzhanibekov](./../../examples/animations/dzhanibekov.gif)

docs/src/assets/quadruped_min.gif

@@ -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, 
+function get_dzhanibekov(;
+    timestep=0.01,
+    gravity=-9.81,
     color=magenta) 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=[szeros(6); linear_velocity; angular_velocity])
+    set_minimal_coordinates_velocities!(mechanism, get_joint(mechanism, :fixed))
+end

docs/src/nonlinear_friction.md

@@ -9,19 +9,17 @@ using Dojo
 timestep=0.01
 gravity=0.0
 mech = get_mechanism(:dzhanibekov,
-        timestep=timestep, 
+        timestep=timestep,
         gravity=gravity);
 
 # ## Simulate
-initialize_dzhanibekov!(mech, 
+initialize_dzhanibekov!(mech,
     angular_velocity=[15.0; 0.01; 0.0])
-storage = simulate!(mech, 4.65, 
-    record=true, 
+storage = simulate!(mech, 4.00,
+    record=true,
     verbose=false)
 
 # ## Visualizers
-vis=visualizer()
-render(vis)
-visualize(mech, storage, 
-    vis=vis)
-
+vis = Visualizer()
+open(vis)
+visualize(mech, storage, vis=vis)