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