コード例 #1
0
    def angle_divide(self,angle_start,angle_stop):
        angle_list = []
        vel_mag_list = []
        theta_diff = CircleFunctions.circle_dist(angle_start,angle_stop)
        # if abs(theta_diff) < self.on_setpoint_theta_dist:
        #     return angle_list,vel_mag_list
        if 0 < theta_diff:
            if angle_stop < angle_start:
                angle_start = angle_start - 2*math.pi
        else:
            if angle_start < angle_stop:
                angle_stop = angle_stop - 2*math.pi
        # rospy.logwarn("angle_start = \n%s" % (str(angle_start)))
        # rospy.logwarn("angle_stop = \n%s" % (str(angle_stop)))
        # rospy.logwarn("diff = \n%s" % (str(diff)))
        point_count = 0
        r = self.setpoint.radius
        theta_diff_positive = 0 < theta_diff
        finished = False
        if 0 < r:
            vel_mag_list = [1]              # Use dummy first value...
            angle = angle_start
            angle_list.append(angle)
            while (not finished) and (point_count <= self.point_count_max):
                vel_mag = self.find_theta_vel_mag(theta_diff,r)
                point_count += 1
                chord_length = vel_mag/self.ltm.freq
                # rospy.logwarn("chord_length = %s" % (str(chord_length)))
                angle_inc = chord_length/r
                if not theta_diff_positive:
                    angle_inc = - angle_inc
                angle += angle_inc
                theta_diff = CircleFunctions.circle_dist(angle,angle_stop)
                if (abs(theta_diff) < self.on_setpoint_theta_mag) or \
                   ((theta_diff < 0) and theta_diff_positive) or \
                   ((0 < theta_diff) and (not theta_diff_positive)):
                    finished = True
                else:
                    vel_mag_list.append(vel_mag)
                    angle_list.append(angle)

            # point_count = math.ceil(abs(diff)/angle_inc)
            # if self.point_count_max < point_count:
            #     point_count = self.point_count_max
            # angle_list = list(numpy.linspace(angle_start,angle_stop,num=point_count,endpoint=True))
        # rospy.logwarn("angle_list = \n%s" % (str(angle_list)))
        return angle_list,vel_mag_list,theta_diff_positive
コード例 #2
0
ファイル: Fly.py プロジェクト: sagrawal/Flylab
    def GetNextFlipValue(self):
        # Get the prior flip value.
        flipValuePrev = self.lpFlip.GetValue()
        if flipValuePrev is None:
            flipValuePrev = 0.0

        # angleContour is ambiguous mod pi radians
        eccmetric = (self.contour.ecc + 1/self.contour.ecc) - 1
        #rospy.loginfo('%s: eccmetric=%0.2f' % (self.name, eccmetric))
        if (eccmetric > 1.5):
            angleContour         = self.contour.angle #self.YawFromQuaternion(self.state.pose.orientation)
            angleContour_flipped = angleContour + N.pi
            
            # Most recent angle of travel.
            if self.angleOfTravelRecent is not None:
                angleOfTravel = self.angleOfTravelRecent
            else:
                angleOfTravel = angleContour
                
                
            # Compare distances between angles of (travel - orientation) and (travel - flippedorientation)        
            dist         = N.abs(CircleFunctions.circle_dist(angleContour,         angleOfTravel))
            dist_flipped = N.abs(CircleFunctions.circle_dist(angleContour_flipped, angleOfTravel))
            
            # Vote for flipped or non-flipped.
            if dist < dist_flipped:
                flipValueNew = -1.0 # Not flipped
            else:
                flipValueNew =  1.0 # Flipped
        else:
            flipValueNew = flipValuePrev

        # Weight the prev/new values based on speed.                
        speed = N.linalg.norm([self.state.velocity.linear.x, self.state.velocity.linear.y])
        alpha = 10.0/(10.0+speed)
        flipValueWeighted = (alpha * flipValuePrev) + ((1.0-alpha) * flipValueNew) 

        
        #if 'Fly1' in self.name:
        #    rospy.logwarn('%s: flipValue %0.2f, %0.2f, %0.2f' % (self.name, flipValuePrev,flipValueWeighted,flipValueNew))


        return flipValueWeighted
コード例 #3
0
 def angle_divide(self, angle_start, angle_stop):
     diff = CircleFunctions.circle_dist(angle_start, angle_stop)
     if 0 < diff:
         if angle_stop < angle_start:
             angle_start = angle_start - 2 * math.pi
     else:
         if angle_start < angle_stop:
             angle_stop = angle_stop - 2 * math.pi
     # rospy.logwarn("angle_start = \n%s" % (str(angle_start)))
     # rospy.logwarn("angle_stop = \n%s" % (str(angle_stop)))
     # rospy.logwarn("diff = \n%s" % (str(diff)))
     r = self.setpoint.radius
     if 0 < r:
         angle_inc = self.chord_length / r
         point_count = math.ceil(abs(diff) / angle_inc)
         if self.point_count_max < point_count:
             point_count = self.point_count_max
         angle_list = list(numpy.linspace(angle_start, angle_stop, num=point_count, endpoint=True))
     else:
         angle_list = []
     # rospy.logwarn("angle_list = \n%s" % (str(angle_list)))
     return angle_list
