Merge pull request #128 from JuliaComputing/wheelsurface

add surface parameter to rolling wheel

Author: baggepinnen

Project.toml

@@ -24,7 +24,7 @@ Render = ["Makie"]
 
 [compat]
 CoordinateTransformations = "0.6"
-DataInterpolations = "5"
+DataInterpolations = "5, 6"
 FileIO = "1"
 JuliaSimCompiler = "0.1.12"
 LinearAlgebra = "1.6"

src/wheels.jl

@@ -21,9 +21,9 @@ this frame.
 
 # Arguments and parameters:
 
-name: Name of the rolling wheel joint component
-radius: Radius of the wheel
-angles: Angles to rotate world-frame into frame_a around y-, z-, x-axis
+- `radius`: Radius of the wheel
+- `angles`: Angles to rotate world-frame into frame_a around y-, z-, x-axis
+- `surface`: By default, the wheel is rolling on a flat xz plane. A function `surface = (x, z)->y` may be provided to define a road surface. The function should return the height of the road at `(x, z)`.
 
 # Variables:
 - `x`: x-position of the wheel axis
@@ -52,7 +52,7 @@ angles: Angles to rotate world-frame into frame_a around y-, z-, x-axis
 # Connector frames
 - `frame_a`: Frame for the wheel joint
 """
-@component function RollingWheelJoint(; name, radius, angles = zeros(3), der_angles=zeros(3), x0=0, y0 = radius, z0=0, sequence = [2, 3, 1], iscut=false)
+@component function RollingWheelJoint(; name, radius, angles = zeros(3), der_angles=zeros(3), x0=0, y0 = radius, z0=0, sequence = [2, 3, 1], iscut=false, surface = nothing)
     @parameters begin radius = radius, [description = "Radius of the wheel"] end
     @variables begin
         (x(t) = x0), [state_priority = 15, description = "x-position of the wheel axis"]
@@ -116,18 +116,32 @@ angles: Angles to rotate world-frame into frame_a around y-, z-, x-axis
 
     Rarot = axes_rotations(sequence, angles, -der_angles) # The - is the neg_w change
 
+    equations = if surface === nothing
+        [ # Road description
+            r_road_0 .~ [s, 0, w]
+            e_n_0 .~ [0, 1, 0]
+            e_s_0 .~ [1, 0, 0]
+        ]
+     else
+        sy = surface(s, w)
+        e_w_0 = _normalize([0, expand_derivatives(Differential(w)(sy)), 1])
+        # @show sy, expand_derivatives(Differential(s)(sy)), expand_derivatives(Differential(w)(sy))
+        [
+            r_road_0 .~ [s, sy, w]
+            e_s_0 .~ _normalize([1, expand_derivatives(Differential(s)(sy)), 0])
+            e_n_0 .~ _normalize(cross(e_w_0, e_s_0))
+        ]
+     end
+
     equations = [
+                equations;
                 connect_orientation(Ra, Rarot; iscut)   # Ra ~ Rarot
                 Ra.w ~ Rarot.w
 
                  # frame_a.R is computed from generalized coordinates
                  collect(frame_a.r_0) .~ [x, y, z]
                  der_angles .~ D.(angles)
 
-                 # Road description
-                 r_road_0 .~ [s, 0, w]
-                 e_n_0 .~ [0, 1, 0]
-                 e_s_0 .~ [1, 0, 0]
 
                  # Coordinate system at contact point (e_long_0, e_lat_0, e_n_0)
                  e_axis_0 .~ resolve1(Ra, [0, 0, 1])

test/test_wheels.jl

@@ -47,6 +47,74 @@ sol = solve(prob, Tsit5(), abstol=1e-8, reltol=1e-8)
 # ])
 
 
+# ==============================================================================
+## Rolling wheel on interesting surface ===============================================================
+# ==============================================================================
+using LinearAlgebra
+# The wheel does not need the world
+# @testset "Rolling wheel" begin
+@mtkmodel WheelInWorld begin
+    @structural_parameters begin
+        surface
+    end
+    @components begin
+        # world = World(n=[0,0,-1])
+        world = W()
+        wheel = RollingWheel(; radius = 0.3, m = 2, I_axis = 0.06,
+                            I_long = 0.12,
+                            x0 = 0.2,
+                            z0 = 0.2,
+                            surface,
+                            der_angles = [0, 0, 0])
+    end
+end
+
+@named worldwheel = WheelInWorld(surface = (x,z)->0)
+worldwheel = complete(worldwheel)
+
+# pars = collect(worldwheel.world.n) .=> [0,0,-1];
+defs = Dict([
+    # collect(worldwheel.world.n) .=> [0,0,-1];
+    worldwheel.wheel.body.r_0[1] => 0.2;
+    worldwheel.wheel.body.r_0[2] => 0.3;
+    worldwheel.wheel.body.r_0[3] => 0.2;
+    collect(worldwheel.wheel.rollingWheel.der_angles) .=> [0, 5, 1];
+    # collect(D.(cwheel.rollingWheel.angles)) .=> [0, 5, 1]
+])
+
+ssys = structural_simplify(IRSystem(worldwheel))
+prob = ODEProblem(ssys, defs, (0, 4))
+sol = solve(prob, FBDF(autodiff=false), abstol=1e-8, reltol=1e-8)
+@test SciMLBase.successful_retcode(sol)
+# first(Multibody.render(worldwheel, sol, 0, show_axis=true))
+@test sol(4, idxs=[worldwheel.wheel.x; worldwheel.wheel.z]) ≈ [0.162547, -2.23778] atol=1e-3
+
+@named worldwheel = WheelInWorld(surface = (x,z)->x)
+worldwheel = complete(worldwheel)
+
+defs = Dict([
+    # collect(worldwheel.world.n) .=> [0,0,-1];
+    worldwheel.wheel.body.r_0[1] => 0.0;
+    worldwheel.wheel.body.r_0[2] => 0.3/sqrt(2);
+    worldwheel.wheel.body.r_0[3] => 0.0;
+    collect(worldwheel.wheel.rollingWheel.der_angles) .=> [0, 0, 0];
+])
+
+ssys = structural_simplify(IRSystem(worldwheel))
+prob = ODEProblem(ssys, defs, (0, 4))
+sol = solve(prob, FBDF(autodiff=false), abstol=1e-8, reltol=1e-8)
+# plot(sol)
+tv = 0:0.5:4
+@test sol(tv, idxs=worldwheel.wheel.body.r_0[1]) ≈ sol(tv, idxs=worldwheel.wheel.body.r_0[2]) .- 0.3*sqrt(2) rtol=1e-6 # The sqrt(2) is to account for the shifted contact point at a 45 degree plane
+
+dd = diff(sol(tv, idxs=worldwheel.wheel.rollingWheel.der_angles[2]).u) # angular acceleration
+@test norm(dd .- dd[1]) < 1e-10 # constant acceleration
+@test abs(dd[1]) < 9.81
+@test abs(dd[1]) > 5
+
+
+
+
 ## Rolling wheel with axis ====================================================
 import ModelingToolkitStandardLibrary.Blocks
 @mtkmodel WheelWithAxis begin