LIBRARY
SlipWheelJoint
Joint for a wheel with slip-dependent friction rolling on the flat plane y = 0.
Unlike RollingWheelJoint, this joint does not impose strict no-slip constraints. Instead, the contact patch can slip and the resulting tangential force is computed from a velocity-dependent friction curve evaluated by MultibodyComponents.limit_S_triple. Reference: https://people.inf.ethz.ch/fcellier/MS/andres_ms.pdf
Integrator choice
The slip model contains a discontinuity in the second derivative at the transitions between adhesion and sliding. This can cause problems for BDF-type integrators; explicit RK methods (e.g. Tsit5) work well.
Normal force
The wheel cannot leave the ground. Make sure that the normal force f_n never becomes negative.
This component extends from Renderable This component extends from CutJoint
Usage
MultibodyComponents.SlipWheelJoint(render=true, color=[1, 0, 0, 1], specular_coefficient=1.5, radius=0.3, width=0.035, c_z=250000, d_z=500, vAdhesion_min=0.05, vSlide_min=0.15, sAdhesion=0.04, sSlide=0.12, mu_A=0.8, mu_S=0.6, v_small=1e-5)
Parameters:
| Name | Description | Units | Default value |
|---|---|---|---|
iscut | – | false | |
residual | – | zeros(3) | |
surface | Use an external road surface supplied via surface_frame (else flat ground y=0) | – | false |
elastic_contact | Use a compliant (elastic) normal contact instead of the rigid no-penetration constraint: f_n = max(0, c_z_pen + d_z_der(pen)) is then explicit, so f_n and the contact-point acceleration leave the coupled algebraic block (this decouples free-floating multi-wheel cars). The wheel vertical position becomes a dynamic tire-compression state. | – | false |
angular_state | Include the wheel's angular state (Cardan angles). Set false when the wheel is mounted on an already-rooted axis: the orientation is then taken from frame_a and no orientation loop/state is introduced (avoids redundant constraint forces in free-floating multi-wheel assemblies). | – | true |
sequence | Rotation axis sequence for the Cardan angles that orient frame_a (default y, z, x) | – | [2, 3, 1] |
render | – | true | |
color | – | [1, 0, 0, 1] | |
specular_coefficient | – | 1.5 | |
radius | Radius of the wheel | m | 0.3 |
width | Width of the wheel in animations | – | 0.035 |
c_z | Vertical contact stiffness, used only when elastic_contact = true [N/m] | – | 250000 |
d_z | Vertical contact damping, used only when elastic_contact = true [N*s/m] | – | 500 |
vAdhesion_min | Minimum velocity for the peak of the adhesion curve (regularization) | m/s | 0.05 |
vSlide_min | Minimum velocity for the start of the flat region of the slip curve (regularization) | m/s | 0.15 |
sAdhesion | Adhesion slippage: the peak of the adhesion curve appears when the wheel slip equals sAdhesion | – | 0.04 |
sSlide | Sliding slippage: the flat region of the slip curve appears when the wheel slip exceeds sSlide | – | 0.12 |
mu_A | Friction coefficient at the adhesion peak | – | 0.8 |
mu_S | Friction coefficient at full sliding | – | 0.6 |
v_small | Small value added to v_slip to avoid division by zero in the slip model | – | 1e-5 |
Connectors
frame_a- Frame3D is the fundamental 3D connector used for 6DOF motion. Most components have one or severalFrame
connectors that can be connected together (Frame3D)
surface_frame- Frame3D is the fundamental 3D connector used for 6DOF motion. Most components have one or severalFrame
connectors that can be connected together (Frame3D)
Variables
| Name | Description | Units |
|---|---|---|
Rleaf | – | |
Rroot | – | |
x | x-position of the wheel axis | m |
y | y-position of the wheel axis | m |
z | z-position of the wheel axis | m |
angles | Angles that rotate the world frame into frame_a around the y-, z-, x-axis | rad |
der_angles | Time derivatives of angles | rad/s |
phi_roll | Wheel rolling angle (angles[2]) | rad |
w_roll | Wheel rolling angular velocity | rad/s |
r_road_0 | Position vector from world frame to contact point on the road, resolved in world frame | m |
f_wheel_0 | Force vector on the wheel at the contact point, resolved in world frame | – |
f_n | Contact force acting on the wheel in the normal direction | N |
f_lat | Contact force acting on the wheel in the lateral direction | N |
f_long | Contact force acting on the wheel in the longitudinal direction | N |
e_axis_0 | Unit vector along the wheel axis, resolved in world frame | – |
delta_0 | Distance vector from wheel center to contact point, resolved in world frame | – |
pen | Tire penetration depth (radius minus center-to-road normal distance); >0 when compressed | – |
e_n_0 | Unit vector normal to the road at the contact point, resolved in world frame | – |
e_lat_0 | Unit vector in the lateral direction of the road at the contact point, resolved in world frame | – |
e_long_0 | Unit vector in the longitudinal direction of the road at the contact point, resolved in world frame | – |
e_s_0 | Road heading at (s, w), resolved in world frame (unit vector) | – |
s | Road surface parameter 1 | – |
w | Road surface parameter 2 | – |
v_0 | Velocity of wheel center, resolved in world frame | m/s |
w_0 | Angular velocity of the wheel, resolved in world frame | rad/s |
vContact_0 | Velocity of the contact point, resolved in world frame | m/s |
aux | Auxiliary vector for building the contact frame | – |
v_slip_long | Slip velocity in the longitudinal direction | m/s |
v_slip_lat | Slip velocity in the lateral direction | m/s |
v_slip | Slip velocity magnitude (with v_small regularization) | m/s |
slip_ratio | Slip ratio = v_slip_long / (v_0 · e_long_0) | – |
f | Total traction force | N |
vAdhesion | Slip velocity at which adhesion is maximized | m/s |
vSlide | Slip velocity at which the flat region of the slip model starts | m/s |
Behavior
Source
dyad
"""
Joint for a wheel with slip-dependent friction rolling on the flat plane y = 0.
Unlike `RollingWheelJoint`, this joint does not impose strict no-slip
constraints. Instead, the contact patch can slip and the resulting tangential
force is computed from a velocity-dependent friction curve evaluated by
`MultibodyComponents.limit_S_triple`. Reference:
https://people.inf.ethz.ch/fcellier/MS/andres_ms.pdf
!!! tip "Integrator choice"
The slip model contains a discontinuity in the second derivative at the
transitions between adhesion and sliding. This can cause problems for
BDF-type integrators; explicit RK methods (e.g. Tsit5) work well.
!!! warn "Normal force"
The wheel cannot leave the ground. Make sure that the normal force `f_n`
never becomes negative.
"""
component SlipWheelJoint
extends Renderable(color = [1, 0, 0, 1])
extends CutJoint()
frame_a = Frame3D() {}
"""
Optional road-surface frame: when surface=true the contact patch position
equals this frame's position, so a connected component can define the road
height y = f(x, z). When false, the wheel rolls on the flat plane y = 0.
"""
surface_frame = Frame3D() if surface {}
"Tire visualization: cylinder centered on frame_a with axis along z"
tire_shape = CylinderShape(render = render, color = color, r = frame_a.r_0, R = transpose(frame_a.R), r_shape = [0, 0, -width / 2], length_direction = [0, 0, 1], length = width, width = 2 * radius, height = 2 * radius)
"Rim cross-cylinder 1: thin spoke spanning the wheel diameter; rotates with the wheel to make the spin visible"
rim1_shape = CylinderShape(render = render, color = [0.8, 0.8, 0.8, 1.0], r = frame_a.r_0, R = transpose(frame_a.R), r_shape = [-radius, 0, 0], length_direction = [1, 0, 0], length = 2 * radius, width = radius / 3, height = radius / 3)
"Rim cross-cylinder 2: second spoke, perpendicular to the first"
rim2_shape = CylinderShape(render = render, color = [0.8, 0.8, 0.8, 1.0], r = frame_a.r_0, R = transpose(frame_a.R), r_shape = [0, -radius, 0], length_direction = [0, 1, 0], length = 2 * radius, width = radius / 3, height = radius / 3)
"Use an external road surface supplied via surface_frame (else flat ground y=0)"
structural parameter surface::Boolean = false
"""
Use a compliant (elastic) normal contact instead of the rigid no-penetration
constraint: f_n = max(0, c_z*pen + d_z*der(pen)) is then explicit, so f_n and
the contact-point acceleration leave the coupled algebraic block (this decouples
free-floating multi-wheel cars). The wheel vertical position becomes a dynamic
tire-compression state.
"""
structural parameter elastic_contact::Boolean = false
"""
Include the wheel's angular state (Cardan angles). Set false when the wheel is
mounted on an already-rooted axis: the orientation is then taken from frame_a
and no orientation loop/state is introduced (avoids redundant constraint forces
in free-floating multi-wheel assemblies).
"""
structural parameter angular_state::Boolean = true
"Rotation axis sequence for the Cardan angles that orient `frame_a` (default y, z, x)"
structural parameter sequence::Integer[3] = [2, 3, 1]
"Radius of the wheel"
parameter radius::Length = 0.3
"Width of the wheel in animations"
parameter width::Real = 0.035
"Vertical contact stiffness, used only when elastic_contact = true [N/m]"
parameter c_z::Real = 250000
"Vertical contact damping, used only when elastic_contact = true [N*s/m]"
parameter d_z::Real = 500
"Minimum velocity for the peak of the adhesion curve (regularization)"
parameter vAdhesion_min::Velocity = 0.05
"Minimum velocity for the start of the flat region of the slip curve (regularization)"
parameter vSlide_min::Velocity = 0.15
"Adhesion slippage: the peak of the adhesion curve appears when the wheel slip equals `sAdhesion`"
parameter sAdhesion::Real = 0.04
"Sliding slippage: the flat region of the slip curve appears when the wheel slip exceeds `sSlide`"
parameter sSlide::Real = 0.12
"Friction coefficient at the adhesion peak"
parameter mu_A::Real = 0.8
"Friction coefficient at full sliding"
parameter mu_S::Real = 0.6
"Small value added to `v_slip` to avoid division by zero in the slip model"
parameter v_small::Real = 1e-5
"x-position of the wheel axis"
variable x::Length(statePriority = 15)
"y-position of the wheel axis"
variable y::Length(statePriority = 0)
"z-position of the wheel axis"
variable z::Length(statePriority = 15)
"Angles that rotate the world frame into `frame_a` around the y-, z-, x-axis"
variable angles::Angle(statePriority = 15)[3] if angular_state
"Time derivatives of `angles`"
variable der_angles::AngularVelocity(statePriority = 30)[3] if angular_state
"Wheel rolling angle (`angles[2]`)"
variable phi_roll::Angle if angular_state
"Wheel rolling angular velocity"
variable w_roll::AngularVelocity
"Position vector from world frame to contact point on the road, resolved in world frame"
variable r_road_0::Position[3]
"Force vector on the wheel at the contact point, resolved in world frame"
variable f_wheel_0::Real[3]
"Contact force acting on the wheel in the normal direction"
variable f_n::Dyad.Force
"Contact force acting on the wheel in the lateral direction"
variable f_lat::Dyad.Force
"Contact force acting on the wheel in the longitudinal direction"
variable f_long::Dyad.Force
"Unit vector along the wheel axis, resolved in world frame"
variable e_axis_0::Real[3]
"Distance vector from wheel center to contact point, resolved in world frame"
variable delta_0::Real[3]
"Tire penetration depth (radius minus center-to-road normal distance); >0 when compressed"
variable pen::Real
"Unit vector normal to the road at the contact point, resolved in world frame"
variable e_n_0::Real[3]
"Unit vector in the lateral direction of the road at the contact point, resolved in world frame"
variable e_lat_0::Real[3]
"Unit vector in the longitudinal direction of the road at the contact point, resolved in world frame"
variable e_long_0::Real[3]
"Road heading at (s, w), resolved in world frame (unit vector)"
variable e_s_0::Real[3]
"Road surface parameter 1"
variable s::Real
"Road surface parameter 2"
variable w::Real
"Velocity of wheel center, resolved in world frame"
variable v_0::Velocity[3]
"Angular velocity of the wheel, resolved in world frame"
variable w_0::AngularVelocity[3]
"Velocity of the contact point, resolved in world frame"
variable vContact_0::Velocity[3]
"Auxiliary vector for building the contact frame"
variable aux::Real[3]
"Slip velocity in the longitudinal direction"
variable v_slip_long::Velocity
"Slip velocity in the lateral direction"
variable v_slip_lat::Velocity
"Slip velocity magnitude (with `v_small` regularization)"
variable v_slip::Velocity
"Slip ratio = v_slip_long / (v_0 · e_long_0)"
variable slip_ratio::Real
"Total traction force"
variable f::Dyad.Force
"Slip velocity at which adhesion is maximized"
variable vAdhesion::Velocity
"Slip velocity at which the flat region of the slip model starts"
variable vSlide::Velocity
relations
# Solver guesses for algebraic intermediates (mirrors Multibody.jl defaults)
guess x = 0
guess y = radius
guess z = 0
if angular_state
guess angles = [0, 0, 0]
guess der_angles = [0, 0, 0]
guess phi_roll = 0
end
guess w_roll = 0
guess f_n = 1.0
guess delta_0 = [0, -radius, 0]
guess pen = 0
guess e_n_0 = [0, 1, 0]
guess aux = [1, 0, 0]
guess v_slip_long = 0
guess v_slip_lat = 0
# Road description
if surface
# The surface frame is a kinematic road reference: it exerts no force.
surface_frame.f = [0, 0, 0]
surface_frame.tau = [0, 0, 0]
# Contact patch position is the surface-frame position; s, w (= x, z) remain
# free and are solved by the contact constraints, while the connected road
# component imposes the height (y of the frame position).
s = surface_frame.r_0[1]
w = surface_frame.r_0[3]
r_road_0 = surface_frame.r_0
# Contact tangent/normal come from the road frame's orientation: the connected
# road component orients surface_frame so its y-axis is the surface normal and
# its x-axis is the heading. This is exact for any surface the road describes
# (a flat/excitation road keeps R = I → vertical normal).
e_n_0 = resolve1(surface_frame.R, [0, 1, 0])
e_s_0 = resolve1(surface_frame.R, [1, 0, 0])
else
# Flat road description
r_road_0 = [s, 0, w]
e_n_0 = [0, 1, 0]
e_s_0 = [1, 0, 0]
end
# Pose / orientation.
frame_a.r_0 = [x, y, z]
Rleaf = frame_a.R
if angular_state
# Wheel carries its own Cardan-angle orientation state. It goes through
# CutJoint: when iscut is true the orientation is imposed as a 3-residue cut
# (opening the loop with an attached rooted axis) instead of full matrix
# equality. Angular velocity is derived from der_angles (avoids state
# selection picking D(R)-based states).
Rroot = RR(axes_rotations(sequence, angles, der_angles))
der_angles = der(angles)
phi_roll = angles[2]
w_roll = der(phi_roll)
w_0 = resolve1(frame_a.R, angular_velocity2(axes_rotations(sequence, angles, der_angles)))
else
# No angular state: orientation (and its angular velocity) come from frame_a,
# which is rooted by the axis the wheel is mounted on. No orientation loop is
# introduced, so there is nothing to cut.
Rroot = frame_a.R
w_roll = dot(angular_velocity2(ori(frame_a)), [0, 0, 1])
w_0 = angular_velocity1(ori(frame_a))
end
# Contact-point coordinate frame (e_long_0, e_lat_0, e_n_0)
e_axis_0 = resolve1(frame_a.R, [0, 0, 1])
assert(abs(dot(e_n_0, e_axis_0)) < 0.99, "Wheel lays nearly on the ground (which is a singularity)")
aux = cross(e_n_0, e_axis_0)
e_long_0 = aux / norm_(aux)
e_lat_0 = -cross(e_long_0, e_n_0) # wheel rotation axis and lateral axis are opposite
# Contact-point geometry
delta_0 = r_road_0 - frame_a.r_0
0 = dot(delta_0, e_axis_0)
0 = dot(delta_0, e_long_0)
# Normal contact along (radius minus center-to-road normal distance). Rigid:
# no penetration, f_n is the reaction that enforces it. Elastic: f_n is an
# explicit one-sided spring-damper in pen, so it leaves the algebraic block.
pen = radius - dot(delta_0, cross(e_long_0, e_axis_0))
if elastic_contact
f_n = max(0, c_z * pen + d_z * der(pen))
else
0 = pen
end
# Velocities
v_0 = der(frame_a.r_0)
vContact_0 = v_0 + cross(w_0, delta_0)
# Slip kinematics
v_slip_lat = dot(vContact_0, e_lat_0)
v_slip_long = dot(vContact_0, e_long_0)
v_slip = sqrt(v_slip_long ^ 2 + v_slip_lat ^ 2) + v_small
# The denominator (longitudinal rolling speed) is legitimately zero for a
# stationary wheel; regularize it (sign-preserving) so this purely-diagnostic
# quantity cannot poison initialization, which evaluates all observed
# equations at the initial guess.
slip_ratio = v_slip_long / ifelse(abs(dot(v_0, e_long_0)) < v_small, v_small, dot(v_0, e_long_0))
# Adhesion/sliding thresholds depend on rolling speed
vAdhesion = max(vAdhesion_min, sAdhesion * abs(radius * w_roll))
vSlide = max(vSlide_min, sSlide * abs(radius * w_roll))
# Total tangential force from slip-curve friction model
f = f_n * MultibodyComponents.limit_S_triple(vAdhesion, vSlide, mu_A, mu_S, v_slip)
f_long = f * v_slip_long / v_slip
f_lat = f * v_slip_lat / v_slip
# Contact force in world frame (signs match Multibody.jl SlipWheelJoint)
f_wheel_0 = f_n * e_n_0 - f_lat * e_lat_0 - f_long * e_long_0
# Force and torque balance at the wheel center
[0, 0, 0] = frame_a.f + resolve2(frame_a.R, f_wheel_0)
[0, 0, 0] = frame_a.tau + resolve2(frame_a.R, cross(delta_0, f_wheel_0))
metadata {
"Dyad": {
"icons": {"default": "dyad://MultibodyComponents/SlipWheelJoint.svg"},
"labels": [
{
"label": "$(instance)",
"x": 500,
"y": 200,
"rot": 0,
"attrs": {"font-size": "160"}
}
]
}
}
endFlattened Source
dyad
"""
Joint for a wheel with slip-dependent friction rolling on the flat plane y = 0.
Unlike `RollingWheelJoint`, this joint does not impose strict no-slip
constraints. Instead, the contact patch can slip and the resulting tangential
force is computed from a velocity-dependent friction curve evaluated by
`MultibodyComponents.limit_S_triple`. Reference:
https://people.inf.ethz.ch/fcellier/MS/andres_ms.pdf
!!! tip "Integrator choice"
The slip model contains a discontinuity in the second derivative at the
transitions between adhesion and sliding. This can cause problems for
BDF-type integrators; explicit RK methods (e.g. Tsit5) work well.
!!! warn "Normal force"
The wheel cannot leave the ground. Make sure that the normal force `f_n`
never becomes negative.
"""
component SlipWheelJoint
parameter render::Boolean = true
parameter color::Real[4] = [0.5, 0.5, 0.5, 1.0]
parameter specular_coefficient::Real = 1.5
structural parameter iscut::Boolean = false
final structural parameter residual::Real[3] = zeros(3) if iscut
variable Rleaf::Real[3, 3]
variable Rroot::Real[3, 3]
frame_a = Frame3D() {}
"""
Optional road-surface frame: when surface=true the contact patch position
equals this frame's position, so a connected component can define the road
height y = f(x, z). When false, the wheel rolls on the flat plane y = 0.
"""
surface_frame = Frame3D() if surface {}
"Tire visualization: cylinder centered on frame_a with axis along z"
tire_shape = CylinderShape(render = render, color = color, r = frame_a.r_0, R = transpose(frame_a.R), r_shape = [0, 0, -width / 2], length_direction = [0, 0, 1], length = width, width = 2 * radius, height = 2 * radius)
"Rim cross-cylinder 1: thin spoke spanning the wheel diameter; rotates with the wheel to make the spin visible"
rim1_shape = CylinderShape(render = render, color = [0.8, 0.8, 0.8, 1.0], r = frame_a.r_0, R = transpose(frame_a.R), r_shape = [-radius, 0, 0], length_direction = [1, 0, 0], length = 2 * radius, width = radius / 3, height = radius / 3)
"Rim cross-cylinder 2: second spoke, perpendicular to the first"
rim2_shape = CylinderShape(render = render, color = [0.8, 0.8, 0.8, 1.0], r = frame_a.r_0, R = transpose(frame_a.R), r_shape = [0, -radius, 0], length_direction = [0, 1, 0], length = 2 * radius, width = radius / 3, height = radius / 3)
"Use an external road surface supplied via surface_frame (else flat ground y=0)"
structural parameter surface::Boolean = false
"""
Use a compliant (elastic) normal contact instead of the rigid no-penetration
constraint: f_n = max(0, c_z*pen + d_z*der(pen)) is then explicit, so f_n and
the contact-point acceleration leave the coupled algebraic block (this decouples
free-floating multi-wheel cars). The wheel vertical position becomes a dynamic
tire-compression state.
"""
structural parameter elastic_contact::Boolean = false
"""
Include the wheel's angular state (Cardan angles). Set false when the wheel is
mounted on an already-rooted axis: the orientation is then taken from frame_a
and no orientation loop/state is introduced (avoids redundant constraint forces
in free-floating multi-wheel assemblies).
"""
structural parameter angular_state::Boolean = true
"Rotation axis sequence for the Cardan angles that orient `frame_a` (default y, z, x)"
structural parameter sequence::Integer[3] = [2, 3, 1]
"Radius of the wheel"
parameter radius::Length = 0.3
"Width of the wheel in animations"
parameter width::Real = 0.035
"Vertical contact stiffness, used only when elastic_contact = true [N/m]"
parameter c_z::Real = 250000
"Vertical contact damping, used only when elastic_contact = true [N*s/m]"
parameter d_z::Real = 500
"Minimum velocity for the peak of the adhesion curve (regularization)"
parameter vAdhesion_min::Velocity = 0.05
"Minimum velocity for the start of the flat region of the slip curve (regularization)"
parameter vSlide_min::Velocity = 0.15
"Adhesion slippage: the peak of the adhesion curve appears when the wheel slip equals `sAdhesion`"
parameter sAdhesion::Real = 0.04
"Sliding slippage: the flat region of the slip curve appears when the wheel slip exceeds `sSlide`"
parameter sSlide::Real = 0.12
"Friction coefficient at the adhesion peak"
parameter mu_A::Real = 0.8
"Friction coefficient at full sliding"
parameter mu_S::Real = 0.6
"Small value added to `v_slip` to avoid division by zero in the slip model"
parameter v_small::Real = 1e-5
"x-position of the wheel axis"
variable x::Length(statePriority = 15)
"y-position of the wheel axis"
variable y::Length(statePriority = 0)
"z-position of the wheel axis"
variable z::Length(statePriority = 15)
"Angles that rotate the world frame into `frame_a` around the y-, z-, x-axis"
variable angles::Angle(statePriority = 15)[3] if angular_state
"Time derivatives of `angles`"
variable der_angles::AngularVelocity(statePriority = 30)[3] if angular_state
"Wheel rolling angle (`angles[2]`)"
variable phi_roll::Angle if angular_state
"Wheel rolling angular velocity"
variable w_roll::AngularVelocity
"Position vector from world frame to contact point on the road, resolved in world frame"
variable r_road_0::Position[3]
"Force vector on the wheel at the contact point, resolved in world frame"
variable f_wheel_0::Real[3]
"Contact force acting on the wheel in the normal direction"
variable f_n::Dyad.Force
"Contact force acting on the wheel in the lateral direction"
variable f_lat::Dyad.Force
"Contact force acting on the wheel in the longitudinal direction"
variable f_long::Dyad.Force
"Unit vector along the wheel axis, resolved in world frame"
variable e_axis_0::Real[3]
"Distance vector from wheel center to contact point, resolved in world frame"
variable delta_0::Real[3]
"Tire penetration depth (radius minus center-to-road normal distance); >0 when compressed"
variable pen::Real
"Unit vector normal to the road at the contact point, resolved in world frame"
variable e_n_0::Real[3]
"Unit vector in the lateral direction of the road at the contact point, resolved in world frame"
variable e_lat_0::Real[3]
"Unit vector in the longitudinal direction of the road at the contact point, resolved in world frame"
variable e_long_0::Real[3]
"Road heading at (s, w), resolved in world frame (unit vector)"
variable e_s_0::Real[3]
"Road surface parameter 1"
variable s::Real
"Road surface parameter 2"
variable w::Real
"Velocity of wheel center, resolved in world frame"
variable v_0::Velocity[3]
"Angular velocity of the wheel, resolved in world frame"
variable w_0::AngularVelocity[3]
"Velocity of the contact point, resolved in world frame"
variable vContact_0::Velocity[3]
"Auxiliary vector for building the contact frame"
variable aux::Real[3]
"Slip velocity in the longitudinal direction"
variable v_slip_long::Velocity
"Slip velocity in the lateral direction"
variable v_slip_lat::Velocity
"Slip velocity magnitude (with `v_small` regularization)"
variable v_slip::Velocity
"Slip ratio = v_slip_long / (v_0 · e_long_0)"
variable slip_ratio::Real
"Total traction force"
variable f::Dyad.Force
"Slip velocity at which adhesion is maximized"
variable vAdhesion::Velocity
"Slip velocity at which the flat region of the slip model starts"
variable vSlide::Velocity
relations
if iscut
residue(Rleaf, Rroot) = residual
else
Rleaf = Rroot
end
# Solver guesses for algebraic intermediates (mirrors Multibody.jl defaults)
guess x = 0
guess y = radius
guess z = 0
if angular_state
guess angles = [0, 0, 0]
guess der_angles = [0, 0, 0]
guess phi_roll = 0
end
guess w_roll = 0
guess f_n = 1.0
guess delta_0 = [0, -radius, 0]
guess pen = 0
guess e_n_0 = [0, 1, 0]
guess aux = [1, 0, 0]
guess v_slip_long = 0
guess v_slip_lat = 0
# Road description
if surface
# The surface frame is a kinematic road reference: it exerts no force.
surface_frame.f = [0, 0, 0]
surface_frame.tau = [0, 0, 0]
# Contact patch position is the surface-frame position; s, w (= x, z) remain
# free and are solved by the contact constraints, while the connected road
# component imposes the height (y of the frame position).
s = surface_frame.r_0[1]
w = surface_frame.r_0[3]
r_road_0 = surface_frame.r_0
# Contact tangent/normal come from the road frame's orientation: the connected
# road component orients surface_frame so its y-axis is the surface normal and
# its x-axis is the heading. This is exact for any surface the road describes
# (a flat/excitation road keeps R = I → vertical normal).
e_n_0 = resolve1(surface_frame.R, [0, 1, 0])
e_s_0 = resolve1(surface_frame.R, [1, 0, 0])
else
# Flat road description
r_road_0 = [s, 0, w]
e_n_0 = [0, 1, 0]
e_s_0 = [1, 0, 0]
end
# Pose / orientation.
frame_a.r_0 = [x, y, z]
Rleaf = frame_a.R
if angular_state
# Wheel carries its own Cardan-angle orientation state. It goes through
# CutJoint: when iscut is true the orientation is imposed as a 3-residue cut
# (opening the loop with an attached rooted axis) instead of full matrix
# equality. Angular velocity is derived from der_angles (avoids state
# selection picking D(R)-based states).
Rroot = RR(axes_rotations(sequence, angles, der_angles))
der_angles = der(angles)
phi_roll = angles[2]
w_roll = der(phi_roll)
w_0 = resolve1(frame_a.R, angular_velocity2(axes_rotations(sequence, angles, der_angles)))
else
# No angular state: orientation (and its angular velocity) come from frame_a,
# which is rooted by the axis the wheel is mounted on. No orientation loop is
# introduced, so there is nothing to cut.
Rroot = frame_a.R
w_roll = dot(angular_velocity2(ori(frame_a)), [0, 0, 1])
w_0 = angular_velocity1(ori(frame_a))
end
# Contact-point coordinate frame (e_long_0, e_lat_0, e_n_0)
e_axis_0 = resolve1(frame_a.R, [0, 0, 1])
assert(abs(dot(e_n_0, e_axis_0)) < 0.99, "Wheel lays nearly on the ground (which is a singularity)")
aux = cross(e_n_0, e_axis_0)
e_long_0 = aux / norm_(aux)
e_lat_0 = -cross(e_long_0, e_n_0) # wheel rotation axis and lateral axis are opposite
# Contact-point geometry
delta_0 = r_road_0 - frame_a.r_0
0 = dot(delta_0, e_axis_0)
0 = dot(delta_0, e_long_0)
# Normal contact along (radius minus center-to-road normal distance). Rigid:
# no penetration, f_n is the reaction that enforces it. Elastic: f_n is an
# explicit one-sided spring-damper in pen, so it leaves the algebraic block.
pen = radius - dot(delta_0, cross(e_long_0, e_axis_0))
if elastic_contact
f_n = max(0, c_z * pen + d_z * der(pen))
else
0 = pen
end
# Velocities
v_0 = der(frame_a.r_0)
vContact_0 = v_0 + cross(w_0, delta_0)
# Slip kinematics
v_slip_lat = dot(vContact_0, e_lat_0)
v_slip_long = dot(vContact_0, e_long_0)
v_slip = sqrt(v_slip_long ^ 2 + v_slip_lat ^ 2) + v_small
# The denominator (longitudinal rolling speed) is legitimately zero for a
# stationary wheel; regularize it (sign-preserving) so this purely-diagnostic
# quantity cannot poison initialization, which evaluates all observed
# equations at the initial guess.
slip_ratio = v_slip_long / ifelse(abs(dot(v_0, e_long_0)) < v_small, v_small, dot(v_0, e_long_0))
# Adhesion/sliding thresholds depend on rolling speed
vAdhesion = max(vAdhesion_min, sAdhesion * abs(radius * w_roll))
vSlide = max(vSlide_min, sSlide * abs(radius * w_roll))
# Total tangential force from slip-curve friction model
f = f_n * MultibodyComponents.limit_S_triple(vAdhesion, vSlide, mu_A, mu_S, v_slip)
f_long = f * v_slip_long / v_slip
f_lat = f * v_slip_lat / v_slip
# Contact force in world frame (signs match Multibody.jl SlipWheelJoint)
f_wheel_0 = f_n * e_n_0 - f_lat * e_lat_0 - f_long * e_long_0
# Force and torque balance at the wheel center
[0, 0, 0] = frame_a.f + resolve2(frame_a.R, f_wheel_0)
[0, 0, 0] = frame_a.tau + resolve2(frame_a.R, cross(delta_0, f_wheel_0))
metadata {
"Dyad": {
"icons": {"default": "dyad://MultibodyComponents/SlipWheelJoint.svg"},
"labels": [
{
"label": "$(instance)",
"x": 500,
"y": 200,
"rot": 0,
"attrs": {"font-size": "160"}
}
]
}
}
endTest Cases
No test cases defined.
Related
Examples
Experiments
Analyses