def new_goal(self): goal = self.moveto_as.accept_new_goal() self.desired_position = tools.position_from_posetwist(goal.posetwist) self.desired_orientation = tools.orientation_from_posetwist( goal.posetwist) #self.linear_tolerance = goal.linear_tolerance #self.angular_tolerance = goal.angular_tolerance rospy.loginfo('Desired position:' + str(self.desired_position)) rospy.loginfo('Current position:' + str(self.current_position)) rospy.loginfo('Desired orientation:' + str(self.desired_orientation)) rospy.loginfo('Current orientation:' + str(self.current_orientation)) rospy.loginfo('linear_tolerance:' + str(self.linear_tolerance)) rospy.loginfo('angular_tolerance:' + str(self.angular_tolerance)) # This controller doesn't handle desired twist #self.desired_twist = goal.posetwist.twist if (xyz_array(goal.posetwist.twist.linear).any() or xyz_array(goal.posetwist.twist.angular).any()): rospy.logwarn( 'None zero are not handled by the tank steer trajectory generator. Setting twist to 0' ) if np.linalg.norm(self.current_position - self.desired_position) > self.orientation_radius: self.line = line(self.current_position, self.desired_position) self.redraw_line = True else: self.line = line( self.current_position, tools.normal_vector_from_rotvec(self.desired_orientation) + self.current_position) self.redraw_line = False
def new_goal(self): goal = self.moveto_as.accept_new_goal() self.desired_position = tools.position_from_posetwist(goal.posetwist) self.desired_orientation = tools.orientation_from_posetwist(goal.posetwist) #self.linear_tolerance = goal.linear_tolerance #self.angular_tolerance = goal.angular_tolerance rospy.loginfo('Desired position:' + str(self.desired_position)) rospy.loginfo('Current position:' + str(self.current_position)) rospy.loginfo('Desired orientation:' + str(self.desired_orientation)) rospy.loginfo('Current orientation:' + str(self.current_orientation)) rospy.loginfo('linear_tolerance:' + str(self.linear_tolerance)) rospy.loginfo('angular_tolerance:' + str(self.angular_tolerance)) # This controller doesn't handle desired twist #self.desired_twist = goal.posetwist.twist if (xyz_array(goal.posetwist.twist.linear).any() or xyz_array(goal.posetwist.twist.angular).any() ): rospy.logwarn('None zero are not handled by the tank steer trajectory generator. Setting twist to 0') if np.linalg.norm(self.current_position - self.desired_position) > self.orientation_radius: self.line = line(self.current_position, self.desired_position) self.redraw_line = True else: self.line = line(self.current_position, tools.normal_vector_from_rotvec(self.desired_orientation) + self.current_position) self.redraw_line = False
def new_goal(self): # Lock on odom cb #print 'New goal: wait for lock' self.as_lock.acquire() #print 'New goal lock' goal = self.moveto_as.accept_new_goal() pt = goal.posetwist self.desired_position = tools.position_from_posetwist(pt) self.desired_orientation = tools.orientation_from_posetwist(pt) #self.linear_tolerance = goal.linear_tolerance #self.angular_tolerance = goal.angular_tolerance rospy.logdebug('Desired position:' + str(self.desired_position)) rospy.logdebug('Current position:' + str(self.current_position)) rospy.logdebug('Desired orientation:' + str(self.desired_orientation * 180 / np.pi)) rospy.logdebug('Current orientation:' + str(self.current_orientation * 180 / np.pi)) rospy.logdebug('linear_tolerance:' + str(self.linear_tolerance)) rospy.logdebug('angular_tolerance:' + str(self.angular_tolerance)) # Pass the new goal to the trajectory generator if self.traj_gen is not None: self.traj_gen.new_goal(pt) # Release lock self.as_lock.release()
def new_goal(self, goal): self.desired_position = tools.position_from_posetwist(goal) self.desired_orientation = tools.orientation_from_posetwist(goal) # Twist not supported if (xyz_array(goal.twist.linear).any() or xyz_array(goal.twist.angular).any() ): rospy.logwarn('None zero twist are not handled by the azi goal_pp trajectory generator. Setting twist to 0')
def new_goal(self, goal): self.desired_position = tools.position_from_posetwist(goal) self.desired_orientation = tools.orientation_from_posetwist(goal) # Twist not supported if (xyz_array(goal.twist.linear).any() or xyz_array(goal.twist.angular).any() ): rospy.logwarn('None zero twist are not handled by the azi a_star_rpp trajectory generator. Setting twist to 0')
def start(self, current_pt): # Set current position and twist self.current_position = tools.position_from_posetwist(current_pt) # Zero the Z self.current_position[2] = 0 self.current_orientation = tools.orientation_from_posetwist(current_pt) # Initlize Trajectory generator with current position as goal # Set the line to be along our current orientation self.desired_position = self.current_position self.desired_orientation = self.current_orientation
def start(self, current_pt): # Set current position and twist self.current_position = tools.position_from_posetwist(current_pt) # Zero the Z self.current_position[2] = 0 self.current_orientation = tools.orientation_from_posetwist(current_pt) # Initlize Trajectory generator with current position as goal # Set the line to be along our current orientation self.desired_position = self.current_position self.desired_orientation = self.current_orientation # Make a line along the orientation self.line = line(self.current_position, tools.normal_vector_from_rotvec(self.current_orientation) + self.current_position) self.redraw_line = False
def feedback(self, pt): # Update current pt self.current_position = tools.position_from_posetwist(pt) # Zero the Z self.current_position[2] = 0 self.current_orientation = tools.orientation_from_posetwist(pt) # Get distance to the goal if np.array_equal(self.current_position, self.desired_position): self.line = line(self.desired_position, tools.normal_vector_from_rotvec(self.desired_orientation) + self.desired_position) else: self.line = line(self.current_position, self.desired_position) self.distance_to_goal = np.linalg.norm(self.line.s)
def new_goal(self, goal): self.desired_position = tools.position_from_posetwist(goal) self.desired_orientation = tools.orientation_from_posetwist(goal) # Twist not supported if (xyz_array(goal.twist.linear).any() or xyz_array(goal.twist.angular).any() ): rospy.logwarn('None zero twist are not handled by the azi point and shoot trajectory generator. Setting twist to 0') if np.linalg.norm(self.current_position - self.desired_position) > self.orientation_radius: #print 'Far out dude' self.line = line(self.current_position, self.desired_position) else: #print 'Pretty close' self.line = line(self.desired_position, tools.normal_vector_from_rotvec(self.desired_orientation) + self.desired_position)
def feedback(self, pt): # Update current pt self.current_position = tools.position_from_posetwist(pt) # Zero the Z self.current_position[2] = 0 self.current_orientation = tools.orientation_from_posetwist(pt) # Get distance to the goal if np.array_equal(self.current_position, self.desired_position): self.line = line( self.desired_position, tools.normal_vector_from_rotvec(self.desired_orientation) + self.desired_position) else: self.line = line(self.current_position, self.desired_position) self.distance_to_goal = np.linalg.norm(self.line.s)
def feedback(self, pt): # Update current pt self.current_position = tools.position_from_posetwist(pt) # Zero the Z self.current_position[2] = 0 self.current_orientation = tools.orientation_from_posetwist(pt) # Get distance to the goal vector_to_goal = self.desired_position - self.current_position self.distance_to_goal = np.linalg.norm(vector_to_goal) # overshoot is 1 if behind line drawn perpendicular to the goal line and through the desired position, -1 if on the other # Side of said line overshoot = np.dot(vector_to_goal / self.distance_to_goal, self.line.hat) self.overshoot = overshoot / abs(overshoot) # In the case that overshoot is 0 or otherwise fails default to not overshoot if math.isnan(self.overshoot): self.overshoot = 1
def new_goal(self, goal): self.desired_position = tools.position_from_posetwist(goal) self.desired_orientation = tools.orientation_from_posetwist(goal) # Twist not supported if (xyz_array(goal.twist.linear).any() or xyz_array(goal.twist.angular).any()): rospy.logwarn( 'None zero twist are not handled by the azi point and shoot trajectory generator. Setting twist to 0' ) if np.linalg.norm(self.current_position - self.desired_position) > self.orientation_radius: #print 'Far out dude' self.line = line(self.current_position, self.desired_position) else: #print 'Pretty close' self.line = line( self.desired_position, tools.normal_vector_from_rotvec(self.desired_orientation) + self.desired_position)
def feedback(self, pt): # Update current pt self.current_position = tools.position_from_posetwist(pt) # Zero the Z self.current_position[2] = 0 self.current_orientation = tools.orientation_from_posetwist(pt)
def traj_cb(self, traj): # Timing now = rospy.get_rostime() dt = (now - self.last_time).to_sec() self.last_time = now # Get vectors related to the orientation of the trajectory o = tools.normal_vector_from_posetwist(traj.posetwist) #rospy.loginfo('o : ' + str(o)) o_hat = o / np.linalg.norm(o) #rospy.loginfo('o hat: ' + str(o_hat)) o_norm = np.cross([0, 0, -1], o_hat) #rospy.loginfo('o norm: ' + str(o_norm)) # Overshoot = 1 if the boat is behind a line drawn perpendicular to the trajectory orientation # and through the trajectories position, it is -1 if it is on the other side of the before mentioned line traj_position = tools.position_from_posetwist(traj.posetwist) boat_proj = line(traj_position, traj_position + o_hat).proj_pt(self.current_position) #rospy.loginfo('Boat_proj: ' + str(boat_proj)) #rospy.loginfo('traj: ' + str(traj_position)) #rospy.loginfo('traj - Boat_proj: ' + str(traj_position - boat_proj)) boat_to_traj = traj_position - self.current_position boat_dist_to_traj = np.linalg.norm(boat_to_traj) boat_to_traj_hat = boat_to_traj / boat_dist_to_traj boat_to_traj_norm = np.cross([0,0,-1], boat_to_traj_hat) overshoot = np.dot(boat_to_traj_hat, o_hat) overshoot = overshoot / abs(overshoot) rospy.loginfo('overshoot: ' + str(overshoot)) if math.isnan(overshoot): return # Method 2: # ## P error = - angle(between boat and trajectory) # #enu_angle_to_traj = np.arccos(boat_to_traj_hat, np.array([1, 0, 0]))) #enu_angle_to_traj = math.copysign(enu_angle_to_traj, np.dot(boat_to_traj_norm, self.current_position)) #boat_angle_to_traj = (self.current_orientation[2] - enu_angle_to_traj) % (np.pi) boat_orientation_vec = tools.normal_vector_from_rotvec(self.current_orientation) #angle_error = 0 #torque_dir = 1 if boat_dist_to_traj > self.orientation_radius: rospy.loginfo('Location: Outside orientation radius') # Is the angle to trajectory positive or negative ( dot product of two unit vectors is negative when the # angle between them is greater than 90, by doting the orientation of the boat with a vector that is 90 degrees # to the trajectory and pointed to the right we get positive numbers if we are pointed to the right and negative # if we are on the left side) torque_dir = math.copysign(1, np.dot(boat_to_traj_norm, boat_orientation_vec)) # angle between path to traj and boat orientation angle_error = np.arccos(np.dot(boat_orientation_vec, boat_to_traj_hat)) else: rospy.loginfo('Location: Inside orientation radius') # Same as above but now we are orienting to the desired final orientation instead of towards the trajectory torque_dir = math.copysign(1, np.dot(o_norm, boat_orientation_vec)) # Angle between traj_orientation and boat orientation angle_error = np.arccos(np.dot(boat_orientation_vec, o_hat)) #rospy.loginfo('enu_angle_to_traj:\t' + str(enu_angle_to_traj * 180 / np.pi)) #rospy.loginfo('Boat orientation:\t' + str(self.current_orientation[2] * 180 / np.pi)) #rospy.loginfo('Boat angle to traj\t' + str((boat_angle_to_traj) * 180 / np.pi )) torque = angle_error * torque_dir * self.p force = traj.posetwist.twist.linear.x if boat_dist_to_traj < self.orientation_radius and overshoot == -1: rospy.loginfo('Status: Overshoot in orientation radius') force = force * -1 elif abs(angle_error) > np.pi / 2: rospy.loginfo('Status: angle error > 90') force = 0 else: rospy.loginfo('Status: Normal') rospy.loginfo('Angle error: ' + str(angle_error * torque_dir * 180 / np.pi)) rospy.loginfo('torque: ' + str(torque)) rospy.loginfo('Force: ' + str(force)) """ Method 1: # ## P error = - perpendicular_velocity * p_gain * overshoot # # Velocity error = velocity perpendicular to trajectory orientation velocity_error = self.current_velocity - (np.dot(self.current_velocity, o_hat) * o_hat) v_dir = np.dot(velocity_error, o_norm) velocity_error = math.copysign(np.linalg.norm(velocity_error), v_dir) rospy.loginfo('V_perp = ' + str(velocity_error)) velocity_error = -1 * velocity_error * self.p * overshoot rospy.loginfo('P error = ' + str(velocity_error)) # ## I error = - position * i_gain * overshoot * ??? # # Positional error # boat_position parralel portion of boat position pos_error = self.current_position - boat_proj p_dir = np.dot(pos_error, o_norm) pos_error = math.copysign(np.linalg.norm(pos_error), p_dir) rospy.loginfo('Perp_position = ' + str(pos_error)) pos_error = -1 * pos_error * self.i * overshoot rospy.loginfo('I error = ' + str(pos_error)) # ## D error # # Acceleration error acceleration_error = self.d * (velocity_error - self.last_perp_velocity) / dt self.last_perp_velocity = velocity_error torque = velocity_error + pos_error + acceleration_error # Reverse force if overshoot force = traj.posetwist.twist.linear.x * overshoot rospy.loginfo('torque: ' + str(torque)) """ # Output a wrench! if not self.killed: self.wrench_pub.publish( WrenchStamped( header = Header( frame_id = '/base_link', stamp = now), wrench = Wrench( force = Vector3(force, 0, 0), torque = Vector3(0, 0, torque)))) else: # Publish zero wrench self.wrench_pub.publish(WrenchStamped())
def traj_cb(self, traj): # Timing now = rospy.get_rostime() dt = (now - self.last_time).to_sec() self.last_time = now # Get vectors related to the orientation of the trajectory o = tools.normal_vector_from_posetwist(traj.posetwist) #rospy.loginfo('o : ' + str(o)) o_hat = o / np.linalg.norm(o) #rospy.loginfo('o hat: ' + str(o_hat)) o_norm = np.cross([0, 0, -1], o_hat) #rospy.loginfo('o norm: ' + str(o_norm)) # Overshoot = 1 if the boat is behind a line drawn perpendicular to the trajectory orientation # and through the trajectories position, it is -1 if it is on the other side of the before mentioned line traj_position = tools.position_from_posetwist(traj.posetwist) boat_proj = line(traj_position, traj_position + o_hat).proj_pt(self.current_position) #rospy.loginfo('Boat_proj: ' + str(boat_proj)) #rospy.loginfo('traj: ' + str(traj_position)) #rospy.loginfo('traj - Boat_proj: ' + str(traj_position - boat_proj)) boat_to_traj = traj_position - self.current_position boat_dist_to_traj = np.linalg.norm(boat_to_traj) boat_to_traj_hat = boat_to_traj / boat_dist_to_traj boat_to_traj_norm = np.cross([0, 0, -1], boat_to_traj_hat) overshoot = np.dot(boat_to_traj_hat, o_hat) overshoot = overshoot / abs(overshoot) rospy.loginfo('overshoot: ' + str(overshoot)) if math.isnan(overshoot): return # Method 2: # ## P error = - angle(between boat and trajectory) # #enu_angle_to_traj = np.arccos(boat_to_traj_hat, np.array([1, 0, 0]))) #enu_angle_to_traj = math.copysign(enu_angle_to_traj, np.dot(boat_to_traj_norm, self.current_position)) #boat_angle_to_traj = (self.current_orientation[2] - enu_angle_to_traj) % (np.pi) boat_orientation_vec = tools.normal_vector_from_rotvec( self.current_orientation) #angle_error = 0 #torque_dir = 1 if boat_dist_to_traj > self.orientation_radius: rospy.loginfo('Location: Outside orientation radius') # Is the angle to trajectory positive or negative ( dot product of two unit vectors is negative when the # angle between them is greater than 90, by doting the orientation of the boat with a vector that is 90 degrees # to the trajectory and pointed to the right we get positive numbers if we are pointed to the right and negative # if we are on the left side) torque_dir = math.copysign( 1, np.dot(boat_to_traj_norm, boat_orientation_vec)) # angle between path to traj and boat orientation angle_error = np.arccos( np.dot(boat_orientation_vec, boat_to_traj_hat)) else: rospy.loginfo('Location: Inside orientation radius') # Same as above but now we are orienting to the desired final orientation instead of towards the trajectory torque_dir = math.copysign(1, np.dot(o_norm, boat_orientation_vec)) # Angle between traj_orientation and boat orientation angle_error = np.arccos(np.dot(boat_orientation_vec, o_hat)) #rospy.loginfo('enu_angle_to_traj:\t' + str(enu_angle_to_traj * 180 / np.pi)) #rospy.loginfo('Boat orientation:\t' + str(self.current_orientation[2] * 180 / np.pi)) #rospy.loginfo('Boat angle to traj\t' + str((boat_angle_to_traj) * 180 / np.pi )) torque = angle_error * torque_dir * self.p force = traj.posetwist.twist.linear.x if boat_dist_to_traj < self.orientation_radius and overshoot == -1: rospy.loginfo('Status: Overshoot in orientation radius') force = force * -1 elif abs(angle_error) > np.pi / 2: rospy.loginfo('Status: angle error > 90') force = 0 else: rospy.loginfo('Status: Normal') rospy.loginfo('Angle error: ' + str(angle_error * torque_dir * 180 / np.pi)) rospy.loginfo('torque: ' + str(torque)) rospy.loginfo('Force: ' + str(force)) """ Method 1: # ## P error = - perpendicular_velocity * p_gain * overshoot # # Velocity error = velocity perpendicular to trajectory orientation velocity_error = self.current_velocity - (np.dot(self.current_velocity, o_hat) * o_hat) v_dir = np.dot(velocity_error, o_norm) velocity_error = math.copysign(np.linalg.norm(velocity_error), v_dir) rospy.loginfo('V_perp = ' + str(velocity_error)) velocity_error = -1 * velocity_error * self.p * overshoot rospy.loginfo('P error = ' + str(velocity_error)) # ## I error = - position * i_gain * overshoot * ??? # # Positional error # boat_position parralel portion of boat position pos_error = self.current_position - boat_proj p_dir = np.dot(pos_error, o_norm) pos_error = math.copysign(np.linalg.norm(pos_error), p_dir) rospy.loginfo('Perp_position = ' + str(pos_error)) pos_error = -1 * pos_error * self.i * overshoot rospy.loginfo('I error = ' + str(pos_error)) # ## D error # # Acceleration error acceleration_error = self.d * (velocity_error - self.last_perp_velocity) / dt self.last_perp_velocity = velocity_error torque = velocity_error + pos_error + acceleration_error # Reverse force if overshoot force = traj.posetwist.twist.linear.x * overshoot rospy.loginfo('torque: ' + str(torque)) """ # Output a wrench! if not self.killed: self.wrench_pub.publish( WrenchStamped(header=Header(frame_id='/base_link', stamp=now), wrench=Wrench(force=Vector3(force, 0, 0), torque=Vector3(0, 0, torque)))) else: # Publish zero wrench self.wrench_pub.publish(WrenchStamped())