cleaning pendulum example

Author: simon-lc

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 
+### Tippe top
+<img src="examples/animations/pendulum.gif" width="150"/>
+
+## Trajectory Optimization
 
-### Box 
+### 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/define_controller.md

@@ -1 +1,58 @@
-# 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.
+
+
+
+
+# ## Setup
+using Dojo
+
+# ## Mechanism
+mechanism = get_mechanism(:pendulum,
+    timestep=0.01,
+    gravity=-9.81,
+    damper=5.0,
+    spring=0.0)
+
+# ## Controller
+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
+
+    off = 0
+    for joint in mechanism.joints
+        nu = input_dimension(joint)
+
+        ## Get joint configuration + velocity
+        xi = x[off .+ (1:2nu)]
+        xi_goal = x_goal[off .+ (1:2nu)]
+
+        ## Control
+        ui = -K * (xi - xi_goal)
+        set_input!(joint, ui)
+
+        off += nu
+    end
+end
+
+# ##Simulate
+initialize!(mechanism, :pendulum,
+    angle=0.0 * π,
+    angular_velocity=0.0);
+
+storage = simulate!(mechanism, 2.0, controller!,
+    record=true,
+    verbose=true);
+
+# ## Visualize
+vis = Visualizer()
+render(vis)
+visualize(mechanism, storage, vis=vis);

environments/pendulum/methods/initialize.jl

