Merge pull request #78 from dojo-sim/jan/input_update

External forcing

DojoEnvironments/src/mechanisms/cartpole/mechanism.jl

@@ -17,13 +17,13 @@ function get_cartpole(;
     # mechanism
     origin = Origin{Float64}()
     slider = Capsule(1.5 * radius, 1, slider_mass; 
-        orientation_offset=Dojo.RotX(0.5 * π), color)
-    pendulum = Capsule(radius, pendulum_length, pendulum_mass; color)
+        orientation_offset=Dojo.RotX(0.5 * π), color, name=:cart)
+    pendulum = Capsule(radius, pendulum_length, pendulum_mass; color, name=:pole)
     bodies = [slider, pendulum]
 
-    joint_origin_slider = JointConstraint(Prismatic(origin, slider, Y_AXIS))
+    joint_origin_slider = JointConstraint(Prismatic(origin, slider, Y_AXIS); name=:cart_joint)
     joint_slider_pendulum = JointConstraint(Revolute(slider, pendulum, X_AXIS; 
-        child_vertex=-0.5*pendulum_length*Z_AXIS))
+        child_vertex=-0.5*pendulum_length*Z_AXIS); name=:pole_joint)
     joints = [joint_origin_slider, joint_slider_pendulum]
 
     mechanism = Mechanism(origin, bodies, joints;

docs/src/creating_simulation/define_controller.md

@@ -1,54 +1,59 @@
 # 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. The controller is a method that always takes 2 input arguments:
+We focus on the stabilization of the cartpole, which has two joints but only a single input on the cart. The controller is a method that always takes 2 input arguments:
 - a [`Mechanism`](@ref),
-- an integer `t` indicating the current simulation step.
-For the pendulum, the controller computes the control inputs based on the current state `x`, the goal state `x_goal` and a PID controller.
+- an integer `k` indicating the current simulation step.
+For the cartpole, the controller computes the control input based on the current state `x`, the goal state `x_goal` and an LQR controller. The simulation step is not used in this example.
+
+There are three ways to apply inputs to the system
+- set an input directly to a joint
+- set a set of inputs to all joints of the mechanism
+- set an external force on bodies
 
 ```julia
 # ### Setup
 using Dojo
 using DojoEnvironments
 
 # ### Mechanism
-mechanism = get_mechanism(:pendulum)
+mechanism = get_mechanism(:cartpole)
 
 # ### Controller
-summed_error = 0 # for integral part
+K = [-0.948838; -2.54837; 48.6627; 10.871]
 
 function controller!(mechanism, k)
     ## Target state
-    x_goal = [π/2; 0.0]
+    x_goal = [0;0; 0.0;0]
 
     ## Current state
     x = get_minimal_state(mechanism)
 
-    error = (x_goal-x)
-    global summed_error += error[1]*mechanism.timestep
+    ## Control inputs
+    u = -K' * (x - x_goal)
 
-    ## Gains
-    Kp = 25
-    Ki = 50
-    Kd = 5
+    ## 3 ways to set input:
 
-    ## Control inputs
-    u = Kp*error[1] + Ki*summed_error + Kd*error[2]
-    set_input!(mechanism, [u])
-end
+    ## 1: get joint and set input
+    cart_joint = get_joint(mechanism, :cart_joint)
+    set_input!(cart_joint, [u])
 
-# ### Simulate
-initialize!(mechanism, :pendulum,
-    angle=0.0, angular_velocity=0.0)
+    ## 2: set input for all joints at once
+    ## set_input!(mechanism, [u;0]) # need to know joint order
 
-storage = simulate!(mechanism, 5.0, controller!, record=true)
+    ## 3: direct external force on body
+    ## cart = get_body(mechanism, :cart)
+    ## set_external_force!(cart; force=[0;u;0])
 
+end
+
+# ### Simulate 
+initialize!(mechanism, :cartpole; position=0, orientation=pi/4)
+
+storage = simulate!(mechanism, 10.0, controller!, record=true)
+
+
 # ### Visualize
 vis = visualize(mechanism, storage)
 render(vis)
 ```
-
-By changing the parameters, you should be able to perform a swing up. You might need to be carful due to a possible sign-change in the top position. 
-```@raw html
-<img src="../assets/animations/pendulum.gif" width="300"/>
-```

examples/control/cartpole_lqr.jl

@@ -25,7 +25,20 @@ function controller!(mechanism, k)
 
     ## Control inputs
     u = -K * (x - x_goal)
-    set_input!(mechanism, [u;0]) # input only to cart
+
+    ## 3 ways to set input:
+
+    ## 1: get joint and set input
+    cart_joint = get_joint(mechanism, :cart_joint)
+    set_input!(cart_joint, u)
+
+    ## 2: set input for all joints at once
+    ## set_input!(mechanism, [u;0]) # need to know joint order
+
+    ## 3: direct external force on body
+    ## cart = get_body(mechanism, :cart)
+    ## set_external_force!(cart; force=[0;u;0])
+
 end
 
 # ### Simulate 

src/Dojo.jl

@@ -156,7 +156,8 @@ export
     Mesh,
     CombinedShapes,
     get_body,
-    get_node
+    get_node,
+    set_external_force!
 
 # Joints
 export

src/bodies/set.jl

@@ -96,3 +96,27 @@ function set_maximal_velocities!(pbody::Node, cbody::Body;
 
     return set_maximal_velocities!(cbody; v = v2, ω = ω2)
 end
+
+"""
+    set_external_force!(body; force, torque, vertex)
+
+    applies an external force on a body
+
+    body: Body 
+    force: force in body frame
+    torque: torque in local frame
+    vertex: point where force is applied in local frame
+"""
+function set_external_force!(body::Body; force=zeros(3), torque=zeros(3), vertex=zeros(3))
+    body.state.Fext = vector_rotate(force, body.state.q2)
+    body.state.τext = torque + cross(vertex, force)
+
+    return
+end
+
+function clear_external_force!(body::Body{T}) where T
+    body.state.Fext = szeros(T,3)
+    body.state.τext = szeros(T,3)
+
+    return
+end

src/bodies/state.jl

@@ -13,6 +13,8 @@
     q2: orientation (Quaternion) at current time step 
     JF2: linear impulse (force * time step) applied at current time step 
     Jτ2: angular impulse (torque * timestep) applied at current time step 
+    Fext: external force applied at current time step 
+    τext: external torque applied at current time step 
 
     vsol: linear velocity at time step 2.5 (midpoint); contains current value (index 1) and candidate value (index 2)
     ωsol: angular velocity at time step 2.5 (midpoint); contains current value (index 1) and candidate value (index 2)
@@ -32,6 +34,8 @@ mutable struct State{T}
     q2::Quaternion{T}
     JF2::SVector{3,T}
     Jτ2::SVector{3,T}
+    Fext::SVector{3,T}
+    τext::SVector{3,T}
 
     # solution estimate [before step; after step]
     vsol::Vector{SVector{3,T}}
@@ -51,14 +55,16 @@ mutable struct State{T}
         q2 = one(Quaternion{T})
         JF2 = szeros(T, 3)
         Jτ2 = szeros(T, 3)
+        Fext = szeros(T, 3)
+        τext = szeros(T, 3)
 
         vsol = [szeros(T, 3) for i=1:2]
         ωsol = [szeros(T, 3) for i=1:2]
 
         d = szeros(T, 6)
         D = szeros(T, 6, 6)
 
-        new{T}(x1, q1, v15, ω15, x2, q2, JF2, Jτ2, vsol, ωsol, d, D)
+        new{T}(x1, q1, v15, ω15, x2, q2, JF2, Jτ2, Fext, τext, vsol, ωsol, d, D)
     end
 end
 
@@ -73,6 +79,8 @@ function Base.show(io::IO, mime::MIME{Symbol("text/plain")}, state::State{T}) wh
     println(io, "q2:   "*string(state.q2))
     println(io, "JF2:   "*string(state.JF2))
     println(io, "Jτ2:   "*string(state.Jτ2))
+    println(io, "Fext: "*string(state.Fext))
+    println(io, "τext: "*string(state.τext))
     println(io, "vsol: "*string(state.vsol))
     println(io, "ωsol: "*string(state.ωsol))
     println(io, "d:    "*string(state.d))

src/integrators/constraint.jl

@@ -11,10 +11,10 @@ function constraint(mechanism::Mechanism{T,Nn,Ne,Nb}, body::Body{T}) where {T,Nn
     x3, q3 = next_configuration(state, timestep)
 
     # dynamics
-    D1x = - 1.0 / timestep * mass * (x2 - x1) - 0.5 * timestep * mass * gravity
-    D2x =   1.0 / timestep * mass * (x3 - x2) - 0.5 * timestep * mass * gravity
-    D1q = -2.0 / timestep * LVᵀmat(q2)' * Lmat(q1) * Vᵀmat() * inertia * Vmat() * Lmat(q1)' * vector(q2)
-    D2q = -2.0 / timestep * LVᵀmat(q2)' * Tmat() * Rmat(q3)' * Vᵀmat() * inertia * Vmat() * Lmat(q2)' * vector(q3)
+    D1x = - 1.0 / timestep * mass * (x2 - x1) - 0.5 * timestep * (mass * gravity + state.Fext)
+    D2x =   1.0 / timestep * mass * (x3 - x2) - 0.5 * timestep * (mass * gravity + state.Fext)
+    D1q = -2.0 / timestep * LVᵀmat(q2)' * Lmat(q1) * Vᵀmat() * inertia * Vmat() * Lmat(q1)' * vector(q2) - 0.5 * timestep * state.τext
+    D2q = -2.0 / timestep * LVᵀmat(q2)' * Tmat() * Rmat(q3)' * Vᵀmat() * inertia * Vmat() * Lmat(q2)' * vector(q3) - 0.5 * timestep * state.τext
 
     dynT = D2x + D1x
     dynR = D2q + D1q

src/mechanics/momentum.jl

@@ -26,8 +26,8 @@ function momentum(mechanism::Mechanism{T}, body::Body{T}) where T
     v15 = body.state.vsol[2] # v1.5
     ω15 = body.state.ωsol[2] # ω1.5
 
-    D2x = 1 / timestep * mass * (x3 - x2) - 0.5 * timestep * mass * mechanism.gravity
-    D2q = -2.0 / timestep * LVᵀmat(q2)' * Tmat() * Rmat(q3)' * Vᵀmat() * inertia * Vmat() * Lmat(q2)' * vector(q3)
+    D2x = 1 / timestep * mass * (x3 - x2) - 0.5 * timestep * (mass * mechanism.gravity + state.Fext)
+    D2q = -2.0 / timestep * LVᵀmat(q2)' * Tmat() * Rmat(q3)' * Vᵀmat() * inertia * Vmat() * Lmat(q2)' * vector(q3) - 0.5 * timestep * state.τext
     p_linear_body = D2x - 0.5 * state.JF2
     p_angular_body = D2q - 0.5 * state.Jτ2
 

src/simulation/simulate.jl

@@ -26,6 +26,7 @@ function simulate!(mechanism::Mechanism, steps::AbstractUnitRange, storage::Stor
         control!(mechanism, k)
         for joint in mechanism.joints input_impulse!(joint, mechanism) end
         status = mehrotra!(mechanism, opts=opts)
+        for body in mechanism.bodies clear_external_force!(body) end
         abort_upon_failure && (status == :failed) && break
         record && save_to_storage!(mechanism, storage, k)
         (k != steps[end]) && (for body in mechanism.bodies update_state!(body, mechanism.timestep) end)

test/behaviors.jl

@@ -39,6 +39,21 @@ end
     end
 end
 
+@testset "Box external force" begin
+    mech = get_mechanism(:block;gravity=0, contact=false, mass=1)
+    mech.bodies[1].inertia = diagm(ones(3))
+
+    controller!(mechanism, k) = (k<=50 ? set_external_force!(mechanism.bodies[1];force=[1;0;0],vertex=[0.5;0;0]) : nothing)
+    initialize!(mech, :block; position=zeros(3), orientation=Dojo.RotZ(pi/2), velocity=zeros(3), angular_velocity=zeros(3))    
+    storage = simulate!(mech, 1.0, controller!, record=true)
+    @test norm(mech.bodies[1].state.vsol[1][2] - 0.5) < 1.0e-3
+
+    controller!(mechanism, k) = (k<=50 ? set_external_force!(mechanism.bodies[1];torque=[1;0;0],vertex=[0.5;0;0]) : nothing)
+    initialize!(mech, :block; position=zeros(3), orientation=Dojo.RotZ(pi/2), velocity=zeros(3), angular_velocity=zeros(3))
+    storage = simulate!(mech, 1.0, controller!, record=true)
+    @test norm(mech.bodies[1].state.ωsol[1][1] - 0.5) < 1.0e-3
+end
+
 @testset "Four-bar linkage" begin
     for timestep in [0.10, 0.05, 0.01, 0.005]
         mech = get_mechanism(:fourbar;