fixing tippetop

Author: simon-lc

docs/src/define_mechanism.md

@@ -1 +1,4 @@
-# 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. 

environments/snake/methods/initialize.jl

@@ -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

environments/tippetop/methods/initialize.jl

@@ -1,23 +1,23 @@
-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=cyan), Sphere(radius*α, mass*α^3, name=:sphere2, color=cyan)]
+    joints = [JointConstraint(Floating(origin, bodies[1]),
                 name=:floating_joint),
-              JointConstraint(Fixed(bodies[1], bodies[2], 
-                parent_vertex=[0,0,radius], 
+              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
@@ -27,58 +27,69 @@ function get_tippetop(;
         contact = [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, 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,
+                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), 
+    q=one(UnitQuaternion),
     v=zeros(3),
     ω=zeros(3)) where T
 
     joint2 = get_joint(mechanism, :fixed_joint)
-    radius = joint2.constraints[1].vertices[1][3]
+    radius = joint2.translational.vertices[1][3]
     origin = mechanism.origin
     pbody = get_body(mech, :sphere1)
     cbody = get_body(mech, :sphere2)
 
     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, 
+
+    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], 
+    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!(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], 
+    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])
     return nothing
 end
+
+
+
+mech = get_tippetop(timestep=0.01)
+initialize!(mech, :tippetop, ω=[0,0.05,5.0])
+
+storage = simulate!(mech, 3.0, verbose=false, record=true)
+
+# vis = Visualizer()
+# open(vis)
+visualize(mech, storage, vis=vis)

examples/simulation/tippetop.jl

@@ -6,25 +6,25 @@
 using Dojo
 
 # ## Mechanism
-mech = get_mechanism(:tippetop, 
-    timestep=0.01, 
-    gravity=-9.81, 
-    contact=true, 
+mech = get_mechanism(:tippetop,
+    timestep=0.01,
+    gravity=-9.81,
+    friction_coefficient=0.4,
+    contact=true,
     contact_type=:nonlinear)
 
 # ## Simulate
-initialize!(mech, :tippetop, 
-    x=[0.0, 0.0, 1.0], 
-    q=UnitQuaternion(RotX(0.01 * π)), 
+initialize!(mech, :tippetop,
+    x=[0.0, 0.0, 1.0],
+    q=UnitQuaternion(RotX(0.01 * π)),
     ω=[0.0, 0.01, 50.0])
 
-storage = simulate!(mech, 25.0, 
-    record=true, 
-    verbose=false, 
-    opts=SolverOptions(verbose=false, btol=1e-6))
+storage = simulate!(mech, 10.0,
+    record=true,
+    verbose=false,
+    opts=SolverOptions(verbose=false, btol=1e-5))
 
 # ## Open visualizer
-vis=visualizer()
-open(vis)
-visualize(mech, storage, 
-    vis=vis)
+vis = Visualizer()
+render(vis)
+visualize(mech, storage, vis=vis)