Coverage for src/pyroboplan/planning/path_shortcutting.py: 93%

1"""Capabilities to shorten paths produced by motion planners.""" 

2 

3import numpy as np 

4 

5from pyroboplan.core.utils import configuration_distance, get_path_length 

6from pyroboplan.planning.utils import ( 

7 check_collisions_along_path, 

8 discretize_joint_space_path, 

9) 

10 

11 

12def get_normalized_path_scaling(q_path): 

13 """ 

14 Returns a list of length-normalized scaling values along a joint configuration path, 

15 from 0.0 at the start to 1.0 at the end. 

16 

17 Parameters 

18 ---------- 

19 q_path : list[array-like] 

20 The list of joint configurations describing the path. 

21 

22 Return 

23 ------ 

24 list[float] 

25 The list of scaling values, between 0.0 and 1.0, at each of the path waypoints. 

26 """ 

27 if len(q_path) == 0: 

28 return [] 

29 if len(q_path) == 1: 

30 return [1.0] 

31 

32 path_length = 0.0 

33 path_length_list = [path_length] 

34 for idx in range(len(q_path) - 1): 

35 path_length += configuration_distance(q_path[idx], q_path[idx + 1]) 

36 path_length_list.append(path_length) 

37 return np.array(path_length_list) / path_length 

38 

39 

40def get_configuration_from_normalized_path_scaling(q_path, path_scalings, value): 

41 """ 

42 Given a path and its corresponding length-normalized scalings between 0.0 and 1.0, 

43 get the joint configuration at a specific scale value. 

44 

45 For example: 

46 * 0.0 will return the start configuration of the path 

47 * 1.0 will return the end configuration of the path 

48 * 0.5 will return the configuration halfway along the length of the path 

49 

50 Parameters 

51 ---------- 

52 q_path : list[array-like] 

53 The list of joint configurations describing the path. 

54 path_scalings : list[float] 

55 The list of scaling values, between 0.0 and 1.0, at each of the path waypoints. 

56 value : float 

57 The value, between 0.0 and 1.0, at which to get the joint configuration along the path. 

58 

59 Return 

60 ------ 

61 tuple (array-like, int) 

62 A tuple containing the joint configuration at the specified scaling value along the path, 

63 as well as the index corresponding to the next point along the path. 

64 """ 

65 if value < 0 or value > 1: 

66 raise ValueError("Scaling value must be in the range [0, 1]") 

67 

68 for idx in range(len(path_scalings)): 68 ↛ exitline 68 didn't return from function 'get_configuration_from_normalized_path_scaling' because the loop on line 68 didn't complete

69 if value > path_scalings[idx]: 

70 continue 

71 

72 delta_scale = (value - path_scalings[idx - 1]) / ( 

73 path_scalings[idx] - path_scalings[idx - 1] 

74 ) 

75 return q_path[idx - 1] + delta_scale * (q_path[idx] - q_path[idx - 1]), idx 

76 

77 

78def shortcut_path(model, collision_model, q_path, max_iters=100, max_step_size=0.05): 

79 """ 

80 Performs path shortcutting by sampling two random points along the path and verifying 

81 whether those two points can be connected directly without collisions. 

82 

83 This implementation is based on section 3.5.3 of: 

84 https://motion.cs.illinois.edu/RoboticSystems/MotionPlanningHigherDimensions.html 

85 

86 Parameters 

87 ---------- 

88 model : `pinocchio.Model` 

89 The model of the robot. 

90 collision_model : `pinocchio.Model`. 

91 The model to use for collision checking. 

92 q_path : list[array-like] 

93 The list of joint configurations describing the path. 

94 max_iters : int 

95 The maximum iterations for randomly sampling shortcut points. 

96 max_step_size : float 

97 Maximum joint configuration step size for collision checking along path segments. 

98 

99 Return 

100 ------ 

101 list[array-like] 

102 The shortest length path over all the shortcutting attempts. 

103 """ 

104 # If the original path has 2 points or less, this is already a shortest path. 

105 if len(q_path) < 3: 

106 return q_path 

107 

108 q_shortened = q_path 

109 for _ in range(max_iters): 

110 # If the path has been shortened to 2 points or less, this is already a shortest path. 

111 if len(q_shortened) < 3: 111 ↛ 112line 111 didn't jump to line 112 because the condition on line 111 was never true

112 break 

113 

114 # Sample two points along the path length 

115 path_scalings = get_normalized_path_scaling(q_shortened) 

116 

117 low_point, high_point = sorted(np.random.random(2)) 

118 q_low, idx_low = get_configuration_from_normalized_path_scaling( 

119 q_shortened, path_scalings, low_point 

120 ) 

121 q_high, idx_high = get_configuration_from_normalized_path_scaling( 

122 q_shortened, path_scalings, high_point 

123 ) 

124 # there is nothing to shorten on a linear path segment 

125 if idx_low == idx_high: 

126 continue 

127 

128 # Check if the sampled segment is collision free. If it is, shortcut the path. 

129 path_to_goal = discretize_joint_space_path([q_low, q_high], max_step_size) 

130 if not check_collisions_along_path(model, collision_model, path_to_goal): 130 ↛ 109line 130 didn't jump to line 109 because the condition on line 130 was always true

131 q_shortened = ( 

132 q_shortened[:idx_low] + [q_low, q_high] + q_shortened[idx_high:] 

133 ) 

134 return q_shortened