def dock(self):
Vr_to_t = self.search()
#angle_g_to_t=math.atan2((self.goal[1] - self.search()[1]), (self.goal[0] - self.search()[0]))
#angle_r_to_t=math.atan2((self.X[1] - self.search()[1]), (self.X[0] - self.search()[0]))
#theta = angle_r_to_t - angle_g_to_t
box_location = BoxLocation(x=self.box_pos[0], y=self.box_pos[1])
goal = Point(x=self.goal[0], y=self.goal[1])
robot_location = RobotLocation(x=self.X[0], y=self.X[1])
angle_g_to_t = Motion.angle_between_points(box_location, goal)
angle_r_to_t = Motion.angle_between_points(box_location,
robot_location)
theta = Motion.delta_between_angles(angle_g_to_t, angle_r_to_t)
if theta < 0:
Vperp = np.array([-Vr_to_t[1], Vr_to_t[0],
Vr_to_t[2]]) # turn left?
theta = math.pi + theta
else:
Vperp = np.array([Vr_to_t[1], -Vr_to_t[0],
Vr_to_t[2]]) # turn right?
theta = math.pi - theta
alpha = theta / THETA_MAX
if theta > THETA_MAX:
#print 'v perp', Vperp
return Vperp
else:
#print 'v perp + ...', alpha*Vperp + (1-alpha)*Vr_to_t
return alpha * Vperp + (1 - alpha) * Vr_to_t
def angle_to_point_behind_box(self):
angle_g_to_t = Motion.angle_between_points(self.boxLocation,
self.target)
angle_r_to_t = Motion.angle_between_points(self.boxLocation,
self.robotLocation)
theta = Motion.delta_between_angles(angle_g_to_t, angle_r_to_t)
theta = Motion.fix_angle_180(theta)
return theta
def dock(self): Vr_to_t = self.search() #angle_g_to_t=math.atan2((self.goal[1] - self.search()[1]), (self.goal[0] - self.search()[0])) #angle_r_to_t=math.atan2((self.X[1] - self.search()[1]), (self.X[0] - self.search()[0])) #theta = angle_r_to_t - angle_g_to_t box_location = BoxLocation(x=self.box_pos[0],y=self.box_pos[1]) goal=Point(x=self.goal[0],y=self.goal[1]) robot_location=RobotLocation(x=self.X[0],y=self.X[1]) angle_g_to_t= Motion.angle_between_points(box_location, goal) angle_r_to_t= Motion.angle_between_points(box_location, robot_location) theta = Motion.delta_between_angles(angle_g_to_t,angle_r_to_t) if theta < 0: Vperp = np.array([-Vr_to_t[1],Vr_to_t[0],Vr_to_t[2]]) # turn left? theta = math.pi + theta else: Vperp = np.array([Vr_to_t[1],-Vr_to_t[0],Vr_to_t[2]]) # turn right? theta = math.pi - theta alpha = theta/THETA_MAX if theta > THETA_MAX: #print 'v perp', Vperp return Vperp else: #print 'v perp + ...', alpha*Vperp + (1-alpha)*Vr_to_t return alpha*Vperp + (1-alpha)*Vr_to_t
def angle_to_point_behind_box(self):
angle_g_to_t= Motion.angle_between_points(self.boxLocation, self.target)
angle_r_to_t= Motion.angle_between_points(self.boxLocation, self.robotLocation)
theta = Motion.delta_between_angles(angle_g_to_t,angle_r_to_t)
theta = Motion.fix_angle_180(theta)
return theta
