Ejemplo n.º 1
0
    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
Ejemplo n.º 2
0
	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
Ejemplo n.º 3
0
    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()
Ejemplo n.º 4
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 goal_pp trajectory generator. Setting twist to 0')
Ejemplo n.º 5
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')
Ejemplo n.º 6
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
Ejemplo n.º 7
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
Ejemplo n.º 8
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
        #   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
Ejemplo n.º 9
0
    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)
Ejemplo n.º 10
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
        #   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
Ejemplo n.º 11
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 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)
Ejemplo n.º 12
0
    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)
Ejemplo n.º 13
0
    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
Ejemplo n.º 14
0
    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
Ejemplo n.º 15
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 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)
Ejemplo n.º 16
0
 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)
Ejemplo n.º 17
0
	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())
Ejemplo n.º 18
0
    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())