diff --git a/examples/robot/test_components.jl b/examples/robot/test_components.jl index 0f3b6f87..3cbe3a8c 100644 --- a/examples/robot/test_components.jl +++ b/examples/robot/test_components.jl @@ -19,17 +19,22 @@ using ModelingToolkitStandardLibrary.Blocks myfixed(args...; kwargs...) = ModelingToolkitStandardLibrary.Mechanical.Rotational.Fixed(args...; kwargs...) +myinertia(args...; kwargs...) = ModelingToolkitStandardLibrary.Mechanical.Rotational.Inertia(args...; kwargs...) + +mytorque(args...; kwargs...) = ModelingToolkitStandardLibrary.Mechanical.Rotational.ConstantTorque(args...; kwargs...) ## @mtkmodel MotorTest begin @components begin motor = Motor() - fixed = myfixed() # bug in @mtkmodel + # fixed = myfixed() # bug in @mtkmodel + inertia = myinertia(J=1) constant = Constant(k=1) end @equations begin - connect(motor.flange_motor, fixed.flange) + # connect(motor.flange_motor, fixed.flange) + connect(motor.flange_motor, inertia.flange_a) constant.output.u ~ motor.axisControlBus.current_ref end end @@ -37,22 +42,42 @@ end @named motorTest = MotorTest() m = structural_simplify(motorTest) @test length(states(m)) == 3 + # D(motorTest.motor.gear.bearingFriction.w) => 0 +cm = complete(motorTest) + +prob = ODEProblem(m, [ + D(D(cm.motor.Jmotor.phi)) => 0 +], (0.0, 5.0)) +sol = solve(prob, Rodas4()) +plot(sol, idxs=cm.motor.phi.phi.u) ## @mtkmodel GearTest begin @components begin - gear2 = GearType2() - fixed = myfixed() # bug in @mtkmodel + motor = Motor() + inertia = myinertia(J=1) + gear = GearType2() + # fixed = myfixed() # bug in @mtkmodel constant = Constant(k=1) end @equations begin - connect(gear2.flange_a, fixed.flange) + constant.output.u ~ motor.axisControlBus.current_ref + connect(motor.flange_motor, gear.flange_a) + connect(gear.flange_b, inertia.flange_a) + # connect(gear2.flange_a, fixed.flange) end end @named gearTest = GearTest() m = structural_simplify(gearTest) +cm = complete(gearTest) + +prob = ODEProblem(m, [ + ModelingToolkit.missing_variable_defaults(m); +], (0.0, 5.0)) +sol = solve(prob, Rodas4()) +plot(sol, idxs=cm.motor.phi.phi.u) @mtkmodel GearTest begin @@ -92,31 +117,30 @@ m = structural_simplify(controllerTest) m = structural_simplify(IRSystem(controllerTest)) -## -myinertia(args...; kwargs...) = ModelingToolkitStandardLibrary.Mechanical.Rotational.Inertia(args...; kwargs...) +## Test Axis -mytorque(args...; kwargs...) = ModelingToolkitStandardLibrary.Mechanical.Rotational.ConstantTorque(args...; kwargs...) @mtkmodel AxisTest2 begin @components begin axis2 = AxisType2() - fixed = myfixed() # bug in @mtkmodel + # fixed = myfixed() # bug in @mtkmodel # fixed = mytorque(tau_constant=1, use_support=true) # bug in @mtkmodel - # inertia = myinertia(J=1) + inertia = myinertia(J=1) # constant1 = Constant(k=1) constant2 = Constant(k=0) - constant3 = Constant(k=1) + constant3 = Constant(k=2) constant4 = Constant(k=1) constant5 = Constant(k=0) end @equations begin - connect(axis2.flange, fixed.flange) - # connect(axis2.flange, inertia.flange_a) + # connect(axis2.flange, fixed.flange) + connect(axis2.flange, inertia.flange_a) # constant1.output.u ~ motor.axisControlBus.current_ref # This is connected to the controller output constant2.output.u ~ axis2.axisControlBus.speed_ref constant3.output.u ~ axis2.axisControlBus.angle_ref constant4.output.u ~ axis2.axisControlBus.motion_ref constant5.output.u ~ axis2.axisControlBus.acceleration_ref + # axis2.motor.emf.support.phi ~ 0 end end @@ -125,14 +149,30 @@ m = structural_simplify(axisTest) # m = structural_simplify(IRSystem(axisTest)) # Yingbo: solution unstable with IRSystem simplification cm = complete(axisTest) +tspan = (0.0, 5.0) prob = ODEProblem(m, [ - D(D(cm.axis2.gear.gear.phi_b)) => 0 -], (0.0, 5)) + ModelingToolkit.missing_variable_defaults(m); + # D(cm.axis2.gear.bearingFriction.w) => 0 +], tspan) sol = solve(prob, Rodas4()) @test SciMLBase.successful_retcode(sol) -plot(sol, layout=length(states(m))) -u = cm.axis2.controller.PI.ctr_output.u + +@test sol(0.0, idxs=cm.axis2.motor.emf.phi) == 0 +# @test sol(tspan[2], idxs=cm.axis2.motor.emf.phi) == 0 + +isinteractive() && plot(sol, layout=length(states(m))) +plot(sol, idxs=[ + cm.axis2.gear.gear.phi_a + cm.axis2.gear.gear.phi_b + cm.axis2.gear.gear.flange_b.phi + # cm.axis2.gear.bearingFriction.flange_a.phi + cm.axis2.gear.flange_b.phi + cm.axis2.gear.gear.phi_support + cm.axis2.angleSensor.phi.u + cm.axis2.motor.phi.phi.u +], layout=8, size=(800, 800)) @test abs(sol(prob.tspan[2], idxs=u)) < 1e-6 +@info "add tests" ## @@ -152,7 +192,7 @@ ssys = structural_simplify(oneaxis)#, allow_parameters = false) @named robot = FullRobot() -ssys = structural_simplify(robot, allow_parameters = false) +# ssys = structural_simplify(robot, allow_parameters = false) ssys = structural_simplify(IRSystem(robot)) diff --git a/examples/robot/utilities/components.jl b/examples/robot/utilities/components.jl index 5d9d159e..efbc93d2 100644 --- a/examples/robot/utilities/components.jl +++ b/examples/robot/utilities/components.jl @@ -64,7 +64,9 @@ Ideal sensor to measure the absolute flange angular acceleration @variables w(t) [description = "Absolute angular velocity of flange"] @named a = Blocks.RealOutput() #[description = "Absolute angular acceleration of flange"] eqs = [D(flange.phi) ~ w - a.u ~ D(w)] + a.u ~ D(w) + flange.tau ~ 0 + ] return ODESystem(eqs, t, [], []; name = name, systems = [flange, a]) end @@ -103,8 +105,10 @@ function AxisType2(; name, kp = 10, ks = 1, Ts = 0.01, k = 1.1616, w = 4590, D = end eqs = [ - connect(gear.flange_b, flange, angleSensor.flange, speedSensor.flange, accSensor.flange) + connect(flange, gear.flange_b, angleSensor.flange, speedSensor.flange, accSensor.flange) connect(motor.flange_motor, gear.flange_a) + + connect(motor.axisControlBus, axisControlBus) (angleSensor.phi.u ~ axisControlBus.angle) (speedSensor.w.u ~ axisControlBus.speed) @@ -182,13 +186,17 @@ function GearType2(; name, i = -99, flange_b = Rotational.Flange() gear = Rotational.IdealGear(; ratio = i, use_support = false) # bearingFriction = Rotational.BearingFriction(; tau_pos=[0, Rv0; 1, (Rv0 + Rv1*unitAngularVelocity)], peak=peak, useSupport=false) # Not yet supported - bearingFriction = Rotational.RotationalFriction(; f = Rv1, tau_brk = peak * Rv0, - tau_c = Rv0, w_brk = 0.1) # NOTE: poorly chosen w_brk + # bearingFriction = BearingFriction(; f = Rv1, tau_brk = peak * Rv0, tau_c = Rv0, w_brk = 0.1) # NOTE: poorly chosen w_brk + bearingFriction = BearingFriction(;) end + #= + NOTE: We do not yet have the bearingFriction component, bearingFriction this component extends PartialElementaryTwoFlangesAndSupport2 which is implicitly grounded when use_support=false. This component has a relative angle state. Instead, we use a RotationalFriction component, which extends PartialCompliantWithRelativeStates that does not have implicit grounding. We therefore add the explicit grounding using a fixed component + =# eqs = [ + connect(flange_a, gear.flange_a) connect(gear.flange_b, bearingFriction.flange_a) connect(bearingFriction.flange_b, flange_b) - connect(gear.flange_a, flange_a) + # Equations below are the save as above, but without the gear # connect(bearingFriction.flange_b, flange_b) @@ -197,6 +205,51 @@ function GearType2(; name, i = -99, ODESystem(eqs, t; name, systems) end +using ModelingToolkitStandardLibrary.Mechanical.Rotational: PartialElementaryTwoFlangesAndSupport2 +import ModelingToolkitStandardLibrary.Mechanical.Rotational.Flange as fl +@mtkmodel BearingFriction begin + # @extend flange_a, flange_b, phi_support = partial_comp = PartialElementaryTwoFlangesAndSupport2(;use_support = false) + # @parameters begin + # f, [description = "Viscous friction coefficient"] + # tau_c, [description = "Coulomb friction torque"] + # w_brk, [description = "Breakaway friction velocity"] + # tau_brk, [description = "Breakaway friction torque"] + # end + # @variables begin + # phi(t) = 0.0, [description = "Angle between shaft flanges (flange_a, flange_b) and support"] + # tau(t) = 0.0, [description = "Torque between flanges"] + # w(t) = 0.0 + # a(t) = 0.0 + # end + + # begin + # str_scale = sqrt(2 * exp(1)) * (tau_brk - tau_c) + # w_st = w_brk * sqrt(2) + # w_coul = w_brk / 10 + # end + # @equations begin + # tau ~ str_scale * (exp(-(w / w_st)^2) * w / w_st) + + # tau_c * tanh(w / w_coul) + f * w # Stribeck friction + Coulomb friction + Viscous friction + + # phi ~ flange_a.phi - phi_support; + # flange_b.phi ~ flange_a.phi; + + # # Angular velocity and angular acceleration of flanges + # w ~ D(phi) + # a ~ D(w) + + # flange_a.tau + flange_b.tau - tau ~ 0 + # end + @components begin + flange_a = fl() + flange_b = fl() + end + @equations begin + connect(flange_a, flange_b) + end +end + + function GearType1(; name, i = -105, c = 43, d = 0.005, Rv0 = 0.4, Rv1 = (0.13 / 160),