Virtual Model Control of a Simulated Robot

What is a virtual mechanism system?

A virtual mechanism system is contains a model of the robot, a model of a virtual mechanism and interface components that act between the robot and the virtual mechanism. It can be used for two purposes: either for simulation or for control of a real robot. If used for simulation, a full robot model is needed, with dynamic components (like mass/inertia). If used for control, the robot model is still needed, but at a minimum only a kinematics model is needed (frames, joints and coordinates) so no inertial model is nescessary. When controlling a real robot, components on the robot are ignored.

The VirtualMechanismSystem thus contains a robot::Mechanism, a virtual_mechanism::Mechanism, which can be built just like a robot, and also contains it's own coordinates and components that connect between the robot and virtual mechanism.

Connecting a spring/damper between a robot and a virtual mechanism.

First, lets build a super simple virtual mechanism system. It will consist of a 1DOF robot, and a 1DOF virtual mechanism. A virtual spring and damper will join the two together.

First, we make the robot, virtual mechanism, and virtual mechanism system.

using VMRobotControl, StaticArrays

robot = Mechanism{Float64}("Piston")
add_frame!(robot, "L1")
add_joint!(robot, Prismatic(SVector(1.0, 0.0, 0.0)); parent="root_frame", child="L1", id="J1")
add_coordinate!(robot, FrameOrigin("L1"); id="ee_pos")
virtual_mechanism = deepcopy(robot)
vms = VirtualMechanismSystem("MySystem", robot, virtual_mechanism)

VirtualMechanismSystem 'MySystem' with robot `Piston`, and virtual mechanism `Piston`.

Then, we add a coordinate to the virtual mechanism system using CoordDifference which will is the typical way to define a coordinate between the robot and virtual mechanism. It represents a vector from the tip of the robot end-effector to the tip of the virtual mechanism end-effector. To identify robot/virtual mechanism coordinates from the virtual-mechanism-system, the prefixes ".robot." or ".virtual_mechanism." must be used.

add_coordinate!(vms, CoordDifference(".robot.ee_pos", ".virtual_mechanism.ee_pos"); id="ee_err")
add_component!(vms, LinearSpring(100.0, "ee_err"); id="ee_spring")
add_component!(vms, LinearDamper(1.0, "ee_err"); id="ee_damper")

"ee_damper"

We can only add spring/damper like components to the virtual mechanism system, and not inertias/ masses/inertances. This is because the robot/virtual mechanism both act as admitances (taking forces as input, and computing accelerations as outputs), so the interface between them must act as an impedance (taking motion as input and returning a force).

Using a virtual mechanism system

The interface for using a virtual mechanism system is broadly the same as for using mechanism. However, joint configurations, velocities and actuation torques must now be tuples, with the first entry corresponding to the robot and the second to the virtual mechanism

