Experiment with a scalar formulation

ext/Render.jl

@@ -341,7 +341,7 @@ function render!(scene, ::typeof(Frame), sys, sol, t)
     true
 end
 
-function render!(scene, T::Union{typeof(Revolute), typeof(RevolutePlanarLoopConstraint)}, sys, sol, t)
+function render!(scene, ::Union{typeof(Revolute), typeof(RevolutePlanarLoopConstraint)}, sys, sol, t)
     r_0 = get_fun(sol, collect(sys.frame_a.r_0))
     n = get_fun(sol, collect(sys.n))
     color = get_color(sys, sol, :red)
@@ -533,7 +533,34 @@ function render!(scene, ::typeof(UniversalSpherical), sys, sol, t)
         Sphere(r2, sphere_diameter/2)
     end
     mesh!(scene, thing; color=sphere_color, specular = Vec3f(1.5))
+    true
+end
 
+function render!(scene, ::typeof(RollingWheelJoint), sys, sol, t)
+
+    r_0 = get_fun(sol, collect(sys.frame_a.r_0))
+    # framefun = get_frame_fun(sol, sys.frame_a)
+    rotfun = get_rot_fun(sol, sys.frame_a)
+    color = get_color(sys, sol, :red)
+
+    radius = try
+        sol(sol.t[1], idxs=sys.radius)
+    catch
+        0.05f0
+    end |> Float32
+    thing = @lift begin
+        # radius = sol($t, idxs=sys.radius)
+        O = r_0($t)
+        # T_w_a = framefun($t)
+        R_w_a = rotfun($t)
+        n_w = R_w_a[:, 3] # Wheel rotates around z axis
+        # n_w = R_w_a*n_a # Rotate to the world frame
+        width = radius/10
+        p1 = Point3f(O + width*n_w)
+        p2 = Point3f(O - width*n_w)
+        Makie.GeometryBasics.Cylinder(p1, p2, radius)
+    end
+    mesh!(scene, thing; color, specular = Vec3f(1.5), shininess=20f0, diffuse=Vec3f(1))
     true
 end
 

src/Multibody.jl

@@ -1,5 +1,6 @@
 module Multibody
-
+# Find variables that are both array form and scalarized / collected
+# foreach(println, sort(unknowns(IRSystem(model)), by=string))
 using LinearAlgebra
 using ModelingToolkit
 using JuliaSimCompiler
@@ -172,6 +173,4 @@ include("robot/path_planning.jl")
 include("robot/robot_components.jl")
 include("robot/FullRobot.jl")
 
-
-
 end

src/components.jl

@@ -38,23 +38,27 @@ end
 """
     World(; name, render=true)
 """
-@component function World(; name, render=true, point_gravity=false)
+@component function World(; name, render=true, point_gravity=false, n = [0.0, -1.0, 0.0], g=9.80665, mu=3.986004418e14)
     # World should have
     # 3+3+9+3 // r_0+f+R.R+τ
     # - (3+3) // (f+t)
     # = 12 equations 
+    n0 = n
+    g0 = g
+    mu0 = mu
     @named frame_b = Frame()
-    @parameters n[1:3]=[0, -1, 0] [description = "gravity direction of world"]
-    @parameters g=9.80665 [description = "gravitational acceleration of world"]
-    @parameters mu=3.986004418e14 [description = "Gravity field constant [m³/s²] (default = field constant of earth)"]
+    @parameters n[1:3]=n0 [description = "gravity direction of world"]
+    @parameters g=g0 [description = "gravitational acceleration of world"]
+    @parameters mu=mu0 [description = "Gravity field constant [m³/s²] (default = field constant of earth)"]
     @parameters render=render
     @parameters point_gravity = point_gravity
+    n = Symbolics.scalarize(n)
     O = ori(frame_b)