コード例 #4
0
ファイル: Fly.py プロジェクト: sagrawal/Flylab
    def SetFlipState(self):
        # Update the flip filter, and convert it to a flip state.
        flipValuePre = self.GetNextFlipValue()
        flipValuePost = self.lpFlip.Update(flipValuePre, self.contour.header.stamp.to_sec())
        if flipValuePost > 0.0:
            flipNew = True
        else:
            flipNew = False

        speed = N.linalg.norm([self.state.velocity.linear.x, self.state.velocity.linear.y])
        if speed > 3.0: # Deadband.
            self.flip = flipNew    
                
        # contour angle only ranges on [-pi,-0].  If wrapped, then change the flip state.
        #if 'Fly1' in self.name:
        #    rospy.logwarn('self.flip=%s'%self.flip)
        if (self.contourPrev is not None) and (isinstance(self.contourPrev.angle,float)) and (self.contour is not None) and (isinstance(self.contour.angle,float)):
            #rospy.logwarn('contour.angle=%s, contourPrev.angle=%s'%(self.contour.angle, self.contourPrev.angle))
            d = N.abs(CircleFunctions.circle_dist(self.contour.angle, self.contourPrev.angle))
            if (d > (N.pi/2.0)):
                #if 'Fly1' in self.name:
                #    rospy.logwarn('%s: wrap %0.2f, %0.2f, d=%0.2f'%(self.name, self.contour.angle, self.contourPrev.angle,d))
                self.lpFlip.SetValue(-self.lpFlip.GetValue())
                self.flip = not self.flip
コード例 #5
0
    def find_robot_setpoint_error(self):
        self.robot_arena = self.convert_to_arena(self.robot_origin)
        self.robot_control_frame = self.convert_to_control_frame(self.robot_origin)
        dx = self.robot_control_frame.point.x
        dy = self.robot_control_frame.point.y
        self.robot_control_frame_radius = math.sqrt(dx**2 + dy**2)
        self.robot_control_frame_theta = math.atan2(dy,dx)
        self.radius_error = self.setpoint.radius - self.robot_control_frame_radius
        self.theta_error = CircleFunctions.circle_dist(self.robot_control_frame_theta,self.setpoint.theta)
        # rospy.logwarn("radius_error = %s" % (str(radius_error)))
        # rospy.logwarn("theta_error = %s" % (str(theta_error)))

        # self.on_setpoint_radius = abs(radius_error) < self.on_setpoint_radius_dist
        # self.on_setpoint_theta = abs(theta_error) < self.on_setpoint_theta_dist

        # if (not self.on_setpoint_radius) and (not self.on_setpoint_theta):
        #     if abs(radius_error) < self.on_setpoint_radius_dist:
        #         self.on_setpoint_radius = True
        #         # rospy.logwarn("on setpoint radius, not on setpoint theta")
        # elif self.on_setpoint_radius and (not self.on_setpoint_theta):
        #     if abs(theta_error) < self.on_setpoint_theta_dist:
        #         self.on_setpoint_theta = True
        #         # rospy.logwarn("on setpoint radius, on setpoint theta")
        # elif self.on_setpoint_theta:
        #     self.on_setpoint_radius = abs(self.radius_error) < self.on_setpoint_radius_dist
        if not self.ltm.in_progress:
            self.on_setpoint_radius = abs(self.radius_error) < self.on_setpoint_dist
            if self.on_setpoint_radius:
                self.near_setpoint_radius = False
            else:
                self.near_setpoint_radius = abs(self.radius_error) < self.near_setpoint_dist
        else:
            self.on_setpoint_radius = abs(self.radius_error) < self.lookup_table_move_setpoint_dist_multiplier*self.on_setpoint_dist
            if self.on_setpoint_radius:
                self.near_setpoint_radius = False
            else:
                self.near_setpoint_radius = abs(self.radius_error) < self.lookup_table_move_setpoint_dist_multiplier*self.near_setpoint_dist

        if self.setpoint_radius_thresh < self.setpoint.radius:
            self.on_setpoint_theta_mag = 2*math.atan(self.on_setpoint_dist/(2*self.setpoint.radius))
            self.near_setpoint_theta_mag = 2*math.atan(self.near_setpoint_dist/(2*self.setpoint.radius))

            self.on_setpoint_theta = abs(self.theta_error) < self.on_setpoint_theta_mag
            if self.on_setpoint_theta:
                self.near_setpoint_theta = False
            else:
                self.near_setpoint_theta = abs(self.theta_error) < self.near_setpoint_theta_mag

            if self.ltm.in_progress and \
               ((not self.on_setpoint_theta) and (not self.near_setpoint_theta)) and \
               (((self.theta_error < 0) and self.ltm.direction_positive) or ((0 < self.theta_error) and (not self.ltm.direction_positive))):
                rospy.logwarn("using theta_error and ltm.direction_positive...")
                rospy.logwarn("theta_error = %s" % (str(self.theta_error)))
                rospy.logwarn("ltm.direction_positive = %s" % (str(self.ltm.direction_positive)))
                # self.ltm.stop_move()
                # self.on_setpoint_theta = False
                # self.near_setpoint_theta = True

        else:
            self.on_setpoint_theta_mag = 0
            self.near_setpoint_theta_mag = 0
            self.on_setpoint_theta = True
            self.near_setpoint_theta = False