julia> m = compile(vms)CompiledVirtualMechanismSystem{Float64, CompiledMechanism{Float64, VMRobotControl.RBTree{Float64, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{CompiledMechanismJoint(Float64, PrismaticData{Float64})}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{}}}, CompiledMechanism{Float64, VMRobotControl.RBTree{Float64, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{CompiledMechanismJoint(Float64, PrismaticData{Float64})}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}}}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{LinearSpring{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}}, Vector{LinearDamper{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}}}}}("MySystem", CompiledMechanism{Float64, VMRobotControl.RBTree{Float64, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{CompiledMechanismJoint(Float64, PrismaticData{Float64})}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{}}}("Piston", VMRobotControl.RBTree{Float64, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{CompiledMechanismJoint(Float64, PrismaticData{Float64})}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}}}}(1, [(1, 2)], VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{CompiledMechanismJoint(Float64, PrismaticData{Float64})}}}((CompiledMechanismJoint(Float64, PrismaticData{Float64})[CompiledMechanismJoint(Float64, PrismaticData{Float64})(PrismaticData{Float64}([1.0, 0.0, 0.0], Transform{Float64}([0.0, 0.0, 0.0], Rotor{Float64}(1.0, [0.0, 0.0, 0.0]))), CompiledFrameID(1), CompiledFrameID(2), 1, 1)],)), VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}}}[VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}}}((VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}[VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}(FrameOrigin{CompiledFrameID}(CompiledFrameID(2)), 1:3)],))], Bool[0 1; 0 0], [[1, 2], [2]], Dict{String, CompiledFrameID}("L1" => CompiledFrameID(2), "root_frame" => CompiledFrameID(1)), OrderedCollections.OrderedDict{String, VMRobotControl.CompiledJointID}("J1" => VMRobotControl.CompiledJointID{CompiledMechanismJoint(Float64, PrismaticData{Float64})}(VMRobotControl.TypeStableCollections.TypeStableIdx{CompiledMechanismJoint(Float64, PrismaticData{Float64})}(1))), Dict{String, VMRobotControl.CompiledCoordID}("ee_pos" => VMRobotControl.CompiledCoordID{FrameOrigin{CompiledFrameID}}(1, VMRobotControl.TypeStableCollections.TypeStableIdx{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}(1)))), VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{}}(()), Dict{String, VMRobotControl.CompiledComponentID}()), CompiledMechanism{Float64, VMRobotControl.RBTree{Float64, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{CompiledMechanismJoint(Float64, PrismaticData{Float64})}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{}}}("Piston", VMRobotControl.RBTree{Float64, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{CompiledMechanismJoint(Float64, PrismaticData{Float64})}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}}}}(1, [(1, 2)], VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{CompiledMechanismJoint(Float64, PrismaticData{Float64})}}}((CompiledMechanismJoint(Float64, PrismaticData{Float64})[CompiledMechanismJoint(Float64, PrismaticData{Float64})(PrismaticData{Float64}([1.0, 0.0, 0.0], Transform{Float64}([0.0, 0.0, 0.0], Rotor{Float64}(1.0, [0.0, 0.0, 0.0]))), CompiledFrameID(1), CompiledFrameID(2), 1, 1)],)), VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}}}[VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}}}((VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}[VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}(FrameOrigin{CompiledFrameID}(CompiledFrameID(2)), 1:3)],))], Bool[0 1; 0 0], [[1, 2], [2]], Dict{String, CompiledFrameID}("L1" => CompiledFrameID(2), "root_frame" => CompiledFrameID(1)), OrderedCollections.OrderedDict{String, VMRobotControl.CompiledJointID}("J1" => VMRobotControl.CompiledJointID{CompiledMechanismJoint(Float64, PrismaticData{Float64})}(VMRobotControl.TypeStableCollections.TypeStableIdx{CompiledMechanismJoint(Float64, PrismaticData{Float64})}(1))), Dict{String, VMRobotControl.CompiledCoordID}("ee_pos" => VMRobotControl.CompiledCoordID{FrameOrigin{CompiledFrameID}}(1, VMRobotControl.TypeStableCollections.TypeStableIdx{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}(1)))), VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{}}(()), Dict{String, VMRobotControl.CompiledComponentID}()), VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}}}}}[VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}}}}}((VMRobotControl.CompiledCoord{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}}[VMRobotControl.CompiledCoord{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}}(CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}(VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}(VMRobotControl.CompiledCoordID{FrameOrigin{CompiledFrameID}}(1, VMRobotControl.TypeStableCollections.TypeStableIdx{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}(1))), VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}(VMRobotControl.CompiledCoordID{FrameOrigin{CompiledFrameID}}(1, VMRobotControl.TypeStableCollections.TypeStableIdx{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}(1)))), 1:3)],))], VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{LinearSpring{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}}, Vector{LinearDamper{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}}}}((LinearSpring{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}[LinearSpring{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}(100.0, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}(VMRobotControl.CompiledCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}}(1, VMRobotControl.TypeStableCollections.TypeStableIdx{VMRobotControl.CompiledCoord{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}}}(1))))], LinearDamper{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}[LinearDamper{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}(1.0, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}(VMRobotControl.CompiledCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}}(1, VMRobotControl.TypeStableCollections.TypeStableIdx{VMRobotControl.CompiledCoord{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}}}(1))))])), Dict{String, VMRobotControl.VMSFrameID}(".robot.root_frame" => VMRobotControl.VMSFrameID{:robot_coord}(CompiledFrameID(1)), ".robot.L1" => VMRobotControl.VMSFrameID{:robot_coord}(CompiledFrameID(2)), ".virtual_mechanism.root_frame" => VMRobotControl.VMSFrameID{:virtual_mechanism_coord}(CompiledFrameID(1)), ".virtual_mechanism.L1" => VMRobotControl.VMSFrameID{:virtual_mechanism_coord}(CompiledFrameID(2))), OrderedCollections.OrderedDict{String, VMRobotControl.VMSJointID}(".robot.J1" => VMRobotControl.VMSJointID{:robot_coord, CompiledMechanismJoint(Float64, PrismaticData{Float64})}(VMRobotControl.CompiledJointID{CompiledMechanismJoint(Float64, PrismaticData{Float64})}(VMRobotControl.TypeStableCollections.TypeStableIdx{CompiledMechanismJoint(Float64, PrismaticData{Float64})}(1))), ".virtual_mechanism.J1" => VMRobotControl.VMSJointID{:virtual_mechanism_coord, CompiledMechanismJoint(Float64, PrismaticData{Float64})}(VMRobotControl.CompiledJointID{CompiledMechanismJoint(Float64, PrismaticData{Float64})}(VMRobotControl.TypeStableCollections.TypeStableIdx{CompiledMechanismJoint(Float64, PrismaticData{Float64})}(1)))), Dict{String, VMRobotControl.VMSCoordID}(".robot.ee_pos" => VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}(VMRobotControl.CompiledCoordID{FrameOrigin{CompiledFrameID}}(1, VMRobotControl.TypeStableCollections.TypeStableIdx{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}(1))), "ee_err" => VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}(VMRobotControl.CompiledCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}}(1, VMRobotControl.TypeStableCollections.TypeStableIdx{VMRobotControl.CompiledCoord{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}}}(1))), ".virtual_mechanism.ee_pos" => VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}(VMRobotControl.CompiledCoordID{FrameOrigin{CompiledFrameID}}(1, VMRobotControl.TypeStableCollections.TypeStableIdx{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}(1)))), Dict{String, VMRobotControl.VMSComponentID}("ee_damper" => VMRobotControl.VMSComponentID{LinearDamper{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}, :system_coord}(VMRobotControl.CompiledComponentID{LinearDamper{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}}(VMRobotControl.TypeStableCollections.TypeStableIdx{LinearDamper{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}}(1))), "ee_spring" => VMRobotControl.VMSComponentID{LinearSpring{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}, :system_coord}(VMRobotControl.CompiledComponentID{LinearSpring{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}}(VMRobotControl.TypeStableCollections.TypeStableIdx{LinearSpring{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}}(1)))))
julia> t = 0.00.0
julia> q = zero_q(m)([0.0], [0.0])
julia> kcache = new_kinematics_cache(m)VMRobotControl.VirtualMechanismSystemCacheBundle{CompiledVirtualMechanismSystem{Float64, CompiledMechanism{Float64, VMRobotControl.RBTree{Float64, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{CompiledMechanismJoint(Float64, PrismaticData{Float64})}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{}}}, CompiledMechanism{Float64, VMRobotControl.RBTree{Float64, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{CompiledMechanismJoint(Float64, PrismaticData{Float64})}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}}}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{LinearSpring{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}}, Vector{LinearDamper{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}}}}}, VMRobotControl.VMSKinematicsCache{Float64}}(CompiledVirtualMechanismSystem{Float64, CompiledMechanism{Float64, VMRobotControl.RBTree{Float64, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{CompiledMechanismJoint(Float64, PrismaticData{Float64})}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{}}}, CompiledMechanism{Float64, VMRobotControl.RBTree{Float64, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{CompiledMechanismJoint(Float64, PrismaticData{Float64})}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}}}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{LinearSpring{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}}, Vector{LinearDamper{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}}}}}("MySystem", CompiledMechanism{Float64, VMRobotControl.RBTree{Float64, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{CompiledMechanismJoint(Float64, PrismaticData{Float64})}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{}}}("Piston", VMRobotControl.RBTree{Float64, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{CompiledMechanismJoint(Float64, PrismaticData{Float64})}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}}}}(1, [(1, 2)], VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{CompiledMechanismJoint(Float64, PrismaticData{Float64})}}}((CompiledMechanismJoint(Float64, PrismaticData{Float64})[CompiledMechanismJoint(Float64, PrismaticData{Float64})(PrismaticData{Float64}([1.0, 0.0, 0.0], Transform{Float64}([0.0, 0.0, 0.0], Rotor{Float64}(1.0, [0.0, 0.0, 0.0]))), CompiledFrameID(1), CompiledFrameID(2), 1, 1)],)), VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}}}[VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}}}((VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}[VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}(FrameOrigin{CompiledFrameID}(CompiledFrameID(2)), 1:3)],))], Bool[0 1; 0 0], [[1, 2], [2]], Dict{String, CompiledFrameID}("L1" => CompiledFrameID(2), "root_frame" => CompiledFrameID(1)), OrderedCollections.OrderedDict{String, VMRobotControl.CompiledJointID}("J1" => VMRobotControl.CompiledJointID{CompiledMechanismJoint(Float64, PrismaticData{Float64})}(VMRobotControl.TypeStableCollections.TypeStableIdx{CompiledMechanismJoint(Float64, PrismaticData{Float64})}(1))), Dict{String, VMRobotControl.CompiledCoordID}("ee_pos" => VMRobotControl.CompiledCoordID{FrameOrigin{CompiledFrameID}}(1, VMRobotControl.TypeStableCollections.TypeStableIdx{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}(1)))), VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{}}(()), Dict{String, VMRobotControl.CompiledComponentID}()), CompiledMechanism{Float64, VMRobotControl.RBTree{Float64, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{CompiledMechanismJoint(Float64, PrismaticData{Float64})}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{}}}("Piston", VMRobotControl.RBTree{Float64, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{CompiledMechanismJoint(Float64, PrismaticData{Float64})}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}}}}(1, [(1, 2)], VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{CompiledMechanismJoint(Float64, PrismaticData{Float64})}}}((CompiledMechanismJoint(Float64, PrismaticData{Float64})[CompiledMechanismJoint(Float64, PrismaticData{Float64})(PrismaticData{Float64}([1.0, 0.0, 0.0], Transform{Float64}([0.0, 0.0, 0.0], Rotor{Float64}(1.0, [0.0, 0.0, 0.0]))), CompiledFrameID(1), CompiledFrameID(2), 1, 1)],)), VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}}}[VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}}}((VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}[VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}(FrameOrigin{CompiledFrameID}(CompiledFrameID(2)), 1:3)],))], Bool[0 1; 0 0], [[1, 2], [2]], Dict{String, CompiledFrameID}("L1" => CompiledFrameID(2), "root_frame" => CompiledFrameID(1)), OrderedCollections.OrderedDict{String, VMRobotControl.CompiledJointID}("J1" => VMRobotControl.CompiledJointID{CompiledMechanismJoint(Float64, PrismaticData{Float64})}(VMRobotControl.TypeStableCollections.TypeStableIdx{CompiledMechanismJoint(Float64, PrismaticData{Float64})}(1))), Dict{String, VMRobotControl.CompiledCoordID}("ee_pos" => VMRobotControl.CompiledCoordID{FrameOrigin{CompiledFrameID}}(1, VMRobotControl.TypeStableCollections.TypeStableIdx{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}(1)))), VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{}}(()), Dict{String, VMRobotControl.CompiledComponentID}()), VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}}}}}[VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{VMRobotControl.CompiledCoord{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}}}}}((VMRobotControl.CompiledCoord{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}}[VMRobotControl.CompiledCoord{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}}(CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}(VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}(VMRobotControl.CompiledCoordID{FrameOrigin{CompiledFrameID}}(1, VMRobotControl.TypeStableCollections.TypeStableIdx{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}(1))), VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}(VMRobotControl.CompiledCoordID{FrameOrigin{CompiledFrameID}}(1, VMRobotControl.TypeStableCollections.TypeStableIdx{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}(1)))), 1:3)],))], VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{LinearSpring{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}}, Vector{LinearDamper{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}}}}((LinearSpring{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}[LinearSpring{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}(100.0, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}(VMRobotControl.CompiledCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}}(1, VMRobotControl.TypeStableCollections.TypeStableIdx{VMRobotControl.CompiledCoord{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}}}(1))))], LinearDamper{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}[LinearDamper{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}(1.0, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}(VMRobotControl.CompiledCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}}(1, VMRobotControl.TypeStableCollections.TypeStableIdx{VMRobotControl.CompiledCoord{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}}}(1))))])), Dict{String, VMRobotControl.VMSFrameID}(".robot.root_frame" => VMRobotControl.VMSFrameID{:robot_coord}(CompiledFrameID(1)), ".robot.L1" => VMRobotControl.VMSFrameID{:robot_coord}(CompiledFrameID(2)), ".virtual_mechanism.root_frame" => VMRobotControl.VMSFrameID{:virtual_mechanism_coord}(CompiledFrameID(1)), ".virtual_mechanism.L1" => VMRobotControl.VMSFrameID{:virtual_mechanism_coord}(CompiledFrameID(2))), OrderedCollections.OrderedDict{String, VMRobotControl.VMSJointID}(".robot.J1" => VMRobotControl.VMSJointID{:robot_coord, CompiledMechanismJoint(Float64, PrismaticData{Float64})}(VMRobotControl.CompiledJointID{CompiledMechanismJoint(Float64, PrismaticData{Float64})}(VMRobotControl.TypeStableCollections.TypeStableIdx{CompiledMechanismJoint(Float64, PrismaticData{Float64})}(1))), ".virtual_mechanism.J1" => VMRobotControl.VMSJointID{:virtual_mechanism_coord, CompiledMechanismJoint(Float64, PrismaticData{Float64})}(VMRobotControl.CompiledJointID{CompiledMechanismJoint(Float64, PrismaticData{Float64})}(VMRobotControl.TypeStableCollections.TypeStableIdx{CompiledMechanismJoint(Float64, PrismaticData{Float64})}(1)))), Dict{String, VMRobotControl.VMSCoordID}(".robot.ee_pos" => VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}(VMRobotControl.CompiledCoordID{FrameOrigin{CompiledFrameID}}(1, VMRobotControl.TypeStableCollections.TypeStableIdx{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}(1))), "ee_err" => VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}(VMRobotControl.CompiledCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}}(1, VMRobotControl.TypeStableCollections.TypeStableIdx{VMRobotControl.CompiledCoord{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}}}(1))), ".virtual_mechanism.ee_pos" => VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}(VMRobotControl.CompiledCoordID{FrameOrigin{CompiledFrameID}}(1, VMRobotControl.TypeStableCollections.TypeStableIdx{VMRobotControl.CompiledCoord{FrameOrigin{CompiledFrameID}}}(1)))), Dict{String, VMRobotControl.VMSComponentID}("ee_damper" => VMRobotControl.VMSComponentID{LinearDamper{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}, :system_coord}(VMRobotControl.CompiledComponentID{LinearDamper{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}}(VMRobotControl.TypeStableCollections.TypeStableIdx{LinearDamper{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}}(1))), "ee_spring" => VMRobotControl.VMSComponentID{LinearSpring{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}, :system_coord}(VMRobotControl.CompiledComponentID{LinearSpring{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}}(VMRobotControl.TypeStableCollections.TypeStableIdx{LinearSpring{Float64, Float64, VMRobotControl.VMSCoordID{CoordDifference{VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :robot_coord}, VMRobotControl.VMSCoordID{FrameOrigin{CompiledFrameID}, :virtual_mechanism_coord}}, :system_coord}}}(1))))), VMRobotControl.VMSKinematicsCache{Float64}(Base.RefValue{Float64}(0.0), ([0.0], [0.0]), VMRobotControl.FrameKinematicsCache{Float64}(Transform{Float64}[Transform{Float64}([0.0, 0.0, 0.0], Rotor{Float64}(1.0, [0.0, 0.0, 0.0])), Transform{Float64}([0.0, 0.0, 0.0], Rotor{Float64}(1.0, [0.0, 0.0, 0.0]))]), VMRobotControl.CoordKinematicsCache{Float64}([0.0, 0.0, 0.0]), VMRobotControl.FrameKinematicsCache{Float64}(Transform{Float64}[Transform{Float64}([0.0, 0.0, 0.0], Rotor{Float64}(1.0, [0.0, 0.0, 0.0])), Transform{Float64}([0.0, 0.0, 0.0], Rotor{Float64}(1.0, [0.0, 0.0, 0.0]))]), VMRobotControl.CoordKinematicsCache{Float64}([0.0, 0.0, 0.0]), VMRobotControl.CoordKinematicsCache{Float64}([0.0, 0.0, 0.0])))
julia> kinematics!(kcache, t, q)

Note that q is a tuple