From 163429a0d7905f2e0c3b732509f2205f6b658fc4 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Thu, 9 Nov 2023 11:46:41 +0100 Subject: [PATCH 1/7] move flexibility to the other side of the gearbox --- docs/src/examples/free_motion.md | 1 + src/robot/robot_components.jl | 6 +++--- test/test_robot.jl | 6 ++++++ 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/docs/src/examples/free_motion.md b/docs/src/examples/free_motion.md index 2bed8169..51029aa8 100644 --- a/docs/src/examples/free_motion.md +++ b/docs/src/examples/free_motion.md @@ -83,6 +83,7 @@ plot(sol, idxs = [body.r_0...]) ```@example FREE_MOTION import CairoMakie +import Multibody.Rotations Multibody.render(model, sol, z=-3, R = Rotations.RotXYZ(0.2, -0.2, 0), filename = "free_body.gif") nothing # hide ``` diff --git a/src/robot/robot_components.jl b/src/robot/robot_components.jl index 90e176bd..8b2a8a17 100644 --- a/src/robot/robot_components.jl +++ b/src/robot/robot_components.jl @@ -163,9 +163,9 @@ function AxisType1(; name, c = 43, cd = 0.005, kp = 10, ks = 1, Ts = 0.01, k = 1 end eqs = [ - connect(flange, spring.flange_a) - connect(spring.flange_b, gear.flange_b, angleSensor.flange, speedSensor.flange, accSensor.flange) - connect(motor.flange_motor, gear.flange_a) + connect(flange, gear.flange_b, angleSensor.flange, speedSensor.flange, accSensor.flange) + connect(gear.flange_a, spring.flange_b) + connect(motor.flange_motor, spring.flange_a) connect(motor.axisControlBus, axisControlBus) (angleSensor.phi.u ~ axisControlBus.angle) (speedSensor.w.u ~ axisControlBus.speed) diff --git a/test/test_robot.jl b/test/test_robot.jl index bed77721..f9f6420f 100644 --- a/test/test_robot.jl +++ b/test/test_robot.jl @@ -279,6 +279,12 @@ end robot.pathPlanning.controlBus.axisControlBus5.angle robot.pathPlanning.controlBus.axisControlBus6.angle ], layout=6) + + plot!(sol, idxs = [ + robot.axis1.motor.Jmotor.phi / ( -105) + robot.axis2.motor.Jmotor.phi / (210) + robot.axis3.motor.Jmotor.phi / (60) + ], sp=(1:3)') display(current()) end From 0273e7e8853e2e3f3c898a85da1afc75332bf841 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Thu, 9 Nov 2023 11:46:59 +0100 Subject: [PATCH 2/7] rm files --- examples/robot/utilities/components.jl | 530 ---------------------- examples/robot/utilities/path_planning.jl | 358 --------------- 2 files changed, 888 deletions(-) delete mode 100644 examples/robot/utilities/components.jl delete mode 100644 examples/robot/utilities/path_planning.jl diff --git a/examples/robot/utilities/components.jl b/examples/robot/utilities/components.jl deleted file mode 100644 index 0120d6f9..00000000 --- a/examples/robot/utilities/components.jl +++ /dev/null @@ -1,530 +0,0 @@ -using ModelingToolkit -using Multibody -import ModelingToolkitStandardLibrary.Blocks -using ModelingToolkitStandardLibrary.Electrical -t = Multibody.t -D = Differential(t) - -""" - @connector AxisControlBus(; name) - -- `motion_ref(t) = 0`: = true, if reference motion is not in rest -- `angle_ref(t) = 0`: Reference angle of axis flange -- `angle(t) = 0`: Angle of axis flange -- `speed_ref(t) = 0`: Reference speed of axis flange -- `speed(t) = 0`: Speed of axis flange -- `acceleration_ref(t) = 0`: Reference acceleration of axis flange -- `acceleration(t) = 0`: Acceleration of axis flange -- `current_ref(t) = 0`: Reference current of motor -- `current(t) = 0`: Current of motor -- `motorAngle(t) = 0`: Angle of motor flange -- `motorSpeed(t) = 0`: Speed of motor flange -""" -@connector function AxisControlBus(; name) - vars = @variables begin - (motion_ref(t) = 0), [description = "= true, if reference motion is not in rest"] - (angle_ref(t) = 0), [description = "Reference angle of axis flange"] - (angle(t) = 0), [description = "Angle of axis flange"] - (speed_ref(t) = 0), [description = "Reference speed of axis flange"] - (speed(t) = 0), [description = "Speed of axis flange"] - (acceleration_ref(t) = 0), [description = "Reference acceleration of axis flange"] - (acceleration(t) = 0), [description = "Acceleration of axis flange"] - (current_ref(t) = 0), [description = "Reference current of motor"] - (current(t) = 0), [description = "Current of motor"] - (motorAngle(t) = 0), [description = "Angle of motor flange"] - (motorSpeed(t) = 0), [description = "Speed of motor flange"] - end - ODESystem(Equation[], t, vars, []; name) -end - -@connector function ControlBus(; name) - systems = @named begin - axisControlBus1 = AxisControlBus() - axisControlBus2 = AxisControlBus() - axisControlBus3 = AxisControlBus() - axisControlBus4 = AxisControlBus() - axisControlBus5 = AxisControlBus() - axisControlBus6 = AxisControlBus() - end - ODESystem(Equation[], t; systems, name) -end - -""" - AccSensor(;name) - -Ideal sensor to measure the absolute flange angular acceleration - -# Connectors: - - - `flange`: [Flange](@ref) Flange of shaft from which sensor information shall be measured - - `a`: [RealOutput](@ref) Absolute angular acceleration of flange -""" -@component function AccSensor(; name) - @named flange = Rotational.Flange() - @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) - flange.tau ~ 0 - ] - return ODESystem(eqs, t, [], []; name = name, systems = [flange, a]) -end - -""" - AxisType2(; name) - -Axis model of the r3 joints 4,5,6 -""" -function AxisType2(; name, kp = 10, ks = 1, Ts = 0.01, k = 1.1616, w = 4590, D = 0.6, - J = 0.0013, ratio = -105, Rv0 = 0.4, Rv1 = 0.13 / 160, peak = 1) - # @parameters begin - # kp = kp, [description = "Gain of position controller"] - # ks = ks, [description = "Gain of speed controller"] - # Ts = Ts, [description = "Time constant of integrator of speed controller"] - # k = k, [description = "Gain of motor"] - # w = w, [description = "Time constant of motor"] - # D = D, [description = "Damping constant of motor"] - # J = J, [description = "Moment of inertia of motor"] - # ratio = ratio, [description = "Gear ratio"] - # Rv0 = Rv0, [description = "Viscous friction torque at zero velocity"] - # Rv1 = Rv1, [description = "Viscous friction coefficient"] - # peak = peak, - # [description = "Maximum static friction torque is peak*Rv0 (peak >= 1)"] - # end - - systems = @named begin - flange = Rotational.Flange() - gear = GearType2(; Rv0, Rv1, peak, i = ratio) - motor = Motor(; J, k, w, D) - controller = Controller(; kp, ks, Ts, ratio) - angleSensor = Rotational.AngleSensor() - speedSensor = Rotational.SpeedSensor() - accSensor = AccSensor() # TODO: shift to MTKstdlib version when merged - # Const = Blocks.Constant(k = 0) - axisControlBus = AxisControlBus() - end - - eqs = [ - 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) - (accSensor.a.u ~ axisControlBus.acceleration) - # connect(axisControlBus.angle_ref, initializeFlange.phi_start) - # connect(axisControlBus.speed_ref, initializeFlange.w_start) - # connect(initializeFlange.flange, flange) - # connect(Const.y, initializeFlange.a_start) - connect(controller.axisControlBus, axisControlBus)] - - compose(ODESystem(eqs, t; name), systems) -end - -function AxisType1(; name, c = 43, cd = 0.005, kwargs...) - @parameters begin - c = c, [description = "Spring constant"] - cd = cd, [description = "Damper constant"] - end - @warn "Axis type 1 is currently identical to type 2" maxlog=2 - # @named axisType2 = AxisType2(redeclare GearType1 gear(c=c, d=cd), kwargs...) # TODO: Figure out how to handle the redeclare directive https://github.com/SciML/ModelingToolkit.jl/issues/2038 - axisType2 = AxisType2(; name, kwargs...) -end - -function Controller(; name, kp = 10, ks = 1, Ts = 0.01, ratio = 1) - # @parameters begin - # kp = kp, [description = "Gain of position controller"] - # ks = ks, [description = "Gain of speed controller"] - # Ts = Ts, [description = "Time constant of integrator of speed controller"] - # ratio = ratio, [description = "Gear ratio of gearbox"] - # end - systems = @named begin - gain1 = Blocks.Gain(ratio) - PI = Blocks.PI(gainPI.k = ks, T = Ts) - feedback1 = Blocks.Feedback() - P = Blocks.Gain(kp) - add3 = Blocks.Add3(k3 = -1) - gain2 = Blocks.Gain(ratio) - axisControlBus = AxisControlBus() - end - - eqs = [connect(gain1.output, feedback1.input1) - connect(feedback1.output, P.input) - connect(P.output, add3.input2) - connect(gain2.output, add3.input1) - connect(add3.output, :e, PI.err_input) - (gain2.input.u ~ axisControlBus.speed_ref) - (gain1.input.u ~ axisControlBus.angle_ref) - (feedback1.input2.u ~ axisControlBus.motorAngle) - (add3.input3.u ~ axisControlBus.motorSpeed) - (PI.ctr_output.u ~ axisControlBus.current_ref)] - - compose(ODESystem(eqs, t; name), systems) -end - -function GearType2(; name, i = -99, - Rv0 = 21.8, - Rv1 = 9.8, - peak = (26.7 / 21.8)) - # Modelica.Mechanics.Rotational.Components.BearingFriction bearingFriction( - # tau_pos=[0, - # Rv0/unitTorque; 1, (Rv0 + Rv1*unitAngularVelocity)/unitTorque], peak=peak, - # useSupport=false) - - unitAngularVelocity = 1 - - # @parameters begin - # i = i, [description = "Gear ratio"] - # Rv0 = Rv0, [description = "Viscous friction torque at zero velocity"] - # Rv1 = Rv1, [description = "Viscous friction coefficient (R=Rv0+Rv1*abs(qd))"] - # peak = peak, - # [description = "Maximum static friction torque is peak*Rv0 (peak >= 1)"] - # end - systems = @named begin - flange_a = Rotational.Flange() - 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 = 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) - - - # Equations below are the save as above, but without the gear - # connect(bearingFriction.flange_b, flange_b) - # connect(bearingFriction.flange_a, flange_a) - ] - 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), - peak = 1) - unitAngularVelocity = 1 - unitTorque = 1 - # pars = @parameters begin - # i = i, [description = "Gear ratio"] - # c = c, [description = "Spring constant"] - # d = d, [description = "Damper constant"] - # Rv0 = Rv0, [description = "Viscous friction torque at zero velocity"] - # Rv1 = Rv1, - # [description = "Viscous friction coefficient (R=Rv0+Rv1*abs(qd))"] - # peak = peak, - # [description = "Maximum static friction torque is peak*Rv0 (peak >= 1)"] - # end - - # Modelica.Mechanics.Rotational.Components.BearingFriction bearingFriction( - # tau_pos=[0, - # Rv0/unitTorque; 1, (Rv0 + Rv1*unitAngularVelocity)/unitTorque], - # useSupport=false) - - systems = @named begin - flange_a = Rotational.Flange() - flange_b = Rotational.Flange() - gear = Rotational.IdealGear(; ratio = i, use_support = false) - spring = Rotational.SpringDamper(; c, d) - # bearingFriction = Rotational.BearingFriction(; tau_pos=[0, Rv0; 1, (Rv0 + Rv1*unitAngularVelocity)], 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 - end - # vars = @variables a_rel(t)=D(spring.w_rel) [ # This is only used inside "initial equation" block - # description = "Relative angular acceleration of spring", - # ] - - eqs = [connect(spring.flange_b, gear.flange_a) - connect(bearingFriction.flange_b, spring.flange_a) - connect(gear.flange_b, flange_b) - connect(bearingFriction.flange_a, flange_a)] - ODESystem(eqs, t; name, systems) -end - -function Motor(; name, J = 0.0013, k = 1.1616, w = 4590, D = 0.6, w_max = 315, i_max = 9) - # @parameters begin - # J = J, [description = "Moment of inertia of motor"] - # k = k, [description = "Gain of motor"] - # w = w, [description = "Time constant of motor"] - # D = D, [description = "Damping constant of motor"] - # w_max = w_max, [description = "Maximum speed of motor"] - # i_max = i_max, [description = "Maximum current of motor"] - # end - - # Electrical.Analog.Basic.RotationalEMF emf(k=k, useSupport=false) - - systems = @named begin - flange_motor = Rotational.Flange() - Vs = Voltage() # was SignalVoltage - power = IdealOpAmp() - diff = IdealOpAmp() - fixed = Rotational.Fixed() # NOTE: this was added to account for the fixed frame that is added to RotationalEMF when useSupport=false - emf = Electrical.EMF(; k = k) # Was RotationalEMF(k, useSupport=false), which differs from EMF in that it can be instantiated without support - La = Inductor(; L = (250 / (2 * D * w))) - Ra = Resistor(; R = 250) - Rd2 = Resistor(; R = 100) - C = Capacitor(C = 0.004 * D / w) - OpI = IdealOpAmp() - Ri = Resistor(; R = 10) - Rd1 = Resistor(; R = 100) - Rp1 = Resistor(; R = 200) - Rp2 = Resistor(; R = 50) - Rd4 = Resistor(; R = 100) - hall2 = Voltage() # was SignalVoltage - Rd3 = Resistor(; R = 100) - g1 = Ground() - g2 = Ground() - g3 = Ground() - hall1 = CurrentSensor() - g4 = Ground() - g5 = Ground() - phi = Rotational.AngleSensor() - speed = Rotational.SpeedSensor() - Jmotor = Rotational.Inertia(; J = J) - axisControlBus = AxisControlBus() - convert1 = Blocks.Gain(1) - convert2 = Blocks.Gain(1) - end - - eqs = [connect(fixed.flange, emf.support) # NOTE: extra equation added - connect(La.n, emf.p) - connect(Ra.n, La.p) - connect(Rd2.n, diff.n1) - connect(C.n, OpI.p2) - connect(OpI.p2, power.p1) - connect(Vs.p, Rd2.p) - connect(diff.n1, Rd1.p) - connect(Rd1.n, diff.p2) - connect(diff.p2, Ri.p) - connect(Ri.n, OpI.n1) - connect(OpI.n1, C.p) - connect(power.n1, Rp1.p) - connect(power.p2, Rp1.n) - connect(Rp1.p, Rp2.p) - connect(power.p2, Ra.p) - connect(Rd3.p, hall2.p) - connect(Rd3.n, diff.p1) - connect(Rd3.n, Rd4.p) - connect(Vs.n, g1.g) - connect(g2.g, hall2.n) - connect(Rd4.n, g3.g) - connect(g3.g, OpI.p1) - connect(g5.g, Rp2.n) - connect(emf.n, hall1.p) - connect(hall1.n, g4.g) - connect(emf.flange, phi.flange) - connect(emf.flange, speed.flange) - connect(OpI.n2, power.n2) - connect(OpI.p1, OpI.n2) - connect(OpI.p1, diff.n2) - connect(Jmotor.flange_b, flange_motor) - (phi.phi.u ~ axisControlBus.motorAngle) - (speed.w.u ~ axisControlBus.motorSpeed) - (hall1.i ~ axisControlBus.current) - (hall1.i ~ convert1.u) - (convert1.y ~ hall2.v) - (convert2.u ~ axisControlBus.current_ref) - (convert2.y ~ Vs.v) - connect(emf.flange, Jmotor.flange_a)] - - compose(ODESystem(eqs, t; name), systems) -end - -function MechanicalStructure(; name, mLoad = 15, rLoad = [0, 0.25, 0], g = 9.81) - # @parameters begin - # mLoad = mLoad, [description = "Mass of load"] - # rLoad[1:3] = rLoad, [description = "Distance from last flange to load mass"] - # g = g, [description = "Gravity acceleration"] - # end - - @variables begin - (q(t)[1:6] = 0), [state_priority = typemax(Int), description = "Joint angles"] - (qd(t)[1:6] = 0), [state_priority = typemax(Int), description = "Joint speeds"] - (qdd(t)[1:6] = 0), - [state_priority = typemax(Int), description = "Joint accelerations"] - (tau(t)[1:6] = 0), - [state_priority = typemax(Int), description = "Joint driving torques"] - end - - systems = @named begin - axis1 = Rotational.Flange() - axis2 = Rotational.Flange() - axis3 = Rotational.Flange() - axis4 = Rotational.Flange() - axis5 = Rotational.Flange() - axis6 = Rotational.Flange() - r1 = Revolute(n = [0, 1, 0], useAxisFlange = true, isroot = false) - r2 = Revolute(n = [1, 0, 0], useAxisFlange = true, isroot = false) - r3 = Revolute(n = [1, 0, 0], useAxisFlange = true, isroot = false) - r4 = Revolute(n = [0, 1, 0], useAxisFlange = true, isroot = false) - r5 = Revolute(n = [1, 0, 0], useAxisFlange = true, isroot = false) - r6 = Revolute(n = [0, 1, 0], useAxisFlange = true, isroot = false) - b0 = BodyShape(r = [0, 0.351, 0], - # r_shape = [0, 0, 0], - # lengthDirection = [1, 0, 0], - # widthDirection = [0, 1, 0], - # length = 0.225, - # width = 0.3, - # height = 0.3, - r_cm = [0, 0, 0], - m = 1) - b1 = BodyShape(r = [0, 0.324, 0.3], - I_22 = 1.16, - # lengthDirection = [1, 0, 0], - # widthDirection = [0, 1, 0], - # length = 0.25, - # width = 0.15, - # height = 0.2, - # color = [255, 0, 0], - r_cm = [0, 0, 0], - m = 1) - b2 = BodyShape(r = [0, 0.65, 0], - r_cm = [0.172, 0.205, 0], - m = 56.5, - I_11 = 2.58, - I_22 = 0.64, - I_33 = 2.73, - I_21 = -0.46 - # lengthDirection = [1, 0, 0], - # widthDirection = [0, 1, 0], - # length = 0.5, - # width = 0.2, - # height = 0.15, - # color = [255, 178, 0], - ) - b3 = BodyShape(r = [0, 0.414, -0.155], - r_cm = [0.064, -0.034, 0], - m = 26.4, - I_11 = 0.279, - I_22 = 0.245, - I_33 = 0.413, - I_21 = -0.070 - # lengthDirection = [1, 0, 0], - # widthDirection = [0, 1, 0], - # length = 0.15, - # width = 0.15, - # height = 0.15, - # color = [255, 0, 0], - ) - b4 = BodyShape(r = [0, 0.186, 0], - r_cm = [0, 0, 0], - m = 28.7, - I_11 = 1.67, - I_22 = 0.081, - I_33 = 1.67 - # lengthDirection = [1, 0, 0], - # widthDirection = [0, 1, 0], - # length = 0.73, - # width = 0.1, - # height = 0.1, - # color = [255, 178, 0], - ) - b5 = BodyShape(r = [0, 0.125, 0], - r_cm = [0, 0, 0], - m = 5.2, - I_11 = 1.25, - I_22 = 0.81, - I_33 = 1.53 - # lengthDirection = [1, 0, 0], - # widthDirection = [0, 1, 0], - # length = 0.225, - # width = 0.075, - # height = 0.1, - # color = [0, 0, 255], - ) - b6 = BodyShape(r = [0, 0, 0], - r_cm = [0.05, 0.05, 0.05], - m = 0.5 - # lengthDirection = [1, 0, 0], - # widthDirection = [0, 1, 0], - # color = [0, 0, 255], - ) - load = BodyShape(r = [0, 0, 0], - r_cm = rLoad, - m = mLoad - # widthDirection = [1, 0, 0], - # width = 0.05, - # height = 0.05, - # color = [255, 0, 0], - # lengthDirection = to_unit1(rLoad), - # length = length(rLoad), - ) - end - eqs = [q .~ [r1.phi, r2.phi, r3.phi, r4.phi, r5.phi, r6.phi] - qd .~ D.(q) - qdd .~ D.(qd) - tau .~ [r1.tau, r2.tau, r3.tau, r4.tau, r5.tau, r6.tau] - connect(load.frame_a, b6.frame_b) - connect(world.frame_b, b0.frame_a) - connect(b0.frame_b, r1.frame_a) - connect(b1.frame_b, r2.frame_a) - connect(r1.frame_b, b1.frame_a) - connect(r2.frame_b, b2.frame_a) - connect(b2.frame_b, r3.frame_a) - connect(r2.axis, axis2) - connect(r1.axis, axis1) - connect(r3.frame_b, b3.frame_a) - connect(b3.frame_b, r4.frame_a) - connect(r3.axis, axis3) - connect(r4.axis, axis4) - connect(r4.frame_b, b4.frame_a) - connect(b4.frame_b, r5.frame_a) - connect(r5.axis, axis5) - connect(r5.frame_b, b5.frame_a) - connect(b5.frame_b, r6.frame_a) - connect(r6.axis, axis6) - connect(r6.frame_b, b6.frame_a)] - - compose(ODESystem(eqs, t; name), [world; systems]) -end diff --git a/examples/robot/utilities/path_planning.jl b/examples/robot/utilities/path_planning.jl deleted file mode 100644 index 83d2be5b..00000000 --- a/examples/robot/utilities/path_planning.jl +++ /dev/null @@ -1,358 +0,0 @@ -using DataInterpolations -using ModelingToolkitStandardLibrary.Blocks: RealInput, RealOutput -"Generate reference angles for specified kinematic movement" -function PathPlanning1(; name, angleBegDeg = 0, angleEndDeg = 1, time = 0:0.01:10, - swingTime = 0.5, kwargs...) - # @parameters begin - # angleBegDeg = angleBegDeg, [description = "Start angle"] - # angleEndDeg = angleEndDeg, [description = "End angle"] - # speedMax = speedMax, [description = "Maximum axis speed"] - # accMax = accMax, [description = "Maximum axis acceleration"] - # startTime = startTime, [description = "Start time of movement"] - # swingTime = swingTime, - # [ - # description = "Additional time after reference motion is in rest before simulation is stopped", - # ] - # angleBeg = deg2rad(angleBegDeg), [description = "Start angles"] - # angleEnd = deg2rad(angleEndDeg), [description = "End angles"] - # end - - systems = @named begin - controlBus = ControlBus() - path = KinematicPTP(; q_end = deg2rad.(angleEndDeg), - time, - # qd_max = speedMax, - # qdd_max = accMax, - # startTime = startTime, - q_begin = deg2rad.(angleBegDeg), kwargs...) - pathToAxis1 = PathToAxisControlBus(nAxis = 1, axisUsed = 1) - # terminateSimulation = TerminateSimulation(condition = time >= - # path.endTime + swingTime) - end - - eqs = [connect(path.q, pathToAxis1.q) - connect(path.qd, pathToAxis1.qd) - connect(path.qdd, pathToAxis1.qdd) - # connect(path.moving, pathToAxis1.moving) - connect(pathToAxis1.axisControlBus, controlBus.axisControlBus1)] - ODESystem(eqs, t; name, systems) -end - -function PathPlanning6(; name, naxis = 6, angleBegDeg = zeros(naxis), - angleEndDeg = ones(naxis), time = 0:0.01:10, - speedMax = fill(3, naxis), - accMax = fill(2.5, naxis), startTime = 0, swingTime = 0.5, kwargs...) - # @parameters begin - # naxis = naxis, [description = "Number of driven axis"] - # angleBegDeg[1:naxis] = angleBegDeg, [description = "Start angles"] - # angleEndDeg[1:naxis] = angleEndDeg, [description = "End angles"] - # # speedMax[1:naxis] = speedMax, [description = "Maximum axis speed"] - # # accMax[1:naxis] = accMax, [description = "Maximum axis acceleration"] - # # startTime = startTime, [description = "Start time of movement"] - # swingTime = swingTime, - # [ - # description = "Additional time after reference motion is in rest before simulation is stopped", - # ] - # angleBeg[1:6] = deg2rad.(angleBegDeg), [description = "Start angles"] - # angleEnd[1:6] = deg2rad.(angleEndDeg), [description = "End angles"] - # end - - systems = @named begin - controlBus = ControlBus() - path = KinematicPTP(; q_end = deg2rad.(angleEndDeg), - time, - # qd_max = speedMax, - # qdd_max = accMax, - # startTime = startTime, - q_begin = deg2rad.(angleBegDeg), kwargs...) - pathToAxis1 = PathToAxisControlBus(nAxis = naxis, axisUsed = 1) - pathToAxis2 = PathToAxisControlBus(nAxis = naxis, axisUsed = 2) - pathToAxis3 = PathToAxisControlBus(nAxis = naxis, axisUsed = 3) - pathToAxis4 = PathToAxisControlBus(nAxis = naxis, axisUsed = 4) - pathToAxis5 = PathToAxisControlBus(nAxis = naxis, axisUsed = 5) - pathToAxis6 = PathToAxisControlBus(nAxis = naxis, axisUsed = 6) - # terminateSimulation = TerminateSimulation(condition = time >= - # path.endTime + swingTime) - end - - eqs = [connect(path.q, pathToAxis1.q) - connect(path.qd, pathToAxis1.qd) - connect(path.qdd, pathToAxis1.qdd) - # connect(path.moving, pathToAxis1.moving) - connect(path.q, pathToAxis2.q) - connect(path.qd, pathToAxis2.qd) - connect(path.qdd, pathToAxis2.qdd) - # connect(path.moving, pathToAxis2.moving) - connect(path.q, pathToAxis3.q) - connect(path.qd, pathToAxis3.qd) - connect(path.qdd, pathToAxis3.qdd) - # connect(path.moving, pathToAxis3.moving) - connect(path.q, pathToAxis4.q) - connect(path.qd, pathToAxis4.qd) - connect(path.qdd, pathToAxis4.qdd) - # connect(path.moving, pathToAxis4.moving) - connect(path.q, pathToAxis5.q) - connect(path.qd, pathToAxis5.qd) - connect(path.qdd, pathToAxis5.qdd) - # connect(path.moving, pathToAxis5.moving) - connect(path.q, pathToAxis6.q) - connect(path.qd, pathToAxis6.qd) - connect(path.qdd, pathToAxis6.qdd) - # connect(path.moving, pathToAxis6.moving) - connect(pathToAxis1.axisControlBus, controlBus.axisControlBus1) - connect(pathToAxis2.axisControlBus, controlBus.axisControlBus2) - connect(pathToAxis3.axisControlBus, controlBus.axisControlBus3) - connect(pathToAxis4.axisControlBus, controlBus.axisControlBus4) - connect(pathToAxis5.axisControlBus, controlBus.axisControlBus5) - connect(pathToAxis6.axisControlBus, controlBus.axisControlBus6)] - - ODESystem(eqs, t; name, systems) -end - -"Map path planning to one axis control bus" -function PathToAxisControlBus(; name, nAxis = 6, axisUsed = 1) - # @parameters begin - # nAxis = nAxis, [description = "Number of driven axis"] - # axisUsed = axisUsed, - # [description = "Map path planning of axisUsed to axisControlBus"] - # end - - systems = @named begin - q = Blocks.RealInput(nin = nAxis) - qd = Blocks.RealInput(nin = nAxis) - qdd = Blocks.RealInput(nin = nAxis) - axisControlBus = AxisControlBus() - q_axisUsed = RealPassThrough() - qd_axisUsed = RealPassThrough() - qdd_axisUsed = RealPassThrough() - moving = Blocks.Constant(k = 1) # Blocks.BooleanInput(nAxis) # NOTE - motion_ref_axisUsed = RealPassThrough() # Blocks.BooleanPassThrough() - end - - eqs = [(q_axisUsed.input.u ~ q.u[axisUsed]) - (qd_axisUsed.input.u ~ qd.u[axisUsed]) - (qdd_axisUsed.input.u ~ qdd.u[axisUsed]) - connect(motion_ref_axisUsed.input, moving.output) - (motion_ref_axisUsed.output.u ~ axisControlBus.motion_ref) - (qdd_axisUsed.output.u ~ axisControlBus.acceleration_ref) - (qd_axisUsed.output.u ~ axisControlBus.speed_ref) - (q_axisUsed.output.u ~ axisControlBus.angle_ref)] - ODESystem(eqs, t; systems, name) -end - -""" - q, qd, qdd = traj5(t; q0, q1, q̇0 = zero(q0), q̇1 = zero(q0), q̈0 = zero(q0), q̈1 = zero(q0)) - -Generate a 5:th order polynomial trajectory with specified end points, vels and accs. -""" -function traj5(t; q0 = 0.0, q1 = one(q0), q̇0 = zero(q0), q̇1 = zero(q0), q̈0 = zero(q0), - q̈1 = zero(q0)) - t[1] == 0 || throw(ArgumentError("t must start at 0")) - tf = t[end] - a0 = q0 - a1 = q̇0 - a2 = @. q̈0 / 2 - a3 = @. (20q1 - 20q0 - (8q̇1 + 12q̇0) * tf - (3q̈0 - q̈1) * tf^2) / 2tf^3 - a4 = @. (30q0 - 30q1 + (14q̇1 + 16q̇0) * tf + (3q̈0 - 2q̈1) * tf^2) / 2tf^4 - a5 = @. (12q1 - 12q0 - (6q̇1 + 6q̇0) * tf - (q̈0 - q̈1) * tf^2) / 2tf^5 - evalpoly.(t, ((a0, a1, a2, a3, a4, a5),)), - evalpoly.(t, ((a1, 2a2, 3a3, 4a4, 5a5),)), - evalpoly.(t, ((2a2, 6a3, 12a4, 20a5),)) -end -# t = 0:100; -# q, qd, qdd = traj5(t, q0 = 1, q1 = 2, q̇0 = 3, q̇1 = 4, q̈0 = 5, q̈1 = 6); -# plot(t, q, label = "q", layout = (3, 1), sp = 1) -# plot!(t, qd, label = "qd", sp = 2) -# plot!(t, qdd, label = "qdd", sp = 3) -# plot!(t, centraldiff(q), sp = 2) -# plot!(t, centraldiff(centraldiff(q)), sp = 3) - -""" - KinematicPTP(; time, name, q_begin = 0, q_end = 1, qd_begin = 0, qd_end = 0, qdd_begin = 0, qdd_end = 0) - -A simple trajectory planner that plans a 5:th order polynomial trajectory between two points, subject to specified boundary conditions on the position, velocity and acceleration. -""" -function KinematicPTP(; time, name, q_begin = 0, q_end = 1, qd_begin = 0, qd_end = 0, - qdd_begin = 0, qdd_end = 0, trivial = false) - nout = max(length(q_begin), length(q_end), length(qd_end), length(qdd_end)) - # parameter Real p_q_begin[nout]=(if size(q_begin, 1) == 1 then ones(nout)* - # q_begin[1] else q_begin); - # parameter Real p_q_end[nout]=(if size(q_end, 1) == 1 then ones(nout)*q_end[ - # 1] else q_end); - # parameter Real p_qd_max[nout]=(if size(qd_max, 1) == 1 then ones(nout)* - # qd_max[1] else qd_max); - # parameter Real p_qdd_max[nout]=(if size(qdd_max, 1) == 1 then ones(nout)* - # qdd_max[1] else qdd_max); - # parameter Real p_deltaq[nout]=p_q_end - p_q_begin; - - # @parameters begin - # q_begin = q_begin, [description = "Start position"] - # q_end = q_end, [description = "End position"] - # qd_max = qd_max, [description = "Maximum velocities der(q)"] - # qdd_max = qdd_max, [description = "Maximum accelerations der(qd)"] - # startTime = startTime, [description = "Time instant at which movement starts"] - # p_q_begin[1:nout] = ones(nout) .* q_begin - # p_q_end[1:nout] = ones(nout) .* q_end - # p_qd_max[1:nout] = ones(nout) .* qd_max - # p_qdd_max[1:nout] = ones(nout) .* qdd_max - # p_deltaq[1:nout] = p_q_end - p_q_begin - # end - - # output endTime "Time instant at which movement stops"; - - # Modelica.Blocks.Interfaces.RealOutput q[nout] - # "Reference position of path planning" annotation (Placement( - # transformation(extent={{100,70},{120,90}}))); - # Modelica.Blocks.Interfaces.RealOutput qd[nout] - # "Reference speed of path planning" annotation (Placement(transformation( - # extent={{100,20},{120,40}}))); - # Modelica.Blocks.Interfaces.RealOutput qdd[nout] - # "Reference acceleration of path planning" annotation (Placement( - # transformation(extent={{100,-40},{120,-20}}))); - # Modelica.Blocks.Interfaces.BooleanOutput moving[nout] - # "= true, if end position not yet reached; = false, if end position reached or axis is completely at rest" - - systems = @named begin - q = RealOutput(; nout) - qd = RealOutput(; nout) - qdd = RealOutput(; nout) - # moving = BooleanOutput(; nout) - end - - # for i in 1:nout - # aux1[i] = p_deltaq[i] / p_qd_max[i] - # aux2[i] = p_deltaq[i] / p_qdd_max[i] - # end - - # sd_max_inv = max(abs(aux1)) - # sdd_max_inv = max(abs(aux2)) - - # if sd_max_inv <= eps || sdd_max_inv <= eps - # sd_max = 0 - # sdd_max = 0 - # Ta1 = 0 - # Ta2 = 0 - # noWphase = false - # Tv = 0 - # Te = 0 - # Ta1s = 0 - # Ta2s = 0 - # Tvs = 0 - # Tes = 0 - # sd_max2 = 0 - # s1 = 0 - # s2 = 0 - # s3 = 0 - # s = 0 - # else - # sd_max = 1 / max(abs(aux1)) - # sdd_max = 1 / max(abs(aux2)) - # Ta1 = sqrt(1 / sdd_max) - # Ta2 = sd_max / sdd_max - # noWphase = Ta2 >= Ta1 - # Tv = if noWphase - # Ta1 - # else - # 1 / sd_max - # end - # Te = if noWphase - # Ta1 + Ta1 - # else - # Tv + Ta2 - # end - # Ta1s = Ta1 + startTime - # Ta2s = Ta2 + startTime - # Tvs = Tv + startTime - # Tes = Te + startTime - # sd_max2 = sdd_max * Ta1 - # s1 = sdd_max * (noWphase ? - # Ta1 * Ta1 : Ta2 * Ta2) / 2 - # s2 = s1 + (noWphase ? sd_max2 * (Te - Ta1) - (sdd_max / 2) * (Te - Ta1)^2 : - # sd_max * (Tv - Ta2)) - - # s3 = s2 + sd_max * (Te - Tv) - (sdd_max / 2) * (Te - Tv) * (Te - Tv) - - # if time < startTime - # s = 0 - # elseif noWphase - # if time < Ta1s - # s = (sdd_max / 2) * (time - startTime) * (time - startTime) - # elseif time < Tes - # s = s1 + sd_max2 * (time - Ta1s) - - # (sdd_max / 2) * (time - Ta1s) * (time - - # Ta1s) - # else - # s = s2 - # end - # elseif time < Ta2s - # s = (sdd_max / 2) * (time - startTime) * (time - startTime) - # elseif time < Tvs - # s = s1 + sd_max * (time - Ta2s) - # elseif time < Tes - # s = s2 + sd_max * (time - Tvs) - (sdd_max / 2) * (time - Tvs) * (time - Tvs) - # else - # s = s3 - # end - # end - - # sd = D(s) - # sdd = D(sd) - - # qdd = p_deltaq * sdd - # qd = p_deltaq * sd - # q = p_q_begin + p_deltaq * s - # endTime = Tes - - # # report when axis is moving - # motion_ref = time < endTime - # for i in 1:nout - # loop - # moving[i] = abs(q_begin[i] - q_end[i]) > eps ? motion_ref : false - # end - - startTime = time[1] - time0 = time .- startTime # traj5 wants time vector to start at 0 - - interp_eqs = map(1:nout) do i - q_vec, qd_vec, qdd_vec = traj5(time0; q0 = q_begin[i], q1 = q_end[i], - q̇0 = zero(q_begin[i]), - q̇1 = zero(q_begin[i]), - q̈0 = zero(q_begin[i]), - q̈1 = zero(q_begin[i])) - - qfun = CubicSpline(q_vec, time) - qdfun = CubicSpline(qd_vec, time) - qddfun = CubicSpline(qdd_vec, time) - - if trivial - [q.u[i] ~ 1 # TODO: SymbolicIR does not handle the interpolation https://github.com/JuliaComputing/SymbolicIR.jl/issues/2 - qd.u[i] ~ 0 - qdd.u[i] ~ 0] - else - [q.u[i] ~ qfun(t) # TODO: SymbolicIR does not handle the interpolation https://github.com/JuliaComputing/SymbolicIR.jl/issues/2 - qd.u[i] ~ qdfun(t) - qdd.u[i] ~ qddfun(t)] - end - end - eqs = reduce(vcat, interp_eqs) - - # push!(eqs, moving.u ~ (time[1] < t < time[end])) - - ODESystem(eqs, t; name, systems) -end - -""" - RealPassThrough(; name) - -Pass a Real signal through without modification - -# Connectors -- `input` -- `output` -""" -@component function RealPassThrough(; name) - @named siso = Blocks.SISO() - @unpack u, y = siso - eqs = [y ~ u] - extend(ODESystem(eqs, t; name), siso) -end From f309e3a23b74e969167939cb9169c99b463a668b Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Thu, 9 Nov 2023 12:25:03 +0100 Subject: [PATCH 3/7] JSComp bug --- docs/src/examples/robot.md | 14 +++++++++++++- src/robot/robot_components.jl | 24 ++++++++++-------------- 2 files changed, 23 insertions(+), 15 deletions(-) diff --git a/docs/src/examples/robot.md b/docs/src/examples/robot.md index 81a758be..1ea716d6 100644 --- a/docs/src/examples/robot.md +++ b/docs/src/examples/robot.md @@ -68,7 +68,19 @@ plot!(sol, idxs = [ ], sp=7:12, lab="Position error", link=:x) plot!(xlabel=[fill("", 1, 9) fill("Time [s]", 1, 3)]) ``` -We see that after an initial transient, the robot controller converges to tracking the reference trajectory well. +We see that after an initial transient, the robot controller converges to tracking the reference trajectory well. However, since the first three axes of the robot are modeled as slightly flexible, and we are ultimately interested in the tracking performance _after_ the gear box, we plot also this tracking error +```@example robot +plot(sol, idxs = [ + robot.axis1.controller.feedback1.output.u + robot.axis2.controller.feedback1.output.u + robot.axis3.controller.feedback1.output.u +], layout=(1,3), lab="Position error, motor side", link=:x) +plot!(sol, idxs = [ + robot.pathPlanning.controlBus.axisControlBus1.angle_ref - robot.axis1.motor.Jmotor.phi / ( -105) + robot.pathPlanning.controlBus.axisControlBus2.angle_ref - robot.axis2.motor.Jmotor.phi / (210) + robot.pathPlanning.controlBus.axisControlBus3.angle_ref - robot.axis3.motor.Jmotor.phi / (60) +], lab="Position error, arm side") +``` diff --git a/src/robot/robot_components.jl b/src/robot/robot_components.jl index 8b2a8a17..13917548 100644 --- a/src/robot/robot_components.jl +++ b/src/robot/robot_components.jl @@ -125,18 +125,14 @@ function AxisType2(; name, kp = 10, ks = 1, Ts = 0.01, k = 1.1616, w = 4590, D = end eqs = [ - connect(flange, gear.flange_b, angleSensor.flange, speedSensor.flange, accSensor.flange) - connect(motor.flange_motor, gear.flange_a) + connect(flange, gear.flange_b) + connect(motor.flange_motor, gear.flange_a, angleSensor.flange, speedSensor.flange, accSensor.flange) connect(motor.axisControlBus, axisControlBus) - (angleSensor.phi.u ~ axisControlBus.angle) - (speedSensor.w.u ~ axisControlBus.speed) - (accSensor.a.u ~ axisControlBus.acceleration) - # connect(axisControlBus.angle_ref, initializeFlange.phi_start) - # connect(axisControlBus.speed_ref, initializeFlange.w_start) - # connect(initializeFlange.flange, flange) - # connect(Const.y, initializeFlange.a_start) + (angleSensor.phi.u*ratio ~ axisControlBus.angle) + (speedSensor.w.u*ratio ~ axisControlBus.speed) + (accSensor.a.u*ratio ~ axisControlBus.acceleration) connect(controller.axisControlBus, axisControlBus)] ODESystem(eqs, t; name, systems) @@ -163,13 +159,13 @@ function AxisType1(; name, c = 43, cd = 0.005, kp = 10, ks = 1, Ts = 0.01, k = 1 end eqs = [ - connect(flange, gear.flange_b, angleSensor.flange, speedSensor.flange, accSensor.flange) + connect(flange, gear.flange_b) connect(gear.flange_a, spring.flange_b) - connect(motor.flange_motor, spring.flange_a) + connect(motor.flange_motor, spring.flange_a, angleSensor.flange, speedSensor.flange, accSensor.flange) connect(motor.axisControlBus, axisControlBus) - (angleSensor.phi.u ~ axisControlBus.angle) - (speedSensor.w.u ~ axisControlBus.speed) - (accSensor.a.u ~ axisControlBus.acceleration) + (angleSensor.phi.u*ratio ~ axisControlBus.angle) + (speedSensor.w.u*ratio ~ axisControlBus.speed) + (accSensor.a.u*ratio ~ axisControlBus.acceleration) connect(controller.axisControlBus, axisControlBus) ] From 9c0edb9482122f6461b36064f3c61875f5295a78 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 5 Jun 2024 06:18:19 +0200 Subject: [PATCH 4/7] up manifest --- Manifest.toml | 434 ++++++++++++++++++++++++++++++-------------------- 1 file changed, 262 insertions(+), 172 deletions(-) diff --git a/Manifest.toml b/Manifest.toml index 0ec38415..7264b79e 100644 --- a/Manifest.toml +++ b/Manifest.toml @@ -1,13 +1,18 @@ # This file is machine-generated - editing it directly is not advised -julia_version = "1.10.2" +julia_version = "1.10.3" manifest_format = "2.0" project_hash = "efeef262c86b4eafbecc9a61dea3d204d89a22d1" [[deps.ADTypes]] -git-tree-sha1 = "016833eb52ba2d6bea9fcb50ca295980e728ee24" +git-tree-sha1 = "daf26bbdec60d9ca1c0003b70f389d821ddb4224" uuid = "47edcb42-4c32-4615-8424-f2b9edc5f35b" -version = "0.2.7" +version = "1.2.1" +weakdeps = ["ChainRulesCore", "EnzymeCore"] + + [deps.ADTypes.extensions] + ADTypesChainRulesCoreExt = "ChainRulesCore" + ADTypesEnzymeCoreExt = "EnzymeCore" [[deps.AbstractTrees]] git-tree-sha1 = "2d9c9a55f9c93e8887ad391fbae72f8ef55e1177" @@ -45,26 +50,33 @@ weakdeps = ["StaticArrays"] [deps.Adapt.extensions] AdaptStaticArraysExt = "StaticArrays" +[[deps.AliasTables]] +deps = ["PtrArrays", "Random"] +git-tree-sha1 = "9876e1e164b144ca45e9e3198d0b689cadfed9ff" +uuid = "66dad0bd-aa9a-41b7-9441-69ab47430ed8" +version = "1.1.3" + [[deps.ArgTools]] uuid = "0dad84c5-d112-42e6-8d28-ef12dabb789f" version = "1.1.1" [[deps.ArnoldiMethod]] deps = ["LinearAlgebra", "Random", "StaticArrays"] -git-tree-sha1 = "62e51b39331de8911e4a7ff6f5aaf38a5f4cc0ae" +git-tree-sha1 = "d57bd3762d308bded22c3b82d033bff85f6195c6" uuid = "ec485272-7323-5ecc-a04f-4719b315124d" -version = "0.2.0" +version = "0.4.0" [[deps.ArrayInterface]] deps = ["Adapt", "LinearAlgebra", "SparseArrays", "SuiteSparse"] -git-tree-sha1 = "44691067188f6bd1b2289552a23e4b7572f4528d" +git-tree-sha1 = "133a240faec6e074e07c31ee75619c90544179cf" uuid = "4fba245c-0d91-5ea0-9b3e-6abc04ee57a9" -version = "7.9.0" +version = "7.10.0" [deps.ArrayInterface.extensions] ArrayInterfaceBandedMatricesExt = "BandedMatrices" ArrayInterfaceBlockBandedMatricesExt = "BlockBandedMatrices" ArrayInterfaceCUDAExt = "CUDA" + ArrayInterfaceCUDSSExt = "CUDSS" ArrayInterfaceChainRulesExt = "ChainRules" ArrayInterfaceGPUArraysCoreExt = "GPUArraysCore" ArrayInterfaceReverseDiffExt = "ReverseDiff" @@ -75,6 +87,7 @@ version = "7.9.0" BandedMatrices = "aae01518-5342-5314-be14-df237901396f" BlockBandedMatrices = "ffab5731-97b5-5995-9138-79e8c1846df0" CUDA = "052768ef-5323-5732-b1bb-66c8b64840ba" + CUDSS = "45b445bb-4962-46a0-9369-b4df9d0f772e" ChainRules = "082447d4-558c-5d27-93f4-14fc19e9eca2" GPUArraysCore = "46192b85-c4d5-4398-a991-12ede77f4527" ReverseDiff = "37e2e3b7-166d-5795-8a7a-e32c996b4267" @@ -83,9 +96,9 @@ version = "7.9.0" [[deps.ArrayLayouts]] deps = ["FillArrays", "LinearAlgebra"] -git-tree-sha1 = "6404a564c24a994814106c374bec893195e19bac" +git-tree-sha1 = "29649b61e0313db0a7ad5ecf41210e4e85aea234" uuid = "4c555306-a7a7-4459-81d9-ec55ddd5c99a" -version = "1.8.0" +version = "1.9.3" weakdeps = ["SparseArrays"] [deps.ArrayLayouts.extensions] @@ -115,15 +128,15 @@ version = "0.5.0" [[deps.CPUSummary]] deps = ["CpuId", "IfElse", "PrecompileTools", "Static"] -git-tree-sha1 = "601f7e7b3d36f18790e2caf83a882d88e9b71ff1" +git-tree-sha1 = "585a387a490f1c4bd88be67eea15b93da5e85db7" uuid = "2a0fbf3d-bb9c-48f3-b0a9-814d99fd7ab9" -version = "0.2.4" +version = "0.2.5" [[deps.CSTParser]] deps = ["Tokenize"] -git-tree-sha1 = "b544d62417a99d091c569b95109bc9d8c223e9e3" +git-tree-sha1 = "0157e592151e39fa570645e2b2debcdfb8a0f112" uuid = "00ebfdb7-1f24-5e51-bd34-a7502290713f" -version = "3.4.2" +version = "3.4.3" [[deps.Calculus]] deps = ["LinearAlgebra"] @@ -133,15 +146,15 @@ version = "0.5.1" [[deps.ChainRules]] deps = ["Adapt", "ChainRulesCore", "Compat", "Distributed", "GPUArraysCore", "IrrationalConstants", "LinearAlgebra", "Random", "RealDot", "SparseArrays", "SparseInverseSubset", "Statistics", "StructArrays", "SuiteSparse"] -git-tree-sha1 = "4e42872be98fa3343c4f8458cbda8c5c6a6fa97c" +git-tree-sha1 = "5ec157747036038ec70b250f578362268f0472f1" uuid = "082447d4-558c-5d27-93f4-14fc19e9eca2" -version = "1.63.0" +version = "1.68.0" [[deps.ChainRulesCore]] deps = ["Compat", "LinearAlgebra"] -git-tree-sha1 = "575cd02e080939a33b6df6c5853d14924c08e35b" +git-tree-sha1 = "71acdbf594aab5bbb2cec89b208c41b4c411e49f" uuid = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" -version = "1.23.0" +version = "1.24.0" weakdeps = ["SparseArrays"] [deps.ChainRulesCore.extensions] @@ -177,9 +190,9 @@ version = "0.3.0" [[deps.Compat]] deps = ["TOML", "UUIDs"] -git-tree-sha1 = "c955881e3c981181362ae4088b35995446298b80" +git-tree-sha1 = "b1c55339b7c6c350ee89f2c1604299660525b248" uuid = "34da2185-b29b-5c13-b0c7-acf172513d20" -version = "4.14.0" +version = "4.15.0" weakdeps = ["Dates", "LinearAlgebra"] [deps.Compat.extensions] @@ -188,12 +201,12 @@ weakdeps = ["Dates", "LinearAlgebra"] [[deps.CompilerSupportLibraries_jll]] deps = ["Artifacts", "Libdl"] uuid = "e66e0078-7015-5450-92f7-15fbd957f2ae" -version = "1.1.0+0" +version = "1.1.1+0" [[deps.CompositeTypes]] -git-tree-sha1 = "02d2316b7ffceff992f3096ae48c7829a8aa0638" +git-tree-sha1 = "bce26c3dab336582805503bed209faab1c279768" uuid = "b152e2b5-7a66-4b01-a709-34e65c35f657" -version = "0.1.3" +version = "0.1.4" [[deps.CompositionsBase]] git-tree-sha1 = "802bb88cd69dfd1509f6670416bd4434015693ad" @@ -243,29 +256,28 @@ uuid = "9a962f9c-6df0-11e9-0e5d-c546b8b5ee8a" version = "1.16.0" [[deps.DataInterpolations]] -deps = ["FindFirstFunctions", "LinearAlgebra", "PrettyTables", "RecipesBase", "Reexport"] -git-tree-sha1 = "e31c460739fb1beb9d84cc851b670b43ef493fda" +deps = ["FindFirstFunctions", "ForwardDiff", "LinearAlgebra", "PrettyTables", "RecipesBase", "Reexport"] +git-tree-sha1 = "b580ef00ec248aeb137b4ef3a4f751a567d35556" uuid = "82cc6244-b520-54b8-b5a6-8a565e85f1d0" -version = "4.7.1" +version = "5.0.0" [deps.DataInterpolations.extensions] DataInterpolationsChainRulesCoreExt = "ChainRulesCore" - DataInterpolationsOptimExt = ["ForwardDiff", "Optim"] + DataInterpolationsOptimExt = "Optim" DataInterpolationsRegularizationToolsExt = "RegularizationTools" DataInterpolationsSymbolicsExt = "Symbolics" [deps.DataInterpolations.weakdeps] ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" - ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" Optim = "429524aa-4258-5aef-a3af-852621145aeb" RegularizationTools = "29dad682-9a27-4bc3-9c72-016788665182" Symbolics = "0c5d862f-8b57-4792-8d23-62f2024744c7" [[deps.DataStructures]] deps = ["Compat", "InteractiveUtils", "OrderedCollections"] -git-tree-sha1 = "0f4b5d62a88d8f59003e43c25a8a90de9eb76317" +git-tree-sha1 = "1d0a14036acb104d9e89698bd408f63ab58cdc82" uuid = "864edb3b-99cc-5e75-8d2d-829cb0a9cfe8" -version = "0.18.18" +version = "0.18.20" [[deps.DataValueInterfaces]] git-tree-sha1 = "bfc1187b79289637fa0ef6d4436ebdfe6905cbd6" @@ -278,11 +290,12 @@ uuid = "ade2ca70-3891-5945-98fb-dc099432e06a" [[deps.DiffEqBase]] deps = ["ArrayInterface", "ConcreteStructs", "DataStructures", "DocStringExtensions", "EnumX", "EnzymeCore", "FastBroadcast", "FastClosures", "ForwardDiff", "FunctionWrappers", "FunctionWrappersWrappers", "LinearAlgebra", "Logging", "Markdown", "MuladdMacro", "Parameters", "PreallocationTools", "PrecompileTools", "Printf", "RecursiveArrayTools", "Reexport", "SciMLBase", "SciMLOperators", "Setfield", "SparseArrays", "Static", "StaticArraysCore", "Statistics", "Tricks", "TruncatedStacktraces"] -git-tree-sha1 = "4fa023dbb15b3485426bbc6c43e030c14250d664" +git-tree-sha1 = "37d49a1f8eedfe68b7622075ff3ebe3dd0e8f327" uuid = "2b5f629d-d688-5b77-993f-72d75c75574e" -version = "6.149.0" +version = "6.151.2" [deps.DiffEqBase.extensions] + DiffEqBaseCUDAExt = "CUDA" DiffEqBaseChainRulesCoreExt = "ChainRulesCore" DiffEqBaseDistributionsExt = "Distributions" DiffEqBaseEnzymeExt = ["ChainRulesCore", "Enzyme"] @@ -295,6 +308,7 @@ version = "6.149.0" DiffEqBaseUnitfulExt = "Unitful" [deps.DiffEqBase.weakdeps] + CUDA = "052768ef-5323-5732-b1bb-66c8b64840ba" ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" Distributions = "31c24e10-a181-5473-b8eb-7969acd0382f" Enzyme = "7da242da-08ed-463a-9acd-ee780be4f1d9" @@ -328,6 +342,42 @@ git-tree-sha1 = "23163d55f885173722d1e4cf0f6110cdbaf7e272" uuid = "b552c78f-8df3-52c6-915a-8e097449b14b" version = "1.15.1" +[[deps.DifferentiationInterface]] +deps = ["ADTypes", "Compat", "DocStringExtensions", "FillArrays", "LinearAlgebra", "PackageExtensionCompat", "SparseArrays", "SparseMatrixColorings"] +git-tree-sha1 = "b4ce591b91f500126c9c5030aa5b8a710c7db7a0" +uuid = "a0c0ee7d-e4b9-4e03-894e-1c5f64a51d63" +version = "0.4.2" + + [deps.DifferentiationInterface.extensions] + DifferentiationInterfaceChainRulesCoreExt = "ChainRulesCore" + DifferentiationInterfaceDiffractorExt = "Diffractor" + DifferentiationInterfaceEnzymeExt = "Enzyme" + DifferentiationInterfaceFastDifferentiationExt = "FastDifferentiation" + DifferentiationInterfaceFiniteDiffExt = "FiniteDiff" + DifferentiationInterfaceFiniteDifferencesExt = "FiniteDifferences" + DifferentiationInterfaceForwardDiffExt = "ForwardDiff" + DifferentiationInterfacePolyesterForwardDiffExt = "PolyesterForwardDiff" + DifferentiationInterfaceReverseDiffExt = "ReverseDiff" + DifferentiationInterfaceSymbolicsExt = "Symbolics" + DifferentiationInterfaceTapirExt = "Tapir" + DifferentiationInterfaceTrackerExt = "Tracker" + DifferentiationInterfaceZygoteExt = "Zygote" + + [deps.DifferentiationInterface.weakdeps] + ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" + Diffractor = "9f5e2b26-1114-432f-b630-d3fe2085c51c" + Enzyme = "7da242da-08ed-463a-9acd-ee780be4f1d9" + FastDifferentiation = "eb9bf01b-bf85-4b60-bf87-ee5de06c00be" + FiniteDiff = "6a86dc24-6348-571c-b903-95158fe2bd41" + FiniteDifferences = "26cc04aa-876d-5657-8c51-4c34ba976000" + ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" + PolyesterForwardDiff = "98d1487c-24ca-40b6-b7ab-df2af84e126b" + ReverseDiff = "37e2e3b7-166d-5795-8a7a-e32c996b4267" + Symbolics = "0c5d862f-8b57-4792-8d23-62f2024744c7" + Tapir = "07d77754-e150-4737-8c94-cd238a1fb45b" + Tracker = "9f7883ad-71c0-57eb-9f7f-b5c9e6d3789c" + Zygote = "e88e6eb3-aa80-5325-afca-941959d7151f" + [[deps.Distances]] deps = ["LinearAlgebra", "Statistics", "StatsAPI"] git-tree-sha1 = "66c4c81f259586e8f002eacebc177e1fb06363b0" @@ -344,10 +394,10 @@ deps = ["Random", "Serialization", "Sockets"] uuid = "8ba89e20-285c-5b6f-9357-94700520ee1b" [[deps.Distributions]] -deps = ["FillArrays", "LinearAlgebra", "PDMats", "Printf", "QuadGK", "Random", "SpecialFunctions", "Statistics", "StatsAPI", "StatsBase", "StatsFuns"] -git-tree-sha1 = "7c302d7a5fec5214eb8a5a4c466dcf7a51fcf169" +deps = ["AliasTables", "FillArrays", "LinearAlgebra", "PDMats", "Printf", "QuadGK", "Random", "SpecialFunctions", "Statistics", "StatsAPI", "StatsBase", "StatsFuns"] +git-tree-sha1 = "9c405847cc7ecda2dc921ccf18b47ca150d7317e" uuid = "31c24e10-a181-5473-b8eb-7969acd0382f" -version = "0.25.107" +version = "0.25.109" [deps.Distributions.extensions] DistributionsChainRulesCoreExt = "ChainRulesCore" @@ -367,9 +417,15 @@ version = "0.9.3" [[deps.DomainSets]] deps = ["CompositeTypes", "IntervalSets", "LinearAlgebra", "Random", "StaticArrays"] -git-tree-sha1 = "9fd332fb3b276a080e3ebccf0dcd98f4a10bf6a6" +git-tree-sha1 = "490392af2c7d63183bfa2c8aaa6ab981c5ba7561" uuid = "5b8099bc-c8ec-5219-889f-1d9e522a28bf" -version = "0.7.10" +version = "0.7.14" + + [deps.DomainSets.extensions] + DomainSetsMakieExt = "Makie" + + [deps.DomainSets.weakdeps] + Makie = "ee78f7c6-11fb-53f2-987a-cfe4a2b5a57a" [[deps.Downloads]] deps = ["ArgTools", "FileWatching", "LibCURL", "NetworkOptions"] @@ -384,9 +440,9 @@ version = "0.6.8" [[deps.DynamicPolynomials]] deps = ["Future", "LinearAlgebra", "MultivariatePolynomials", "MutableArithmetics", "Pkg", "Reexport", "Test"] -git-tree-sha1 = "0bb0a6f812213ecc8fbbcf472f4a993036858971" +git-tree-sha1 = "30a1848c4f4fc35d1d4bbbd125650f6a11b5bc6c" uuid = "7c1d4256-1411-5781-91ec-d7bc3513ac07" -version = "0.5.5" +version = "0.5.7" [[deps.DynamicQuantities]] deps = ["Compat", "PackageExtensionCompat", "Tricks"] @@ -412,9 +468,9 @@ uuid = "4e289a0a-7415-4d19-859d-a7e5c4648b56" version = "1.0.4" [[deps.EnzymeCore]] -git-tree-sha1 = "1bc328eec34ffd80357f84a84bb30e4374e9bd60" +git-tree-sha1 = "0910982db2490a20f81dc7db5d4bbea236c027b3" uuid = "f151be2c-9106-41f4-ab19-57ee4f262869" -version = "0.6.6" +version = "0.7.3" weakdeps = ["Adapt"] [deps.EnzymeCore.extensions] @@ -439,9 +495,9 @@ version = "0.8.5" [[deps.FastBroadcast]] deps = ["ArrayInterface", "LinearAlgebra", "Polyester", "Static", "StaticArrayInterface", "StrideArraysCore"] -git-tree-sha1 = "a6e756a880fc419c8b41592010aebe6a5ce09136" +git-tree-sha1 = "e17367f052035620d832499496080f792fa7ea47" uuid = "7034ab61-46d4-4ed7-9d0f-46aef9175898" -version = "0.2.8" +version = "0.3.2" [[deps.FastClosures]] git-tree-sha1 = "acebe244d53ee1b461970f8910c235b259e772ef" @@ -450,18 +506,18 @@ version = "0.3.2" [[deps.FastLapackInterface]] deps = ["LinearAlgebra"] -git-tree-sha1 = "0a59c7d1002f3131de53dc4568a47d15a44daef7" +git-tree-sha1 = "cbf5edddb61a43669710cbc2241bc08b36d9e660" uuid = "29a986be-02c6-4525-aec4-84b980013641" -version = "2.0.2" +version = "2.0.4" [[deps.FileWatching]] uuid = "7b1f6079-737a-58dc-b8bc-7a2ca5c1b5ee" [[deps.FillArrays]] -deps = ["LinearAlgebra", "Random"] -git-tree-sha1 = "5b93957f6dcd33fc343044af3d48c215be2562f1" +deps = ["LinearAlgebra"] +git-tree-sha1 = "0653c0a2396a6da5bc4766c43041ef5fd3efbe57" uuid = "1a297f60-69ca-5386-bcde-b61e274b549b" -version = "1.9.3" +version = "1.11.0" weakdeps = ["PDMats", "SparseArrays", "Statistics"] [deps.FillArrays.extensions] @@ -476,9 +532,9 @@ version = "1.2.0" [[deps.FiniteDiff]] deps = ["ArrayInterface", "LinearAlgebra", "Requires", "Setfield", "SparseArrays"] -git-tree-sha1 = "bc0c5092d6caaea112d3c8e3b238d61563c58d5f" +git-tree-sha1 = "2de436b72c3422940cbe1367611d137008af7ec3" uuid = "6a86dc24-6348-571c-b903-95158fe2bd41" -version = "2.23.0" +version = "2.23.1" [deps.FiniteDiff.extensions] FiniteDiffBandedMatricesExt = "BandedMatrices" @@ -518,9 +574,9 @@ version = "0.1.3" [[deps.Functors]] deps = ["LinearAlgebra"] -git-tree-sha1 = "8ae30e786837ce0a24f5e2186938bf3251ab94b2" +git-tree-sha1 = "8a66c07630d6428eaab3506a0eabfcf4a9edea05" uuid = "d9f16b24-f501-4c13-a1f2-28368ffc5196" -version = "0.4.8" +version = "0.4.11" [[deps.Future]] deps = ["Random"] @@ -534,9 +590,9 @@ version = "0.1.6" [[deps.GPUCompiler]] deps = ["ExprTools", "InteractiveUtils", "LLVM", "Libdl", "Logging", "Scratch", "TimerOutputs", "UUIDs"] -git-tree-sha1 = "706309c25a6fac3e332b3e436c4077716f266a1f" +git-tree-sha1 = "1600477fba37c9fc067b9be21f5e8101f24a8865" uuid = "61eb1bfa-7361-4325-ad38-22787b887f55" -version = "0.26.2" +version = "0.26.4" [[deps.GenericSchur]] deps = ["LinearAlgebra", "Printf"] @@ -551,9 +607,9 @@ version = "1.3.1" [[deps.Graphs]] deps = ["ArnoldiMethod", "Compat", "DataStructures", "Distributed", "Inflate", "LinearAlgebra", "Random", "SharedArrays", "SimpleTraits", "SparseArrays", "Statistics"] -git-tree-sha1 = "899050ace26649433ef1af25bc17a815b3db52b7" +git-tree-sha1 = "4f2b57488ac7ee16124396de4f2bbdd51b2602ad" uuid = "86223c79-3864-5bf0-83f7-82e725a168b6" -version = "1.9.0" +version = "1.11.0" [[deps.HostCPUFeatures]] deps = ["BitTwiddlingConvenienceFunctions", "IfElse", "Libdl", "Static"] @@ -573,15 +629,15 @@ uuid = "615f187c-cbe4-4ef1-ba3b-2fcf58d6d173" version = "0.1.1" [[deps.Inflate]] -git-tree-sha1 = "ea8031dea4aff6bd41f1df8f2fdfb25b33626381" +git-tree-sha1 = "d1b1b796e47d94588b3757fe84fbf65a5ec4a80d" uuid = "d25df0c9-e2be-5dd7-82c8-3ad0b3e990b9" -version = "0.1.4" +version = "0.1.5" [[deps.IntelOpenMP_jll]] deps = ["Artifacts", "JLLWrappers", "Libdl"] -git-tree-sha1 = "5fdf2fe6724d8caabf43b557b84ce53f3b7e2f6b" +git-tree-sha1 = "be50fe8df3acbffa0274a744f1a99d29c45a57f4" uuid = "1d5cc7b8-4909-519e-a0f8-d0f5ad9712d0" -version = "2024.0.2+0" +version = "2024.1.0+0" [[deps.InteractiveUtils]] deps = ["Markdown"] @@ -600,9 +656,9 @@ weakdeps = ["Random", "RecipesBase", "Statistics"] [[deps.InverseFunctions]] deps = ["Test"] -git-tree-sha1 = "896385798a8d49a255c398bd49162062e4a4c435" +git-tree-sha1 = "e7cbed5032c4c397a6ac23d1493f3289e01231c4" uuid = "3587e190-3f89-42d0-90ee-14403ec27112" -version = "0.1.13" +version = "0.1.14" weakdeps = ["Dates"] [deps.InverseFunctions.extensions] @@ -632,28 +688,39 @@ version = "0.21.4" [[deps.JuliaFormatter]] deps = ["CSTParser", "CommonMark", "DataStructures", "Glob", "Pkg", "PrecompileTools", "Tokenize"] -git-tree-sha1 = "e07d6fd7db543b11cd90ed764efec53f39851f09" +git-tree-sha1 = "1c4880cb70a5c6c87ea36deccc3d7f9e7969c18c" uuid = "98e50ef6-434e-11e9-1051-2b60c6c9e899" -version = "1.0.54" +version = "1.0.56" + +[[deps.JuliaSimBase]] +deps = ["Preferences"] +git-tree-sha1 = "a2cce509f83f17761da1a5963e7bf807d020b05a" +uuid = "9c9cc66b-a38b-4ec4-8b59-87b54775939f" +version = "0.1.0" [[deps.JuliaSimCompiler]] -deps = ["AbstractTrees", "ChainRules", "ConstructionBase", "DataStructures", "DiffEqCallbacks", "DocStringExtensions", "Expronicon", "ForwardDiff", "GPUCompiler", "Graphs", "IfElse", "JuliaFormatter", "LLVM", "Libdl", "LinearAlgebra", "ModelingToolkit", "NaNMath", "OffsetArrays", "Preferences", "RecursiveFactorization", "RuntimeGeneratedFunctions", "SciMLBase", "Setfield", "SparseArrays", "SparseDiffTools", "SpecialFunctions", "StaticArrays", "StrideArraysCore", "SymbolicIndexingInterface", "Symbolics", "UnPack"] -git-tree-sha1 = "d689b6ccb69f74b3d89a031e1cfb752cc4ff3d43" +deps = ["AbstractTrees", "ChainRules", "ConstructionBase", "DataStructures", "DiffEqCallbacks", "DocStringExtensions", "Expronicon", "ForwardDiff", "GPUCompiler", "Graphs", "IfElse", "JuliaFormatter", "JuliaSimBase", "JuliaSimCompilerRuntime", "LLVM", "Libdl", "LinearAlgebra", "ModelingToolkit", "NaNMath", "OffsetArrays", "RuntimeGeneratedFunctions", "SciMLBase", "Setfield", "SnoopPrecompile", "SparseArrays", "SparseDiffTools", "SpecialFunctions", "StaticArrays", "StrideArraysCore", "SymbolicIndexingInterface", "SymbolicUtils", "Symbolics", "UnPack"] +git-tree-sha1 = "e948d4218b8860e7c980d4738806ae2a8df3cbbc" repo-rev = "master" repo-url = "https://github.com/JuliaComputing/JuliaSimCompiler.jl.git" uuid = "8391cb6b-4921-5777-4e45-fd9aab8cb88d" -version = "0.1.7" +version = "0.1.11" [deps.JuliaSimCompiler.extensions] AcausalVisualizationExt = "CairoMakie" MethodOfLinesExt = "MethodOfLines" - SundialsExt = "Sundials" [deps.JuliaSimCompiler.weakdeps] CairoMakie = "13f3f980-e62b-5c42-98c6-ff1f3baf88f0" MethodOfLines = "94925ecb-adb7-4558-8ed8-f975c56a0bf4" Sundials = "c3572dad-4567-51f8-b174-8c6c989267f4" +[[deps.JuliaSimCompilerRuntime]] +deps = ["LLVM", "Libdl"] +git-tree-sha1 = "da3ccbe6483e6bc86dee9b3e11f268d99e956ebd" +uuid = "9cbdfd5a-25c0-4dde-8b55-206f91b28bd9" +version = "1.0.0" + [[deps.JumpProcesses]] deps = ["ArrayInterface", "DataStructures", "DiffEqBase", "DocStringExtensions", "FunctionWrappers", "Graphs", "LinearAlgebra", "Markdown", "PoissonRandom", "Random", "RandomNumbers", "RecursiveArrayTools", "Reexport", "SciMLBase", "StaticArrays", "SymbolicIndexingInterface", "UnPack"] git-tree-sha1 = "ed08d89318be7d625613f3c435d1f6678fba4850" @@ -672,15 +739,15 @@ version = "0.6.0" [[deps.Krylov]] deps = ["LinearAlgebra", "Printf", "SparseArrays"] -git-tree-sha1 = "8a6837ec02fe5fb3def1abc907bb802ef11a0729" +git-tree-sha1 = "267dad6b4b7b5d529c76d40ff48d33f7e94cb834" uuid = "ba0b0d4f-ebba-5204-a429-3ac8c609bfb7" -version = "0.9.5" +version = "0.9.6" [[deps.LLVM]] deps = ["CEnum", "LLVMExtra_jll", "Libdl", "Preferences", "Printf", "Requires", "Unicode"] -git-tree-sha1 = "ab01dde107f21aa76144d0771dccc08f152ccac7" +git-tree-sha1 = "839c82932db86740ae729779e610f07a1640be9a" uuid = "929cbde3-209d-540e-8aea-75f648917ca0" -version = "6.6.2" +version = "6.6.3" [deps.LLVM.extensions] BFloat16sExt = "BFloat16s" @@ -701,9 +768,9 @@ version = "1.3.1" [[deps.LabelledArrays]] deps = ["ArrayInterface", "ChainRulesCore", "ForwardDiff", "LinearAlgebra", "MacroTools", "PreallocationTools", "RecursiveArrayTools", "StaticArrays"] -git-tree-sha1 = "d1f981fba6eb3ec393eede4821bca3f2b7592cd4" +git-tree-sha1 = "e459fda6b68ea8684b3fcd513d2fd1e5130c4402" uuid = "2ee39098-c373-598a-b85f-a56591580800" -version = "1.15.1" +version = "1.16.0" [[deps.LambertW]] git-tree-sha1 = "c5ffc834de5d61d00d2b0e18c96267cffc21f648" @@ -712,9 +779,9 @@ version = "0.4.6" [[deps.Latexify]] deps = ["Format", "InteractiveUtils", "LaTeXStrings", "MacroTools", "Markdown", "OrderedCollections", "Requires"] -git-tree-sha1 = "cad560042a7cc108f5a4c24ea1431a9221f22c1b" +git-tree-sha1 = "e0b5cd21dc1b44ec6e64f351976f961e6f31d6c4" uuid = "23fbe1c1-3f47-55db-b15f-69d7ec21a316" -version = "0.16.2" +version = "0.16.3" [deps.Latexify.extensions] DataFramesExt = "DataFrames" @@ -731,15 +798,23 @@ uuid = "10f19ff3-798f-405d-979b-55457f8fc047" version = "0.1.15" [[deps.LazyArrays]] -deps = ["ArrayLayouts", "FillArrays", "LinearAlgebra", "MacroTools", "MatrixFactorizations", "SparseArrays"] -git-tree-sha1 = "9cfca23ab83b0dfac93cb1a1ef3331ab9fe596a5" +deps = ["ArrayLayouts", "FillArrays", "LinearAlgebra", "MacroTools", "SparseArrays"] +git-tree-sha1 = "899d44fa1a575653df5721a7fccb4988f7f09b62" uuid = "5078a376-72f3-5289-bfd5-ec5146d43c02" -version = "1.8.3" -weakdeps = ["StaticArrays"] +version = "2.0.4" [deps.LazyArrays.extensions] + LazyArraysBandedMatricesExt = "BandedMatrices" + LazyArraysBlockArraysExt = "BlockArrays" + LazyArraysBlockBandedMatricesExt = "BlockBandedMatrices" LazyArraysStaticArraysExt = "StaticArrays" + [deps.LazyArrays.weakdeps] + BandedMatrices = "aae01518-5342-5314-be14-df237901396f" + BlockArrays = "8e7c35d0-a365-5155-bbbb-fb81a777f24e" + BlockBandedMatrices = "ffab5731-97b5-5995-9138-79e8c1846df0" + StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" + [[deps.LazyArtifacts]] deps = ["Artifacts", "Pkg"] uuid = "4af54fe1-eca0-43a8-85a7-787d91b784e3" @@ -783,14 +858,15 @@ uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" [[deps.LinearSolve]] deps = ["ArrayInterface", "ChainRulesCore", "ConcreteStructs", "DocStringExtensions", "EnumX", "FastLapackInterface", "GPUArraysCore", "InteractiveUtils", "KLU", "Krylov", "LazyArrays", "Libdl", "LinearAlgebra", "MKL_jll", "Markdown", "PrecompileTools", "Preferences", "RecursiveFactorization", "Reexport", "SciMLBase", "SciMLOperators", "Setfield", "SparseArrays", "Sparspak", "StaticArraysCore", "UnPack"] -git-tree-sha1 = "775e5e5d9ace42ef8deeb236587abc69e70dc455" +git-tree-sha1 = "7648cc20100504f4b453917aacc8520e9c0ecfb3" uuid = "7ed4a6bd-45f5-4d41-b270-4a48e9bafcae" -version = "2.28.0" +version = "2.30.1" [deps.LinearSolve.extensions] LinearSolveBandedMatricesExt = "BandedMatrices" LinearSolveBlockDiagonalsExt = "BlockDiagonals" LinearSolveCUDAExt = "CUDA" + LinearSolveCUDSSExt = "CUDSS" LinearSolveEnzymeExt = ["Enzyme", "EnzymeCore"] LinearSolveFastAlmostBandedMatricesExt = ["FastAlmostBandedMatrices"] LinearSolveHYPREExt = "HYPRE" @@ -805,6 +881,7 @@ version = "2.28.0" BandedMatrices = "aae01518-5342-5314-be14-df237901396f" BlockDiagonals = "0a1fb500-61f7-11e9-3c65-f5ef3456f9f0" CUDA = "052768ef-5323-5732-b1bb-66c8b64840ba" + CUDSS = "45b445bb-4962-46a0-9369-b4df9d0f772e" Enzyme = "7da242da-08ed-463a-9acd-ee780be4f1d9" EnzymeCore = "f151be2c-9106-41f4-ab19-57ee4f262869" FastAlmostBandedMatrices = "9d29842c-ecb8-4973-b1e9-a27b1157504e" @@ -818,9 +895,9 @@ version = "2.28.0" [[deps.LogExpFunctions]] deps = ["DocStringExtensions", "IrrationalConstants", "LinearAlgebra"] -git-tree-sha1 = "18144f3e9cbe9b15b070288eef858f71b291ce37" +git-tree-sha1 = "a2d09619db4e765091ee5c6ffe8872849de0feea" uuid = "2ab3a3ac-af41-5b50-aa03-7779005ae688" -version = "0.3.27" +version = "0.3.28" [deps.LogExpFunctions.extensions] LogExpFunctionsChainRulesCoreExt = "ChainRulesCore" @@ -837,9 +914,9 @@ uuid = "56ddb016-857b-54e1-b83d-db4d58db5568" [[deps.LoopVectorization]] deps = ["ArrayInterface", "CPUSummary", "CloseOpenIntervals", "DocStringExtensions", "HostCPUFeatures", "IfElse", "LayoutPointers", "LinearAlgebra", "OffsetArrays", "PolyesterWeave", "PrecompileTools", "SIMDTypes", "SLEEFPirates", "Static", "StaticArrayInterface", "ThreadingUtilities", "UnPack", "VectorizationBase"] -git-tree-sha1 = "0f5648fbae0d015e3abe5867bca2b362f67a5894" +git-tree-sha1 = "8f6786d8b2b3248d79db3ad359ce95382d5a6df8" uuid = "bdcacae8-1622-11e9-2a5c-532679323890" -version = "0.12.166" +version = "0.12.170" weakdeps = ["ChainRulesCore", "ForwardDiff", "SpecialFunctions"] [deps.LoopVectorization.extensions] @@ -847,10 +924,10 @@ weakdeps = ["ChainRulesCore", "ForwardDiff", "SpecialFunctions"] SpecialFunctionsExt = "SpecialFunctions" [[deps.MKL_jll]] -deps = ["Artifacts", "IntelOpenMP_jll", "JLLWrappers", "LazyArtifacts", "Libdl"] -git-tree-sha1 = "72dc3cf284559eb8f53aa593fe62cb33f83ed0c0" +deps = ["Artifacts", "IntelOpenMP_jll", "JLLWrappers", "LazyArtifacts", "Libdl", "oneTBB_jll"] +git-tree-sha1 = "80b2833b56d466b3858d565adcd16a4a05f2089b" uuid = "856f044c-d86e-5d09-b602-aeab76dc8ba7" -version = "2024.0.0+0" +version = "2024.1.0+0" [[deps.MLStyle]] git-tree-sha1 = "bc38dff0548128765760c79eb7388a4b37fae2c8" @@ -872,17 +949,11 @@ version = "0.1.8" deps = ["Base64"] uuid = "d6f4376e-aef5-505a-96c1-9c027394607a" -[[deps.MatrixFactorizations]] -deps = ["ArrayLayouts", "LinearAlgebra", "Printf", "Random"] -git-tree-sha1 = "78f6e33434939b0ac9ba1df81e6d005ee85a7396" -uuid = "a3b82374-2e81-5b9e-98ce-41277c0e4c87" -version = "2.1.0" - [[deps.MaybeInplace]] deps = ["ArrayInterface", "LinearAlgebra", "MacroTools", "SparseArrays"] -git-tree-sha1 = "b1f2f92feb0bc201e91c155ef575bcc7d9cc3526" +git-tree-sha1 = "1b9e613f2ca3b6cdcbfe36381e17ca2b66d4b3a1" uuid = "bb5d69b7-63fc-4a16-80bd-7e42200c7bdb" -version = "0.1.2" +version = "0.1.3" [[deps.MbedTLS_jll]] deps = ["Artifacts", "Libdl"] @@ -891,18 +962,18 @@ version = "2.28.2+1" [[deps.Missings]] deps = ["DataAPI"] -git-tree-sha1 = "f66bdc5de519e8f8ae43bdc598782d35a25b1272" +git-tree-sha1 = "ec4f7fbeab05d7747bdf98eb74d130a2a2ed298d" uuid = "e1d29d7a-bbdc-5cf2-9ac0-f12de2c33e28" -version = "1.1.0" +version = "1.2.0" [[deps.Mmap]] uuid = "a63ad114-7e13-5084-954f-fe012c677804" [[deps.ModelingToolkit]] -deps = ["AbstractTrees", "ArrayInterface", "Combinatorics", "Compat", "ConstructionBase", "DataStructures", "DiffEqBase", "DiffEqCallbacks", "DiffRules", "Distributed", "Distributions", "DocStringExtensions", "DomainSets", "DynamicQuantities", "ExprTools", "FindFirstFunctions", "ForwardDiff", "FunctionWrappersWrappers", "Graphs", "InteractiveUtils", "JuliaFormatter", "JumpProcesses", "LabelledArrays", "Latexify", "Libdl", "LinearAlgebra", "MLStyle", "NaNMath", "OrdinaryDiffEq", "PrecompileTools", "RecursiveArrayTools", "Reexport", "RuntimeGeneratedFunctions", "SciMLBase", "SciMLStructures", "Serialization", "Setfield", "SimpleNonlinearSolve", "SparseArrays", "SpecialFunctions", "StaticArrays", "SymbolicIndexingInterface", "SymbolicUtils", "Symbolics", "URIs", "UnPack", "Unitful"] -git-tree-sha1 = "ef2002247f9d9c1df72f5502243e1b7493337de6" +deps = ["AbstractTrees", "ArrayInterface", "Combinatorics", "Compat", "ConstructionBase", "DataStructures", "DiffEqBase", "DiffEqCallbacks", "DiffRules", "Distributed", "Distributions", "DocStringExtensions", "DomainSets", "DynamicQuantities", "ExprTools", "FindFirstFunctions", "ForwardDiff", "FunctionWrappersWrappers", "Graphs", "InteractiveUtils", "JuliaFormatter", "JumpProcesses", "LabelledArrays", "Latexify", "Libdl", "LinearAlgebra", "MLStyle", "NaNMath", "OrderedCollections", "OrdinaryDiffEq", "PrecompileTools", "RecursiveArrayTools", "Reexport", "RuntimeGeneratedFunctions", "SciMLBase", "SciMLStructures", "Serialization", "Setfield", "SimpleNonlinearSolve", "SparseArrays", "SpecialFunctions", "StaticArrays", "SymbolicIndexingInterface", "SymbolicUtils", "Symbolics", "URIs", "UnPack", "Unitful"] +git-tree-sha1 = "81ffc7d059a10669a809a4dad2576998b196a43b" uuid = "961ee093-0014-501f-94e3-6117800e7a78" -version = "9.7.1" +version = "9.15.0" [deps.ModelingToolkit.extensions] MTKBifurcationKitExt = "BifurcationKit" @@ -914,11 +985,11 @@ version = "9.7.1" [[deps.ModelingToolkitStandardLibrary]] deps = ["ChainRulesCore", "DiffEqBase", "IfElse", "LinearAlgebra", "ModelingToolkit", "Symbolics"] -git-tree-sha1 = "d4e187c31982c8fb31362093625adc50c1209576" -repo-rev = "main" +git-tree-sha1 = "d2c67a4fde6f3ca0e18b603c918855862e7c0e0d" +repo-rev = "baggepinnen-patch-1" repo-url = "https://github.com/SciML/ModelingToolkitStandardLibrary.jl.git" uuid = "16a59e39-deab-5bd0-87e4-056b12336739" -version = "2.6.0" +version = "2.7.1" [[deps.MozillaCACerts_jll]] uuid = "14a3606d-f60d-562e-9121-12d972cd8159" @@ -931,15 +1002,15 @@ version = "0.2.4" [[deps.MultivariatePolynomials]] deps = ["ChainRulesCore", "DataStructures", "LinearAlgebra", "MutableArithmetics"] -git-tree-sha1 = "769c9175942d91ed9b83fa929eee4fe6a1d128ad" +git-tree-sha1 = "dad7be0c92b688bf8f24af170825ccedc104b116" uuid = "102ac46a-7ee4-5c85-9060-abc95bfdeaa3" -version = "0.5.4" +version = "0.5.5" [[deps.MutableArithmetics]] deps = ["LinearAlgebra", "SparseArrays", "Test"] -git-tree-sha1 = "2d106538aebe1c165e16d277914e10c550e9d9b7" +git-tree-sha1 = "a3589efe0005fc4718775d8641b2de9060d23f73" uuid = "d8a4904e-b15c-11e9-3269-09a3773c0cb0" -version = "1.4.2" +version = "1.4.4" [[deps.NLSolversBase]] deps = ["DiffResults", "Distributed", "FiniteDiff", "ForwardDiff"] @@ -964,10 +1035,10 @@ uuid = "ca575930-c2e3-43a9-ace4-1e988b2c1908" version = "1.2.0" [[deps.NonlinearSolve]] -deps = ["ADTypes", "ArrayInterface", "ConcreteStructs", "DiffEqBase", "FastBroadcast", "FastClosures", "FiniteDiff", "ForwardDiff", "LazyArrays", "LineSearches", "LinearAlgebra", "LinearSolve", "MaybeInplace", "PrecompileTools", "Preferences", "Printf", "RecursiveArrayTools", "Reexport", "SciMLBase", "SimpleNonlinearSolve", "SparseArrays", "SparseDiffTools", "StaticArraysCore", "TimerOutputs"] -git-tree-sha1 = "c32743c2321e99adf8bbe03145ce8e2bf2fcfbf9" +deps = ["ADTypes", "ArrayInterface", "ConcreteStructs", "DiffEqBase", "FastBroadcast", "FastClosures", "FiniteDiff", "ForwardDiff", "LazyArrays", "LineSearches", "LinearAlgebra", "LinearSolve", "MaybeInplace", "PrecompileTools", "Preferences", "Printf", "RecursiveArrayTools", "Reexport", "SciMLBase", "SimpleNonlinearSolve", "SparseArrays", "SparseDiffTools", "StaticArraysCore", "SymbolicIndexingInterface", "TimerOutputs"] +git-tree-sha1 = "ed5500c66b726ec9fe8c1796c0a600353246ecf8" uuid = "8913a72c-1f9b-4ce2-8d82-65094dcecaec" -version = "3.9.0" +version = "3.12.4" [deps.NonlinearSolve.extensions] NonlinearSolveBandedMatricesExt = "BandedMatrices" @@ -996,9 +1067,9 @@ version = "3.9.0" Zygote = "e88e6eb3-aa80-5325-afca-941959d7151f" [[deps.OffsetArrays]] -git-tree-sha1 = "6a731f2b5c03157418a20c12195eb4b74c8f8621" +git-tree-sha1 = "e64b4f5ea6b7389f6f046d13d4896a8f9c1ba71e" uuid = "6fe1bfb0-de20-5000-8ca7-80f57d26f881" -version = "1.13.0" +version = "1.14.0" weakdeps = ["Adapt"] [deps.OffsetArrays.extensions] @@ -1026,10 +1097,10 @@ uuid = "bac558e1-5e72-5ebc-8fee-abe8a469f55d" version = "1.6.3" [[deps.OrdinaryDiffEq]] -deps = ["ADTypes", "Adapt", "ArrayInterface", "DataStructures", "DiffEqBase", "DocStringExtensions", "ExponentialUtilities", "FastBroadcast", "FastClosures", "FillArrays", "FiniteDiff", "ForwardDiff", "FunctionWrappersWrappers", "IfElse", "InteractiveUtils", "LineSearches", "LinearAlgebra", "LinearSolve", "Logging", "MacroTools", "MuladdMacro", "NonlinearSolve", "Polyester", "PreallocationTools", "PrecompileTools", "Preferences", "RecursiveArrayTools", "Reexport", "SciMLBase", "SciMLOperators", "SimpleNonlinearSolve", "SimpleUnPack", "SparseArrays", "SparseDiffTools", "StaticArrayInterface", "StaticArrays", "TruncatedStacktraces"] -git-tree-sha1 = "91079af18db922354197eeae2a17b177079e24c1" +deps = ["ADTypes", "Adapt", "ArrayInterface", "DataStructures", "DiffEqBase", "DocStringExtensions", "EnumX", "ExponentialUtilities", "FastBroadcast", "FastClosures", "FillArrays", "FiniteDiff", "ForwardDiff", "FunctionWrappersWrappers", "IfElse", "InteractiveUtils", "LineSearches", "LinearAlgebra", "LinearSolve", "Logging", "MacroTools", "MuladdMacro", "NonlinearSolve", "Polyester", "PreallocationTools", "PrecompileTools", "Preferences", "RecursiveArrayTools", "Reexport", "SciMLBase", "SciMLOperators", "SciMLStructures", "SimpleNonlinearSolve", "SimpleUnPack", "SparseArrays", "SparseDiffTools", "StaticArrayInterface", "StaticArrays", "TruncatedStacktraces"] +git-tree-sha1 = "75b0d2bf28d0df92931919004a5be5304c38cca2" uuid = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed" -version = "6.74.1" +version = "6.80.1" [[deps.PDMats]] deps = ["LinearAlgebra", "SparseArrays", "SuiteSparse"] @@ -1068,9 +1139,9 @@ version = "0.4.4" [[deps.Polyester]] deps = ["ArrayInterface", "BitTwiddlingConvenienceFunctions", "CPUSummary", "IfElse", "ManualMemory", "PolyesterWeave", "Requires", "Static", "StaticArrayInterface", "StrideArraysCore", "ThreadingUtilities"] -git-tree-sha1 = "5d8a46101b622927a87fe3553ea697e606d9a3c5" +git-tree-sha1 = "b3e2bae88cf07baf0a051fe09666b8ef97aefe93" uuid = "f517fe37-dbe3-4b94-8317-1923a5111588" -version = "0.7.11" +version = "0.7.14" [[deps.PolyesterWeave]] deps = ["BitTwiddlingConvenienceFunctions", "CPUSummary", "IfElse", "Static", "ThreadingUtilities"] @@ -1080,9 +1151,9 @@ version = "0.2.1" [[deps.PreallocationTools]] deps = ["Adapt", "ArrayInterface", "ForwardDiff"] -git-tree-sha1 = "b6665214f2d0739f2d09a17474dd443b9139784a" +git-tree-sha1 = "406c29a7f46706d379a3bce45671b4e3a39ddfbc" uuid = "d236fae5-4411-538c-8e31-a6e3d9e00b46" -version = "0.4.20" +version = "0.4.22" [deps.PreallocationTools.extensions] PreallocationToolsReverseDiffExt = "ReverseDiff" @@ -1104,14 +1175,19 @@ version = "1.4.3" [[deps.PrettyTables]] deps = ["Crayons", "LaTeXStrings", "Markdown", "PrecompileTools", "Printf", "Reexport", "StringManipulation", "Tables"] -git-tree-sha1 = "88b895d13d53b5577fd53379d913b9ab9ac82660" +git-tree-sha1 = "66b20dd35966a748321d3b2537c4584cf40387c7" uuid = "08abe8d2-0d0c-5749-adfa-8a2ac140af0d" -version = "2.3.1" +version = "2.3.2" [[deps.Printf]] deps = ["Unicode"] uuid = "de0858da-6303-5e67-8744-51eddeeeb8d7" +[[deps.PtrArrays]] +git-tree-sha1 = "f011fbb92c4d401059b2212c05c0601b70f8b759" +uuid = "43287f4e-b6f4-7ad1-bb20-aadabca52c3d" +version = "1.2.0" + [[deps.QuadGK]] deps = ["DataStructures", "LinearAlgebra"] git-tree-sha1 = "9b23c31e76e333e6fb4c1595ae6afa74966a729e" @@ -1152,9 +1228,9 @@ version = "1.3.4" [[deps.RecursiveArrayTools]] deps = ["Adapt", "ArrayInterface", "DocStringExtensions", "GPUArraysCore", "IteratorInterfaceExtensions", "LinearAlgebra", "RecipesBase", "SparseArrays", "StaticArraysCore", "Statistics", "SymbolicIndexingInterface", "Tables"] -git-tree-sha1 = "d8f131090f2e44b145084928856a561c83f43b27" +git-tree-sha1 = "2cea01606a852c2431ded77293eb533b511b19e6" uuid = "731186ca-8d62-57ce-b412-fbd966d074cd" -version = "3.13.0" +version = "3.22.0" [deps.RecursiveArrayTools.extensions] RecursiveArrayToolsFastBroadcastExt = "FastBroadcast" @@ -1176,9 +1252,9 @@ version = "3.13.0" [[deps.RecursiveFactorization]] deps = ["LinearAlgebra", "LoopVectorization", "Polyester", "PrecompileTools", "StrideArraysCore", "TriangularSolve"] -git-tree-sha1 = "8bc86c78c7d8e2a5fe559e3721c0f9c9e303b2ed" +git-tree-sha1 = "6db1a75507051bc18bfa131fbc7c3f169cc4b2f6" uuid = "f2c3362d-daeb-58d1-803e-2bc74f2840b4" -version = "0.2.21" +version = "0.2.23" [[deps.Reexport]] git-tree-sha1 = "45e428421666073eab6f2da5c9d310d99bb12f9b" @@ -1198,16 +1274,16 @@ uuid = "79098fc4-a85e-5d69-aa6a-4863f24498fa" version = "0.7.1" [[deps.Rmath_jll]] -deps = ["Artifacts", "JLLWrappers", "Libdl", "Pkg"] -git-tree-sha1 = "6ed52fdd3382cf21947b15e8870ac0ddbff736da" +deps = ["Artifacts", "JLLWrappers", "Libdl"] +git-tree-sha1 = "d483cd324ce5cf5d61b77930f0bbd6cb61927d21" uuid = "f50d1b31-88e8-58de-be2c-1cc44531875f" -version = "0.4.0+0" +version = "0.4.2+0" [[deps.Rotations]] deps = ["LinearAlgebra", "Quaternions", "Random", "StaticArrays"] -git-tree-sha1 = "2a0a5d8569f481ff8840e3b7c84bbf188db6a3fe" +git-tree-sha1 = "5680a9276685d392c87407df00d57c9924d9f11e" uuid = "6038ab10-8711-5258-84ad-4b1120ba62dc" -version = "1.7.0" +version = "1.7.1" weakdeps = ["RecipesBase"] [deps.Rotations.extensions] @@ -1215,9 +1291,9 @@ weakdeps = ["RecipesBase"] [[deps.RuntimeGeneratedFunctions]] deps = ["ExprTools", "SHA", "Serialization"] -git-tree-sha1 = "6aacc5eefe8415f47b3e34214c1d79d2674a0ba2" +git-tree-sha1 = "04c968137612c4a5629fa531334bb81ad5680f00" uuid = "7e49a35a-f44a-4d26-94aa-eba1b4ca6b47" -version = "0.5.12" +version = "0.5.13" [[deps.SHA]] uuid = "ea8e919c-243c-51af-8825-aaa63cd721ce" @@ -1236,9 +1312,9 @@ version = "0.6.42" [[deps.SciMLBase]] deps = ["ADTypes", "ArrayInterface", "CommonSolve", "ConstructionBase", "Distributed", "DocStringExtensions", "EnumX", "FunctionWrappersWrappers", "IteratorInterfaceExtensions", "LinearAlgebra", "Logging", "Markdown", "PrecompileTools", "Preferences", "Printf", "RecipesBase", "RecursiveArrayTools", "Reexport", "RuntimeGeneratedFunctions", "SciMLOperators", "SciMLStructures", "StaticArraysCore", "Statistics", "SymbolicIndexingInterface", "Tables"] -git-tree-sha1 = "d15c65e25615272e1b1c5edb1d307484c7942824" +git-tree-sha1 = "9f59654e2a85017ee27b0f59c7fac5a57aa10ced" uuid = "0bca4576-84f4-4d90-8ffe-ffa030f20462" -version = "2.31.0" +version = "2.39.0" [deps.SciMLBase.extensions] SciMLBaseChainRulesCoreExt = "ChainRulesCore" @@ -1266,9 +1342,9 @@ uuid = "c0aeaf25-5076-4817-a8d5-81caf7dfa961" version = "0.3.8" [[deps.SciMLStructures]] -git-tree-sha1 = "5833c10ce83d690c124beedfe5f621b50b02ba4d" +git-tree-sha1 = "d778a74df2f64059c38453b34abad1953b2b8722" uuid = "53ae85a6-f571-4167-b2af-e1d143709226" -version = "1.1.0" +version = "1.2.0" [[deps.Scratch]] deps = ["Dates"] @@ -1290,24 +1366,20 @@ deps = ["Distributed", "Mmap", "Random", "Serialization"] uuid = "1a1011a3-84de-559e-8e89-a11a2f7dc383" [[deps.SimpleNonlinearSolve]] -deps = ["ADTypes", "ArrayInterface", "ConcreteStructs", "DiffEqBase", "DiffResults", "FastClosures", "FiniteDiff", "ForwardDiff", "LinearAlgebra", "MaybeInplace", "PrecompileTools", "Reexport", "SciMLBase", "StaticArraysCore"] -git-tree-sha1 = "a535ae5083708f59e75d5bb3042c36d1be9bc778" +deps = ["ADTypes", "ArrayInterface", "ConcreteStructs", "DiffEqBase", "DiffResults", "DifferentiationInterface", "FastClosures", "FiniteDiff", "ForwardDiff", "LinearAlgebra", "MaybeInplace", "PrecompileTools", "Reexport", "SciMLBase", "Setfield", "StaticArraysCore"] +git-tree-sha1 = "f3c50acd5145f2c6ee85343ce6f433dd2caab41e" uuid = "727e6d20-b764-4bd8-a329-72de5adea6c7" -version = "1.6.0" +version = "1.9.0" [deps.SimpleNonlinearSolve.extensions] SimpleNonlinearSolveChainRulesCoreExt = "ChainRulesCore" - SimpleNonlinearSolvePolyesterForwardDiffExt = "PolyesterForwardDiff" SimpleNonlinearSolveReverseDiffExt = "ReverseDiff" - SimpleNonlinearSolveStaticArraysExt = "StaticArrays" SimpleNonlinearSolveTrackerExt = "Tracker" SimpleNonlinearSolveZygoteExt = "Zygote" [deps.SimpleNonlinearSolve.weakdeps] ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" - PolyesterForwardDiff = "98d1487c-24ca-40b6-b7ab-df2af84e126b" ReverseDiff = "37e2e3b7-166d-5795-8a7a-e32c996b4267" - StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" Tracker = "9f7883ad-71c0-57eb-9f7f-b5c9e6d3789c" Zygote = "e88e6eb3-aa80-5325-afca-941959d7151f" @@ -1322,6 +1394,12 @@ git-tree-sha1 = "58e6353e72cde29b90a69527e56df1b5c3d8c437" uuid = "ce78b400-467f-4804-87d8-8f486da07d0a" version = "1.1.0" +[[deps.SnoopPrecompile]] +deps = ["Preferences"] +git-tree-sha1 = "e760a70afdcd461cf01a575947738d359234665c" +uuid = "66db9d55-30c0-4569-8b51-7e840670fc0c" +version = "1.0.3" + [[deps.Sockets]] uuid = "6462fe0b-24de-5631-8697-dd941f90decc" @@ -1338,9 +1416,9 @@ version = "1.10.0" [[deps.SparseDiffTools]] deps = ["ADTypes", "Adapt", "ArrayInterface", "Compat", "DataStructures", "FiniteDiff", "ForwardDiff", "Graphs", "LinearAlgebra", "PackageExtensionCompat", "Random", "Reexport", "SciMLOperators", "Setfield", "SparseArrays", "StaticArrayInterface", "StaticArrays", "Tricks", "UnPack", "VertexSafeGraphs"] -git-tree-sha1 = "a616ac46c38da60ac05cecf52064d44732edd05e" +git-tree-sha1 = "469f51f8c4741ce944be2c0b65423b518b1405b0" uuid = "47a9eef4-7e08-11e9-0b38-333d64bd3804" -version = "2.17.0" +version = "2.19.0" [deps.SparseDiffTools.extensions] SparseDiffToolsEnzymeExt = "Enzyme" @@ -1362,6 +1440,12 @@ git-tree-sha1 = "52962839426b75b3021296f7df242e40ecfc0852" uuid = "dc90abb0-5640-4711-901d-7e5b23a2fada" version = "0.1.2" +[[deps.SparseMatrixColorings]] +deps = ["ADTypes", "Compat", "DocStringExtensions", "LinearAlgebra", "Random", "SparseArrays"] +git-tree-sha1 = "d4adedbcc8776c567e0e22ef19f13cf10cb6ecaa" +uuid = "0a514795-09f3-496d-8182-132a7b665d35" +version = "0.3.2" + [[deps.Sparspak]] deps = ["Libdl", "LinearAlgebra", "Logging", "OffsetArrays", "Printf", "SparseArrays", "Test"] git-tree-sha1 = "342cf4b449c299d8d1ceaf00b7a49f4fbc7940e7" @@ -1370,9 +1454,9 @@ version = "0.3.9" [[deps.SpecialFunctions]] deps = ["IrrationalConstants", "LogExpFunctions", "OpenLibm_jll", "OpenSpecFun_jll"] -git-tree-sha1 = "e2cfc4012a19088254b3950b85c3c1d8882d864d" +git-tree-sha1 = "2f5d4697f21388cbe1ff299430dd169ef97d7e14" uuid = "276daf66-3868-5448-9aa4-cd146d93841b" -version = "2.3.1" +version = "2.4.0" weakdeps = ["ChainRulesCore"] [deps.SpecialFunctions.extensions] @@ -1397,9 +1481,9 @@ weakdeps = ["OffsetArrays", "StaticArrays"] [[deps.StaticArrays]] deps = ["LinearAlgebra", "PrecompileTools", "Random", "StaticArraysCore"] -git-tree-sha1 = "bf074c045d3d5ffd956fa0a461da38a44685d6b2" +git-tree-sha1 = "9ae599cd7529cfce7fea36cf00a62cfc56f0f37c" uuid = "90137ffa-7385-5640-81b9-e52037218182" -version = "1.9.3" +version = "1.9.4" weakdeps = ["ChainRulesCore", "Statistics"] [deps.StaticArrays.extensions] @@ -1424,9 +1508,9 @@ version = "1.7.0" [[deps.StatsBase]] deps = ["DataAPI", "DataStructures", "LinearAlgebra", "LogExpFunctions", "Missings", "Printf", "Random", "SortingAlgorithms", "SparseArrays", "Statistics", "StatsAPI"] -git-tree-sha1 = "1d77abd07f617c4868c33d4f5b9e1dbb2643c9cf" +git-tree-sha1 = "5cf7606d6cef84b543b483848d4ae08ad9832b21" uuid = "2913bbd2-ae8a-5f71-8c99-4fb6c76f3a91" -version = "0.34.2" +version = "0.34.3" [[deps.StatsFuns]] deps = ["HypergeometricFunctions", "IrrationalConstants", "LogExpFunctions", "Reexport", "Rmath", "SpecialFunctions"] @@ -1440,10 +1524,10 @@ weakdeps = ["ChainRulesCore", "InverseFunctions"] StatsFunsInverseFunctionsExt = "InverseFunctions" [[deps.StrideArraysCore]] -deps = ["ArrayInterface", "CloseOpenIntervals", "IfElse", "LayoutPointers", "ManualMemory", "SIMDTypes", "Static", "StaticArrayInterface", "ThreadingUtilities"] -git-tree-sha1 = "d6415f66f3d89c615929af907fdc6a3e17af0d8c" +deps = ["ArrayInterface", "CloseOpenIntervals", "IfElse", "LayoutPointers", "LinearAlgebra", "ManualMemory", "SIMDTypes", "Static", "StaticArrayInterface", "ThreadingUtilities"] +git-tree-sha1 = "25349bf8f63aa36acbff5e3550a86e9f5b0ef682" uuid = "7792a7ef-975c-4747-a70f-980b88e8d1da" -version = "0.5.2" +version = "0.5.6" [[deps.StringManipulation]] deps = ["PrecompileTools"] @@ -1474,16 +1558,16 @@ uuid = "bea87d4a-7f5b-5778-9afe-8cc45184846c" version = "7.2.1+1" [[deps.SymbolicIndexingInterface]] -deps = ["Accessors", "ArrayInterface", "MacroTools", "RuntimeGeneratedFunctions", "StaticArraysCore"] -git-tree-sha1 = "4b7f4c80449d8baae8857d55535033981862619c" +deps = ["Accessors", "ArrayInterface", "RuntimeGeneratedFunctions", "StaticArraysCore"] +git-tree-sha1 = "a5f6f138b740c9d93d76f0feddd3092e6ef002b7" uuid = "2efcf032-c050-4f8e-a9bb-153293bab1f5" -version = "0.3.15" +version = "0.3.22" [[deps.SymbolicLimits]] deps = ["SymbolicUtils"] -git-tree-sha1 = "89aa6b25a75418c8fffc42073b2e7dce69847394" +git-tree-sha1 = "fb099adbd7504f1e68b4512828e9d94197a8b889" uuid = "19f23fe9-fdab-4a78-91af-e7b7767979c3" -version = "0.2.0" +version = "0.2.1" [[deps.SymbolicUtils]] deps = ["AbstractTrees", "Bijections", "ChainRulesCore", "Combinatorics", "ConstructionBase", "DataStructures", "DocStringExtensions", "DynamicPolynomials", "IfElse", "LabelledArrays", "LinearAlgebra", "MultivariatePolynomials", "NaNMath", "Setfield", "SparseArrays", "SpecialFunctions", "StaticArrays", "SymbolicIndexingInterface", "TimerOutputs", "Unityper"] @@ -1493,9 +1577,9 @@ version = "1.5.1" [[deps.Symbolics]] deps = ["ArrayInterface", "Bijections", "ConstructionBase", "DataStructures", "DiffRules", "Distributions", "DocStringExtensions", "DomainSets", "DynamicPolynomials", "ForwardDiff", "IfElse", "LaTeXStrings", "LambertW", "Latexify", "Libdl", "LinearAlgebra", "LogExpFunctions", "MacroTools", "Markdown", "NaNMath", "PrecompileTools", "RecipesBase", "Reexport", "Requires", "RuntimeGeneratedFunctions", "SciMLBase", "Setfield", "SparseArrays", "SpecialFunctions", "StaticArrays", "SymbolicIndexingInterface", "SymbolicLimits", "SymbolicUtils"] -path = "../Symbolics" +git-tree-sha1 = "4104548fff14d7370b278ee767651d6ec61eb195" uuid = "0c5d862f-8b57-4792-8d23-62f2024744c7" -version = "5.25.2" +version = "5.28.0" [deps.Symbolics.extensions] SymbolicsGroebnerExt = "Groebner" @@ -1543,20 +1627,20 @@ version = "0.5.2" [[deps.TimerOutputs]] deps = ["ExprTools", "Printf"] -git-tree-sha1 = "f548a9e9c490030e545f72074a41edfd0e5bcdd7" +git-tree-sha1 = "5a13ae8a41237cff5ecf34f73eb1b8f42fff6531" uuid = "a759f4b9-e2f1-59dc-863e-4aeb61b1ea8f" -version = "0.5.23" +version = "0.5.24" [[deps.Tokenize]] -git-tree-sha1 = "5b5a892ba7704c0977013bd0f9c30f5d962181e0" +git-tree-sha1 = "468b4685af4abe0e9fd4d7bf495a6554a6276e75" uuid = "0796e94c-ce3b-5d07-9a54-7f471281c624" -version = "0.5.28" +version = "0.5.29" [[deps.TriangularSolve]] deps = ["CloseOpenIntervals", "IfElse", "LayoutPointers", "LinearAlgebra", "LoopVectorization", "Polyester", "Static", "VectorizationBase"] -git-tree-sha1 = "7ee8ed8904e7dd5d31bb46294ef5644d9e2e44e4" +git-tree-sha1 = "66c68a20907800c0b7c04ff8a6164115e8747de2" uuid = "d5829a12-d9aa-46ab-831f-fb7c9ab06edf" -version = "0.1.21" +version = "0.2.0" [[deps.Tricks]] git-tree-sha1 = "eae1bb484cd63b36999ee58be2de6c178105112f" @@ -1588,9 +1672,9 @@ uuid = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5" [[deps.Unitful]] deps = ["Dates", "LinearAlgebra", "Random"] -git-tree-sha1 = "3c793be6df9dd77a0cf49d80984ef9ff996948fa" +git-tree-sha1 = "dd260903fdabea27d9b6021689b3cd5401a57748" uuid = "1986cc42-f94f-5a68-af5c-568840ba703d" -version = "1.19.0" +version = "1.20.0" weakdeps = ["ConstructionBase", "InverseFunctions"] [deps.Unitful.extensions] @@ -1605,9 +1689,9 @@ version = "0.1.6" [[deps.VectorizationBase]] deps = ["ArrayInterface", "CPUSummary", "HostCPUFeatures", "IfElse", "LayoutPointers", "Libdl", "LinearAlgebra", "SIMDTypes", "Static", "StaticArrayInterface"] -git-tree-sha1 = "7209df901e6ed7489fe9b7aa3e46fb788e15db85" +git-tree-sha1 = "e863582a41c5731f51fd050563ae91eb33cf09be" uuid = "3d5dd08c-fd9d-11e8-17fa-ed2836048c2f" -version = "0.21.65" +version = "0.21.68" [[deps.VertexSafeGraphs]] deps = ["Graphs"] @@ -1630,6 +1714,12 @@ deps = ["Artifacts", "Libdl"] uuid = "8e850ede-7688-5339-a07c-302acd2aaf8d" version = "1.52.0+1" +[[deps.oneTBB_jll]] +deps = ["Artifacts", "JLLWrappers", "Libdl"] +git-tree-sha1 = "7d0ea0f4895ef2f5cb83645fa689e52cb55cf493" +uuid = "1317d2d5-d96f-522e-a858-c73665f53c3e" +version = "2021.12.0+0" + [[deps.p7zip_jll]] deps = ["Artifacts", "Libdl"] uuid = "3f19e933-33d8-53b3-aaab-bd5110c3b7a0" From 66a95a49e6f02ceb8dfe722e9333395adce848a1 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 5 Jun 2024 08:07:28 +0200 Subject: [PATCH 5/7] update sensor ratio --- Manifest.toml | 4 +--- docs/src/examples/robot.md | 16 +++++++-------- src/robot/robot_components.jl | 12 +++++------ test/test_robot.jl | 38 +++++++++++++++++------------------ 4 files changed, 34 insertions(+), 36 deletions(-) diff --git a/Manifest.toml b/Manifest.toml index 7264b79e..af90b200 100644 --- a/Manifest.toml +++ b/Manifest.toml @@ -700,9 +700,7 @@ version = "0.1.0" [[deps.JuliaSimCompiler]] deps = ["AbstractTrees", "ChainRules", "ConstructionBase", "DataStructures", "DiffEqCallbacks", "DocStringExtensions", "Expronicon", "ForwardDiff", "GPUCompiler", "Graphs", "IfElse", "JuliaFormatter", "JuliaSimBase", "JuliaSimCompilerRuntime", "LLVM", "Libdl", "LinearAlgebra", "ModelingToolkit", "NaNMath", "OffsetArrays", "RuntimeGeneratedFunctions", "SciMLBase", "Setfield", "SnoopPrecompile", "SparseArrays", "SparseDiffTools", "SpecialFunctions", "StaticArrays", "StrideArraysCore", "SymbolicIndexingInterface", "SymbolicUtils", "Symbolics", "UnPack"] -git-tree-sha1 = "e948d4218b8860e7c980d4738806ae2a8df3cbbc" -repo-rev = "master" -repo-url = "https://github.com/JuliaComputing/JuliaSimCompiler.jl.git" +path = "/home/fredrikb/.julia/dev/JuliaSimCompiler" uuid = "8391cb6b-4921-5777-4e45-fd9aab8cb88d" version = "0.1.11" diff --git a/docs/src/examples/robot.md b/docs/src/examples/robot.md index d0ce75ee..003f84e4 100644 --- a/docs/src/examples/robot.md +++ b/docs/src/examples/robot.md @@ -12,7 +12,7 @@ using Test t = Multibody.t D = Differential(t) -@named robot = Multibody.Robot6DOF(trivial=true) +@named robot = Multibody.Robot6DOF(trivial=false) robot = complete(robot) length(equations(robot)) @@ -68,17 +68,17 @@ plot!(sol, idxs = [ ], sp=7:12, lab="Position error", link=:x) plot!(xlabel=[fill("", 1, 9) fill("Time [s]", 1, 3)]) ``` -We see that after an initial transient, the robot controller converges to tracking the reference trajectory well. However, since the first three axes of the robot are modeled as slightly flexible, and we are ultimately interested in the tracking performance _after_ the gear box, we plot also this tracking error +We see that after an initial transient, the robot controller converges to tracking the reference trajectory well. However, since the first three axes of the robot are modeled as slightly flexible, and we are ultimately interested in the tracking performance _after_ the gear box and any flexibilities it may suffer from, we plot also this tracking error ```@example robot plot(sol, idxs = [ - robot.axis1.controller.feedback1.output.u - robot.axis2.controller.feedback1.output.u - robot.axis3.controller.feedback1.output.u + robot.axis1.controller.feedback1.output.u #/ ( -105) + robot.axis2.controller.feedback1.output.u #/ (210) + robot.axis3.controller.feedback1.output.u #/ (60) ], layout=(1,3), lab="Position error, motor side", link=:x) plot!(sol, idxs = [ - robot.pathPlanning.controlBus.axisControlBus1.angle_ref - robot.axis1.motor.Jmotor.phi / ( -105) - robot.pathPlanning.controlBus.axisControlBus2.angle_ref - robot.axis2.motor.Jmotor.phi / (210) - robot.pathPlanning.controlBus.axisControlBus3.angle_ref - robot.axis3.motor.Jmotor.phi / (60) + robot.pathPlanning.controlBus.axisControlBus1.angle_ref - robot.mechanics.r1.phi # + robot.pathPlanning.controlBus.axisControlBus2.angle_ref - robot.mechanics.r2.phi # + robot.pathPlanning.controlBus.axisControlBus3.angle_ref - robot.mechanics.r3.phi # ], lab="Position error, arm side") ``` diff --git a/src/robot/robot_components.jl b/src/robot/robot_components.jl index 05577eb7..906680ee 100644 --- a/src/robot/robot_components.jl +++ b/src/robot/robot_components.jl @@ -130,9 +130,9 @@ function AxisType2(; name, kp = 10, ks = 1, Ts = 0.01, k = 1.1616, w = 4590, D = connect(motor.axisControlBus, axisControlBus) - (angleSensor.phi.u*ratio ~ axisControlBus.angle) - (speedSensor.w.u*ratio ~ axisControlBus.speed) - (accSensor.a.u*ratio ~ axisControlBus.acceleration) + (angleSensor.phi.u/ratio ~ axisControlBus.angle) + (speedSensor.w.u/ratio ~ axisControlBus.speed) + (accSensor.a.u/ratio ~ axisControlBus.acceleration) connect(controller.axisControlBus, axisControlBus)] ODESystem(eqs, t; name, systems) @@ -163,9 +163,9 @@ function AxisType1(; name, c = 43, cd = 0.005, kp = 10, ks = 1, Ts = 0.01, k = 1 connect(gear.flange_a, spring.flange_b) connect(motor.flange_motor, spring.flange_a, angleSensor.flange, speedSensor.flange, accSensor.flange) connect(motor.axisControlBus, axisControlBus) - (angleSensor.phi.u*ratio ~ axisControlBus.angle) - (speedSensor.w.u*ratio ~ axisControlBus.speed) - (accSensor.a.u*ratio ~ axisControlBus.acceleration) + (angleSensor.phi.u/ratio ~ axisControlBus.angle) + (speedSensor.w.u/ratio ~ axisControlBus.speed) + (accSensor.a.u/ratio ~ axisControlBus.acceleration) connect(controller.axisControlBus, axisControlBus) ] diff --git a/test/test_robot.jl b/test/test_robot.jl index ce3a9727..09651d1f 100644 --- a/test/test_robot.jl +++ b/test/test_robot.jl @@ -44,7 +44,7 @@ end @named motorTest = MotorTest() m = structural_simplify(IRSystem(motorTest)) -# @test length(states(m)) == 3 +# @test length(unknowns(m)) == 3 # D(motorTest.motor.gear.bearingFriction.w) => 0 cm = complete(motorTest) @@ -159,6 +159,7 @@ tspan = (0.0, 5.0) prob = ODEProblem(m, [ # ModelingToolkit.missing_variable_defaults(m); # D(cm.axis2.gear.bearingFriction.w) => 0 + cm.axis2.motor.Jmotor.phi => deg2rad(20) * 0, cm.axis2.gear.gear.phi_b => 0, D(cm.axis2.gear.gear.phi_b) => 0, ], tspan) @@ -168,7 +169,7 @@ sol = solve(prob, Rodas4()) @test sol(0.0, idxs=cm.axis2.motor.emf.phi) == 0 # @test sol(tspan[2], idxs=cm.axis2.motor.emf.phi) == 0 -doplot() && plot(sol, layout=length(states(m))) +doplot() && plot(sol, layout=length(unknowns(m))) doplot() && plot(sol, idxs=[ cm.axis2.gear.gear.phi_a cm.axis2.gear.gear.phi_b @@ -200,7 +201,7 @@ u = cm.axis2.controller.PI.ctr_output.u @testset "one axis" begin @info "Testing one axis" - @named oneaxis = RobotAxis(trivial=true) + @named oneaxis = RobotAxis(trivial=false) oneaxis = complete(oneaxis) op = Dict([ oneaxis.axis.flange.phi => 0 @@ -244,7 +245,7 @@ u = cm.axis2.controller.PI.ctr_output.u # @test sol(10, idxs=oneaxis.axis.controller.PI.err_input.u) ≈ 0 atol=1e-8 tv = 0:0.1:10.0 - control_error = sol(tv, idxs=oneaxis.pathPlanning.controlBus.axisControlBus1.angle_ref-oneaxis.pathPlanning.controlBus.axisControlBus1.angle) + control_error = sol(tv, idxs=oneaxis.pathPlanning.controlBus.axisControlBus1.angle_ref-oneaxis.load.phi) @test sol(tv[1], idxs=oneaxis.pathPlanning.controlBus.axisControlBus1.angle_ref) ≈ deg2rad(0) atol=1e-8 @test sol(tv[end], idxs=oneaxis.pathPlanning.controlBus.axisControlBus1.angle_ref) ≈ deg2rad(120) @@ -256,7 +257,7 @@ end @testset "full robot" begin @info "Testing full robot" - @named robot = Robot6DOF(trivial=true) + @named robot = Robot6DOF(trivial=false) robot = complete(robot) @time "full robot" begin @@ -269,9 +270,9 @@ end robot.mechanics.r5.phi => deg2rad(-110) robot.mechanics.r6.phi => deg2rad(0) - robot.axis1.motor.Jmotor.phi => deg2rad(-60) * -105 # Multiply by gear ratio - robot.axis2.motor.Jmotor.phi => deg2rad(20) * 210 - robot.axis3.motor.Jmotor.phi => deg2rad(90) * 60 + robot.axis1.motor.Jmotor.phi => deg2rad(-60) * (-105) # Multiply by gear ratio + robot.axis2.motor.Jmotor.phi => deg2rad(20) * (210) + robot.axis3.motor.Jmotor.phi => deg2rad(90) * (60) ], (0.0, 4.0)) @time "simulation (solve)" sol = solve(prob, Rodas5P(autodiff=false)); @test SciMLBase.successful_retcode(sol) @@ -280,12 +281,12 @@ end if doplot() # plot(sol, layout=30, size=(1900,1200), legend=false) plot(sol, idxs = [ - robot.pathPlanning.controlBus.axisControlBus1.angle_ref - robot.pathPlanning.controlBus.axisControlBus2.angle_ref - robot.pathPlanning.controlBus.axisControlBus3.angle_ref - robot.pathPlanning.controlBus.axisControlBus4.angle_ref - robot.pathPlanning.controlBus.axisControlBus5.angle_ref - robot.pathPlanning.controlBus.axisControlBus6.angle_ref + robot.pathPlanning.controlBus.axisControlBus1.angle_ref# * (-105) + robot.pathPlanning.controlBus.axisControlBus2.angle_ref# * (210) + robot.pathPlanning.controlBus.axisControlBus3.angle_ref# * (60) + robot.pathPlanning.controlBus.axisControlBus4.angle_ref# * (-99) + robot.pathPlanning.controlBus.axisControlBus5.angle_ref# * (79.2) + robot.pathPlanning.controlBus.axisControlBus6.angle_ref# * (-99) ], layout=6, size=(800,800), l=(:black, :dash), legend=false) plot!(sol, idxs = [ robot.pathPlanning.controlBus.axisControlBus1.angle @@ -297,9 +298,9 @@ end ], layout=6) plot!(sol, idxs = [ - robot.axis1.motor.Jmotor.phi / ( -105) - robot.axis2.motor.Jmotor.phi / (210) - robot.axis3.motor.Jmotor.phi / (60) + robot.axis1.motor.Jmotor.phi / ( -105) - robot.pathPlanning.controlBus.axisControlBus1.angle_ref + robot.axis2.motor.Jmotor.phi / (210) - robot.pathPlanning.controlBus.axisControlBus2.angle_ref + robot.axis3.motor.Jmotor.phi / (60) - robot.pathPlanning.controlBus.axisControlBus3.angle_ref ], sp=(1:3)') display(current()) @@ -310,6 +311,5 @@ end @test !all(iszero, angle_ref) control_error = sol(tv, idxs=robot.pathPlanning.controlBus.axisControlBus1.angle_ref-robot.pathPlanning.controlBus.axisControlBus1.angle) - @test_broken maximum(abs, control_error[25:end]) < 1e-4 - @test_broken maximum(abs, control_error) < 1e-4 # Initial condition not respected + @test maximum(abs, control_error) < 1e-4 end From eff806c3f3a719c14db41508a2effc3ba607668e Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 11 Jun 2024 11:09:52 +0200 Subject: [PATCH 6/7] add ptp --- src/robot/path_planning.jl | 3 + src/robot/ptp.jl | 122 +++++++++++++++++++++++++++++++++++++ test/runtests.jl | 5 ++ test/test_traj.jl | 49 +++++++++++++++ 4 files changed, 179 insertions(+) create mode 100644 src/robot/ptp.jl create mode 100644 test/test_traj.jl diff --git a/src/robot/path_planning.jl b/src/robot/path_planning.jl index 0fe17dc0..37007071 100644 --- a/src/robot/path_planning.jl +++ b/src/robot/path_planning.jl @@ -1,5 +1,8 @@ using DataInterpolations using ModelingToolkitStandardLibrary.Blocks: RealInput, RealOutput + +include("ptp.jl") + "Generate reference angles for specified kinematic movement" function PathPlanning1(; name, angleBegDeg = 0, angleEndDeg = 1, time = 0:0.01:10, swingTime = 0.5, kwargs...) diff --git a/src/robot/ptp.jl b/src/robot/ptp.jl new file mode 100644 index 00000000..b499359a --- /dev/null +++ b/src/robot/ptp.jl @@ -0,0 +1,122 @@ +function centraldiff(v::AbstractMatrix) + dv = Base.diff(v, dims=1)/2 + a1 = [dv[[1],:];dv] + a2 = [dv;dv[[end],:]] + a = a1+a2 +end + +function centraldiff(v::AbstractVector) + dv = Base.diff(v)/2 + a1 = [dv[1];dv] + a2 = [dv;dv[end]] + a = a1+a2 +end + +function PTP(time; q0 = 0.0, q1 = 1.0, t0 = 0, qd_max = 1, qdd_max = 1) + nout = max(length(q0), length(q1), length(qd_max), length(qdd_max)) + p_q0 = q0 + p_q1 = q1 + p_qd_max = qd_max + p_qdd_max = qdd_max + p_deltaq = p_q1 .- p_q0 + + T = Base.promote_eltype(q0, q1, t0, qd_max, qdd_max) + + aux1 = p_deltaq ./ p_qd_max + aux2 = p_deltaq ./ p_qdd_max + + sd_max_inv = maximum(abs, aux1) + sdd_max_inv = maximum(abs, aux2) + + if sd_max_inv <= eps() || sdd_max_inv <= eps() + if time isa Number + return q0, zero(q0), zero(q0), float(time) + else + return zeros(T, nout) .+ q0', zeros(T, length(time), nout), zeros(T, length(time), nout) + end + end + + sd_max = 1 / sd_max_inv + sdd_max = 1 / sdd_max_inv + Ta1 = sqrt(1 / sdd_max) + Ta2 = sd_max / sdd_max + noWphase = Ta2 >= Ta1 + Tv = if noWphase + Ta1 + else + 1 / sd_max + end + Te = if noWphase + Ta1 + Ta1 + else + Tv + Ta2 + end + Ta1s = Ta1 + t0 + Ta2s = Ta2 + t0 + Tvs = Tv + t0 + Tes = Te + t0 + sd_max2 = sdd_max * Ta1 + s1 = sdd_max * (noWphase ? + Ta1 * Ta1 : Ta2 * Ta2) / 2 + s2 = s1 + (noWphase ? sd_max2 * (Te - Ta1) - (sdd_max / 2) * (Te - Ta1)^2 : + sd_max * (Tv - Ta2)) + + s3 = s2 + sd_max * (Te - Tv) - (sdd_max / 2) * (Te - Tv) * (Te - Tv) + + if !(time isa Number) + N = length(time) + Q = zeros(T, N, nout) + Qd = zeros(T, N, nout) + Qdd = zeros(T, N, nout) + end + + for (i, t) in enumerate(time) + sd = sdd = zero(T) + + if t < t0 + s = zero(T) + elseif noWphase + if t < Ta1s + s = (sdd_max / 2) * (t - t0)^2 + sd = sdd_max*(t - t0) + sdd = sdd_max + elseif t < Tes + s = s1 + sd_max2 * (t - Ta1s) - + (sdd_max / 2) * (t - Ta1s)^2 + sd = sd_max2 + (Ta1s - t)*sdd_max + sdd = -sdd_max + else + s = s2 + end + elseif t < Ta2s + s = (sdd_max / 2) * (t - t0)^2 + sd = sdd_max*(t - t0) + sdd = sdd_max + elseif t < Tvs + s = s1 + sd_max * (t - Ta2s) + sd = sd_max + elseif t < Tes + s = s2 + sd_max * (t - Tvs) - (sdd_max / 2) * (t - Tvs)^2 + sd = sd_max + (Tvs - t)*sdd_max + sdd = -sdd_max + else + s = s3 + end + + if time isa Number + qdd = p_deltaq * sdd + qd = p_deltaq * sd + t1 = Tes + q = @. p_q0 + p_deltaq * s + return q, qd, qdd, t1 + else + @. Qdd[i, :] = p_deltaq * sdd + @. Qd[i, :] = p_deltaq * sd + @. Q[i, :] = p_q0 + p_deltaq * s + end + end + @assert !(time isa Number) + return Q, Qd, Qdd + +end + diff --git a/test/runtests.jl b/test/runtests.jl index d4c9dda6..144ba078 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -30,6 +30,11 @@ ssys = structural_simplify(model) include("test_robot.jl") end +@testset "traj" begin + @info "Testing traj" + include("test_traj.jl") +end + # ============================================================================== ## Add spring to make a harmonic oscillator ==================================== # ============================================================================== diff --git a/test/test_traj.jl b/test/test_traj.jl new file mode 100644 index 00000000..7ed5ade7 --- /dev/null +++ b/test/test_traj.jl @@ -0,0 +1,49 @@ +using Test + +Ts = 0.001 +t = -1:Ts:3 +q, qd, qdd = PTP(t) + +q1 = [1, 1.2] +qd_max = [0.7, 1.2] +qdd_max = [0.9, 1.1] +q, qd, qdd = PTP(t; q1, qd_max) +@test all(q .<= q1') +@test all(qd .<= qd_max') +@test all(qdd.<= qdd_max') + +_, _, _, t1 = PTP(0; q1, qd_max) +ci = findlast(qd .!= 0) +t02 = t[ci[1]] +@test t02 ≈ t1 rtol=1e-3 + +_qd = Multibody.centraldiff(q) ./ Ts +_qdd = Multibody.centraldiff(qd)./ Ts + +# Plots.plot(t, [q qd qdd], layout=(2,1)) +# Plots.plot!(t, [_qd _qdd], sp=[1 2]) + + +N = 200 +t0 = randn() +q0 = randn(N) +q1 = randn(N) +qd_max = rand(N) +qdd_max = rand(N) +O = zeros(N) +q, qd, qdd, t1 = PTP(0; q0, q1, qd_max, qdd_max, t0) +Ts = 0.01 +Tf = t1 + 0.1 +t = (t0 - 1):Ts:Tf +q, qd, qdd = PTP(t; q0, q1, qd_max, qdd_max, t0) +@test q[1,:] ≈ q0 norm=maximum atol=1e-3 +@test q[end,:] ≈ q1 norm=maximum atol=1e-3 +@test qd[1,:] == O +@test qd[end,:] == O +@test qdd[1,:] == O +@test qdd[end,:] == O +@test all(min.(q0, q1)' .- 1e-4 .<= q .<= max.(q0, q1)' .+ 1e-4) +@test all(abs.(qd) .<= 1.001.*qd_max') +@test all(abs.(qdd) .<= 1.001.*qdd_max') + + From 9adbbbe0fd4a95aa94856182c766c8876e487e52 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 11 Jun 2024 11:56:07 +0200 Subject: [PATCH 7/7] propagate speed and acc bounds --- docs/src/examples/robot.md | 8 +-- src/robot/FullRobot.jl | 4 +- src/robot/path_planning.jl | 139 +++---------------------------------- test/test_robot.jl | 17 ++--- 4 files changed, 26 insertions(+), 142 deletions(-) diff --git a/docs/src/examples/robot.md b/docs/src/examples/robot.md index 003f84e4..79439541 100644 --- a/docs/src/examples/robot.md +++ b/docs/src/examples/robot.md @@ -37,7 +37,7 @@ prob = ODEProblem(ssys, Dict([ robot.axis1.motor.Jmotor.phi => deg2rad(-60) * -105 # Multiply by gear ratio robot.axis2.motor.Jmotor.phi => deg2rad(20) * 210 robot.axis3.motor.Jmotor.phi => deg2rad(90) * 60 -]), (0.0, 4.0)) +]), (0.0, 2.0)) sol = solve(prob, Rodas5P(autodiff=false)); @test SciMLBase.successful_retcode(sol) @@ -71,9 +71,9 @@ plot!(xlabel=[fill("", 1, 9) fill("Time [s]", 1, 3)]) We see that after an initial transient, the robot controller converges to tracking the reference trajectory well. However, since the first three axes of the robot are modeled as slightly flexible, and we are ultimately interested in the tracking performance _after_ the gear box and any flexibilities it may suffer from, we plot also this tracking error ```@example robot plot(sol, idxs = [ - robot.axis1.controller.feedback1.output.u #/ ( -105) - robot.axis2.controller.feedback1.output.u #/ (210) - robot.axis3.controller.feedback1.output.u #/ (60) + robot.axis1.controller.feedback1.output.u / ( -105) + robot.axis2.controller.feedback1.output.u / (210) + robot.axis3.controller.feedback1.output.u / (60) ], layout=(1,3), lab="Position error, motor side", link=:x) plot!(sol, idxs = [ robot.pathPlanning.controlBus.axisControlBus1.angle_ref - robot.mechanics.r1.phi # diff --git a/src/robot/FullRobot.jl b/src/robot/FullRobot.jl index 459c4d0a..5fb3dfba 100644 --- a/src/robot/FullRobot.jl +++ b/src/robot/FullRobot.jl @@ -56,8 +56,8 @@ function RobotAxis(; name, mLoad = 15, kp = 5.0, ks = 0.5, Ts = 0.05, startAngle pathPlanning = PathPlanning1(;swingTime = swingTime, angleBegDeg = startAngle, angleEndDeg = endAngle, - # speedMax = refSpeedMax, - # accMax = refAccMax + speedMax = refSpeedMax, + accMax = refAccMax, kwargs... ) controlBus = ControlBus() diff --git a/src/robot/path_planning.jl b/src/robot/path_planning.jl index 37007071..813b1f02 100644 --- a/src/robot/path_planning.jl +++ b/src/robot/path_planning.jl @@ -64,8 +64,8 @@ function PathPlanning6(; name, naxis = 6, angleBegDeg = zeros(naxis), controlBus = ControlBus() path = KinematicPTP(; q_end = deg2rad.(angleEndDeg), time, - # qd_max = speedMax, - # qdd_max = accMax, + qd_max = speedMax, + qdd_max = accMax, # startTime = startTime, q_begin = deg2rad.(angleBegDeg), kwargs...) pathToAxis1 = PathToAxisControlBus(nAxis = naxis, axisUsed = 1) @@ -191,17 +191,8 @@ end A simple trajectory planner that plans a 5:th order polynomial trajectory between two points, subject to specified boundary conditions on the position, velocity and acceleration. """ function KinematicPTP(; time, name, q_begin = 0, q_end = 1, qd_begin = 0, qd_end = 0, - qdd_begin = 0, qdd_end = 0, trivial = false) + qdd_begin = 0, qdd_end = 0, trivial = false, qd_max=1, qdd_max=1) nout = max(length(q_begin), length(q_end), length(qd_end), length(qdd_end)) - # parameter Real p_q_begin[nout]=(if size(q_begin, 1) == 1 then ones(nout)* - # q_begin[1] else q_begin); - # parameter Real p_q_end[nout]=(if size(q_end, 1) == 1 then ones(nout)*q_end[ - # 1] else q_end); - # parameter Real p_qd_max[nout]=(if size(qd_max, 1) == 1 then ones(nout)* - # qd_max[1] else qd_max); - # parameter Real p_qdd_max[nout]=(if size(qdd_max, 1) == 1 then ones(nout)* - # qdd_max[1] else qdd_max); - # parameter Real p_deltaq[nout]=p_q_end - p_q_begin; # @parameters begin # q_begin = q_begin, [description = "Start position"] @@ -216,19 +207,7 @@ function KinematicPTP(; time, name, q_begin = 0, q_end = 1, qd_begin = 0, qd_end # p_deltaq[1:nout] = p_q_end - p_q_begin # end - # output endTime "Time instant at which movement stops"; - # Modelica.Blocks.Interfaces.RealOutput q[nout] - # "Reference position of path planning" annotation (Placement( - # transformation(extent={{100,70},{120,90}}))); - # Modelica.Blocks.Interfaces.RealOutput qd[nout] - # "Reference speed of path planning" annotation (Placement(transformation( - # extent={{100,20},{120,40}}))); - # Modelica.Blocks.Interfaces.RealOutput qdd[nout] - # "Reference acceleration of path planning" annotation (Placement( - # transformation(extent={{100,-40},{120,-20}}))); - # Modelica.Blocks.Interfaces.BooleanOutput moving[nout] - # "= true, if end position not yet reached; = false, if end position reached or axis is completely at rest" systems = @named begin q = RealOutput(; nout) @@ -237,124 +216,28 @@ function KinematicPTP(; time, name, q_begin = 0, q_end = 1, qd_begin = 0, qd_end # moving = BooleanOutput(; nout) end - # for i in 1:nout - # aux1[i] = p_deltaq[i] / p_qd_max[i] - # aux2[i] = p_deltaq[i] / p_qdd_max[i] - # end - - # sd_max_inv = max(abs(aux1)) - # sdd_max_inv = max(abs(aux2)) - - # if sd_max_inv <= eps || sdd_max_inv <= eps - # sd_max = 0 - # sdd_max = 0 - # Ta1 = 0 - # Ta2 = 0 - # noWphase = false - # Tv = 0 - # Te = 0 - # Ta1s = 0 - # Ta2s = 0 - # Tvs = 0 - # Tes = 0 - # sd_max2 = 0 - # s1 = 0 - # s2 = 0 - # s3 = 0 - # s = 0 - # else - # sd_max = 1 / max(abs(aux1)) - # sdd_max = 1 / max(abs(aux2)) - # Ta1 = sqrt(1 / sdd_max) - # Ta2 = sd_max / sdd_max - # noWphase = Ta2 >= Ta1 - # Tv = if noWphase - # Ta1 - # else - # 1 / sd_max - # end - # Te = if noWphase - # Ta1 + Ta1 - # else - # Tv + Ta2 - # end - # Ta1s = Ta1 + startTime - # Ta2s = Ta2 + startTime - # Tvs = Tv + startTime - # Tes = Te + startTime - # sd_max2 = sdd_max * Ta1 - # s1 = sdd_max * (noWphase ? - # Ta1 * Ta1 : Ta2 * Ta2) / 2 - # s2 = s1 + (noWphase ? sd_max2 * (Te - Ta1) - (sdd_max / 2) * (Te - Ta1)^2 : - # sd_max * (Tv - Ta2)) - - # s3 = s2 + sd_max * (Te - Tv) - (sdd_max / 2) * (Te - Tv) * (Te - Tv) - - # if time < startTime - # s = 0 - # elseif noWphase - # if time < Ta1s - # s = (sdd_max / 2) * (time - startTime) * (time - startTime) - # elseif time < Tes - # s = s1 + sd_max2 * (time - Ta1s) - - # (sdd_max / 2) * (time - Ta1s) * (time - - # Ta1s) - # else - # s = s2 - # end - # elseif time < Ta2s - # s = (sdd_max / 2) * (time - startTime) * (time - startTime) - # elseif time < Tvs - # s = s1 + sd_max * (time - Ta2s) - # elseif time < Tes - # s = s2 + sd_max * (time - Tvs) - (sdd_max / 2) * (time - Tvs) * (time - Tvs) - # else - # s = s3 - # end - # end - - # sd = D(s) - # sdd = D(sd) - - # qdd = p_deltaq * sdd - # qd = p_deltaq * sd - # q = p_q_begin + p_deltaq * s - # endTime = Tes - - # # report when axis is moving - # motion_ref = time < endTime - # for i in 1:nout - # loop - # moving[i] = abs(q_begin[i] - q_end[i]) > eps ? motion_ref : false - # end startTime = time[1] time0 = time .- startTime # traj5 wants time vector to start at 0 + if !trivial + q_vec, qd_vec, qdd_vec = PTP(time; q0 = q_begin, q1 = q_end, qd_max, qdd_max) + end interp_eqs = map(1:nout) do i if trivial - # [q.u[i] ~ 1 # TODO: SymbolicIR does not handle the interpolation https://github.com/JuliaComputing/SymbolicIR.jl/issues/2 - # qd.u[i] ~ 0 - # qdd.u[i] ~ 0] - _q, _qd, _qdd = traj5(t, time[end]; q0 = q_begin[i], q1 = q_end[i], q̇0 = zero(q_begin[i]), q̇1 = zero(q_begin[i]), q̈0 = zero(q_begin[i]), q̈1 = zero(q_begin[i])) - [q.u[i] ~ _q # TODO: SymbolicIR does not handle the interpolation https://github.com/JuliaComputing/SymbolicIR.jl/issues/2 + [q.u[i] ~ _q qd.u[i] ~ _qd qdd.u[i] ~ _qdd] else - q_vec, qd_vec, qdd_vec = traj5(time0; q0 = q_begin[i], q1 = q_end[i], - q̇0 = zero(q_begin[i]), - q̇1 = zero(q_begin[i]), - q̈0 = zero(q_begin[i]), - q̈1 = zero(q_begin[i])) - - qfun = CubicSpline(q_vec, time; extrapolate=true) - qdfun = CubicSpline(qd_vec, time; extrapolate=true) - qddfun = CubicSpline(qdd_vec, time; extrapolate=true) + # q_vec, qd_vec, qdd_vec = PTP(time; q0 = q_begin[i], q1 = q_end[i], qd_max, qdd_max) + qfun = CubicSpline(q_vec[:, i], time; extrapolate=true) + qdfun = LinearInterpolation(qd_vec[:, i], time; extrapolate=true) + qddfun = ConstantInterpolation(qdd_vec[:, i], time; extrapolate=true) [q.u[i] ~ qfun(t) qd.u[i] ~ qdfun(t) qdd.u[i] ~ qddfun(t)] diff --git a/test/test_robot.jl b/test/test_robot.jl index 09651d1f..47f37b92 100644 --- a/test/test_robot.jl +++ b/test/test_robot.jl @@ -273,43 +273,44 @@ end robot.axis1.motor.Jmotor.phi => deg2rad(-60) * (-105) # Multiply by gear ratio robot.axis2.motor.Jmotor.phi => deg2rad(20) * (210) robot.axis3.motor.Jmotor.phi => deg2rad(90) * (60) - ], (0.0, 4.0)) + ], (0.0, 2.0)) @time "simulation (solve)" sol = solve(prob, Rodas5P(autodiff=false)); @test SciMLBase.successful_retcode(sol) end if doplot() # plot(sol, layout=30, size=(1900,1200), legend=false) - plot(sol, idxs = [ + @time "Plotting ref" plot(sol, idxs = [ robot.pathPlanning.controlBus.axisControlBus1.angle_ref# * (-105) robot.pathPlanning.controlBus.axisControlBus2.angle_ref# * (210) robot.pathPlanning.controlBus.axisControlBus3.angle_ref# * (60) robot.pathPlanning.controlBus.axisControlBus4.angle_ref# * (-99) robot.pathPlanning.controlBus.axisControlBus5.angle_ref# * (79.2) robot.pathPlanning.controlBus.axisControlBus6.angle_ref# * (-99) - ], layout=6, size=(800,800), l=(:black, :dash), legend=false) - plot!(sol, idxs = [ + ], layout=9, size=(800,800), l=(:black, :dash), legend=false) + @time "Plotting ang." plot!(sol, idxs = [ robot.pathPlanning.controlBus.axisControlBus1.angle robot.pathPlanning.controlBus.axisControlBus2.angle robot.pathPlanning.controlBus.axisControlBus3.angle robot.pathPlanning.controlBus.axisControlBus4.angle robot.pathPlanning.controlBus.axisControlBus5.angle robot.pathPlanning.controlBus.axisControlBus6.angle - ], layout=6) + ], sp=(1:6)') plot!(sol, idxs = [ robot.axis1.motor.Jmotor.phi / ( -105) - robot.pathPlanning.controlBus.axisControlBus1.angle_ref robot.axis2.motor.Jmotor.phi / (210) - robot.pathPlanning.controlBus.axisControlBus2.angle_ref robot.axis3.motor.Jmotor.phi / (60) - robot.pathPlanning.controlBus.axisControlBus3.angle_ref - ], sp=(1:3)') + ], sp=(7:9)') display(current()) end - tv = 0:0.1:4 + tv = 0:0.1:2 angle_ref = sol(tv, idxs=robot.pathPlanning.controlBus.axisControlBus1.angle_ref) @test !all(iszero, angle_ref) control_error = sol(tv, idxs=robot.pathPlanning.controlBus.axisControlBus1.angle_ref-robot.pathPlanning.controlBus.axisControlBus1.angle) - @test maximum(abs, control_error) < 1e-4 + @test maximum(abs, control_error) < 0.002 end +