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

Author: thowell

docs/src/assets/logo.png

@@ -1 +1,102 @@
-# Defining a Mechanism
+# Defining a Mechanism
+Here, we describe how to define your own dynamical system of [`Mechanism`](@ref). After it has been defined, it will be extremely easy to simulate it, control it, perform trajectory optimization on it, or even policy optimization.
+
+We're going to build a tippe top,
+
+
+```@raw html
+<img src="./assets/tippetop_real.gif" width="300"/>
+```
+
+```@raw html
+<img src="./../../examples/animations/tippetop.gif" width="300"/>
+```
+
+### Build Mechanism
+
+To do this, we need to define two methods. The first one is named `get_...` where `...` is the name of the mechanism. So here we define `get_tippetop`. This function return a [`Mechanism`](@ref) and takes as input a variety of parameters like the simulation time step, gravity etc. You can add as many parameters you want. This example is typical of what you will find in Dojo.
+
+To build the mechanism corresponding to the tippe top, we decompose it into two spherical bodies. Each body has its own spherical contact constraint with the floor. The joint between the two bodies is a `fixed_joint` and the joint between the main body and the [`Origin`](@ref) of the frame is a floating joint.
+
+```julia
+function get_tippetop(;
+    timestep=0.01,
+    gravity=[0.0; 0.0; -9.81],
+    friction_coefficient=0.4,
+    contact_type=:nonlinear,
+    T=Float64)
+
+    # define origin
+    origin = Origin{T}(name=:origin)
+    # define the two bodies which will form the tippe top once assembled
+    radius = 0.5
+    mass = 1.0
+    α = 0.2
+    bodies = [
+        Sphere(radius, mass, name=:sphere1, color=gray_light),
+        Sphere(radius*α, mass*α^3, name=:sphere2, color=gray_light)]
+    # modify inertia of the first body to ensure we obtain the desired behavior
+    bodies[1].inertia = Diagonal([1.9, 2.1, 2.0])
+
+    # define the joints, the 1st joint is between Origin and :sphere1
+    # the 2nd joint is between :sphere1 and :sphere2
+    joints = [
+        JointConstraint(Floating(origin, bodies[1]),
+                name=:floating_joint),
+        JointConstraint(Fixed(bodies[1], bodies[2],
+                parent_vertex=[0,0,radius],
+                child_vertex=zeros(3)),
+                name = :fixed_joint)]
+
+    # define the two spherical contacts, each contact is attached to a body.
+    contact_point = [0.0, 0.0, 0.0] # both contact spheres are located at the center of the bodies they are attached to.
+    normal = [0.0, 0.0, 1.0] # the normal of the contact is always pointing in the upright direction because the floor is flat.
+    # we specify the type of contact (impact, linear, nonlinear), the coefficient of friction, the radius of the contact sphere, etc.
+    contacts = [
+        contact_constraint(get_body(mechanism, :sphere1), normal,
+            friction_coefficient=friction_coefficient,
+            contact_point=contact_point, offset=[0.0, 0.0, radius],
+            contact_type=contact_type),
+        contact_constraint(get_body(mechanism, :sphere2), normal,
+            friction_coefficient=friction_coefficient,
+            contact_point=contact_point,
+            offset=[0.0, 0.0, radius * α],
+            contact_type=contact_type)
+        ]
+
+    # we build the mechanism by combining together the the origin, the bodies, the joints, and the contacts.
+    mechanism = Mechanism(origin, bodies, joints, contacts,
+        gravity=gravity,
+        timestep=timestep)
+
+    # we make sure that the mechanism is never initialized with its contact spheres below the ground.
+    set_minimal_coordinates!(mechanism, get_joint(mechanism, :floating_joint), [0.0; 0.0; radius; zeros(3)])
+    return mechanism
+end
+```
+
+### Initialize Mechanism
+The second method that we need to define is `initialize_...` where `...` is the name of the mechanism. So here we define `initialize_tippetop`. This function initialize the dynamical system to a certain state. This means that we set the position orientation, linear and angular velocity of each body in the mechanism.
+
+
+```julia
+function initialize_tippetop!(mechanism::Mechanism{T};
+    x=zeros(3),
+    θ=zeros(3),
+    v=zeros(3),
+    ω=zeros(3)) where T
+
+    # we retrieve the joints from the mechanism by their names
+    floating_joint = get_joint(mechanism, :floating_joint)
+    fixed_joint = get_joint(mechanism, :fixed_joint)
+    radius = fixed_joint.translational.vertices[1][3]
+
+    # 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; ω])
+    # we set to minimal coordinates of the fixed joint (this joint has zero minimal coordinate).
+    set_minimal_coordinates_velocities!(mechanism, fixed_joint)
+    return nothing
+end
+```

