add trajplanning to docs

docs/src/examples/robot.md

@@ -12,7 +12,7 @@ using Test
 
 t = Multibody.t
 D = Differential(t)
-@named robot = Multibody.Robot6DOF(trivial=false)
+@named robot = Multibody.Robot6DOF()
 robot = complete(robot)
 
 length(equations(robot))

docs/src/index.md

@@ -88,4 +88,22 @@ Pages = ["orientation.jl"]
 ```@autodocs
 Modules = [Multibody]
 Pages = ["interfaces.jl"]
+```
+
+## Trajectory planning
+Two methods of planning trajectories are available
+- [`point_to_point`](@ref): Generate a minimum-time point-to-point trajectory with specified start and endpoints, not exceeding specified speed and acceleration limits.
+- [`traj5`](@ref): Generate a 5:th order polynomial trajectory with specified start and end points. Additionally allows specification of start and end values for velocity and acceleration.
+
+Components that make use of these trajectory generators is provided:
+- [`KinematicPTP`](@ref)
+- [`Kinematic5`](@ref)
+
+These both have output connectors of type `RealOutput` called `q, qd, qdd` for positions, velocities and accelerations.
+
+See [Industrial robot](@ref) for an example making use of the [`point_to_point`](@ref) planner.
+
+```@autodocs
+Modules = [Multibody]
+Pages = ["path_planning.jl"]
 ```

ext/Render.jl

@@ -248,12 +248,14 @@ end
 
 function render!(scene, ::typeof(Spherical), sys, sol, t)
  vars = get_fun(sol, collect(sys.frame_a.r_0))
+ color = get_color(sys, sol, :yellow)
+ radius = sol(sol.t[1], idxs=sys.radius)
  thing = @lift begin
  coords = vars($t)
  point = Point3f(coords)
- Sphere(point, 0.1)
+ Sphere(point, radius)
  end
- mesh!(scene, thing, color=:yellow)
+ mesh!(scene, thing; color)
  true
 end
 

src/Multibody.jl

@@ -144,7 +144,7 @@ include("forces.jl")
 export PartialCutForceBaseSensor, BasicCutForce, BasicCutTorque, CutTorque, CutForce
 include("sensors.jl")
 
-export point_to_point
+export point_to_point, traj5, KinematicPTP, Kinematic5
 include("robot/path_planning.jl")
 include("robot/robot_components.jl")
 include("robot/FullRobot.jl")

src/robot/path_planning.jl

@@ -186,67 +186,83 @@ end
 # plot!(t, centraldiff(centraldiff(q)), sp = 3)
 
 """
- KinematicPTP(; time, name, q0 = 0, q1 = 1, qd0 = 0, qd1 = 0, qdd0 = 0, qdd1 = 0)
+ KinematicPTP(; time, name, q0 = 0, q1 = 1, qd_max=1, qdd_max=1)
 
-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, q0 = 0, q1 = 1, qd0 = 0, qd1 = 0,
- qdd0 = 0, qdd1 = 0, trivial = false, qd_max=1, qdd_max=1)
- nout = max(length(q0), length(q1), length(qd1), length(qdd1))
-
- # @parameters begin
- # q0 = q0, [description = "Start position"]
- # q1 = q1, [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_q0[1:nout] = ones(nout) .* q0
- # p_q1[1:nout] = ones(nout) .* q1
- # p_qd_max[1:nout] = ones(nout) .* qd_max
- # p_qdd_max[1:nout] = ones(nout) .* qdd_max
- # p_deltaq[1:nout] = p_q1 - p_q0
- # end
+A component emitting a trajectory created by the [`point_to_point`](@ref) trajectory generator.
 
+# Arguments
+- `time`: Time vector, e.g., `0:0.01:10`
+- `name`: Name of the component
+- `q0`: Initial position
+- `q1`: Final position
+- `qd_max`: Maximum velocity
+- `qdd_max`: Maximum acceleration
 
+# Outputs
+- `q`: Position
+- `qd`: Velocity
+- `qdd`: Acceleration
+"""
+function KinematicPTP(; time, name, q0 = 0, q1 = 1, qd_max=1, qdd_max=1)
+ nout = max(length(q0), length(q1))
 
  systems = @named begin
  q = RealOutput(; nout)
  qd = RealOutput(; nout)
  qdd = RealOutput(; nout)
