Skip to content

Commit f89868d

Browse files
baggepinnenbaggepinnen
committed
update robot argument names
1 parent 9f7ff4c commit f89868d

File tree

5 files changed

+28
-67
lines changed

5 files changed

+28
-67
lines changed

‎docs/src/examples/three_springs.md

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -20,13 +20,13 @@ world = Multibody.world
2020
2121
systems = @named begin
2222
body1 = Body(m = 0.8, I_11 = 0.1, I_22 = 0.1, I_33 = 0.1, r_0 = [0.5, -0.3, 0],
23-
r_cm = [0, -0.2, 0], isroot=true, useQuaternions=true)
23+
r_cm = [0, 0, 0], isroot=false, quat=false)
2424
bar1 = FixedTranslation(r = [0.3, 0, 0])
2525
bar2 = FixedTranslation(r = [0, 0, 0.3])
2626
spring1 = Multibody.Spring(c = 20, m = 0, s_unstretched = 0.1,
2727
r_rel_0 = [-0.2, -0.2, 0.2])
28-
spring2 = Multibody.Spring(c = 40, m = 0, s_unstretched = 0.1)
29-
spring3 = Multibody.Spring(c = 20, m = 0, s_unstretched = 0.1, fixedRotationAtFrame_b=true)
28+
spring2 = Multibody.Spring(c = 40, m = 0, s_unstretched = 0.1, fixed_rotation_at_frame_b=true, fixed_rotation_at_frame_a=true)
29+
spring3 = Multibody.Spring(c = 20, m = 0, s_unstretched = 0.1)
3030
end
3131
eqs = [connect(world.frame_b, bar1.frame_a)
3232
connect(world.frame_b, bar2.frame_a)
@@ -44,7 +44,7 @@ sol = solve(prob, FBDF(), u0=prob.u0 .+ 1e-9*randn(length(prob.u0)))
4444
@assert SciMLBase.successful_retcode(sol)
4545
4646
Plots.plot(sol, idxs = [body1.r_0...])
47-
Plots.plot(sol, idxs = ori(body1.frame_a).R[1])
47+
# Plots.plot(sol, idxs = ori(body1.frame_a).R[1])
4848
```
4949

5050
## 3D animation

‎ext/Render.jl

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@ using LinearAlgebra
77
using ModelingToolkit
88
export render
99
using MeshIO, FileIO
10+
using StaticArrays
1011

1112

1213
"""

‎src/robot/FullRobot.jl

Lines changed: 20 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -1,17 +1,21 @@
11
"""
2-
RobotAxis(; name, mLoad = 15, kp = 5, ks = 0.5, Ts = 0.05, q0 = 0, q1 = 120, swingtime = 0.5, refSpeedMax = 3, refAccMax = 10, kwargs)
2+
RobotAxis(; name, mLoad = 15, kp = 5, ks = 0.5, Ts = 0.05, q0 = 0, q1 = 120, qd_max = 3, qdd_max = 10, kwargs)
33
4-
A single robot axis.
4+
A single robot axis with a PID controller configured in a cascaded inner velocity PI loop and outer position P loop. The internal axis is `AxisType1` which includes a flexibility in the transmission.
5+
6+
The controller is following a point-to-point reference trajectory from `q0` to `q1` generated by [`point_to_point`](@ref).
57
68
- `mLoad`: Mass of load
79
- `kp`: Proportional gain of position controller
810
- `ks`: Proportional gain of velocity controller
911
- `Ts`: Time constant of integrator of velocity controller
1012
- `q0`: Start angle in degrees
1113
- `q1`: End angle in degrees
14+
- `qd_max`: Maximum allowed velocity
15+
- `qdd_max`: Maximum allowed acceleration
1216
"""
1317
function RobotAxis(; name, mLoad = 15, kp = 5.0, ks = 0.5, Ts = 0.05, q0 = 0,
14-
q1 = 120, swingtime = 0.5, refSpeedMax = 3, refAccMax = 10, kwargs...)
18+
q1 = 120, qd_max = 3, qdd_max = 10, kwargs...)
1519

1620
# parameter SI.Mass mLoad(min=0)=15 "Mass of load";
1721
# parameter Real kp=5 "Gain of position controller of axis";
@@ -21,10 +25,9 @@ function RobotAxis(; name, mLoad = 15, kp = 5.0, ks = 0.5, Ts = 0.05, q0 = 0,
2125
# parameter Modelica.Units.NonSI.Angle_deg q0 = 0 "Start angle of axis";
2226
# parameter Modelica.Units.NonSI.Angle_deg q1 = 120 "End angle of axis";
2327

24-
# parameter SI.Time swingtime=0.5
2528
# "Additional time after reference motion is in rest before simulation is stopped";
26-
# parameter SI.AngularVelocity refSpeedMax=3 "Maximum reference speed";
27-
# parameter SI.AngularAcceleration refAccMax=10
29+
# parameter SI.AngularVelocity qd_max=3 "Maximum reference speed";
30+
# parameter SI.AngularAcceleration qdd_max=10
2831
# "Maximum reference acceleration";
2932

3033
@parameters begin
@@ -34,12 +37,8 @@ function RobotAxis(; name, mLoad = 15, kp = 5.0, ks = 0.5, Ts = 0.05, q0 = 0,
3437
Ts = Ts, [description = "Time constant of integrator of speed controller of axis"]
3538
# q0 = q0, [description = "Start angle of axis"]
3639
# q1 = q1, [description = "End angle of axis"]
37-
swingtime = swingtime,
38-
[
39-
description = "Additional time after reference motion is in rest before simulation is stopped",
40-
]
41-
# refSpeedMax = refSpeedMax, [description = "Maximum reference speed"]
42-
# refAccMax = refAccMax, [description = "Maximum reference acceleration"]
40+
# qd_max = qd_max, [description = "Maximum reference speed"]
41+
# qdd_max = qdd_max, [description = "Maximum reference acceleration"]
4342
end
4443

4544
systems = @named begin
@@ -53,11 +52,11 @@ function RobotAxis(; name, mLoad = 15, kp = 5.0, ks = 0.5, Ts = 0.05, q0 = 0,
5352
ks = ks,
5453
Ts = Ts)
5554
load = Rotational.Inertia(J = 1.3 * mLoad)
56-
pathPlanning = PathPlanning1(;swingtime = swingtime,
55+
pathPlanning = PathPlanning1(;
5756
q0deg = q0,
5857
q1deg = q1,
59-
speed_max = refSpeedMax,
60-
acc_max = refAccMax,
58+
speed_max = qd_max,
59+
acc_max = qdd_max,
6160
kwargs...
6261
)
6362
controlBus = ControlBus()
@@ -83,10 +82,6 @@ function Robot6DOF(; name, kwargs...)
8382
# [description = "Distance from last flange to load mass"]
8483
# g = 9.81, [description = "Gravity acceleration"]
8584
# refStartTime = 0, [description = "Start time of reference motion"]
86-
# refSwingTime = 0.5,
87-
# [
88-
# description = "Additional time after reference motion is in rest before simulation is stopped",
89-
# ]
9085
# q01 = -60, [description = "Start angle of axis 1"]
9186
# q02 = 20, [description = "Start angle of axis 2"]
9287
# q03 = 90, [description = "Start angle of axis 3"]
@@ -99,9 +94,9 @@ function Robot6DOF(; name, kwargs...)
9994
# q14 = 45, [description = "End angle of axis 4"]
10095
# q15 = 110, [description = "End angle of axis 5"]
10196
# q16 = 45, [description = "End angle of axis 6"]
102-
# refSpeedMax[1:6] = [3, 1.5, 5, 3.1, 3.1, 4.1],
97+
# qd_max[1:6] = [3, 1.5, 5, 3.1, 3.1, 4.1],
10398
# [description = "Maximum reference speeds of all joints"]
104-
# refAccMax[1:6] = [15, 15, 15, 60, 60, 60],
99+
# qdd_max[1:6] = [15, 15, 15, 60, 60, 60],
105100
# [description = "Maximum reference accelerations of all joints"]
106101
# kp1 = 5, [description = "Gain of position controller"]
107102
# ks1 = 0.5, [description = "Gain of speed controller"]
@@ -128,8 +123,8 @@ function Robot6DOF(; name, kwargs...)
128123
g = 9.81 #, [description = "Gravity acceleration"]
129124
refStartTime = 0 #, [description = "Start time of reference motion"]
130125
refSwingTime = 0.5 #, [description = "Additional time after reference motion is in rest before simulation is stopped", ]
131-
refSpeedMax = [3, 1.5, 5, 3.1, 3.1, 4.1] # [description = "Maximum reference speeds of all joints"]
132-
refAccMax = [15, 15, 15, 60, 60, 60] # [description = "Maximum reference accelerations of all joints"]
126+
qd_max = [3, 1.5, 5, 3.1, 3.1, 4.1] # [description = "Maximum reference speeds of all joints"]
127+
qdd_max = [15, 15, 15, 60, 60, 60] # [description = "Maximum reference accelerations of all joints"]
133128
kp1 = 5 #, [description = "Gain of position controller"]
134129
ks1 = 0.5 #, [description = "Gain of speed controller"]
135130
Ts1 = 0.05 #, [description = "Time constant of integrator of speed controller"]
@@ -183,10 +178,8 @@ function Robot6DOF(; name, kwargs...)
183178
q15,
184179
q16,
185180
],
186-
speed_max = refSpeedMax,
187-
acc_max = refAccMax,
188-
startTime = refStartTime,
189-
swingtime = refSwingTime,
181+
speed_max = qd_max,
182+
acc_max = qdd_max,
190183
kwargs...)
191184

192185
axis1 = AxisType1(w = 4590,

‎src/robot/path_planning.jl

Lines changed: 2 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -4,22 +4,7 @@ using ModelingToolkitStandardLibrary.Blocks: RealInput, RealOutput
44
include("ptp.jl")
55

66
"Generate reference angles for specified kinematic movement"
7-
function PathPlanning1(; name, q0deg = 0, q1deg = 1, time = 0:0.01:10,
8-
swingtime = 0.5, speed_max=1, acc_max=1, kwargs...)
9-
# @parameters begin
10-
# q0deg = q0deg, [description = "Start angle"]
11-
# q1deg = q1deg, [description = "End angle"]
12-
# speed_max = speed_max, [description = "Maximum axis speed"]
13-
# acc_max = acc_max, [description = "Maximum axis acceleration"]
14-
# startTime = startTime, [description = "Start time of movement"]
15-
# swingtime = swingtime,
16-
# [
17-
# description = "Additional time after reference motion is in rest before simulation is stopped",
18-
# ]
19-
# angleBeg = deg2rad(q0deg), [description = "Start angles"]
20-
# angleEnd = deg2rad(q1deg), [description = "End angles"]
21-
# end
22-
7+
function PathPlanning1(; name, q0deg = 0, q1deg = 1, time = 0:0.01:10, speed_max=1, acc_max=1, kwargs...)
238
systems = @named begin
249
controlBus = ControlBus()
2510
path = KinematicPTP(; q1 = deg2rad.(q1deg),
@@ -29,8 +14,6 @@ function PathPlanning1(; name, q0deg = 0, q1deg = 1, time = 0:0.01:10,
2914
# startTime = startTime,
3015
q0 = deg2rad.(q0deg), kwargs...)
3116
pathToAxis1 = PathToAxisControlBus(nAxis = 1, axisUsed = 1)
32-
# terminateSimulation = TerminateSimulation(condition = time >=
33-
# path.endTime + swingtime)
3417
end
3518

3619
eqs = [connect(path.q, pathToAxis1.q)
@@ -44,38 +27,21 @@ end
4427
function PathPlanning6(; name, naxis = 6, q0deg = zeros(naxis),
4528
q1deg = ones(naxis), time = 0:0.01:4,
4629
speed_max = fill(3, naxis),
47-
acc_max = fill(2.5, naxis), startTime = 0, swingtime = 0.5, kwargs...)
48-
# @parameters begin
49-
# naxis = naxis, [description = "Number of driven axis"]
50-
# q0deg[1:naxis] = q0deg, [description = "Start angles"]
51-
# q1deg[1:naxis] = q1deg, [description = "End angles"]
52-
# # speed_max[1:naxis] = speed_max, [description = "Maximum axis speed"]
53-
# # acc_max[1:naxis] = acc_max, [description = "Maximum axis acceleration"]
54-
# # startTime = startTime, [description = "Start time of movement"]
55-
# swingtime = swingtime,
56-
# [
57-
# description = "Additional time after reference motion is in rest before simulation is stopped",
58-
# ]
59-
# angleBeg[1:6] = deg2rad.(q0deg), [description = "Start angles"]
60-
# angleEnd[1:6] = deg2rad.(q1deg), [description = "End angles"]
61-
# end
30+
acc_max = fill(2.5, naxis), kwargs...)
6231

6332
systems = @named begin
6433
controlBus = ControlBus()
6534
path = KinematicPTP(; q1 = deg2rad.(q1deg),
6635
time,
6736
qd_max = speed_max,
6837
qdd_max = acc_max,
69-
# startTime = startTime,
7038
q0 = deg2rad.(q0deg), kwargs...)
7139
pathToAxis1 = PathToAxisControlBus(nAxis = naxis, axisUsed = 1)
7240
pathToAxis2 = PathToAxisControlBus(nAxis = naxis, axisUsed = 2)
7341
pathToAxis3 = PathToAxisControlBus(nAxis = naxis, axisUsed = 3)
7442
pathToAxis4 = PathToAxisControlBus(nAxis = naxis, axisUsed = 4)
7543
pathToAxis5 = PathToAxisControlBus(nAxis = naxis, axisUsed = 5)
7644
pathToAxis6 = PathToAxisControlBus(nAxis = naxis, axisUsed = 6)
77-
# terminateSimulation = TerminateSimulation(condition = time >=
78-
# path.endTime + swingtime)
7945
end
8046

8147
eqs = [connect(path.q, pathToAxis1.q)

‎src/robot/robot_components.jl

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -138,6 +138,7 @@ function AxisType2(; name, kp = 10, ks = 1, Ts = 0.01, k = 1.1616, w = 4590, D =
138138
ODESystem(eqs, t; name, systems)
139139
end
140140

141+
141142
function AxisType1(; name, c = 43, cd = 0.005, kp = 10, ks = 1, Ts = 0.01, k = 1.1616, w = 4590, D = 0.6,
142143
J = 0.0013, ratio = -105, Rv0 = 0.4, Rv1 = 0.13 / 160, peak = 1)
143144
# @parameters begin

0 commit comments

Comments
 (0)