diff --git a/docs/src/examples/robot.md b/docs/src/examples/robot.md index 79439541..dc9e690f 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=false) +@named robot = Multibody.Robot6DOF() robot = complete(robot) length(equations(robot)) diff --git a/docs/src/index.md b/docs/src/index.md index 376b6435..c5f7f6b6 100644 --- a/docs/src/index.md +++ b/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"] ``` \ No newline at end of file diff --git a/ext/Render.jl b/ext/Render.jl index 733f2f8a..501c77ac 100644 --- a/ext/Render.jl +++ b/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 diff --git a/src/Multibody.jl b/src/Multibody.jl index 0f4126dc..40e029e5 100644 --- a/src/Multibody.jl +++ b/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") diff --git a/src/robot/path_planning.jl b/src/robot/path_planning.jl index 155aadcf..476951a2 100644 --- a/src/robot/path_planning.jl +++ b/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 diff --git a/test/test_robot.jl b/test/test_robot.jl index 50ec23d5..f1c1ff08 100644 --- a/test/test_robot.jl +++ b/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