def run_checking_sampling_bridge(self, rob, grapple2, ee1flag):
tester = test_robot(self)
tolerate_error = 1e-5
minMax = self.get_min_max_len()
numSeg = self.get_num_segment()
robPos = rob.get_position()
headPos = robPos[0]
endPos = rob.get_EndeePos()
arr = []
con = True
flag, arr = self.check_sampling_bridge(rob, grapple2, ee1flag)
if flag:
return arr
else:
# print(rob)
# print(robPos[-2])
xlen = -(robPos[-2][0] - grapple2[0])
ylen = -(robPos[-2][1] - grapple2[1])
difflen = math.sqrt(xlen * xlen + ylen * ylen)
if (difflen >= minMax[0][-1] and difflen <= minMax[1][-1]):
newAng = Angle.atan2(ylen, xlen)
newAng1 = Angle.atan2(-ylen, -xlen)
if ee1flag:
robArr = rob.str2list()
angSum = 0
for a in range(numSeg - 1):
angSum += robArr[2 + a]
robArr[2 + numSeg - 1] = newAng.in_degrees() - angSum
robArr[2 + numSeg * 2 - 1] = difflen
robNew = make_robot_config_with_arr(
robArr[0], robArr[1], robArr[2:(2 + numSeg)],
robArr[(2 + numSeg):(2 + numSeg * 2)], robArr[-1])
if (tester.self_obstacle_test(robNew)):
# print(robNew)
flagNew, arrNew = self.check_sampling_bridge(
robNew, grapple2, ee1flag)
if flagNew:
return arrNew
else:
#===== here might have problem when grapple is ee2 ====
robArr = rob.str2list()
angSum = 0
for a in range(numSeg - 1):
angSum += robArr[2 + a]
robArr[2 + numSeg - 1] = newAng.in_degrees() - angSum
robArr[2 + numSeg] = difflen
robNew = make_robot_config_with_arr(
robArr[0], robArr[1], robArr[2:(2 + numSeg)],
robArr[(2 + numSeg):(2 + numSeg * 2)], robArr[-1])
robNew1 = make_robot_config_with_arr(
robArr[0], robArr[1], robArr[2:(2 + numSeg)],
robArr[(2 + numSeg):(2 + numSeg * 2)], robArr[-1])
flagNew, arrNew = self.check_sampling_bridge(
robNew, grapple2, ee1flag)
if flagNew:
return arrNew
return arr
def sampling_withinD(self, robot, D, numSampling, eexy, ee1Flag):
minMax = self.get_min_max_len()
numSeg = self.get_num_segment()
grapple_point = self.get_grapple_points()
limitAngle = Angle(radians=D[0])
limitLength = D[1]
# read robot angle and length
robot_ang = robot.get_angle()
robot_len = robot.get_length()
# sampling angles
angles = np.zeros((numSampling, numSeg))
lengths = np.zeros((numSampling, numSeg))
for i in range(numSeg):
tmpAng = np.random.rand(1,numSampling)*limitAngle.in_degrees()*2 \
- limitAngle.in_degrees() +robot_ang[i].in_degrees()
if i == 0:
tmpAng[tmpAng > 180] = 180
tmpAng[tmpAng < -180] = -180
angles[:, i] = tmpAng
else:
tmpAng[tmpAng > 165] = 165
tmpAng[tmpAng < -165] = -165
angles[:, i] = tmpAng
# # sampling length
if (minMax[1][i] - minMax[0][i]) != 0:
tmp = np.random.rand(
1, numSampling) * (D[1] * 2) - D[1] + robot_len[i]
tmp[tmp > minMax[1][i]] = minMax[1][i]
tmp[tmp < minMax[0][i]] = minMax[0][i]
else:
tmp = np.zeros((1, numSampling)) + robot_len[i]
lengths[:, i] = tmp
# grapple
grapples = []
if ee1Flag:
grapples_tmp = np.zeros((1, numSampling))
else:
grapples_tmp = np.ones((1, numSampling))
# grapples_tmp = (np.floor(np.random.rand(1,numSampling)*len(grapple_point)))
for i in range(numSampling):
grapples.append(list(eexy))
output = np.append(np.asarray(grapples), angles, axis=1)
output = np.append(output, lengths, axis=1)
grapples_tmp = grapples_tmp.reshape(numSampling, 1)
output = np.append(output, grapples_tmp, axis=1)
return output
def uniformly_sampling(spec, obstacles, stage):
while True:
sampling_angle = []
sampling_length = []
for i in range(spec.num_segments):
angle = random.uniform(-165, 165)
sampling_angle.append(Angle(degrees=float(angle)))
length = random.uniform(spec.min_lengths[i], spec.max_lengths[i])
sampling_length.append(length)
if stage % 2 == 0:
new_config = make_robot_config_from_ee1(
spec.grapple_points[stage][0],
spec.grapple_points[stage][1],
sampling_angle,
sampling_length,
ee1_grappled=True)
else:
new_config = make_robot_config_from_ee2(
spec.grapple_points[stage][0],
spec.grapple_points[stage][1],
sampling_angle,
sampling_length,
ee2_grappled=True)
if test_obstacle_collision(new_config, spec, obstacles) and test_self_collision(new_config, spec) and \
test_environment_bounds(new_config):
return new_config
def ass_key2list(self, in_id):
# not include the last flag of ee
arr = []
tmp = in_id.split(' ')
segNum = int((len(tmp) - 3) / 2)
for i in range(len(tmp) - 1):
if i < 2:
qq = float(tmp[i].replace(';', ''))
qq = qq * 10000
elif i > 1 and i <= 1 + segNum:
qq = float(tmp[i].replace(';', ''))
tmpAng = Angle(degrees=qq)
qq = tmpAng.in_radians()
else:
qq = float(tmp[i].replace(';', ''))
arr.append(qq)
return arr
def make_robot_config_with_arr(x, y, anglesArr, lengthsArr, grappled):
# grappled = 1-> ee1, 2 -> ee2
angles = [Angle(degrees=float(i)) for i in anglesArr]
if grappled == 1:
return make_robot_config_from_ee1(x, y, angles, lengthsArr, ee1_grappled=True)
else:
return make_robot_config_from_ee2(x, y, angles, lengthsArr, ee2_grappled=True)
def str2robotConfigqq(self, stringInput):
ee1_xy_str, ee1_angles_str, lengths_str, ee1_grappled = stringInput.strip().split(';')
ee1x, ee1y = tuple([float(i) for i in ee1_xy_str.split(' ')])
ee1_angles = [Angle(degrees=float(i)) for i in ee1_angles_str.strip().split(' ')]
lengths = [float(i) for i in lengths_str.strip().split(' ')]
if int(ee1_grappled) == 1:
return rob_conf_ee1(ee1x, ee1y, ee1_angles, lengths, ee1_grappled=True)
else:
return rob_conf_ee2(ee1x, ee1y, ee1_angles, lengths, ee2_grappled=True)
def load_output(filename):
# return a list of RobotConfig objects
robot_configs = []
f = open(filename, 'r')
for line in f:
ee1_xy_str, ee1_angles_str, lengths_str = line.strip().split(';')
ee1x, ee1y = tuple([float(i) for i in ee1_xy_str.split(' ')])
ee1_angles = [Angle(degrees=float(i)) for i in ee1_angles_str.strip().split(' ')]
lengths = [float(i) for i in lengths_str.strip().split(' ')]
robot_configs.append(make_robot_config_from_ee1(ee1x, ee1y, ee1_angles, lengths))
return robot_configs
def __init__(self,
lengths,
ee1x=None,
ee1y=None,
ee1_angles=None,
ee2x=None,
ee2y=None,
ee2_angles=None,
ee1_grappled=False,
ee2_grappled=False):
"""
Constructor for RobotConfig - we suggest using make_robot_config_from_ee1() or make_robot_config_from_ee2()
to construct new instances of RobotConfig rather than calling this function directly.
"""
self.lengths = lengths
self.ee1_grappled = ee1_grappled
self.ee2_grappled = ee2_grappled
if ee1x is not None and ee1y is not None and ee1_angles is not None:
points = [(ee1x, ee1y)]
net_angle = Angle(radians=0)
for i in range(len(ee1_angles)):
x, y = points[-1]
net_angle = net_angle + ee1_angles[i]
x_new = x + (lengths[i] * math.cos(net_angle.in_radians()))
y_new = y + (lengths[i] * math.sin(net_angle.in_radians()))
points.append((x_new, y_new))
self.ee1_angles = ee1_angles
# 1st angle is last angle of e1_angles + pi, others are all -1 * e1_angles (in reverse order)
self.ee2_angles = [math.pi + net_angle] + \
[-ee1_angles[i] for i in range(len(ee1_angles) - 1, 0, -1)]
self.points = points
elif ee2x is not None and ee2y is not None and ee2_angles is not None:
points = [(ee2x, ee2y)]
net_angle = Angle(radians=0)
for i in range(len(ee2_angles)):
x, y = points[0]
net_angle = net_angle + ee2_angles[i]
x_new = x + (lengths[-i - 1] *
math.cos(net_angle.in_radians()))
y_new = y + (lengths[-i - 1] *
math.sin(net_angle.in_radians()))
points.insert(0, (x_new, y_new))
# 1st angle is last angle of e2_angles + pi, others are all -1 * e2_angles (in reverse order)
self.ee1_angles = [math.pi + sum(ee2_angles)] + \
[-ee2_angles[i] for i in range(len(ee2_angles) - 1, 0, -1)]
self.ee2_angles = ee2_angles
self.points = points
else:
raise Exception(
"Could not create RobotConfig - Insufficient information given"
)
def assign_config(self, sample_config, id):
numSeg = self.get_num_segment()
x = sample_config[id][0]
y = sample_config[id][1]
angles = []
lengths = []
for i in range(numSeg):
angles.append(Angle(degrees=sample_config[id][2 + i]))
lengths.append(sample_config[id][2 + numSeg + i])
if (int(sample_config[id][-1]) == 0):
return rob_conf_ee1(x, y, angles, lengths, ee1_grappled=True)
else:
return rob_conf_ee2(x, y, angles, lengths, ee2_grappled=True)
def state_graph(n, k, r, init, goal, spec):
"""
Construct a graph by performing Probabilistic Roadmap.
:param n: number of nodes to put in the roadmap
:param k: number of closest neighbors to examine for each configuration
:param r: the maximum distance between each configurations (in radians)
:param init: the initial configuration
:param spec: the specification of the problem
:return: (list, list) vertices and edges that represents the graph.
"""
vertices = []
edges = []
current = init.config
while len(vertices) < n:
if current.ee1_grappled:
collision = True
while collision:
angles = []
lengths = []
for i in current.ee1_angles:
angles.append(Angle(degrees=random.randint(-165, 165)))
if spec.min_lengths[0] != spec.max_lengths[0]:
for i in range(len(spec.min_lengths)):
lengths.append(random.uniform(spec.min_lengths[i], spec.max_lengths[i]))
else:
lengths = current.lengths
next_node = make_robot_config_from_ee1(
current.points[0][0],
current.points[0][1],
angles,
lengths,
ee1_grappled=True,
)
if test_obstacle_collision(next_node, spec, spec.obstacles):
vertices.append(GraphNode(spec, next_node))
collision = False
vertices.append(init)
vertices.append(goal)
for q in vertices:
closest = find_closest(q, k, vertices)
for target in closest:
if (q, target) not in edges:
iteration_local_planner(q, target, spec)
###
# edges.append((target, q))
return vertices, edges
def get_nearest_obstacle_angle(q, obstacles):
lst = []
p = get_tip(q)
for o in obstacles:
for e in o.edges:
x0 = p[0]
y0 = p[1]
x1 = e[0][0]
y1 = e[0][1]
x2 = e[1][0]
y2 = e[1][1]
yf = abs(y1 - y2)
xf = abs(x1 - x2)
# print("yf = " + str(yf))
# print("xf = " + str(xf))
dist = Angle.atan2(yf, xf)
# print("dist = " + str(dist))
lst.append(dist)
return min(lst)
def create_mid(q, target, spec):
mid_angles = []
mid_lengths = []
for i, j in zip(q.config.ee1_angles, target.config.ee1_angles):
k = (i.in_degrees() + j.in_degrees()) / 2
mid_angles.append(Angle(degrees=k))
for i, j in zip(q.config.lengths, target.config.lengths):
k = (i + j) / 2
mid_lengths.append(k)
mid_conf = make_robot_config_from_ee1(
q.config.points[0][0],
q.config.points[0][1],
mid_angles,
mid_lengths,
ee1_grappled=True,
)
return GraphNode(spec, mid_conf)
def pointsToConfig(pointsConfig):
angles = []
length = []
for i in range(0, len(pointsConfig) - 1):
angles.append(
getAngleM((pointsConfig[i][0], pointsConfig[i][1]), (pointsConfig[i + 1][0], pointsConfig[i + 1][1])))
length.append(
distancePoints((pointsConfig[i][0], pointsConfig[i][1]), (pointsConfig[i + 1][0], pointsConfig[i + 1][1])))
for i in range(1, len(angles)):
angles[i] -= sum(angles[0:i])
anglesAngles = []
for i in range(0, len(angles)):
anglesAngles.append(Angle(degrees=angles[i]))
# Making it from ee1
config = make_robot_config_from_ee1(pointsConfig[0][0], pointsConfig[0][1], anglesAngles, length, ee1_grappled=True)
return config
def __init__(self, input_file):
# parse input file
f = open(input_file, 'r')
# parse arm constraints
try:
self.num_segments = int(next_valid_line(f))
except Exception:
print("Invalid value for number of segments")
sys.exit(1)
self.min_lengths = [float(i) for i in next_valid_line(f).split(' ')]
assert len(self.min_lengths) == self.num_segments, \
"Number of minimum lengths does not match number of segments"
self.max_lengths = [float(i) for i in next_valid_line(f).split(' ')]
assert len(self.max_lengths) == self.num_segments, \
"Number of maximum lengths does not match number of segments"
# parse initial configuration
initial_grappled = int(next_valid_line(f))
assert initial_grappled == 1 or initial_grappled == 2, "Initial end effector number is not 1 or 2"
try:
initial_eex, initial_eey = [
float(i) for i in next_valid_line(f).split(' ')
]
except Exception:
print("Invalid value(s) for initial end effector position")
sys.exit(1)
initial_angles = [
Angle(degrees=float(i)) for i in next_valid_line(f).split(' ')
]
assert len(initial_angles) == self.num_segments, \
"Number of initial angles does not match number of segments"
initial_lengths = [float(i) for i in next_valid_line(f).split(' ')]
assert len(initial_lengths) == self.num_segments, \
"Number of initial lengths does not match number of segments"
if initial_grappled == 1:
self.initial = make_robot_config_from_ee1(initial_eex,
initial_eey,
initial_angles,
initial_lengths,
ee1_grappled=True)
else:
self.initial = make_robot_config_from_ee2(initial_eex,
initial_eey,
initial_angles,
initial_lengths,
ee2_grappled=True)
# parse goal configuration
goal_grappled = int(next_valid_line(f))
assert goal_grappled == 1 or goal_grappled == 2, "Goal end effector number is not 1 or 2"
try:
goal_eex, goal_eey = [
float(i) for i in next_valid_line(f).split(' ')
]
except Exception:
print("Invalid value(s) for goal end effector 1 position")
sys.exit(1)
goal_angles = [
Angle(degrees=float(i)) for i in next_valid_line(f).split(' ')
]
assert len(goal_angles) == self.num_segments, \
"Number of goal ee1 angles does not match number of segments"
goal_lengths = [float(i) for i in next_valid_line(f).split(' ')]
assert len(goal_lengths) == self.num_segments, \
"Number of goal lengths does not match number of segments"
if goal_grappled == 1:
self.goal = make_robot_config_from_ee1(goal_eex,
goal_eey,
goal_angles,
goal_lengths,
ee1_grappled=True)
else:
self.goal = make_robot_config_from_ee2(goal_eex,
goal_eey,
goal_angles,
goal_lengths,
ee2_grappled=True)
# parse grapple points
try:
self.num_grapple_points = int(next_valid_line(f))
except Exception:
print("Invalid value for number of grapple points")
sys.exit(1)
grapple_points = []
for i in range(self.num_grapple_points):
try:
grapple_points.append(
tuple([float(i) for i in next_valid_line(f).split(' ')]))
except Exception:
print("Invalid value(s) for grapple point " + str(i) +
" position")
sys.exit(1)
self.grapple_points = grapple_points
# parse obstacles
try:
self.num_obstacles = int(next_valid_line(f))
except Exception:
print("Invalid value for number of obstacles")
sys.exit(1)
obstacles = []
for i in range(self.num_obstacles):
try:
x1, y1, x2, y2 = [
float(i) for i in next_valid_line(f).split(' ')
]
obstacles.append(Obstacle(x1, y1, x2, y2))
except Exception:
print("Invalid value(s) for obstacle " + str(i))
sys.exit(1)
self.obstacles = obstacles
def generate_sub_goals(spec, stage, obstacles):
start_point = spec.grapple_points[stage]
goal_point = spec.grapple_points[stage+1]
sub_goals = []
for i in range(10):
find_goal = False
while find_goal is False:
sampling_angle = []
sampling_length = []
for j in range(spec.num_segments - 1):
angle = random.uniform(-165, 165)
sampling_angle.append(Angle(degrees=float(angle)))
length = random.uniform(spec.min_lengths[j], spec.max_lengths[j])
sampling_length.append(length)
if stage % 2 == 0:
x1, y1 = start_point
x2, y2 = goal_point
partial_config = make_robot_config_from_ee1(x1, y1, sampling_angle, sampling_length, ee1_grappled=True)
if (partial_config.points[-1][0] - x2) ** 2 + (partial_config.points[-1][1] - y2) ** 2 < spec.max_lengths[spec.num_segments-1] ** 2:
sampling_length.append(math.sqrt((partial_config.points[-1][0] - x2) ** 2 + (partial_config.points[-1][1] - y2) ** 2))
required_net_angle = math.atan((partial_config.points[-1][1] - y2) / (partial_config.points[-1][0] - x2))
if y2 > partial_config.points[-1][1] and x2 < partial_config.points[-1][0]:
required_net_angle += math.pi
elif y2 < partial_config.points[-1][1] and x2 < partial_config.points[-1][0]:
required_net_angle -= math.pi
current_net_angle = 0
for angle in sampling_angle:
current_net_angle += angle.in_radians()
sampling_angle.append(Angle(radians=float(required_net_angle-current_net_angle)))
sub_goal_config = make_robot_config_from_ee1(x1, y1, sampling_angle, sampling_length, ee1_grappled=True, ee2_grappled=True)
if test_obstacle_collision(sub_goal_config, spec, obstacles) and test_self_collision(sub_goal_config, spec)\
and test_environment_bounds(sub_goal_config):
sub_goals.append(sub_goal_config)
find_goal = True
else:
x1, y1 = start_point
x2, y2 = goal_point
partial_config = make_robot_config_from_ee2(x1, y1, sampling_angle, sampling_length, ee2_grappled=True)
if (partial_config.points[0][0] - x2) ** 2 + (partial_config.points[0][1] - y2) ** 2 < spec.max_lengths[0] ** 2:
sampling_length.insert(0, math.sqrt((partial_config.points[0][0] - x2) ** 2 + (partial_config.points[0][1] - y2) ** 2))
required_net_angle = math.atan((partial_config.points[0][1] - y2) / (partial_config.points[0][0] - x2))
if required_net_angle < 0:
required_net_angle += math.pi
current_net_angle = 0
for angle in sampling_angle:
current_net_angle += angle.in_radians()
sampling_angle.append(Angle(radians=float(required_net_angle-current_net_angle)))
sub_goal_config = make_robot_config_from_ee2(x1, y1, sampling_angle, sampling_length, ee1_grappled=True, ee2_grappled=True)
if test_obstacle_collision(sub_goal_config, spec, obstacles) and test_self_collision(sub_goal_config, spec)\
and test_environment_bounds(sub_goal_config):
sub_goals.append(sub_goal_config)
find_goal = True
return sub_goals
def primitive_step(configs):
result = []
result.append(configs[0])
for x in range(len(configs) - 1):
current_angles = configs[x].ee1_angles
next_angles = configs[x+1].ee1_angles
current_lengths = configs[x].lengths
next_lengths = configs[x+1].lengths
abs_difference_angles = []
abs_difference_lengths = []
difference_angles = []
difference_lengths = []
# length of lengths and angles are the same
for y in range(len(current_angles)):
diff_angle = next_angles[y].in_radians() - current_angles[y].in_radians()
abs_diff_angle = abs(diff_angle)
abs_difference_angles.append(abs_diff_angle)
difference_angles.append(diff_angle)
diff_length = next_lengths[y] - current_lengths[y]
abs_diff_length = abs(diff_length)
abs_difference_lengths.append(abs_diff_length)
difference_lengths.append(diff_length)
# print("length : " + str(abs_difference_lengths))
# print("angle : " + str(abs_difference_angles))
segments = max(max(abs_difference_angles), max(abs_difference_lengths))
# print("segments : " + str(segments))
# print()
configs_needed = int(math.ceil(segments/0.001))
direction_vector_angles = []
direction_vector_lengths = []
for dv in range(len(abs_difference_angles)):
direction_vector_angles.append(difference_angles[dv] / configs_needed)
direction_vector_lengths.append(difference_lengths[dv] / configs_needed)
# print("dvdirection_vector_angles)
for step in range(configs_needed):
angles = []
lengths = []
for i in range(len(direction_vector_angles)):
angle = Angle(radians=current_angles[i].in_radians() + (step * direction_vector_angles[i]))
angles.append(angle)
length = current_lengths[i] + (step * direction_vector_lengths[i])
lengths.append(length)
new_config = make_robot_config_from_ee1(configs[0].get_ee1()[0], configs[0].get_ee1()[1], angles,
lengths, configs[0].ee1_grappled, ee2_grappled=False)
result.append(new_config)
result.append(configs[x+1])
return result