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