@@ -1,62 +1,64 @@
-function get_pendulum(; 
-    timestep=0.01, 
-    gravity=-9.81, 
-    mass=1.0, 
+function get_pendulum(;
+    timestep=0.01,
+    gravity=-9.81,
+    mass=1.0,
     len=1.0,
-    spring=0.0, 
-    damper=0.0, 
-    spring_offset=szeros(1), 
+    spring=0.0,
+    damper=0.0,
+    spring_offset=szeros(1),
     axis_offset=one(UnitQuaternion),
     T=Float64)
 
     # Parameters
     joint_axis = [1.0; 0.0; 0.0]
     width, depth = 0.1, 0.1
-    child_vertex = [0.0; 0.0; len / 2.0] # joint connection point
+    parent_vertex = [0.0; 0.0; 1.1 * len] # joint connection point
+    child_vertex = [0.0; 0.0; 0.5 * len] # joint connection point
 
     # Links
     origin = Origin{T}()
-    pbody = Box(width, depth, len, mass)
+    body = Box(width, depth, len, mass, name=:pendulum)
 
     # Constraints
-    joint_between_origin_and_pbody = JointConstraint(Revolute(origin, pbody, joint_axis; 
-        child_vertex=child_vertex, 
-        spring=spring, 
-        damper=damper, 
+    joint = JointConstraint(Revolute(origin, body, joint_axis;
+        parent_vertex=parent_vertex,
+        child_vertex=child_vertex,
+        spring=spring,
+        damper=damper,
         rot_spring_offset=spring_offset,
-        axis_offset=axis_offset))
+        axis_offset=axis_offset), name=:joint)
 
-    bodies = [pbody]
-    joints = [joint_between_origin_and_pbody]
+    bodies = [body]
+    joints = [joint]
 
-    mech = Mechanism(origin, bodies, joints, 
-        gravity=gravity, 
-        timestep=timestep, 
-        spring=spring, 
+    mech = Mechanism(origin, bodies, joints,
+        gravity=gravity,
+        timestep=timestep,
+        spring=spring,
         damper=damper)
 
     return mech
 end
 
-function initialize_pendulum!(mechanism::Mechanism; 
-    angle=0.7, 
+function initialize_pendulum!(mechanism::Mechanism;
+    angle=0.7,
     angular_velocity=0.0) where T
     joint = mechanism.joints[1]
-    set_minimal_coordinates_velocities!(mechanism, joint; 
+    set_minimal_coordinates_velocities!(mechanism, joint;
         xmin=[angle, angular_velocity])
 end
 
-function get_npendulum(; 
-    timestep=0.01, 
-    gravity=-9.81, 
-    mass=1.0, 
+function get_npendulum(;
+    timestep=0.01,
+    gravity=-9.81,
+    mass=1.0,
     len=1.0,
-    spring=0.0, 
-    damper=0.0, 
+    spring=0.0,
+    damper=0.0,
     num_bodies=5,
-    basetype=:Revolute, 
+    basetype=:Revolute,
     joint_type=:Revolute,
-    T=Float64) 
+    T=Float64)
 
     # Parameters
     ex = [1.0; 0.0; 0.0]
@@ -69,31 +71,31 @@ function get_npendulum(;
     bodies = [Box(r, r, len, mass, color=RGBA(1.0, 0.0, 0.0)) for i = 1:num_bodies]
 
     # Constraints
-    jointb1 = JointConstraint(Prototype(basetype, origin, bodies[1], ex; 
-        child_vertex=vert11, 
-        spring=spring, 
+    jointb1 = JointConstraint(Prototype(basetype, origin, bodies[1], ex;
+        child_vertex=vert11,
+        spring=spring,
         damper=damper))
     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
         joints = [jointb1]
     end
 
-    mech = Mechanism(origin, bodies, joints, 
-        gravity=gravity, 
+    mech = Mechanism(origin, bodies, joints,
+        gravity=gravity,
         timestep=timestep)
     return mech
 end
 
-function initialize_npendulum!(mechanism::Mechanism{T}; 
-    base_angle=π / 4.0, 
+function initialize_npendulum!(mechanism::Mechanism{T};
+    base_angle=π / 4.0,
     base_angular_velocity=[0.0, 0.0, 0.0],
-    relative_linear_velocity=[0.0, 0.0, 0.0], 
+    relative_linear_velocity=[0.0, 0.0, 0.0],
     relative_angular_velocity=[0.0, 0.0, 0.0]) where T
 
     pbody = mechanism.bodies[1]
@@ -102,19 +104,19 @@ function initialize_npendulum!(mechanism::Mechanism{T};
     vert12 = -vert11
 
     # set position and velocities
-    set_maximal_configurations!(mechanism.origin, pbody, 
-        child_vertex=vert11, 
+    set_maximal_configurations!(mechanism.origin, pbody,
+        child_vertex=vert11,
         Δq=UnitQuaternion(RotX(base_angle)))
-    set_maximal_velocities!(pbody, 
+    set_maximal_velocities!(pbody,
         ω=base_angular_velocity)
 
     previd = pbody.id
     for (i, body) in enumerate(Iterators.drop(mechanism.bodies, 1))
-        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, Δω=1 / i * relative_angular_velocity)
         previd = body.id

examples/animations/pendulum.gif

@@ -14,12 +14,12 @@ mech = get_mechanism(:dzhanibekov,
 
 # ## Simulate
 initialize_dzhanibekov!(mech,
-    angular_velocity=[8.0; 0.01; 0.0])
+    angular_velocity=[8.0; 0.01; 0.0]);
 storage = simulate!(mech, 8.00,
     record=true,
-    verbose=false)
+    verbose=false);
 
 # ## Visualizers
 vis = Visualizer()
-open(vis)
-visualize(mech, storage, vis=vis)
+render(vis)
+visualize(mech, storage, vis=vis);

examples/animations/pendulum.mp4

@@ -6,49 +6,49 @@
 using Dojo
 
 # ## Mechanism
-mechanism = get_mechanism(:pendulum, 
-    timestep=0.01, 
-    gravity=-9.81, 
-    spring)
+mechanism = get_mechanism(:pendulum,
+    timestep=0.01,
+    gravity=-9.81,
+    damper=5.0,
+    spring=0.0)
 
 # ## Controller
 function controller!(mechanism, k)
     ## Target state
     x_goal = [1.0 * π; 0.0]
 
     ## Current state
-    x = get_minimal_state(mechanism) 
+    x = get_minimal_state(mechanism)
 
-    ## Gains 
-    K = [10.0 0.5] * 0.1
+    ## Gains
+    K = [5.0 0.5] * 0.1
 
     off = 0
     for joint in mechanism.joints
         nu = input_dimension(joint)
-        
+
         ## Get joint configuration + velocity
         xi = x[off .+ (1:2nu)]
         xi_goal = x_goal[off .+ (1:2nu)]
-        
+
         ## Control
-        ui = -K * (xi - xi_goal) 
+        ui = -K * (xi - xi_goal)
         set_input!(joint, ui)
 
         off += nu
     end
 end
 
 # ##Simulate
-initialize!(mechanism, :pendulum, 
-    angle=0.0 * π, 
-    angular_velocity=0.0)
+initialize!(mechanism, :pendulum,
+    angle=0.0 * π,
+    angular_velocity=0.0);
 
-storage = simulate!(mechanism, 10.0, controller!, 
-    record=true, 
-    verbose=true)
+storage = simulate!(mechanism, 2.0, controller!,
+    record=true,
+    verbose=true);
 
 # ## Visualize
-vis=visualizer()
+vis = Visualizer()
 render(vis)
-visualize(mechanism, storage, 
-    vis=vis)
+visualize(mechanism, storage, vis=vis);