External forcing reset, offset, docs

src/bodies/set.jl

@@ -97,7 +97,26 @@ function set_maximal_velocities!(pbody::Node, cbody::Body;
     return set_maximal_velocities!(cbody; v = v2, ω = ω2)
 end
 
-function set_external_force!(body::Body; force=zeros(3), torque=zeros(3))
-    body.state.Fext = force
-    body.state.τext = torque
+"""
+    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/integrators/constraint.jl

@@ -11,8 +11,8 @@ 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 * mechanism.gravity + state.Fext)
-    D2x =   1.0 / timestep * mass * (x3 - x2) - 0.5 * timestep * (mass * mechanism.gravity + state.Fext)
+    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
 

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

@@ -43,15 +43,15 @@ end
     mech = get_mechanism(:block;gravity=0, contact=false, mass=1)
     mech.bodies[1].inertia = diagm(ones(3))
 
-    initialize!(mech, :block; position=zeros(3), orientation=one(Quaternion), velocity=zeros(3), angular_velocity=zeros(3))
-    set_external_force!(mech.bodies[1];force=[1;0;0])
-    storage = simulate!(mech, 1.0, record=true)
-    @test norm(mech.bodies[1].state.vsol[1][1] - 1.0) < 1.0e-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
 
-    initialize!(mech, :block; position=zeros(3), orientation=one(Quaternion), velocity=zeros(3), angular_velocity=zeros(3))
-    set_external_force!(mech.bodies[1];torque=[1;0;0])
-    storage = simulate!(mech, 1.0, record=true)
-    @test norm(mech.bodies[1].state.ωsol[1][1] - 1.0) < 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