def getVelocity(self, cur, goal, prev): desired = Pose() d = self.getGoalDistance(cur, goal) self.theta_err = atan2(goal.y - cur.y, goal.x - cur.x) - self.theta_change # if self.theta_err > 3.14: # self.theta_err = self.theta_err - 6.28 # if self.theta_err < -3.14: # self.theta_err = self.theta_err + 6.28 desired.thetaVel = self.adjust_omega(self.theta_err) self.theta_change = atan2(cur.y - prev.y, cur.x - prev.x) if desired.thetaVel > self.OneDegree * 5: desired.xVel = self.k2 / abs(self.theta_err) if desired.xVel > 0.12: desired.xVel = 0.12 if desired.xVel < 0.07: desired.xVel = 0.07 elif abs(d) > self.linearTolerance: desired.xVel = self.k3 * d if desired.xVel > 0.12: desired.xVel = 0.12 if desired.xVel < 0.07: desired.xVel = 0.07 else: desired.xVel = 0 return desired # def getVelocity(self, cur, goal, prev, dT): # desired = Pose() # d = self.getGoalDistance(cur, goal) # a = atan2(goal.y - cur.y, goal.x - cur.x)# - cur.theta # b = atan2(cur.y - prev.y, cur.x - prev.x) # desired.thetaVel = self.adjust_omega(a, b) # # if a > 0: # # if a > (0.0872665 * 2 ): # # desired.thetaVel = 0.0872665 * 2 #+ self.kB*b # # else: # # desired.thetaVel = 0.0174533 * 2 # # elif a < 0: # # if a < (-0.0872665 * 2): # # desired.thetaVel = -0.0872665 * 2 #+ self.kB* # # else: # # desired.thetaVel = -0.0174533 * 2 # # else: # # desired.thetaVel = 0 # if abs(d) > self.linearTolerance: # desired.xVel = 0.2 # else: # desired.xVel = 0 # def adjust_omega(self, theta_final, theta_counter): # if abs(theta_final) > 0.349066: # limit = 0.0872665 # else: # limit = 0.0349066 # if (theta_final > theta_counter): # if (theta_final - theta_counter) > limit: # w_rand = limit # else: # w_rand = .01745 # elif (theta_final < theta_counter): # if (theta_final - theta_counter) < -limit: # w_rand = -limit # else: # w_rand = -.01745 # else: # w_rand = 0 # if abs(desired.thetaVel) > 1: # desired.thetaVel = 1 # return w_rand
def get_velocity(self, cur, goal, dT): desired = Pose() goal_heading = atan2(goal.y - cur.y, goal.x - cur.x) a = -cur.theta + goal_heading # In Automomous Mobile Robots, they assume theta_G=0. So for # the error in heading, we have to adjust theta based on the # (possibly non-zero) goal theta. theta = self.normalize_pi(cur.theta - goal.theta) b = -theta - a # rospy.loginfo('cur=%f goal=%f a=%f b=%f', cur.theta, goal_heading, # a, b) d = self.get_goal_distance(cur, goal) if self.forward_movement_only: direction = 1 a = self.normalize_pi(a) b = self.normalize_pi(b) else: direction = self.sign(cos(a)) a = self.normalize_half_pi(a) b = self.normalize_half_pi(b) # rospy.loginfo('After normalization, a=%f b=%f', a, b) if abs(d) < self.linear_tolerance: desired.xVel = 0 desired.thetaVel = self.kB * theta else: desired.xVel = self.kP * d * direction + (-self.error_prev + d) * 7 * direction print('iii: {}'.format(-self.error_prev + d)) desired.thetaVel = self.kA * a + self.kB * b self.error_prev = d # Adjust velocities if X velocity is too high. if abs(desired.xVel) > self.max_linear_speed: ratio = self.max_linear_speed / abs(desired.xVel) desired.xVel *= ratio desired.thetaVel *= ratio # Adjust velocities if turning velocity too high. if abs(desired.thetaVel) > self.max_angular_speed: ratio = self.max_angular_speed / abs(desired.thetaVel) desired.xVel *= ratio desired.thetaVel *= ratio # TBD: Adjust velocities if linear or angular acceleration # too high. # Adjust velocities if too low, so robot does not stall. if abs(desired.xVel) > 0 and abs(desired.xVel) < self.min_linear_speed: ratio = self.min_linear_speed / abs(desired.xVel) desired.xVel *= ratio desired.thetaVel *= ratio elif desired.xVel == 0 and abs( desired.thetaVel) < self.min_angular_speed: ratio = self.min_angular_speed / abs(desired.thetaVel) desired.xVel *= ratio desired.thetaVel *= ratio return desired