Adding external force RigidBodyDynamics mechansms

General Usage
,
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