├── .gitattributes ├── Position_SetPoint ├── JLT_Pos_Setpoint.m ├── computeT2_2.m ├── computeT3.m ├── computeT6_2.m ├── computeT7.m ├── 位置设定情况的三阶JLT算法流程图.jpg ├── computeT4.m ├── binarySearchUpdateDurationsGivenTotalTime.m ├── binarySearchUpdateDurationsMinimizeTotalTime.m ├── evaluatePoly.m ├── computeDirection.m ├── saturateT1ForAccel.m ├── saturateT5ForAccel.m ├── computeVelAtZeroAcc.m ├── updateTrajCruise.m ├── updateDurationsMinimizeTotalTimeDecleration.m ├── updateDurationsMinimizeTotalTimeAccleration.m ├── updateTrajDec.m ├── updateTrajAcc.m ├── computeT5_1.m ├── computeT1_1.m ├── computeT2_1.m ├── computeT6_1.m ├── computeT5_2.m ├── computeT1_2.m ├── timeSynchronization.m ├── updateTraj.m └── updateDurations_Position_Setpoint.m ├── Velocity_SetPoint ├── JLT_Vel_Setpoint.m ├── computeT2_2.m ├── computeT3.m ├── 速度设定情况的三阶JLT算法流程图.jpg ├── evaluatePoly.m ├── computeDirection.m ├── saturateT1ForAccel.m ├── computeVelAtZeroAcc.m ├── updateDurationsGivenTotalTime.m ├── updateDurationsMinimizeTotalTime.m ├── updateDurations_Velocity_Setpoint.m ├── computeT1_1.m ├── timeSynchronization.m ├── updateTraj.m ├── computeT2_1.m └── computeT1_2.m └── README.md /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /Position_SetPoint/JLT_Pos_Setpoint.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DistantUtopia/JerkLimitedTrajectory/HEAD/Position_SetPoint/JLT_Pos_Setpoint.m -------------------------------------------------------------------------------- /Position_SetPoint/computeT2_2.m: -------------------------------------------------------------------------------- 1 | function [ T2 ] = computeT2_2( T123, T1, T3 ) 2 | T2 = T123 - T1 - T3; 3 | T2 = max(T2, 0); 4 | end 5 | 6 | -------------------------------------------------------------------------------- /Position_SetPoint/computeT3.m: -------------------------------------------------------------------------------- 1 | function [ T3 ] = computeT3( T1, a0, j_max ) 2 | T3 = a0 / j_max + T1; 3 | T3 = max(T3, 0); 4 | end 5 | 6 | -------------------------------------------------------------------------------- /Position_SetPoint/computeT6_2.m: -------------------------------------------------------------------------------- 1 | function [ T6 ] = computeT6_2( T567, T5, T7 ) 2 | T6 = T567 - T5 - T7; 3 | T6 = max(T6, 0); 4 | end 5 | 6 | -------------------------------------------------------------------------------- /Position_SetPoint/computeT7.m: -------------------------------------------------------------------------------- 1 | function [ T7 ] = computeT7( T5, a0, j_max ) 2 | T7 = a0 / j_max + T5; 3 | T7 = max(T7, 0); 4 | end 5 | 6 | -------------------------------------------------------------------------------- /Velocity_SetPoint/JLT_Vel_Setpoint.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DistantUtopia/JerkLimitedTrajectory/HEAD/Velocity_SetPoint/JLT_Vel_Setpoint.m -------------------------------------------------------------------------------- /Velocity_SetPoint/computeT2_2.m: -------------------------------------------------------------------------------- 1 | function [ T2 ] = computeT2_2( T123, T1, T3 ) 2 | T2 = T123 - T1 - T3; 3 | T2 = max(T2, 0); 4 | end 5 | 6 | -------------------------------------------------------------------------------- /Velocity_SetPoint/computeT3.m: -------------------------------------------------------------------------------- 1 | function [ T3 ] = computeT3( T1, a0, j_max ) 2 | T3 = a0 / j_max + T1; 3 | T3 = max(T3, 0); 4 | end 5 | 6 | -------------------------------------------------------------------------------- /Position_SetPoint/位置设定情况的三阶JLT算法流程图.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DistantUtopia/JerkLimitedTrajectory/HEAD/Position_SetPoint/位置设定情况的三阶JLT算法流程图.jpg -------------------------------------------------------------------------------- /Velocity_SetPoint/速度设定情况的三阶JLT算法流程图.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DistantUtopia/JerkLimitedTrajectory/HEAD/Velocity_SetPoint/速度设定情况的三阶JLT算法流程图.jpg -------------------------------------------------------------------------------- /Position_SetPoint/computeT4.m: -------------------------------------------------------------------------------- 1 | function [ T4 ] = computeT4( delt_p_acc, delt_p_dec, v_i, delt_p ) 2 | T4 = (delt_p - delt_p_acc - delt_p_dec) / v_i; 3 | T4 = max(T4, 0); 4 | end -------------------------------------------------------------------------------- /Position_SetPoint/binarySearchUpdateDurationsGivenTotalTime.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DistantUtopia/JerkLimitedTrajectory/HEAD/Position_SetPoint/binarySearchUpdateDurationsGivenTotalTime.m -------------------------------------------------------------------------------- /Position_SetPoint/binarySearchUpdateDurationsMinimizeTotalTime.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DistantUtopia/JerkLimitedTrajectory/HEAD/Position_SetPoint/binarySearchUpdateDurationsMinimizeTotalTime.m -------------------------------------------------------------------------------- /Position_SetPoint/evaluatePoly.m: -------------------------------------------------------------------------------- 1 | function [ p, v, a, j ] = evaluatePoly( jerk, a0, v0, x0, t, d ) 2 | jt = d * jerk; 3 | t2 = t * t; 4 | t3 = t2 * t; 5 | 6 | j = jt; 7 | a = a0 + jt * t; 8 | v = v0 + a0 * t + 0.5 * jt * t2; 9 | p = x0 + v0 * t + 0.5 * a0 * t2 + 1 / 6 * jt * t3; 10 | 11 | end 12 | 13 | -------------------------------------------------------------------------------- /Velocity_SetPoint/evaluatePoly.m: -------------------------------------------------------------------------------- 1 | function [ p, v, a, j ] = evaluatePoly( jerk, a0, v0, x0, t, d ) 2 | jt = d * jerk; 3 | t2 = t * t; 4 | t3 = t2 * t; 5 | 6 | j = jt; 7 | a = a0 + jt * t; 8 | v = v0 + a0 * t + 0.5 * jt * t2; 9 | p = x0 + v0 * t + 0.5 * a0 * t2 + 1 / 6 * jt * t3; 10 | 11 | end 12 | 13 | -------------------------------------------------------------------------------- /Position_SetPoint/computeDirection.m: -------------------------------------------------------------------------------- 1 | function [ direction ] = computeDirection( vel_sp, state_a, state_v, max_jerk ) 2 | vel_zero_acc = computeVelAtZeroAcc(state_v, state_a, max_jerk); 3 | 4 | direction = sign(vel_sp - vel_zero_acc); 5 | 6 | if direction == 0 7 | direction = sign(state_a); 8 | end 9 | end 10 | 11 | -------------------------------------------------------------------------------- /Velocity_SetPoint/computeDirection.m: -------------------------------------------------------------------------------- 1 | function [ direction ] = computeDirection( vel_sp, state_a, state_v, max_jerk ) 2 | vel_zero_acc = computeVelAtZeroAcc(state_v, state_a, max_jerk); 3 | 4 | direction = sign(vel_sp - vel_zero_acc); 5 | 6 | if direction == 0 7 | direction = sign(state_a); 8 | end 9 | end 10 | 11 | -------------------------------------------------------------------------------- /Position_SetPoint/saturateT1ForAccel.m: -------------------------------------------------------------------------------- 1 | function [ T1_new ] = saturateT1ForAccel( a0, j_max, T1, a_max ) 2 | accel_T1 = a0 + j_max * T1; 3 | T1_new = T1; 4 | 5 | if (accel_T1 > a_max) 6 | T1_new = (a_max - a0) / j_max; 7 | else 8 | if (accel_T1 < -a_max) 9 | T1_new = (-a_max - a0) / j_max; 10 | end 11 | end 12 | end 13 | 14 | -------------------------------------------------------------------------------- /Position_SetPoint/saturateT5ForAccel.m: -------------------------------------------------------------------------------- 1 | function [ T5_new ] = saturateT5ForAccel( a0, j_max, T5, a_max ) 2 | accel_T5 = a0 + j_max * T5; 3 | T5_new = T5; 4 | 5 | if (accel_T5 > a_max) 6 | T5_new = (a_max - a0) / j_max; 7 | else 8 | if (accel_T5 < -a_max) 9 | T5_new = (-a_max - a0) / j_max; 10 | end 11 | end 12 | end 13 | 14 | -------------------------------------------------------------------------------- /Velocity_SetPoint/saturateT1ForAccel.m: -------------------------------------------------------------------------------- 1 | function [ T1_new ] = saturateT1ForAccel( a0, j_max, T1, a_max ) 2 | accel_T1 = a0 + j_max * T1; 3 | T1_new = T1; 4 | 5 | if (accel_T1 > a_max) 6 | T1_new = (a_max - a0) / j_max; 7 | else 8 | if (accel_T1 < -a_max) 9 | T1_new = (-a_max - a0) / j_max; 10 | end 11 | end 12 | end 13 | 14 | -------------------------------------------------------------------------------- /Position_SetPoint/computeVelAtZeroAcc.m: -------------------------------------------------------------------------------- 1 | function [ vel_zero_acc ] = computeVelAtZeroAcc( state_v, state_a, max_jerk ) 2 | vel_zero_acc = state_v; 3 | if abs(state_a) > 0 4 | j_zero_acc = - sign(state_a) * max_jerk; 5 | t_zero_acc = - state_a / j_zero_acc; 6 | vel_zero_acc = state_v + state_a * t_zero_acc + 0.5 * j_zero_acc * t_zero_acc * t_zero_acc; 7 | end 8 | end 9 | 10 | -------------------------------------------------------------------------------- /Velocity_SetPoint/computeVelAtZeroAcc.m: -------------------------------------------------------------------------------- 1 | function [ vel_zero_acc ] = computeVelAtZeroAcc( state_v, state_a, max_jerk ) 2 | vel_zero_acc = state_v; 3 | if abs(state_a) > 1.192093e-007 4 | j_zero_acc = - sign(state_a) * max_jerk; 5 | t_zero_acc = - state_a / j_zero_acc; 6 | vel_zero_acc = state_v + state_a * t_zero_acc + 0.5 * j_zero_acc * t_zero_acc * t_zero_acc; 7 | end 8 | end 9 | 10 | -------------------------------------------------------------------------------- /Position_SetPoint/updateTrajCruise.m: -------------------------------------------------------------------------------- 1 | function [ p, v, a, j ] = updateTrajCruise( timestamp, T4, a0, v0, p0, direction, max_jerk ) 2 | t_remain = timestamp; 3 | 4 | t4 = min(t_remain, T4); 5 | [p, v, a, j] = evaluatePoly( max_jerk, a0, v0, p0, t4, direction ); 6 | t_remain = t_remain - t4; 7 | 8 | if (t_remain > 0) 9 | [p, v, a, j] = evaluatePoly( 0, 0, v, p, t_remain, 0 ); 10 | end 11 | end 12 | 13 | -------------------------------------------------------------------------------- /Velocity_SetPoint/updateDurationsGivenTotalTime.m: -------------------------------------------------------------------------------- 1 | function [ T1, T2, T3 ] = updateDurationsGivenTotalTime( T123, direction, max_jerk, max_accel, vel_sp, state_a, state_v ) 2 | jerk_max_T1 = direction * max_jerk; 3 | delta_v = vel_sp - state_v; 4 | 5 | T1 = computeT1_2(T123, state_a, delta_v, jerk_max_T1, max_accel); 6 | T3 = computeT3(T1, state_a, jerk_max_T1); 7 | T2 = computeT2_2(T123, T1, T3); 8 | 9 | end 10 | 11 | -------------------------------------------------------------------------------- /Velocity_SetPoint/updateDurationsMinimizeTotalTime.m: -------------------------------------------------------------------------------- 1 | function [ T1, T2, T3 ] = updateDurationsMinimizeTotalTime( direction, max_jerk, max_accel, vel_sp, state_a, state_v ) 2 | jerk_max_T1 = direction * max_jerk; 3 | delta_v = vel_sp - state_v; 4 | 5 | T1 = computeT1_1(state_a, delta_v, jerk_max_T1, max_accel); 6 | T3 = computeT3(T1, state_a, jerk_max_T1); 7 | T2 = computeT2_1(T1, T3, state_a, delta_v, jerk_max_T1); 8 | 9 | end 10 | 11 | -------------------------------------------------------------------------------- /Position_SetPoint/updateDurationsMinimizeTotalTimeDecleration.m: -------------------------------------------------------------------------------- 1 | function [ T5, T6, T7, delt_p_dec, p, v, a, j ] = updateDurationsMinimizeTotalTimeDecleration( direction, max_jerk, max_accel, vel_sp, state_a, state_v, state_p ) 2 | jerk_max_T5 = direction * max_jerk; 3 | delta_v = vel_sp - state_v; 4 | 5 | T5 = computeT5_1(state_a, delta_v, jerk_max_T5, max_accel); 6 | T7 = computeT7(T5, state_a, jerk_max_T5); 7 | T6 = computeT6_1(T5, T7, state_a, delta_v, jerk_max_T5); 8 | 9 | delt_p_dec = 0; 10 | [p, v, a, j] = updateTrajDec( T5 + T6 + T7, T5, T6, T7, state_a, state_v, state_p, direction, max_jerk ); 11 | delt_p_dec = p - state_p; 12 | end 13 | -------------------------------------------------------------------------------- /Position_SetPoint/updateDurationsMinimizeTotalTimeAccleration.m: -------------------------------------------------------------------------------- 1 | function [ T1, T2, T3, delt_p_acc, p, v, a, j ] = updateDurationsMinimizeTotalTimeAccleration( direction, max_jerk, max_accel, vel_sp, state_a, state_v, state_p ) 2 | jerk_max_T1 = direction * max_jerk; 3 | delta_v = vel_sp - state_v; 4 | 5 | T1 = computeT1_1(state_a, delta_v, jerk_max_T1, max_accel); 6 | T3 = computeT3(T1, state_a, jerk_max_T1); 7 | T2 = computeT2_1(T1, T3, state_a, delta_v, jerk_max_T1); 8 | 9 | delt_p_acc = 0; 10 | [p, v, a, j] = updateTrajAcc( T1 + T2 + T3, T1, T2, T3, state_a, state_v, state_p, direction, max_jerk ); 11 | delt_p_acc = p - state_p; 12 | end 13 | 14 | -------------------------------------------------------------------------------- /Velocity_SetPoint/updateDurations_Velocity_Setpoint.m: -------------------------------------------------------------------------------- 1 | function [ T1, T2, T3, direction ] = updateDurations_Velocity_Setpoint( vel_sp, state_a, state_v, max_jerk, max_accel, max_velocity ) 2 | if vel_sp > max_velocity 3 | vel_sp = max_velocity; 4 | end 5 | if vel_sp < - max_velocity 6 | vel_sp = - max_velocity; 7 | end 8 | 9 | direction = computeDirection(vel_sp, state_a, state_v, max_jerk); 10 | 11 | if direction ~= 0 12 | [T1, T2, T3] = updateDurationsMinimizeTotalTime(direction, max_jerk, max_accel, vel_sp, state_a, state_v); 13 | else 14 | T1 = 0; 15 | T2 = 0; 16 | T3 = 0; 17 | end 18 | end 19 | 20 | -------------------------------------------------------------------------------- /Position_SetPoint/updateTrajDec.m: -------------------------------------------------------------------------------- 1 | function [ p, v, a, j ] = updateTrajDec( timestamp, T5, T6, T7, a0, v0, p0, direction, max_jerk ) 2 | t_remain = timestamp; 3 | 4 | t5 = min(t_remain, T5); 5 | [p, v, a, j] = evaluatePoly(max_jerk, a0, v0, p0, t5, direction ); 6 | t_remain = t_remain - t5; 7 | 8 | if (t_remain > 0) 9 | t6 = min(t_remain, T6); 10 | [p, v, a, j] = evaluatePoly(0, a, v, p, t6, 0); 11 | t_remain = t_remain - t6; 12 | end 13 | 14 | if (t_remain > 0) 15 | t7 = min(t_remain, T7); 16 | [p, v, a, j] = evaluatePoly(max_jerk, a, v, p, t7, -direction); 17 | t_remain = t_remain - t7; 18 | end 19 | 20 | if (t_remain > 0) 21 | [p, v, a, j] = evaluatePoly(0, 0, v, p, t_remain, 0); 22 | end 23 | end 24 | -------------------------------------------------------------------------------- /Position_SetPoint/updateTrajAcc.m: -------------------------------------------------------------------------------- 1 | function [ p, v, a, j ] = updateTrajAcc( timestamp, T1, T2, T3, a0, v0, p0, direction, max_jerk ) 2 | t_remain = timestamp; 3 | 4 | t1 = min(t_remain, T1); 5 | [p, v, a, j] = evaluatePoly(max_jerk, a0, v0, p0, t1, direction ); 6 | t_remain = t_remain - t1; 7 | 8 | if (t_remain > 0) 9 | t2 = min(t_remain, T2); 10 | [p, v, a, j] = evaluatePoly(0, a, v, p, t2, 0); 11 | t_remain = t_remain - t2; 12 | end 13 | 14 | if (t_remain > 0) 15 | t3 = min(t_remain, T3); 16 | [p, v, a, j] = evaluatePoly(max_jerk, a, v, p, t3, -direction); 17 | t_remain = t_remain - t3; 18 | end 19 | 20 | if (t_remain > 0) 21 | [p, v, a, j] = evaluatePoly(0, 0, v, p, t_remain, 0); 22 | end 23 | end 24 | 25 | -------------------------------------------------------------------------------- /Position_SetPoint/computeT5_1.m: -------------------------------------------------------------------------------- 1 | function [ T5 ] = computeT5_1( a0, v3, j_max, a_max ) 2 | delta = 2 * a0 * a0 + 4 * j_max * v3; 3 | 4 | if delta < 0 5 | T5 = 0; 6 | return; 7 | end 8 | 9 | sqrt_delta = sqrt(delta); 10 | T5_plus = (-a0 + 0.5 * sqrt_delta) / j_max; 11 | T5_minus = (-a0 - 0.5 * sqrt_delta) / j_max; 12 | 13 | T7_plus = a0 / j_max + T5_plus; 14 | T7_minus = a0 / j_max + T5_minus; 15 | 16 | T5 = 0; 17 | 18 | if (T5_plus >= 0 && T7_plus >= 0) 19 | T5 = T5_plus; 20 | else 21 | if (T5_minus >= 0 && T7_minus >= 0) 22 | T5 = T5_minus; 23 | else 24 | 25 | end 26 | end 27 | 28 | T5 = saturateT5ForAccel(a0, j_max, T5, a_max); 29 | 30 | T5 = max(T5, 0); 31 | end 32 | 33 | -------------------------------------------------------------------------------- /Position_SetPoint/computeT1_1.m: -------------------------------------------------------------------------------- 1 | function [ T1 ] = computeT1_1( a0, v3, j_max, a_max ) 2 | delta = 2 * a0 * a0 + 4 * j_max * v3; 3 | 4 | if delta < 0 5 | T1 = 0; 6 | return; 7 | end 8 | 9 | sqrt_delta = sqrt(delta); 10 | T1_plus = (-a0 + 0.5 * sqrt_delta) / j_max; 11 | T1_minus = (-a0 - 0.5 * sqrt_delta) / j_max; 12 | 13 | T3_plus = a0 / j_max + T1_plus; 14 | T3_minus = a0 / j_max + T1_minus; 15 | 16 | T1 = 0; 17 | 18 | if (T1_plus >= 0 && T3_plus >= 0) 19 | T1 = T1_plus; 20 | else 21 | if (T1_minus >= 0 && T3_minus >= 0) 22 | T1 = T1_minus; 23 | else 24 | 25 | end 26 | end 27 | 28 | T1 = saturateT1ForAccel(a0, j_max, T1, a_max); 29 | 30 | T1 = max(T1, 0); 31 | 32 | end 33 | 34 | -------------------------------------------------------------------------------- /Velocity_SetPoint/computeT1_1.m: -------------------------------------------------------------------------------- 1 | function [ T1 ] = computeT1_1( a0, v3, j_max, a_max ) 2 | delta = 2 * a0 * a0 + 4 * j_max * v3; 3 | 4 | if delta < 0 5 | T1 = 0; 6 | return; 7 | end 8 | 9 | sqrt_delta = sqrt(delta); 10 | T1_plus = (-a0 + 0.5 * sqrt_delta) / j_max; 11 | T1_minus = (-a0 - 0.5 * sqrt_delta) / j_max; 12 | 13 | T3_plus = a0 / j_max + T1_plus; 14 | T3_minus = a0 / j_max + T1_minus; 15 | 16 | T1 = 0; 17 | 18 | if (T1_plus >= 0 && T3_plus >= 0) 19 | T1 = T1_plus; 20 | else 21 | if (T1_minus >= 0 && T3_minus >= 0) 22 | T1 = T1_minus; 23 | else 24 | 25 | end 26 | end 27 | 28 | T1 = saturateT1ForAccel(a0, j_max, T1, a_max); 29 | 30 | T1 = max(T1, 0); 31 | 32 | end 33 | 34 | -------------------------------------------------------------------------------- /Velocity_SetPoint/timeSynchronization.m: -------------------------------------------------------------------------------- 1 | function [state, longest_time] = timeSynchronization( state, n_traj ) 2 | desired_time = 0; 3 | longest_traj_index = 0; 4 | longest_time = 0; 5 | for i = 1 : n_traj 6 | T123 = state(i).T1 + state(i).T2 + state(i).T3; 7 | if T123 > desired_time 8 | desired_time = T123; 9 | longest_traj_index = i; 10 | end 11 | end 12 | longest_time = desired_time; 13 | 14 | if desired_time > 1.192093e-007 15 | for i = 1 : n_traj 16 | if i ~= longest_traj_index 17 | [state(i).T1, state(i).T2, state(i).T3] = updateDurationsGivenTotalTime(desired_time, state(i).direction, state(i).j_max, state(i).a_max, state(i).v_sp, state(i).a, state(i).v); 18 | end 19 | end 20 | end 21 | 22 | end 23 | 24 | -------------------------------------------------------------------------------- /Velocity_SetPoint/updateTraj.m: -------------------------------------------------------------------------------- 1 | function [ p, v, a, j, time ] = updateTraj( dt, time_stamp, T1, T2, T3, a0, v0, p0, direction, max_jerk ) 2 | time_stamp = time_stamp + dt; 3 | t_remain = time_stamp; 4 | time = time_stamp; 5 | t1 = min(t_remain, T1); 6 | [p, v, a, j] = evaluatePoly(max_jerk, a0, v0, p0, t1, direction ); 7 | t_remain = t_remain - t1; 8 | 9 | if (t_remain > 0) 10 | t2 = min(t_remain, T2); 11 | [p, v, a, j] = evaluatePoly(0, a, v, p, t2, 0); 12 | t_remain = t_remain - t2; 13 | end 14 | 15 | if (t_remain > 0) 16 | t3 = min(t_remain, T3); 17 | [p, v, a, j] = evaluatePoly(max_jerk, a, v, p, t3, -direction); 18 | t_remain = t_remain - t3; 19 | end 20 | 21 | if (t_remain > 0) 22 | [p, v, a, j] = evaluatePoly(0, 0, v, p, t_remain, 0); 23 | end 24 | 25 | end 26 | 27 | -------------------------------------------------------------------------------- /Position_SetPoint/computeT2_1.m: -------------------------------------------------------------------------------- 1 | function [ T2 ] = computeT2_1( T1, T3, a0, v3, j_max ) 2 | T2 = 0; 3 | den = a0 + j_max * T1; 4 | 5 | if(abs(den) > 0) 6 | T2 = (-0.5 * T1 * T1 * j_max - T1 * T3 * j_max - T1 * a0 + 0.5 * T3 * T3 * j_max - T3 * a0 + v3) / den; 7 | end 8 | 9 | T2 = max(T2, 0); 10 | end 11 | 12 | -------------------------------------------------------------------------------- /Position_SetPoint/computeT6_1.m: -------------------------------------------------------------------------------- 1 | function [ T6 ] = computeT6_1( T5, T7, a0, v3, j_max ) 2 | T6 = 0; 3 | den = a0 + j_max * T5; 4 | 5 | if(abs(den) > 0) 6 | T6 = (-0.5 * T5 * T5 * j_max - T5 * T7 * j_max - T5 * a0 + 0.5 * T7 * T7 * j_max - T7 * a0 + v3) / den; 7 | end 8 | 9 | T6 = max(T6, 0); 10 | end 11 | 12 | -------------------------------------------------------------------------------- /Velocity_SetPoint/computeT2_1.m: -------------------------------------------------------------------------------- 1 | function [ T2 ] = computeT2_1( T1, T3, a0, v3, j_max ) 2 | T2 = 0; 3 | den = a0 + j_max * T1; 4 | 5 | if(abs(den) > 1.192093e-007) 6 | T2 = (-0.5 * T1 * T1 * j_max - T1 * T3 * j_max - T1 * a0 + 0.5 * T3 * T3 * j_max - T3 * a0 + v3) / den; 7 | end 8 | 9 | T2 = max(T2, 0); 10 | end 11 | 12 | -------------------------------------------------------------------------------- /Position_SetPoint/computeT5_2.m: -------------------------------------------------------------------------------- 1 | function [ T5 ] = computeT5_2( T567, a0, v3, j_max, a_max ) 2 | a = -j_max; 3 | b = j_max * T567 - a0; 4 | delta = T567 * T567 * j_max * j_max + 2 * T567 * a0 * j_max - a0 * a0 - 4 * j_max * v3; 5 | 6 | if delta < 0 7 | T5 = 0; 8 | return; 9 | end 10 | 11 | sqrt_delta = sqrt(delta); 12 | denominator_inv = 1 / (2 * a); 13 | T5_plus = max((-b + sqrt_delta) * denominator_inv, 0); 14 | T5_minus = max((-b - sqrt_delta) * denominator_inv, 0); 15 | 16 | T7_plus = a0 / j_max + T5_plus; 17 | T7_minus = a0 / j_max + T5_minus; 18 | 19 | T57_plus = T5_plus + T7_plus; 20 | T57_minus = T5_minus + T7_minus; 21 | 22 | T5 = 0; 23 | 24 | if (T57_plus > T567) 25 | T5 = T5_minus; 26 | else 27 | if (T57_minus > T567) 28 | T5 = T5_plus; 29 | end 30 | end 31 | T5 = saturateT5ForAccel(a0, j_max, T5, a_max); 32 | end 33 | -------------------------------------------------------------------------------- /Position_SetPoint/computeT1_2.m: -------------------------------------------------------------------------------- 1 | function [ T1 ] = computeT1_2( T123, a0, v3, j_max, a_max ) 2 | a = -j_max; 3 | b = j_max * T123 - a0; 4 | delta = T123 * T123 * j_max * j_max + 2 * T123 * a0 * j_max - a0 * a0 - 4 * j_max * v3; 5 | 6 | if delta < 0 7 | T1 = 0; 8 | return; 9 | end 10 | 11 | sqrt_delta = sqrt(delta); 12 | denominator_inv = 1 / (2 * a); 13 | T1_plus = max((-b + sqrt_delta) * denominator_inv, 0); 14 | T1_minus = max((-b - sqrt_delta) * denominator_inv, 0); 15 | 16 | T3_plus = a0 / j_max + T1_plus; 17 | T3_minus = a0 / j_max + T1_minus; 18 | 19 | T13_plus = T1_plus + T3_plus; 20 | T13_minus = T1_minus + T3_minus; 21 | 22 | T1 = 0; 23 | 24 | if (T13_plus > T123) 25 | T1 = T1_minus; 26 | else 27 | if (T13_minus > T123) 28 | T1 = T1_plus; 29 | end 30 | end 31 | T1 = saturateT1ForAccel(a0, j_max, T1, a_max); 32 | end 33 | 34 | -------------------------------------------------------------------------------- /Velocity_SetPoint/computeT1_2.m: -------------------------------------------------------------------------------- 1 | function [ T1 ] = computeT1_2( T123, a0, v3, j_max, a_max ) 2 | a = -j_max; 3 | b = j_max * T123 - a0; 4 | delta = T123 * T123 * j_max * j_max + 2 * T123 * a0 * j_max - a0 * a0 - 4 * j_max * v3; 5 | 6 | if delta < 0 7 | T1 = 0; 8 | return; 9 | end 10 | 11 | sqrt_delta = sqrt(delta); 12 | denominator_inv = 1 / (2 * a); 13 | T1_plus = max((-b + sqrt_delta) * denominator_inv, 0); 14 | T1_minus = max((-b - sqrt_delta) * denominator_inv, 0); 15 | 16 | T3_plus = a0 / j_max + T1_plus; 17 | T3_minus = a0 / j_max + T1_minus; 18 | 19 | T13_plus = T1_plus + T3_plus; 20 | T13_minus = T1_minus + T3_minus; 21 | 22 | T1 = 0; 23 | 24 | if (T13_plus > T123) 25 | T1 = T1_minus; 26 | else 27 | if (T13_minus > T123) 28 | T1 = T1_plus; 29 | end 30 | end 31 | T1 = saturateT1ForAccel(a0, j_max, T1, a_max); 32 | end 33 | 34 | -------------------------------------------------------------------------------- /Position_SetPoint/timeSynchronization.m: -------------------------------------------------------------------------------- 1 | function [state, longest_time] = timeSynchronization( time_stamp, state, n_traj ) 2 | desired_time = 0; 3 | longest_traj_index = 0; 4 | 5 | for i = 1 : n_traj 6 | T1234567 = state(i).T1 + state(i).T2 + state(i).T3 + state(i).T4 + state(i).T5 + state(i).T6 + state(i).T7; 7 | if T1234567 > desired_time 8 | desired_time = T1234567; 9 | longest_traj_index = i; 10 | end 11 | end 12 | longest_time = desired_time; 13 | 14 | if desired_time > 0 15 | for i = 1 : n_traj 16 | if i ~= longest_traj_index 17 | [state(i).T1, state(i).T2, state(i).T3, state(i).T4, state(i).T5, state(i).T6, state(i).T7, state(i).direction_acc, state(i).direction_dec, state(i).v_m ] = ... 18 | binarySearchUpdateDurationsGivenTotalTime( desired_time ,state(i).j_max, state(i).a0, state(i).a_max, state(i).v0, state(i).v_max, state(i).p0, state(i).p_sp ); 19 | end 20 | end 21 | end 22 | 23 | end 24 | 25 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 三阶积分器的JLT项目 2 | ==== 3 | 项目如下： 4 | ------- 5 | >Matlab项目 6 | >>[`Velocity_SetPoint`](https://github.com/DistantUtopia/JerkLimitedTrajectory/tree/master/Velocity_SetPoint) ——速度fixed,位置unfixed,速度、加速度、加加速度为0的情况的Matlab工程 7 | >>[`Position_SetPoint`](https://github.com/DistantUtopia/JerkLimitedTrajectory/blob/master/Position_SetPoint)——位置fixed，速度、加速度、加加速度为0的情况的Matlab工程 8 | 9 | 更新日志 10 | ==== 11 | 12 | 2020-2-13： 13 | ------- 14 | 初次提交位置设定的JLT-Matlab工程 15 | 16 | 2020-2-15： 17 | ------- 18 | 优化速度、位置设定JLT算法，提交对应算法流程图 19 | 20 | 速度设定JLT算法流程图 21 | ------- 22 | ![速度设定JLT算法流程图](https://github.com/DistantUtopia/JerkLimitedTrajectory/blob/master/Velocity_SetPoint/%E9%80%9F%E5%BA%A6%E8%AE%BE%E5%AE%9A%E6%83%85%E5%86%B5%E7%9A%84%E4%B8%89%E9%98%B6JLT%E7%AE%97%E6%B3%95%E6%B5%81%E7%A8%8B%E5%9B%BE.jpg 23 | ) 24 | 25 | 位置设定JLT算法流程图 26 | ------- 27 | ![位置设定JLT算法流程图](https://github.com/DistantUtopia/JerkLimitedTrajectory/blob/master/Position_SetPoint/%E4%BD%8D%E7%BD%AE%E8%AE%BE%E5%AE%9A%E6%83%85%E5%86%B5%E7%9A%84%E4%B8%89%E9%98%B6JLT%E7%AE%97%E6%B3%95%E6%B5%81%E7%A8%8B%E5%9B%BE.jpg 28 | ) 29 | 30 | -------------------------------------------------------------------------------- /Position_SetPoint/updateTraj.m: -------------------------------------------------------------------------------- 1 | function [ p, v, a, j, timestamp ] = updateTraj( dt, timestamp, T1, T2, T3, T4, T5, T6, T7, a0, v0, p0, direction_acc, direction_dec, max_jerk ) 2 | timestamp = timestamp + dt; 3 | p = 0; 4 | a = 0; 5 | v = 0; 6 | j = 0; 7 | if timestamp < (T1 + T2 + T3) 8 | %update accleration trajectory 9 | [ p, v, a, j ] = updateTrajAcc( timestamp, T1, T2, T3, a0, v0, p0, direction_acc, max_jerk ); 10 | end 11 | 12 | if timestamp >= (T1 + T2 + T3) && timestamp < (T1 + T2 + T3 + T4) 13 | %update cruise trajectory 14 | [ p, v, a, j ] = updateTrajAcc( T1 + T2 + T3, T1, T2, T3, a0, v0, p0, direction_acc, max_jerk ); 15 | [ p, v, a, j ] = updateTrajCruise( timestamp - (T1 + T2 + T3), T4, 0, v, p, 0, max_jerk ); 16 | end 17 | if timestamp >= (T1 + T2 + T3 + T4) 18 | %update decleration trajectory 19 | [ p, v, a, j ] = updateTrajAcc( T1 + T2 + T3, T1, T2, T3, a0, v0, p0, direction_acc, max_jerk ); 20 | [ p, v, a, j ] = updateTrajCruise( T4, T4, 0, v, p, 0, max_jerk ); 21 | [ p, v, a, j ] = updateTrajAcc( timestamp - (T1 + T2 + T3 + T4), T5, T6, T7, a, v, p, direction_dec, max_jerk ); 22 | end 23 | end 24 | 25 | -------------------------------------------------------------------------------- /Position_SetPoint/updateDurations_Position_Setpoint.m: -------------------------------------------------------------------------------- 1 | function [ T1, T2, T3, T4, T5, T6, T7, direction_acc, direction_dec, deltP ] = updateDurations_Position_Setpoint( vel_sp, state_a, state_v, max_jerk, max_accel, max_velocity, p0, pt ) 2 | p_acc = 0; v_acc = 0; a_acc = 0; j_acc = 0; delt_p_acc = 0; 3 | p_dec = 0; v_dec = 0; a_dec = 0; j_dec = 0; delt_p_dec = 0; 4 | deltP = 0; 5 | delt_p = pt - p0; 6 | if vel_sp > max_velocity 7 | vel_sp = max_velocity; 8 | end 9 | if vel_sp < - max_velocity 10 | vel_sp = - max_velocity; 11 | end 12 | 13 | direction_acc = computeDirection(vel_sp, state_a, state_v, max_jerk); 14 | if direction_acc ~= 0 15 | [T1, T2, T3, delt_p_acc, p_acc, v_acc, a_acc, j_acc] = updateDurationsMinimizeTotalTimeAccleration( direction_acc, max_jerk, max_accel, vel_sp, state_a, state_v, p0); 16 | else 17 | T1 = 0; 18 | T2 = 0; 19 | T3 = 0; 20 | end 21 | 22 | state_a = 0; 23 | state_v = vel_sp; 24 | vel_sp = 0; 25 | direction_dec = computeDirection(vel_sp, state_a, state_v, max_jerk); 26 | if direction_dec ~= 0 27 | [T5, T6, T7, delt_p_dec, p_dec, v_dec, a_dec, j_dec] = updateDurationsMinimizeTotalTimeDecleration( direction_dec, max_jerk, max_accel, vel_sp, a_acc, v_acc, p_acc); 28 | else 29 | T5 = 0; 30 | T6 = 0; 31 | T7 = 0; 32 | end 33 | 34 | T4 = computeT4( delt_p_acc, delt_p_dec, state_v, delt_p ); 35 | 36 | delt_p_cruise = 0; 37 | [p_cruise, v_cruise, a_cruise, j_cruise] = updateTrajCruise( T4, T4, 0, v_acc, p_acc, 0, max_jerk ); 38 | delt_p_cruise = p_cruise - p_acc; 39 | 40 | deltP = delt_p_acc + delt_p_dec + delt_p_cruise; 41 | end 42 | 43 | --------------------------------------------------------------------------------