Using a sensor

Multibody models are composed out of multibody components, typically by connecting Frames together. To add, e.g., a control system to a multibody model, we often make use of sensors that, just like in the real world, translate between a physical quantity in 3D and a signal in the "signal domain". The signal domain consists of the various blocks defined in ModelingToolkitStandardLibrary.Blocks, and a sensor is simply a component that has two connectors, one Frame connector for attaching to the multibody model, and one Blocks.RealOutput for connecting to the signal domain.

The example below adds a force and a torque sensor to the pivot point of a pendulum. Note how the two sensors are connected in series with each other, just like how we would typically connect them in practice if they are not integrated into the same component.

using Multibody
using ModelingToolkit
using JuliaSimCompiler
using Plots
using OrdinaryDiffEq
using LinearAlgebra

t = Multibody.t
D = Differential(t)
world = Multibody.world

@named joint = Multibody.Revolute(n = [0, 0, 1], isroot = true)
@named body = Body(; m = 1, isroot = false, r_cm = [0.5, 0, 0])
@named torquesensor = CutTorque()
@named forcesensor = CutForce()

connections = [connect(world.frame_b, joint.frame_a)
               connect(joint.frame_b, body.frame_a, torquesensor.frame_a,
                       forcesensor.frame_a)]

connections = [connect(world.frame_b, joint.frame_a)
               connect(joint.frame_b, torquesensor.frame_a)
               connect(torquesensor.frame_b, forcesensor.frame_a)
               connect(forcesensor.frame_b, body.frame_a)]

@named model = ODESystem(connections, t,
                         systems = [world, joint, body, torquesensor, forcesensor])
ssys = structural_simplify(IRSystem(model))


D = Differential(t)
prob = ODEProblem(ssys, [], (0, 3))

using OrdinaryDiffEq
sol = solve(prob, Rodas4())
@assert SciMLBase.successful_retcode(sol)

plot(sol, idxs = [collect(forcesensor.force.u); collect(joint.frame_a.f)])

Note how the force sensor measures a force that appears to equal the cut-force in the joint in magnitude, but the orientation appears to differ. Frame cut forces and toques are resolved in the world frame by default, while the force sensor measures the force in the frame of the sensor. We can choose which frame to resolve the measurements in by using hte keyword argument @named forcesensor = CutForce(; resolve_frame = :world). If we do this, the traces in the plot above will overlap.

Since the torque sensor measures a torque in a revolute joint, it should measure zero torque in this case, no torque is transmitted through the revolute joint since the rotational axis is perpendicular to the gravitational force:

all(x -> abs(x) < 1e-3, reduce(hcat, sol[torquesensor.torque.u]))

true