Adding external force RigidBodyDynamics mechansms

General Usage
,
1

I am trying to figure out how to add an external force to a specific location on a rigid body. For example,
image

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

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:

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

2

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

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 for an example showing how to pass the external wrench to the forward dynamics algorithm.

1 Like
3

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.

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?

4

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:

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]
1 Like
5

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

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)

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

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);
6

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