Body wrench from forces applied at different points

[ferrolho]

Hello, @tkoolen! :slightly_smiling_face: I am trying to model the surface-to-surface contact between a humanoid's foot and flat ground. For that, I am defining four contact points per foot, similar to what you did for Atlas and Valkyrie. The image below shows the four contact points of the left foot, as well as a triad located at the frame origin of the sole.

This next image shows some forces being applied at each contact point.

My question is: for each foot, how can I compute the total wrench acting on the link given the individual contact forces? I want to do this so I can call dynamics!(result, state, torques, externalwrenches) with the external wrenches acting on each foot.

I will leave the code for a single force applied at the frame origin below, as it may be a helpful reference for the discussion:

frame_foot = findbody(mechanism, "left_sole_link")

force_in_world_frame = FreeVector3D(root_frame(mechanism), [0.0, 0.0, 100.0])
force_in_foot_frame = transform(state, force_in_world_frame, frame_foot)

wrench = Wrench(Point3D(frame_foot, zeros(T, 3)), force_in_foot_frame)

externalwrenches = Dict{BodyID,Wrench{T}}(
    BodyID(findbody(mechanism, "leg_left_6_link")) => transform(state, wrench, root_frame(mechanism)),
)

result = robot.dynamicsresultcache[T]
dynamics!(result, state, torques, externalwrenches)

[ferrolho]

Well, this is embarrassing... I was really stuck with the assumption that the first argument I was passing to Wrench() was the angular component... It turns out I was missing that there is a Wrench() that is dispatched on a Point3D for wrenches defined as a force applied to an application point. (Even though I have this explicitly written in my snippet above...)

Oh well, correct me if I am wrong, but what I was looking for is therefore something like this:

frame_foot = default_frame(findbody(mechanism, "left_sole_link"))

force_1 = transform(state, FreeVector3D(root_frame(mechanism), [0.0, 0.0,  20.0]), frame_foot)
force_2 = transform(state, FreeVector3D(root_frame(mechanism), [0.0, 0.0,  20.0]), frame_foot)
force_3 = transform(state, FreeVector3D(root_frame(mechanism), [0.0, 0.0, 100.0]), frame_foot)
force_4 = transform(state, FreeVector3D(root_frame(mechanism), [0.0, 0.0, 100.0]), frame_foot)

wrench_1 = Wrench(Point3D(frame_foot, [ 0.1,  0.05, 0.0]), force_1)
wrench_2 = Wrench(Point3D(frame_foot, [ 0.1, -0.05, 0.0]), force_2)
wrench_3 = Wrench(Point3D(frame_foot, [-0.1,  0.05, 0.0]), force_3)
wrench_4 = Wrench(Point3D(frame_foot, [-0.1, -0.05, 0.0]), force_4)

wrench = wrench_1 + wrench_2 + wrench_3 + wrench_4

externalwrenches = Dict{BodyID,Wrench{T}}(
    BodyID(findbody(mechanism, "leg_left_6_link")) => transform(state, wrench, root_frame(mechanism)),
)

result = robot.dynamicsresultcache[T]
dynamics!(result, state, torques, externalwrenches)

[tkoolen]

Yep, that's right!

[ferrolho]

:slightly_smiling_face:

[hkolano]

@ferrolho do you have the code somewhere for the visualization of the arrows? I'm trying to do a similar thing with a robot of my own but I'm having a hard time getting arrows and meshcat to work together. (Also this thread has been very helpful for how to use the externalwrenches argument, so thank you for posting!)

[ferrolho]

Hi @hkolano, here's the code I use to show trajectories with forces and CoM projection (like the one above):

function play_motion(mvis, robot, problem, q_mat, λ_mat; fps=60)
    vis = mvis.visualizer["/meshcat"]

    # This is the factor by which forces are scaled so that they can be seen in visualisation.
    force_scaling_factor = 0.005

    material = MeshCat.MeshPhongMaterial(color=colorant"red")
    setobject!(vis["center of mass/com"], HyperSphere(Point(0., 0, 0), 0.03), material)
    setobject!(vis["center of mass/projection"], HyperSphere(Point(0., 0, 0), 0.03), material)

    com_line = ArrowVisualizer(vis["center of mass/line"])
    material = MeshLambertMaterial(color=colorant"blue")
    setobject!(com_line, shaft_material=material, head_material=material)
    delete!(vis["center of mass/line/head"])

    # Trajectories may be discretised at high frequencies (e.g., 400 Hz), so we select a subset to match the desired FPS.
    selected_range = round.(Int, range(1, problem.num_knots, length=round(Int, problem.duration * fps + 1)))
    selected_configurations = q_mat[:, selected_range]
    selected_forces = λ_mat[:, selected_range[1:end - 1]]

    vis_arrows = []
    material = MeshLambertMaterial(color=colorant"yellow")
    for (j, cpⱼ) in enumerate(robot.contact_points)
        arrow = ArrowVisualizer(vis["contact forces/arrow_$(j)"])
        setobject!(arrow, shaft_material=material, head_material=material)
        setprop!(vis["contact forces/arrow_$(j)/head/<object>"], "castShadow", false)
        setprop!(vis["contact forces/arrow_$(j)/shaft/<object>"], "castShadow", false)
        push!(vis_arrows, arrow)
    end

    anim = MeshCat.Animation(fps)

    for i = 1:length(selected_range)
        qᵢ = selected_configurations[:, i]

        atframe(anim, i) do
            # Robot configuration
            set_configuration!(robot.state, qᵢ)
            set_configuration!(mvis, configuration(robot.state))

            # Ground-feet contact forces
            if i < length(selected_range)
                for (j, cpⱼ) in enumerate(robot.contact_points)
                    ind_λᵢⱼ = (1:3) .+ (j - 1) * 3
                    λⱼ = selected_forces[:,i][ind_λᵢⱼ] * force_scaling_factor
                    posⱼ = transform(robot.state, cpⱼ, root_frame(robot.mechanism)).v
                    settransform!(vis_arrows[j], Point(posⱼ...), Point((posⱼ + λⱼ)...)) # , shaft_radius=0.02)
                end
            end

            # Center of Mass (CoM)
            com = center_of_mass(robot.state)
            com_projection = Point3D(com.frame, com.v[1], com.v[2], 0)
            settransform!(vis["center of mass/com"], Translation(com.v))
            settransform!(vis["center of mass/projection"], Translation(com_projection.v))
            settransform!(com_line, Point(com.v), Point(com_projection.v), shaft_radius=0.005)
        end
    end

    setanimation!(vis, anim)
end

It relies on a robot and problem structs which are my own, but I think you should be able to easily adapt the function above to get it working with your stuff.

Glad to see more people using this library! Twan has put a lot of effort on it. RBD.jl and MeshCat really have been crucial tools during my PhD.

[hkolano]

That is super helpful! It seems I was missing "splicing" the main visualizer in order to separate the transforms for the arrows. I really appreciate the help -- RBD.jl has been great, but sometimes it's hard to find working code examples.