robot simplifies again

was missing `tau ~ 0` in sensor

examples/robot/test_components.jl

@@ -19,40 +19,65 @@ 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
 
 @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))
 
 

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),