diff --git a/src/joints.jl b/src/joints.jl index cb1e5813..cd8ebb4e 100644 --- a/src/joints.jl +++ b/src/joints.jl @@ -478,16 +478,16 @@ 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]) +function RollingWheelJoint(; name, radius, angles = zeros(3), der_angles=zeros(3), x0, y0, z0 = 0, sequence = [3, 2, 1]) @named frame_a = Frame(varw=true) @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 = 15, description = "y-position of the wheel axis"] + (z(t) = z0), [state_priority = 0, 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", @@ -521,8 +521,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)"] @@ -628,7 +628,7 @@ with the wheel itself. 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, y0, der_angles, kwargs...) body = Body(r_cm = [0, 0, 0], m = m, I_11 = I_long, @@ -647,11 +647,11 @@ 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.0, description = "x-position of the wheel axis"] + (y(t) = y0), [state_priority = 20.0, description = "y-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.0, description = "Angles to rotate world-frame into frame_a around z-, y-, x-axis"] + (der_angles(t)[1:3] = der_angles), [state_priority = 30.0, description = "Derivatives of angles"] end # sts = reduce(vcat, collect.(sts)) diff --git a/test/runtests.jl b/test/runtests.jl index dbc6146a..d42b3e48 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -672,17 +672,17 @@ wheel = complete(wheel) defs = [ collect(world.n .=> [0, 0, -1]); - vec(ori(wheel.frame_a).R .=> I(3)); - vec(ori(wheel.body.frame_a).R .=> I(3)); + wheel.body.r_0[1] => 0.2; + wheel.body.r_0[2] => 0.2; + wheel.body.r_0[3] => 0.3; # collect(D.(cwheel.rollingWheel.angles)) .=> [0, 5, 1] ] -@test_skip begin # Does not initialize - ssys = structural_simplify(IRSystem(wheel)) - prob = ODEProblem(ssys, defs, (0, 10)) - sol = solve(prob, Rodas5P(autodiff=false)) - @info "Write tests" -end +ssys = structural_simplify(IRSystem(wheel)) +prob = ODEProblem(ssys, defs, (0, 0.8)) +sol = solve(prob, RadauIIA5()) +@test_broken SciMLBase.successful_retcode(sol) +@info "Write tests" end