# Adding external force RigidBodyDynamics mechansms

**URL:** <https://discourse.julialang.org/t/adding-external-force-rigidbodydynamics-mechansms/45233>\
**Category:** General Usage\
**Tags:** question, package\
**Created:** [August 19, 2020, 11:55pm UTC](https://discourse.julialang.org/t/adding-external-force-rigidbodydynamics-mechansms/45233 "2020-08-19T23:55:35Z")\
**Posts on this page:** 6\
**Page:** 1

<div class="post-metadata">

**Author:** ![\_dominic](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/_dominic/32/7845_2.png) [@\_dominic](https://discourse.julialang.org/u/_dominic)\
**Post date:** [August 19, 2020, 11:55pm UTC](https://discourse.julialang.org/t/adding-external-force-rigidbodydynamics-mechansms/45233/1 "2020-08-19T23:55:35Z")

</div>

I am trying to figure out how to add an external force to a specific location on a rigid body. For example,  
 ![image](https://global.discourse-cdn.com/julialang/original/3X/f/a/fa0ae8d59eecd986903e6ff7d6c4173a3174b21f.png)

From the pendulum example, I can add a point to the tip of the second link

```julia
body = findbody(mechanism, "lower_link")
point = Point3D(default_frame(body), 0.0, 0, l_2) # control point
setelement!(vis, point, 0.05)

```

Here what I am not so sure of:

```julia
force = SVector(1.0, 0, 0) # 1N in the x direction
external_wrench = Wrench(f₃, ((transform_to_root(state, body) * point) × force), force)
# Wrench expressed in "world"
# angular: [0.0, 0.0, 0.0], linear: [1.0, 0.0, 0.0]

```

At this point, it’s not clear to me how I apply the force at the right location

---

<div class="post-metadata">

**Author:** ![tkoolen](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/tkoolen/32/1603_2.png) [@tkoolen](https://discourse.julialang.org/u/tkoolen)\
**Post date:** [August 20, 2020, 12:15am UTC](https://discourse.julialang.org/t/adding-external-force-rigidbodydynamics-mechansms/45233/2 "2020-08-20T00:15:22Z")

</div>

There’s actually a `Wrench` constructor specifically for this purpose:

[https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/fdb9e5ab12f0435318e87ee81d5f0a9edc08c762/src/spatial/spatialforce.jl#L179-L186](https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/fdb9e5ab12f0435318e87ee81d5f0a9edc08c762/src/spatial/spatialforce.jl#L179-L186)

After that, see e.g. [https://github.com/JuliaRobotics/RigidBodyDynamics.jl/issues/589](https://github.com/JuliaRobotics/RigidBodyDynamics.jl/issues/589) for an example showing how to pass the external wrench to the forward dynamics algorithm.

---

<div class="post-metadata">

**Author:** ![\_dominic](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/_dominic/32/7845_2.png) [@\_dominic](https://discourse.julialang.org/u/_dominic)\
**Post date:** [August 20, 2020, 12:19pm UTC](https://discourse.julialang.org/t/adding-external-force-rigidbodydynamics-mechansms/45233/3 "2020-08-20T12:19:18Z")

</div>

Julia types vexing me here. How do I declare the `result`, the example uses `result = robot.dynamicsresultcache[T]` but the mechanism does not come with a `dynamicsresultcache` field. I thought `DynamicResult(Float64, state.modcount)` would work, but that doesn’t work either.

```julia
zero!(state)
set_configuration!(vis, configuration(state))

wrench = Wrench(
    transform_to_root(state, body) * point,
    FreeVector3D(default_frame(world), [1.0, 0, 0]),
)
externalwrenches = Dict{BodyID,Wrench{Float64}}(
    BodyID(findbody(mechanism, "lower_link")) => transform(state, wrench, root_frame(mechanism)),
)

K, B = -0.002, 0.002
τsd = K * state.q + B * state.v # spring-damper

# result = robot.dynamicsresultcache[T]
result = DynamicResult(Float64, state.modcount)

# not sure which of these calls is going to work
dynamics!(result, state, state, τsd, wrench)
dynamics!(result, state, state, τsd, externalwrenches)

```

How do I fix this?

---

<div class="post-metadata">

**Author:** ![tkoolen](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/tkoolen/32/1603_2.png) [@tkoolen](https://discourse.julialang.org/u/tkoolen)\
**Post date:** [August 21, 2020, 3:06am UTC](https://discourse.julialang.org/t/adding-external-force-rigidbodydynamics-mechansms/45233/4 "2020-08-21T03:06:18Z")

</div>

Sorry about the rough start. Here’s a somewhat more fleshed out double pendulum example to get you going, also with a physical moment of inertia this time:

```julia
using RigidBodyDynamics
using LinearAlgebra
using StaticArrays
using MeshCatMechanisms

"""
Moment of inertia of a solid cylinder with mass m (uniformly distributed), height h and radius r,
aligned with the z-axis, about the center of mass.
Source: https://en.wikipedia.org/wiki/List_of_moments_of_inertia.
"""
function cylinder_moment_of_inertia(m::Number, h::Number, r::Number)
    I_x_and_y = 1 / 12 * m * (3 * r^2 + h^2)
    I_z = 1/2 * m * r^2
    diagm([I_x_and_y, I_x_and_y, I_z])
end

function double_pendulum()
    g = -9.81 # gravitational acceleration in z-direction
    world = RigidBody{Float64}("world")
    mechanism = Mechanism(world; gravity = [0, 0, g])
    axis = [0., 1., 0.]

    l_1 = 1. # link length
    r_1 = 0.05 # link radius
    m_1 = 1. # mass
    c_1 = l_1 / 2 # center of mass
    I_1 = cylinder_moment_of_inertia(m_1, l_1, r_1) # mass moment of inertia about CoM
    inertia_1 = SpatialInertia(CartesianFrame3D("upper_link"), # the reference frame in which the spatial inertia will be expressed
        moment_about_com=I_1,
        com=[0, 0, -c_1],
        mass=m_1)
    upper_link = RigidBody(inertia_1)
    shoulder = Joint("shoulder", Revolute(axis))
    attach!(mechanism, world, upper_link, shoulder)

    l_2 = 2. # link length
    r_2 = 0.05 # link radius
    m_2 = 2. # mass
    c_2 = l_2 / 2
    I_2 = cylinder_moment_of_inertia(m_2, l_2, r_2) # mass moment of inertia about CoM
    inertia_2 = SpatialInertia(CartesianFrame3D("lower_link"),
        moment_about_com=I_2,
        com=[0, 0, -c_2],
        mass=m_2)
    lower_link = RigidBody(inertia_2)
    elbow = Joint("elbow", Revolute(axis))
    before_elbow_to_after_shoulder = Transform3D(
        frame_before(elbow), frame_after(shoulder), SVector(0, 0, -l_1)) # TODO: still need an SVector here unfortunately.
    attach!(mechanism, upper_link, lower_link, elbow,
        joint_pose = before_elbow_to_after_shoulder)

    tip_frame = CartesianFrame3D("tip")
    tip_to_after_elbow = Transform3D(
        tip_frame, frame_after(elbow), SVector(0, 0, -l_2)) # TODO: still need an SVector here unfortunately.
    add_body_fixed_frame!(mechanism, tip_to_after_elbow)
    return mechanism, tip_frame
end

mechanism, tip_frame = double_pendulum()
state = MechanismState(mechanism)

shoulder, elbow = joints(mechanism)
set_configuration!(state, shoulder, 0.3)
set_configuration!(state, elbow, 0.4)
set_velocity!(state, shoulder, 1.)
set_velocity!(state, elbow, 2.)

vis = MechanismVisualizer(state, Skeleton(inertias=true))
open(vis)
setelement!(vis, frame_after(elbow))
setelement!(vis, tip_frame)

world, upper_link, lower_link = bodies(mechanism)
body = lower_link
# alternatives:
# body = findbody(mechanism, "lower_link")
# body = successor(elbow, mechanism)

point = Point3D(tip_frame, 0., 0., 0.) # control point
force = FreeVector3D(root_frame(mechanism), [1.0, 0, 0])
wrench = Wrench(transform_to_root(state, tip_frame) * point, force)
external_wrenches = Dict(BodyID(body) => wrench)

K, B = -0.002, -0.002
τsd = K * configuration(state) + B * velocity(state) # spring-damper

result = DynamicsResult(mechanism)
dynamics!(result, state, τsd, external_wrenches)
@show result.v̇
@show result.v̇[shoulder]
@show result.v̇[elbow]

```

---

<div class="post-metadata">

**Author:** ![\_dominic](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/_dominic/32/7845_2.png) [@\_dominic](https://discourse.julialang.org/u/_dominic)\
**Post date:** [August 21, 2020, 8:43pm UTC](https://discourse.julialang.org/t/adding-external-force-rigidbodydynamics-mechansms/45233/5 "2020-08-21T20:43:00Z")

</div>

Thanks for taking time to help!

I have played with example a bit but I cannot find a `simulate(...)` that takes external wrenches  
The `Dynamics` objects from `RigidBodySim` does not support external wrenches as well

```julia
struct Dynamics{M, JointCollection, C, P}
      statecache::StateCache{M, JointCollection}
      τcache::SegmentedVectorCache{JointID, Base.OneTo{JointID}}
      resultcache::DynamicsResultCache{M}
      control!::C
      setparams!::P
end

```

but I could replicate `simulate()` functionality (following one of the examples)

```julia
using RigidBodySim
open_loop_dynamics = Dynamics(mechanism)
problem = ODEProblem(Dynamics(mechanism, PeriodicController(τ, Δt, pdcontrol!)), state, (0., 5.));
sol = solve(problem, Tsit5())
setanimation!(vis, sol, realtime_rate=0.5)

```

I decided to modify `simulate()`. Ideally, I would like to get the external wrench from a function similar to `control!()`. For now, this compiles but it doesn’t seem to behave as expected, i.e. pulling the pendulum until it is suspended to the side (like in the image above). It looks like the wrench is not constantly applied

```julia
function simulate2(
    state0::MechanismState{X},
    final_time,
    control! = zero_torque!,
    externalwrenches::AbstractDict{BodyID,<:Wrench} = NullDict{BodyID,Wrench{X}}();
    Δt = 1e-4,
    stabilization_gains = Nothing,
) where {X}
    # T = cache_eltype(state0)
    T = Float64
    result = DynamicsResult{T}(state0.mechanism)
    control_torques = similar(velocity(state0))
    closed_loop_dynamics! =
        let result = result,
            control_torques = control_torques,
            ext_wrenches = ext_wrenches,
            stabilization_gains = stabilization_gains
            # https://github.com/JuliaLang/julia/issues/15276
            function (v̇::AbstractArray, ṡ::AbstractArray, t, state)
                # dynamics!(result, state, control_torques; stabilization_gains=stabilization_gains)
                control!(control_torques, t, state)
                dynamics!(
                    result,
                    state,
                    control_torques,
                    ext_wrenches;
                    stabilization_gains = stabilization_gains,
                )
                copyto!(v̇, result.v̇)
                copyto!(ṡ, result.ṡ)
                nothing
            end
        end
    tableau = runge_kutta_4(T)
    storage = ExpandingStorage{T}(state0, ceil(Int64, final_time / Δt * 1.001)) # very rough overestimate of number of time steps
    integrator = MuntheKaasIntegrator(state0, closed_loop_dynamics!, tableau, storage)
    integrate(integrator, final_time, Δt)
    return storage.ts, storage.qs, storage.vs
end

ts, qs, vs = simulate2(state, 10.0, spring_damper!, external_wrenches, Δt = 1e-3);
MeshCatMechanisms.animate(vis, ts, qs; realtimerate = 1.0);

```

---

<div class="post-metadata">

**Author:** ![\_dominic](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/_dominic/32/7845_2.png) [@\_dominic](https://discourse.julialang.org/u/_dominic)\
**Post date:** [August 21, 2020, 8:44pm UTC](https://discourse.julialang.org/t/adding-external-force-rigidbodydynamics-mechansms/45233/6 "2020-08-21T20:44:19Z")

</div>

It occurs to me that I may have missed something and gone on a tangent. Is this even the right approach?