-    eqs = Equation[collect(frame_b.r_0) .~ 0;
-                   O ~ nullrotation()
-                   # vec(D(O).R .~ 0); # QUESTION: not sure if I should have to add this, should only have 12 equations according to modelica paper
-                   ]
-    ODESystem(eqs, t, [], [n; g; mu; point_gravity; render]; name, systems = [frame_b])
+    eqs = Equation[
+        collect(frame_b.r_0) .~ 0;
+        O ~ nullrotation()
+    ]
+    ODESystem(eqs, t, [], [n; g; mu; point_gravity; render]; name, systems = [frame_b])#, defaults=[n => n0; g => g0; mu => mu0])
 end
 
 """
@@ -119,7 +123,7 @@ Can be thought of as a massless rod. For a massive rod, see [`BodyShape`](@ref)
 @component function FixedTranslation(; name, r, radius=0.02f0, color = purple)
     @named frame_a = Frame()
     @named frame_b = Frame()
-    @parameters r[1:3]=r [
+    @parameters r[1:3]=collect(r) [
         description = "position vector from frame_a to frame_b, resolved in frame_a",
     ]
     r = collect(r)

src/joints.jl

@@ -449,9 +449,9 @@ end
 """
     RollingWheelJoint(; name, radius, angles, x0, y0, z0)
 
-Joint (no mass, no inertia) that describes an ideal rolling wheel (rolling on the plane z=0). See [`RollingWheel`](@ref) for a realistic wheel model with inertia.
+Joint (no mass, no inertia) that describes an ideal rolling wheel (rolling on the plane y=0). See [`RollingWheel`](@ref) for a realistic wheel model with inertia.
 
-A joint for a wheel rolling on the x-y plane of the world frame.
+A joint for a wheel rolling on the x-z plane of the world frame.
 The rolling contact is considered being ideal, i.e. there is no
 slip between the wheel and the ground. This is simply
 gained by two non-holonomic constraint equations on velocity level
@@ -462,22 +462,22 @@ the wheel can not take off.
 
 The origin of the frame `frame_a` is placed in the intersection
 of the wheel spin axis with the wheel middle plane and rotates
-with the wheel itself. The y-axis of `frame_a` is identical with
-the wheel spin axis, i.e. the wheel rotates about y-axis of `frame_a`.
+with the wheel itself. The z-axis of `frame_a` is identical with
+the wheel spin axis, i.e. the wheel rotates about z-axis of `frame_a`.
 A wheel body collecting the mass and inertia should be connected to
 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 z-, y-, x-axis
+angles: Angles to rotate world-frame into frame_a around y-, z-, x-axis
 
 # Variables:
 - `x`: x-position of the wheel axis
 - `y`: y-position of the wheel axis
 - `z`: z-position of the wheel axis
-- `angles`: Angles to rotate world-frame into `frame_a` around z-, y-, x-axis
+- `angles`: Angles to rotate world-frame into `frame_a` around y-, z-, x-axis
 - `der_angles`: Derivatives of angles
 - `r_road_0`: Position vector from world frame to contact point on road, resolved in world frame
 - `f_wheel_0`: Force vector on wheel, resolved in world frame