docs/src/assets/tippetop_real.gif

@@ -1,39 +1,39 @@
-function get_snake(; 
-    timestep=0.01, 
-    gravity=[0.0; 0.0; -9.81], 
-    friction_coefficient=0.8, 
+function get_snake(;
+    timestep=0.01,
+    gravity=[0.0; 0.0; -9.81],
+    friction_coefficient=0.8,
     contact=true,
-    contact_type=:nonlinear, 
-    spring=0.0, 
-    damper=0.0, 
+    contact_type=:nonlinear,
+    spring=0.0,
+    damper=0.0,
     num_bodies=2,
-    joint_type=:Spherical, 
-    height=1.0, 
+    joint_type=:Spherical,
+    length=1.0,
     radius=0.05,
     T=Float64)
 
     # Parameters
-    ex = [1.0; 0.0; 0.0] 
-    ey = [0.0; 1.0; 0.0] 
-    ez = [0.0; 0.0; 1.0] 
+    ex = [1.0; 0.0; 0.0]
+    ey = [0.0; 1.0; 0.0]
+    ez = [0.0; 0.0; 1.0]
 
-    vert11 = [0.0; 0.0; height / 2.0]
+    vert11 = [0.0; 0.0; length / 2.0]
     vert12 = -vert11
 
     # Links
     origin = Origin{T}()
-    bodies = [Box(3 * radius, 2 * radius, height, height, color=RGBA(1.0, 0.0, 0.0)) for i = 1:num_bodies]
+    bodies = [Box(3 * radius, 2 * radius, length, length, color=RGBA(0.9, 0.9, 0.9)) for i = 1:num_bodies]
 
     # Constraints
-    jointb1 = JointConstraint(Floating(origin, bodies[1], 
-        spring=0.0, 
+    jointb1 = JointConstraint(Floating(origin, bodies[1],
+        spring=0.0,
         damper=0.0))
 
     if num_bodies > 1
-        joints = [JointConstraint(Prototype(joint_type, bodies[i - 1], bodies[i], ex; 
-            parent_vertex=vert12, 
-            child_vertex=vert11, 
-            spring=spring, 
+        joints = [JointConstraint(Prototype(joint_type, bodies[i - 1], bodies[i], ex;
+            parent_vertex=vert12,
+            child_vertex=vert11,
+            spring=spring,
             damper=damper)) for i = 2:num_bodies]
         joints = [jointb1; joints]
     else
@@ -45,35 +45,35 @@ function get_snake(;
         normal = [[0.0; 0.0; 1.0] for i = 1:n]
         friction_coefficient = friction_coefficient * ones(n)
 
-        contacts1 = contact_constraint(bodies, normal, 
-            friction_coefficient=friction_coefficient, 
-            contact_points=fill(vert11, n), 
+        contacts1 = contact_constraint(bodies, normal,
+            friction_coefficient=friction_coefficient,
+            contact_points=fill(vert11, n),
             contact_type=contact_type) # we need to duplicate point for prismatic joint for instance
-        contacts2 = contact_constraint(bodies, normal, 
-            friction_coefficient=friction_coefficient, 
-            contact_points=fill(vert12, n), 
+        contacts2 = contact_constraint(bodies, normal,
+            friction_coefficient=friction_coefficient,
+            contact_points=fill(vert12, n),
             contact_type=contact_type)
-        mech = Mechanism(origin, bodies, joints, [contacts1; contacts2], 
-            gravity=gravity, 
-            timestep=timestep, 
-            spring=spring, 
+        mech = Mechanism(origin, bodies, joints, [contacts1; contacts2],
+            gravity=gravity,
+            timestep=timestep,
+            spring=spring,
             damper=damper)
     else
-        mech = Mechanism(origin, bodies, joints, 
-            gravity=gravity, 
-            timestep=timestep, 
-            spring=spring, 
+        mech = Mechanism(origin, bodies, joints,
+            gravity=gravity,
+            timestep=timestep,
+            spring=spring,
             damper=damper)
     end
     return mech
 end
 
-function initialize_snake!(mechanism::Mechanism{T}; 
+function initialize_snake!(mechanism::Mechanism{T};
     base_position=[0.0, -0.5, 0.0],
     base_orientation=UnitQuaternion(RotX(0.6 * π)),
-    base_linear_velocity=zeros(3), 
+    base_linear_velocity=zeros(3),
     base_angular_velocity=zeros(3),
-    relative_linear_velocity=zeros(3), 
+    relative_linear_velocity=zeros(3),
     relative_angular_velocity=zeros(3)) where T
 
     pbody = mechanism.bodies[1]
@@ -82,22 +82,22 @@ function initialize_snake!(mechanism::Mechanism{T};
     vert12 = -vert11
 
     # set position and velocities
-    set_maximal_configurations!(mechanism.origin, pbody, 
-        child_vertex=base_position, 
+    set_maximal_configurations!(mechanism.origin, pbody,
+        child_vertex=base_position,
         Δq=base_orientation)
-    set_maximal_velocities!(pbody, 
-        v=base_linear_velocity, 
+    set_maximal_velocities!(pbody,
+        v=base_linear_velocity,
         ω=base_angular_velocity)
 
     previd = pbody.id
     for body in mechanism.bodies[2:end]
-        set_maximal_configurations!(get_body(mechanism, previd), body, 
-            parent_vertex=vert12, 
+        set_maximal_configurations!(get_body(mechanism, previd), body,
+            parent_vertex=vert12,
             child_vertex=vert11)
-        set_maximal_velocities!(get_body(mechanism, previd), body, 
-            parent_vertex=vert12, 
+        set_maximal_velocities!(get_body(mechanism, previd), body,
+            parent_vertex=vert12,
             child_vertex=vert11,
-            Δv=relative_linear_velocity, 
+            Δv=relative_linear_velocity,
             Δω=relative_angular_velocity)
         previd = body.id
     end

docs/src/define_mechanism.md

@@ -1,84 +1,68 @@
-function get_tippetop(; 
-    timestep=0.01, 
-    gravity=[0.0; 0.0; -9.81], 
-    friction_coefficient=0.8, 
-    contact=true, 
+function get_tippetop(;
+    timestep=0.01,
+    gravity=[0.0; 0.0; -9.81],
+    friction_coefficient=0.4,
+    contact=true,
     contact_type=:nonlinear,
     T=Float64)
 
     origin = Origin{T}(name=:origin)
     radius = 0.5
     mass = 1.0
     α = 0.2
-    bodies = [Sphere(radius, mass, name=:sphere1, color=cyan), Sphere(radius*α, mass*α, name=:sphere2, color=cyan)]
-    joints = [JointConstraint(Floating(origin, bodies[1]), 
+    bodies = [
+        Sphere(radius, mass, name=:sphere1, color=gray_light),
+        Sphere(radius*α, mass*α^3, name=:sphere2, color=gray_light)]
+
+    joints = [
+        JointConstraint(Floating(origin, bodies[1]),
                 name=:floating_joint),
-              JointConstraint(Fixed(bodies[1], bodies[2], 
-                parent_vertex=[0,0,radius], 
-                child_vertex=zeros(3)), name = :fixed_joint),]
+        JointConstraint(Fixed(bodies[1], bodies[2],
+                parent_vertex=[0,0,radius],
+                child_vertex=zeros(3)),
+                name = :fixed_joint)]
     mechanism = Mechanism(origin, bodies, joints,
-        timestep=timestep, 
+        timestep=timestep,
         gravity=gravity)
 
     # modify inertias
     mechanism.bodies[1].inertia = Diagonal([1.9, 2.1, 2.0])
 
     if contact
-        contact = [0.0, 0.0, 0.0]
+        contact_point = [0.0, 0.0, 0.0]
         normal = [0.0, 0.0, 1.0]
         contacts = [
-            contact_constraint(get_body(mechanism, :sphere1), normal, 
-                friction_coefficient=friction_coefficient, 
-                contact_point=contact, offset=[0.0, 0.0, radius], 
+            contact_constraint(get_body(mechanism, :sphere1), normal,
+                friction_coefficient=friction_coefficient,
+                contact_point=contact_point, offset=[0.0, 0.0, radius],
                 contact_type=contact_type),
-            contact_constraint(get_body(mechanism, :sphere2), normal, 
-                friction_coefficient=friction_coefficient, 
-                contact_point=contact, 
-                offset=[0.0, 0.0, radius * α], 
+            contact_constraint(get_body(mechanism, :sphere2), normal,
+                friction_coefficient=friction_coefficient,
+                contact_point=contact_point,
+                offset=[0.0, 0.0, radius * α],
                 contact_type=contact_type)
             ]
         set_minimal_coordinates!(mechanism, get_joint(mechanism, :floating_joint), [0.0; 0.0; radius; zeros(3)])
         mechanism = Mechanism(origin, bodies, joints, contacts,
-            gravity=gravity, 
+            gravity=gravity,
             timestep=timestep)
     end
 
     return mechanism
 end
 
-function initialize_tippetop!(mechanism::Mechanism{T}; 
+function initialize_tippetop!(mechanism::Mechanism{T};
     x=zeros(3),
-    q=one(UnitQuaternion), 
+    θ=zeros(3),
     v=zeros(3),
     ω=zeros(3)) where T
 
-    joint2 = get_joint(mechanism, :fixed_joint)
-    radius = joint2.constraints[1].vertices[1][3]
-    origin = mechanism.origin
-    pbody = get_body(mech, :sphere1)
-    cbody = get_body(mech, :sphere2)
+    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_maximal_configurations!(origin, pbody; 
-        parent_vertex=[0.0; 0.0; radius], 
-        child_vertex=[0.0; 0.0; 0.0], 
-        Δx=x, 
-        Δq=q)
-    set_maximal_configurations!(pbody,  cbody; 
-        parent_vertex=[0.0; 0.0; radius], 
-        child_vertex=[0.0; 0.0; 0.0], 
-        Δx=[0.0; 0.0; 0.0], 
-        Δq=one(UnitQuaternion))
-    set_maximal_velocities!(origin, pbody; 
-        parent_vertex=[0.0; 0.0; radius], 
-        child_vertex=[0.0; 0.0; 0.0], 
-        Δv=v, 
-        Δω=ω)
-    set_maximal_velocities!(pbody,  cbody; 
-        parent_vertex=[0.0; 0.0; radius], 
-        child_vertex=[0.0; 0.0; 0.0], 
-        Δv=[0.0; 0.0; 0.0], 
-        Δω=[0.0; 0.0; 0.0])
+    set_minimal_coordinates_velocities!(mechanism, floating_joint, xmin=[x; θ; v; ω])
+    set_minimal_coordinates_velocities!(mechanism, fixed_joint)
     return nothing
 end