def check_fit_traj_segment(p_start, p_end, v_start, v_end, p_max, v_max, a_max,
j_max):
position, velocity, acceleration, jerk = traj.fit_traj_segment(
p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max)
#### 1) test if the calculated start/end pos/vel are equal to the given ones
t_start = position.boundaries[0]
t_end = position.boundaries[-1]
p_start_computed = position(t_start)
v_start_computed = velocity(t_start)
p_end_computed = position(t_end)[0]
v_end_computed = velocity(t_end)[0]
assert np.isclose(p_start, p_start_computed)
assert np.isclose(p_end, p_end_computed)
assert np.isclose(v_start, v_start_computed)
assert np.isclose(v_end, v_end_computed)
#### 2) test if the calculated pos/vel/acc/jrk [at each time_instatnt has change in segment_phase] are within the given limits p_max, v_max, a_max, j_max
for phase_time in position.boundaries:
p_computed = position(phase_time)
v_computed = velocity(phase_time)
a_computed = acceleration(phase_time)
j_computed = jerk(phase_time)
# print "p_computed, v_computed, a_computed, j_computed :"
# print p_computed, v_computed, a_computed, j_computed
assert np.less_equal(
abs(p_computed), p_max + 1e-6
) #added 1e-6 because sometime it gives fails even if the difference is less than (1e-6)
assert np.less_equal(abs(v_computed), v_max + 1e-6)
assert np.less_equal(abs(a_computed), a_max + 1e-6)
assert np.less_equal(abs(j_computed), j_max + 1e-6)
def check_fit_traj_segment(p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max):
position, velocity, jerk, acceleration = traj.fit_traj_segment(p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max)
t_start = position.boundaries[0]
t_end = position.boundaries[-1]
p_start_computed = position(t_start)
v_start_computed = velocity(t_start)
p_end_computed = position(t_end)[0]
v_end_computed = velocity(t_end)[0]
#print p_start_computed, p_end_computed, v_start_computed, v_end_computed
#print " ", p_start, p_end, v_start, v_end
assert np.isclose(p_start, p_start_computed)
assert np.isclose(p_end, p_end_computed)
assert np.isclose(v_start, v_start_computed)
assert np.isclose(v_end, v_end_computed)
def plot_traj_segment(p_start, p_end, v_start, v_end, p_max, v_max, a_max,
j_max):
'''
this function plots a trajectory segment, given the start/end positions/velocities,
Start and end acceleration/jerk are assumed to be zero.
'''
position, velocity, acceleration, jerk = traj.fit_traj_segment(
p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max)
traj.plot.plot_trajectory(plt,
position,
velocity,
acceleration,
jerk,
n_points=100,
v_max=v_max,
a_max=a_max,
j_max=j_max)
plt.show()
def check_fit_traj_segment(p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max):
start_time = time.time()
traj.fit_traj_segment(p_start, p_end, v_start, v_end, p_max, v_max, a_max, j_max)
execution_time = time.time() - start_time
return execution_time
def test_exception_violate_p_max():
with nose.tools.assert_raises_regexp(ValueError,
"non feasible case: violate p_max"):
position, velocity, jerk, acceleration = traj.fit_traj_segment(
9.0, 8.0, 2.5, -3.0, p_max, v_max, a_max, j_max)
def test_exception_violate_min_pos_to_vf():
with nose.tools.assert_raises_regexp(
ValueError, "non feasible case: violate min_pos_to_vf"):
position, velocity, jerk, acceleration = traj.fit_traj_segment(
4.0, 4.4, 0.2, 2.5, p_max, v_max, a_max, j_max)
def test_exception_vel_motion_opposite_to_pos_motion_2():
with nose.tools.assert_raises_regexp(
ValueError,
"non feasible case: vel_motion opposite to pos_motion"):
position, velocity, jerk, acceleration = traj.fit_traj_segment(
0.0, -5.0, 1.5, 1.0, p_max, v_max, a_max, j_max)
def trajectory_for_path_v2(path, v_start, v_end, max_velocities,
max_accelerations, max_jerks):
path_function = parameterize_path(path)
t = Symbol('t')
s = path_function.independent_variable
# check n_jts, n_segs
n_segs = len(path_function.functions)
# step 1: find Max Forward velocity
s_fw_vel = []
for seg in range(n_segs):
fsegment = path_function.functions[seg]
s0 = path_function.boundaries[seg]
s1 = path_function.boundaries[seg + 1]
# Project joint limits onto this segment's direction to get limits on s
v_max = project_limits_onto_s(max_velocities, fsegment, s1 - s0)
a_max = project_limits_onto_s(max_accelerations, fsegment, s1 - s0)
j_max = project_limits_onto_s(max_jerks, fsegment, s1 - s0)
if seg == 0:
s_v_start = project_limits_onto_s(v_start, fsegment, s1 - s0)
s_fw_vel.append(s_v_start)
tj, ta, tv, s_v_nxt = traj.max_reachable_vel_per_segment(
s1 - s0, s_fw_vel[seg], 30.0, j_max, a_max, j_max)
s_fw_vel.append(s_v_nxt)
rospy.logdebug("\n>>> s_fw_vel: \n {}".format(s_fw_vel))
# step 2: find Max Backward velocity
s_bk_vel = []
for seg in range(n_segs):
fsegment = path_function.functions[n_segs - seg - 1]
s0 = path_function.boundaries[n_segs - seg - 1]
s1 = path_function.boundaries[n_segs - seg]
# Project joint limits onto this segment's direction to get limits on s
v_max = project_limits_onto_s(max_velocities, fsegment, s1 - s0)
a_max = project_limits_onto_s(max_accelerations, fsegment, s1 - s0)
j_max = project_limits_onto_s(max_jerks, fsegment, s1 - s0)
if seg == 0:
s_v_end = project_limits_onto_s(v_end, fsegment, s1 - s0)
s_bk_vel.append(s_v_end)
tj, ta, tv, s_v_nxt = traj.max_reachable_vel_per_segment(
s1 - s0, s_bk_vel[seg], 30.0, v_max, a_max, j_max)
s_bk_vel.append(s_v_nxt)
s_bk_vel.reverse()
rospy.logdebug("\n>>> s_bk_vel: \n {}".format(s_bk_vel))
# step 3: find final max reachable vel
# check condition when v_start or v_end is not feasible:
# v_start > max_v_start calculated using the backward loop or Vs
if s_fw_vel[0] > s_bk_vel[0] or s_bk_vel[-1] > s_fw_vel[-1]:
raise ValueError("combination of v_start({}) & v_end({})"
"is not feasible".format(s_fw_vel[0], s_bk_vel[-1]))
# calcuate max_rechable_vels that grantee v_end at the end of
# the trajectory for this portion of traj
s_estimated_vel = [min(fw, bk) for fw, bk in zip(s_fw_vel, s_bk_vel)]
rospy.logdebug("\n>>> s_estimated_vel: \n {}".format(s_estimated_vel))
# step 4: use the estimated max reachable velocity
trajectory_position_functions = []
trajectory_velocity_functions = []
trajectory_acceleration_functions = []
trajectory_jerk_functions = []
trajectory_boundaries = [0.0]
for segment_i in range(len(path_function.functions)):
fsegment = path_function.functions[segment_i]
rospy.logdebug("\n\n >>>>>>>>> seg: {} ".format(segment_i))
s0 = path_function.boundaries[segment_i]
s1 = path_function.boundaries[segment_i + 1]
p_start = np.array(fsegment.subs(s, 0.0)).astype(np.float64).flatten()
p_end = np.array(fsegment.subs(s,
s1 - s0)).astype(np.float64).flatten()
rospy.logdebug((p_start, p_end))
# Project joint limits onto this segment's direction to get limits on s
v_max = project_limits_onto_s(max_velocities, fsegment, s1 - s0)
a_max = project_limits_onto_s(max_accelerations, fsegment, s1 - s0)
j_max = project_limits_onto_s(max_jerks, fsegment, s1 - s0)
# Compute 7 segment profile for s as a function of time.
this_segment_start_time = trajectory_boundaries[-1]
s_position, s_velocity, s_acceleration, s_jerk = traj.fit_traj_segment(
0, s1 - s0, s_estimated_vel[segment_i],
s_estimated_vel[segment_i + 1], 30.0, v_max, a_max, j_max)
# Substitute time profile for s into the path function to get
# trajectory as a function of t.
for function_i in range(len(s_position.functions)):
position_vs_t = fsegment.subs(s, s_position.functions[function_i])
velocity_vs_t = diff(position_vs_t, t)
acceleration_vs_t = diff(velocity_vs_t, t)
jerk_vs_t = diff(acceleration_vs_t, t)
trajectory_position_functions.append(position_vs_t)
trajectory_velocity_functions.append(velocity_vs_t)
trajectory_acceleration_functions.append(acceleration_vs_t)
trajectory_jerk_functions.append(jerk_vs_t)
trajectory_boundaries.append(s_position.boundaries[function_i +
1] +
this_segment_start_time)
return (PiecewiseFunction(trajectory_boundaries,
trajectory_position_functions, t),
PiecewiseFunction(trajectory_boundaries,
trajectory_velocity_functions, t),
PiecewiseFunction(trajectory_boundaries,
trajectory_acceleration_functions, t),
PiecewiseFunction(trajectory_boundaries, trajectory_jerk_functions,
t))