@@ -500,16 +500,15 @@ angles: Angles to rotate world-frame into frame_a around z-, y-, x-axis
 # Connector frames
 - `frame_a`: Frame for the wheel joint
 """
-function RollingWheelJoint(; name, radius, angles = zeros(3), x0, y0, z0 = 0, sequence = [3, 2, 1])
-    @named frame_a = Frame(varw=true)
+@component function RollingWheelJoint(; name, radius, angles = zeros(3), der_angles=zeros(3), x0=0, y0 = radius, z0=0, sequence = [2, 3, 1], iscut=false)
     @parameters begin radius = radius, [description = "Radius of the wheel"] end
     @variables begin
-        (x(t) = x0), [state_priority = 1, description = "x-position of the wheel axis"]
-        (y(t) = y0), [state_priority = 1, description = "y-position of the wheel axis"]
-        (z(t) = z0), [state_priority = 1, description = "z-position of the wheel axis"]
+        (x(t) = x0), [state_priority = 15, description = "x-position of the wheel axis"]
+        (y(t) = y0), [state_priority = 0, description = "y-position of the wheel axis"]
+        (z(t) = z0), [state_priority = 15, description = "z-position of the wheel axis"]
         (angles(t)[1:3] = angles),
-        [description = "Angles to rotate world-frame into frame_a around z-, y-, x-axis"]
-        (der_angles(t)[1:3] = zeros(3)), [description = "Derivatives of angles"]
+        [state_priority = 5, description = "Angles to rotate world-frame into frame_a around z-, y-, x-axis"]
+        (der_angles(t)[1:3] = der_angles), [state_priority = 5, description = "Derivatives of angles"]
         (r_road_0(t)[1:3] = zeros(3)),
         [
             description = "Position vector from world frame to contact point on road, resolved in world frame",
@@ -528,7 +527,7 @@ function RollingWheelJoint(; name, radius, angles = zeros(3), x0, y0, z0 = 0, se
         # ]
         (e_axis_0(t)[1:3] = zeros(3)),
         [description = "Unit vector along wheel axis, resolved in world frame"]
-        (delta_0(t)[1:3] = [0,0,-radius]),
+        (delta_0(t)[1:3] = [0,-radius, 0]),
         [description = "Distance vector from wheel center to contact point"]
         (e_n_0(t)[1:3] = zeros(3)),
         [
@@ -543,8 +542,8 @@ function RollingWheelJoint(; name, radius, angles = zeros(3), x0, y0, z0 = 0, se
             description = "Unit vector in longitudinal direction of road at contact point, resolved in world frame",
         ]
 
-        (s(t) = 0), [state_priority = 1, description = "Road surface parameter 1"]
-        (w(t) = 0), [state_priority = 1, description = "Road surface parameter 2"]
+        (s(t) = 0), [description = "Road surface parameter 1"]
+        (w(t) = 0), [description = "Road surface parameter 2"]
         (e_s_0(t)[1:3] = zeros(3)),
         [description = "Road heading at (s,w), resolved in world frame (unit vector)"]
 
@@ -560,25 +559,26 @@ function RollingWheelJoint(; name, radius, angles = zeros(3), x0, y0, z0 = 0, se
 
     angles,der_angles,r_road_0,f_wheel_0,e_axis_0,delta_0,e_n_0,e_lat_0,e_long_0,e_s_0,v_0,w_0,vContact_0,aux = collect.((angles,der_angles,r_road_0,f_wheel_0,e_axis_0,delta_0,e_n_0,e_lat_0,e_long_0,e_s_0,v_0,w_0,vContact_0,aux))
 
+    @named frame_a = Frame(varw=true)
     Ra = ori(frame_a, true)
 
-    Rarot = axes_rotations(sequence, angles, der_angles)
+    Rarot = axes_rotations(sequence, angles, -der_angles) # The - is the neg_w change
 
     equations = [
-                Ra ~ Rarot
+                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, w, 0]
-                 e_n_0 .~ [0, 0, 1]
+                 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, 1, 0])
+                 e_axis_0 .~ resolve1(Ra, [0, 0, 1])
                  aux .~ (cross(e_n_0, e_axis_0))
                  e_long_0 .~ (aux ./ _norm(aux))
                  e_lat_0 .~ (cross(e_long_0, e_n_0))
@@ -616,14 +616,14 @@ end
 """
     RollingWheel(; name, radius, m, I_axis, I_long, width=0.035, x0, y0, kwargs...)
 
-Ideal rolling wheel on flat surface z=0 (5 positional, 3 velocity degrees of freedom)
+Ideal rolling wheel on flat surface y=0 (5 positional, 3 velocity degrees of freedom)
 
-A wheel rolling on the x-y plane of the world frame including wheel mass.
+A wheel rolling on the x-z plane of the world frame including wheel mass.
 The rolling contact is considered being ideal, i.e. there is no
 slip between the wheel and the ground.
 The wheel can not take off but it can incline toward the ground.
-The frame frame_a is placed in the wheel center point and rotates
-with the wheel itself.
+The frame `frame_a` is placed in the wheel center point and rotates
+with the wheel itself. A [`Revolute`](@ref) joint rotationg around `n = [0, 1, 0]` is required to attach the wheel to a wheel axis.
 
 # Arguments and parameters:
 - `name`: Name of the rolling wheel component
@@ -633,29 +633,30 @@ with the wheel itself.
 - `I_long`: Moment of inertia of the wheel perpendicular to its axis
 - `width`: Width of the wheel (default: 0.035)
 - `x0`: Initial x-position of the wheel axis
-- `y0`: Initial y-position of the wheel axis
+- `z0`: Initial z-position of the wheel axis
 - `kwargs...`: Additional keyword arguments passed to the `RollingWheelJoint` function
 
 # Variables:
 - `x`: x-position of the wheel axis