- # moving = BooleanOutput(; nout)
  end
 
-
- startTime = time[1]
- time0 = time .- startTime # traj5 wants time vector to start at 0
- if !trivial
- q_vec, qd_vec, qdd_vec = point_to_point(time; q0 = q0, q1 = q1, qd_max, qdd_max)
- end
+ q_vec, qd_vec, qdd_vec = point_to_point(time; q0 = q0, q1 = q1, qd_max, qdd_max)
 
  interp_eqs = map(1:nout) do i
- if trivial
- _q, _qd, _qdd = traj5(t, time[end]; q0 = q0[i], q1 = q1[i],
- q̇0 = zero(q0[i]),
- q̇1 = zero(q0[i]),
- q̈0 = zero(q0[i]),
- q̈1 = zero(q0[i]))
- [q.u[i] ~ _q 
- qd.u[i] ~ _qd
- qdd.u[i] ~ _qdd]
- else
- # q_vec, qd_vec, qdd_vec = PTP(time; q0 = q0[i], q1 = q1[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)]
- end
+ 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)]
  end
  eqs = reduce(vcat, interp_eqs)
+ ODESystem(eqs, t; name, systems)
+end
+
+
+"""
+ Kinematic5(; time, name, q0 = 0, q1 = 1, qd0 = 0, qd1 = 0, qdd0 = 0, qdd1 = 0)
+
+A component emitting a 5:th order polynomial trajectory created using [`traj5`](@ref). `traj5` is 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.
 
- # push!(eqs, moving.u ~ (time[1] < t < time[end]))
+# Arguments
+- `time`: Time vector, e.g., `0:0.01:10`
+- `name`: Name of the component
 
+# Outputs
+- `q`: Position
+- `qd`: Velocity
+- `qdd`: Acceleration
+"""
+function Kinematic5(; time, name, q0 = 0, q1 = 1, qd0 = 0, qd1 = 0,
+ qdd0 = 0, qdd1 = 0)
+ nout = max(length(q0), length(q1), length(qd1), length(qdd1))
+
+ systems = @named begin
+ q = RealOutput(; nout)
+ qd = RealOutput(; nout)
+ qdd = RealOutput(; nout)
+ end
+
+ interp_eqs = map(1:nout) do i
+ _q, _qd, _qdd = traj5(t, time[end]; q0 = q0[i], q1 = q1[i],
+ q̇0 = zero(q0[i]),
+ q̇1 = zero(q0[i]),
+ q̈0 = zero(q0[i]),
+ q̈1 = zero(q0[i]))
+ [q.u[i] ~ _q 
+ qd.u[i] ~ _qd
+ qdd.u[i] ~ _qdd]
+
+ end
+ eqs = reduce(vcat, interp_eqs)
  ODESystem(eqs, t; name, systems)
 end
 

test/test_robot.jl

@@ -201,7 +201,7 @@ u = cm.axis2.controller.PI.ctr_output.u
 
 @testset "one axis" begin
  @info "Testing one axis"
- @named oneaxis = RobotAxis(trivial=false)
+ @named oneaxis = RobotAxis()
  oneaxis = complete(oneaxis)
  op = Dict([
  oneaxis.axis.flange.phi => 0
@@ -257,7 +257,7 @@ end
 @testset "full robot" begin
  @info "Testing full robot"
 
- @named robot = Robot6DOF(trivial=false)
+ @named robot = Robot6DOF()
  robot = complete(robot)
 
  @time "full robot" begin