def get_pos_cost(pose, target):
diff_pose = ch.get_diff_pose(pose, target)
distance = math.sqrt(diff_pose.position.x**2 + diff_pose.position.y**2)
yaw = ch.get_yaw(diff_pose) % (2*math.pi)
# target should be 0.01 distance and 0.1 yaw - therefore we take 0.01*yaw for equal weighting
yaw_diff = 0.2 * min(yaw, 2*math.pi - yaw)
return math.sqrt(distance**2 + yaw_diff**2)
def sample_directed_control(direction, threshold):
direction_error = float('inf')
while direction_error > threshold:
control = np.random.uniform(0,1,3)
push = control_to_push(control)
push_direction = push.angle + ch.get_yaw(push.approach)
direction_diff = abs(direction - push_direction)
direction_error = min(direction_diff, 2.0 * pi - direction_diff)
return control, push
def sample_predictive_push(req):
global prediction
res = SamplePredictivePushResponse()
res.success = False
if prediction is not None:
pr_cost, pr_push, pr_pose = prediction
pose = req.object_pose
cost = get_pos_cost(pose, req.target)
dt = ch.get_diff_pose(pose, req.target)
dp = ch.get_diff_pose(pr_pose, pose)
print "----------------- cost ", "x ", "y ", "yaw"
print "prediction: ", pr_cost, pr_pose.position.x, pr_pose.position.y, ch.get_yaw(pr_pose)
print "result: ", cost, pose.position.x, pose.position.y, ch.get_yaw(pose)
print
print "prediction error:", abs(pr_cost - cost), dp.position.x, dp.position.y, ch.get_yaw(dp)
print
print "target error: ", dt.position.x, dt.position.y, ch.get_yaw(dt)
print
# try to sample push
prediction = sample_push(req.object_pose, req.target)
if prediction is not None:
cost, push, pose = prediction
#fill push
res.push.mode = 0
res.push.distance = push.distance
#fill push approach
res.push.approach.frame_id = req.object_id
res.push.approach.point = push.approach.position
res.push.approach.normal = push.approach.orientation
res.push.approach.angle = push.angle
#set successful
res.success = True
return res
def get_cor(p1, p2):
# get point in the middle of p1 p2
pm = ch.interpolate_poses(p1, p2)
pm_yaw = ch.get_yaw(pm)
pm_tangent_yaw = atan2(p2.position.y - p1.position.y, p2.position.x - p1.position.x)
# define perpendicular pose at pm
pm_cross = Pose()
pm_cross.position = pm.position
pm_cross.orientation = ch.quat_from_yaw(pm_tangent_yaw + 0.5 * pi)
# find diff angle between tangent and pm orientation
yaw_diff = pm_yaw - pm_tangent_yaw
p1_tangent_yaw = ch.get_yaw(p1) - yaw_diff
# helper pose for line creation
point_x = Pose()
point_x.position.x = 1.0
point_x.orientation.w = 1.0
# define perpendicular pose at p1
p1_cross = Pose()
p1_cross.position = p1.position
p1_cross.orientation = ch.quat_from_yaw(p1_tangent_yaw + 0.5 * pi)
# create lines along p1 pm -> crossing should be cor
p1_center_line = ch.get_line(p1.position, ch.transform_pose(p1_cross, point_x).position)
pm_center_line = ch.get_line(pm.position, ch.transform_pose(pm_cross, point_x).position)
# find crossing and return cor
cor_xy = ch.intersection(p1_center_line, pm_center_line)
cor = False
if(cor_xy):
cor = Pose()
cor.position.x = cor_xy[0]
cor.position.y = cor_xy[1]
cor.orientation.w = 1.0
return cor
def se2_pose_distance(p1, p2):
p_dist = linalg_dist(p1, p2)
o_dist = abs(ch.get_yaw(p1) - ch.get_yaw(p2)) % (2 * pi)
o_dist = o_dist if o_dist < pi else 2 * pi - o_dist
return p_dist + 0.5 * o_dist
def steer_with_cor(req):
global push_predictor
res = SteerPushResponse()
res.success = False
start = req.start
goal = req.goal
goal_threshold = 0.05
for i in range(100):
# sample random push control and predict pose
control = np.random.uniform(0,1,3)
push = control_to_push(control)
pose = push_predictor.predict_pose(push)
# get cor of sampled pose or continue
cor = get_cor(start, ch.transform_pose(start,pose))
if not cor:
continue
radius = linalg_dist(start, cor)
cor_to_goal = ch.get_diff_pose(cor, goal)
# check for goal distance
dist_cor_goal = np.linalg.norm([cor_to_goal.position.x, cor_to_goal.position.y])
goal_distance_error = abs(radius - dist_cor_goal)
# check sampled push might be a candidate
if goal_distance_error < goal_threshold:
cor_to_start = ch.get_diff_pose(cor, start)
start_yaw = ch.get_yaw(cor_to_start)
start_tang_yaw = atan2(cor_to_start.position.y, cor_to_start.position.x) + 0.5*pi
# difference of pose rotation from tangent
diff_yaw = start_tang_yaw - start_yaw
# compute best target candidate
target = Pose()
target.position.x = radius * cor_to_goal.position.x / dist_cor_goal
target.position.y = radius * cor_to_goal.position.y / dist_cor_goal
target_tang_yaw = atan2(target.position.y, target.position.x) + 0.5*pi
target_yaw = target_tang_yaw - diff_yaw
goal_yaw_error = abs(target_yaw - ch.get_yaw(cor_to_goal))
# check if we found a solution
if ( goal_distance_error + 0.5 * goal_yaw_error ) < goal_threshold:
print "Found new steer control"
res.control = control
single_push_yaw = ch.get_yaw(pose)
rotate_positive = single_push_yaw > 0.0
# test if we need angle wrapping
if rotate_positive and target_yaw < start_yaw:
target_yaw += 2 * pi
elif not rotate_positive and target_yaw > start_yaw:
target_yaw -= 2 * pi
# duration is computed by dividing the complete diff angle by step angle
res.duration = abs(target_yaw - start_yaw) / ch.get_yaw(pose)
res.success = True
break
return res
def simple_steer_with_cor(req, range_rejection=True):
global push_predictor
res = SteerPushResponse()
res.success = False
start = Pose()
start.orientation.w = 1.0
goal = ch.transform_pose(req.start, req.goal)
# if target is too far off
start_goal_distance = linalg_dist(start, goal)
if range_rejection and start_goal_distance > 0.05:
return res
# default push direction is straight towards goal
push_direction = atan2(goal.position.y, goal.position.x)
# check if we find a good curve around cor
cor = get_cor(start, goal)
if cor and linalg_dist(start, cor) < 10 * start_goal_distance:
cor_phi = atan2(cor.position.y, cor.position.x)
# flip push direction 'upwards'
push_direction = cor_phi + 0.5 * pi * (1 if cor.position.x > 0 else -1)
goal_yaw = ch.get_yaw(goal) % (2 * pi) - pi
# if the other direction is faster, reverse push angle
if ( pi - goal_yaw ) * push_direction < 0:
push_direction = -push_direction
# if push direction is out of pushable range (+-0.5 rad from surface)
if not push_direction_valid(push_direction):
return res
goal_threshold = 0.1
for i in range(20):
# sample random push control and predict pose
#res.control = np.random.uniform(0,1,3)
#push = control_to_push(res.control)
control, push = sample_directed_control(push_direction, 0.1)
pose = push_predictor.predict_pose(push)
min_dist = se2_pose_distance(start, goal)
res.duration = 0
next_pose = ch.transform_pose(start, pose)
next_dist = se2_pose_distance(next_pose, goal)
while next_dist < min_dist:
res.duration += 1
min_dist = next_dist
next_pose = ch.transform_pose(next_pose, pose)
next_dist = se2_pose_distance(next_pose, goal)
if res.duration > 0 and min_dist < goal_threshold:
res.success = True
res.control = control
print "Found steer control", res.duration
break
return res