-- `y`: y-position of the wheel axis
-- `angles`: Angles to rotate world-frame into `frame_a` around z-, y-, x-axis
-- `der_angles`: Derivatives of angles
+- `z`: z-position of the wheel axis
+- `angles`: Angles to rotate world-frame into `frame_a` around y-, z-, x-axis
+- `der_angles`: Derivatives of angles  (y: like rotational velocity of a spinning coin, z: wheel forward spin speed, x: wheel falling over speed)
 
 # Named components:
 - `frame_a`: Frame for the wheel component
 - `rollingWheel`: Rolling wheel joint representing the wheel's contact with the road surface
 """
-@component function RollingWheel(; name, radius, m, I_axis, I_long, width = 0.035, x0, y0,
+@component function RollingWheel(; name, radius, m, I_axis, I_long, width = 0.035, x0=0, z0=0,
                       angles = zeros(3), der_angles = zeros(3), kwargs...)
     @named begin
         frame_a = Frame()
-        rollingWheel = RollingWheelJoint(; radius, angles, x0, y0, kwargs...)
+        rollingWheel = RollingWheelJoint(; radius, angles, x0, z0, der_angles, kwargs...)
         body = Body(r_cm = [0, 0, 0],
+                    state_priority = 0,
                     m = m,
                     I_11 = I_long,
-                    I_22 = I_axis,
-                    I_33 = I_long,
+                    I_22 = I_long,
+                    I_33 = I_axis,
                     I_21 = 0,
                     I_31 = 0,
                     I_32 = 0)
@@ -669,16 +670,16 @@ with the wheel itself.
         width = width, [description = "Width of the wheel"]
     end
     sts = @variables begin
-        (x(t) = x0), [state_priority = 2.0, description = "x-position of the wheel axis"]
-        (y(t) = y0), [state_priority = 2.0, description = "y-position of the wheel axis"]
+        (x(t) = x0), [state_priority = 20, description = "x-position of the wheel axis"]
+        (z(t) = z0), [state_priority = 20, description = "z-position of the wheel axis"]
         (angles(t)[1:3] = angles),
-        [description = "Angles to rotate world-frame into frame_a around z-, y-, x-axis"]
-        (der_angles(t)[1:3] = der_angles), [state_priority = 3.0, description = "Derivatives of angles"]
+        [state_priority = 30, description = "Angles to rotate world-frame into frame_a around y-, z-, x-axis"]
+        (der_angles(t)[1:3] = der_angles), [state_priority = 30, description = "Derivatives of angles"]
     end
     # sts = reduce(vcat, collect.(sts))
 
     equations = Equation[rollingWheel.x ~ x
-                         rollingWheel.y ~ y
+                         rollingWheel.z ~ z
                          collect(rollingWheel.angles) .~ collect(angles)
                          collect(rollingWheel.der_angles) .~ collect(der_angles)
                          connect(body.frame_a, frame_a)

src/orientation.jl

@@ -145,12 +145,18 @@ end
 skew(s) = [0 -s[3] s[2]; s[3] 0 -s[1]; -s[2] s[1] 0]
 skewcoords(R::AbstractMatrix) = [R[3, 2]; R[1, 3]; R[2, 1]]
 
-function planar_rotation(axis, phi, der_angle)
+
+"""
+    planar_rotation(axis, phi, phid)
+
+Generate a rotation matrix for a rotation around the specified axis (axis-angle representation).
+"""
+function planar_rotation(axis, phi, phid)
     length(axis) == 3 || error("axis must be a 3-vector")
     axis = collect(axis)
     ee = collect(axis * axis')
     R = ee + (I(3) - ee) * cos(phi) - skew(axis) * sin(phi)
-    w = axis * der_angle
+    w = axis * phid
     RotationMatrix(R, w)
 end
 
@@ -279,7 +285,11 @@ to_mb(Q::Rotations.QuatRotation) = to_mb(vec(Q))
 
 
 ## Euler
+"""
+    axes_rotations(sequence, angles, der_angles; name = :R_ar)
 
+Generate a rotation matrix for a rotation around the specified axes (Euler/Cardan angles).
+"""
 function axes_rotations(sequence, angles, der_angles, name = :R_ar)
     R = axis_rotation(sequence[3], angles[3]) *
         axis_rotation(sequence[2], angles[2]) *