diff --git a/src/joints.jl b/src/joints.jl index 6807772b..ebe15add 100644 --- a/src/joints.jl +++ b/src/joints.jl @@ -450,10 +450,10 @@ angles: Angles to rotate world-frame into frame_a around z-, y-, x-axis ] (f_long(t) = 0), [description = "Contact force acting on wheel in longitudinal direction"] - (err(t) = 0), - [ - description = "|r_road_0 - frame_a.r_0| - radius (must be zero; used for checking)", - ] + # (err(t) = 0), + # [ + # description = "|r_road_0 - frame_a.r_0| - radius (must be zero; used for checking)", + # ] (e_axis_0(t)[1:3] = zeros(3)), [description = "Unit vector along wheel axis, resolved in world frame"] (delta_0(t)[1:3] = zeros(3)), @@ -500,7 +500,7 @@ angles: Angles to rotate world-frame into frame_a around z-, y-, x-axis # Coordinate system at contact point (e_long_0, e_lat_0, e_n_0) collect(e_axis_0) .~ resolve1(ori(frame_a), [0, 1, 0]) collect(aux) .~ collect(cross(e_n_0, e_axis_0)) - collect(e_long_0) .~ collect(aux ./ norm(aux)) + collect(e_long_0) .~ collect(aux ./ _norm(aux)) collect(e_lat_0) .~ collect(cross(e_long_0, e_n_0)) # Determine point on road where the wheel is in contact with the road @@ -512,7 +512,7 @@ angles: Angles to rotate world-frame into frame_a around z-, y-, x-axis 0 ~ radius - delta_0'cross(e_long_0, e_axis_0) # only for testing - err ~ norm(delta_0) - radius + # err ~ norm(delta_0) - radius # Slip velocities collect(v_0) .~ D.(frame_a.r_0) @@ -533,6 +533,8 @@ angles: Angles to rotate world-frame into frame_a around z-, y-, x-axis compose(ODESystem(equations, t; name), frame_a) end +_norm(x) = sqrt(sum(abs2.(x))) + """ RollingWheel(; name, radius, m, I_axis, I_long, width=0.035, x0, y0, kwargs...) @@ -603,7 +605,7 @@ with the wheel itself. collect(rollingWheel.der_angles) .~ collect(der_angles) connect(body.frame_a, frame_a) connect(rollingWheel.frame_a, frame_a)] - compose(ODESystem(equations, t, sts, pars; name), frame_a, rollingWheel, body) + compose(ODESystem(equations, t; name), frame_a, rollingWheel, body) end """ diff --git a/test/runtests.jl b/test/runtests.jl index d09286a1..a825a387 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -616,25 +616,28 @@ sol = solve(prob, Rodas4()) # ============================================================================== ## Rolling wheel =============================================================== # ============================================================================== -world = Multibody.world +# The wheel does not need the world @named wheel = RollingWheel(radius = 0.3, m = 2, I_axis = 0.06, - I_long = 0.12, - x0 = 0.2, - y0 = 0.2, - der_angles = [0, 5, 1]) + I_long = 0.12, + x0 = 0.2, + y0 = 0.2, + der_angles = [0, 5, 1]) + + +wheel = complete(wheel) -cwheel = complete(wheel) defs = [ # collect(world.n .=> [0, 0, -1]); # collect(D.(cwheel.rollingWheel.angles)) .=> [0, 5, 1] ] - - ssys = structural_simplify(IRSystem(wheel)) prob = ODEProblem(ssys, defs, (0, 10)) + + +prob.u0 .+= 0.001 .* randn.() @test_skip begin # Does not initialize - sol = solve(prob, Rodas4())#, u0 = prob.u0 .+ 1e-2 .* rand.()) + sol = solve(prob, Rodas4(), u0 = prob.u0 .+ 1e-2 .* rand.()) @info "Write tests" end @@ -726,4 +729,4 @@ sol = solve(prob, Rodas4()) doplot() && plot(sol, idxs = collect(freeMotion.phi), title = "Dzhanibekov effect") @info "Write tests" -@test_broken sol(0, idxs = collect(freeMotion.phi)) != zeros(3) \ No newline at end of file +@test_broken sol(0, idxs = collect(freeMotion.phi)) != zeros(3) # The problem here is that the initial condition is completely ignored \ No newline at end of file