Exemple #1
0
def filter_reference():  # Kombiniert Beschleunigunssensoren und Gyrosensoren
    global sens_ref  # Teilt Werte mit anderen Threads
    offset = [0.0, 0.0]  # Offset definiert Nullpunkt
    sens_ref = [0.0, 0.0]
    out = [0.0, 0.0]
    iterations = 0
    k = 0.025  # Beschleunigt Kalibrierungsvorgang durch mehr Iterations/t
    time.sleep(1)
    while True:
        out[0] = (
            offset[0]
            + 0.99 * (out[0] + reference_gyro_omega[0] * DT - offset[0])
            + 0.01
            * (360 / (2 * math.pi))
            * (math.atan2(reference_accelerometer_acc["y"], reference_accelerometer_acc["z"]) + math.pi)
        )  # Berechnet x-Rotation
        out[1] = (
            offset[1]
            + 0.99 * (out[1] + reference_gyro_omega[1] * DT - offset[1])
            + 0.01
            * (360 / (2 * math.pi))
            * (math.atan2(reference_accelerometer_acc["z"], reference_accelerometer_acc["x"]) + math.pi)
        )  # Berechnet y-Rotation
        iterations += 1  # Zaehlt Ablaeufe
        time.sleep(k * DT)
        if iterations == 30 * (1 / DT):  # Setzt Nullpunkt nach einer Anzahl Zyklen
            offset[0] = 0.0 - out[0]  # Nullpunkt x
            offset[1] = 0.0 - out[1]  # Nullpunkt y
            out[0] = 0  # Setzt Ouput auf 0 um Inkrementierung zu vermeiden -> schnellere Anpassung
            out[1] = 0  # Setzt Ouput auf 0 um Inkrementierung zu vermeiden -> schnellere Anpassung
            k = 1
            print "Sensor 1 calibrated"
        sens_ref[0] = int(out[0])  # Teilt Wert als ganze Zahl
        sens_ref[1] = int(out[1])  # Teilt Wert als ganze Zahl
Exemple #2
0
    def legIK(self, X, Y, Z, resolution):
        """ Compute leg servo positions. """
        ans = [0,0,0,0]    # (coxa, femur, tibia)
       
        try:
            # first, make this a 2DOF problem... by solving coxa
            ans[0] = radToServo(atan2(X,Y), resolution)
            trueX = int(sqrt(sq(X)+sq(Y))) - self.L_COXA
            im = int(sqrt(sq(trueX)+sq(Z)))  # length of imaginary leg
            
            # get femur angle above horizon...
            q1 = -atan2(Z,trueX)
            d1 = sq(self.L_FEMUR)-sq(self.L_TIBIA)+sq(im)
            d2 = 2*self.L_FEMUR*im
            q2 = acos(d1/float(d2))
            ans[1] = radToServo(q1+q2, resolution)  
        
            # and tibia angle from femur...
            d1 = sq(self.L_FEMUR)-sq(im)+sq(self.L_TIBIA)
            d2 = 2*self.L_TIBIA*self.L_FEMUR;
            ans[2] = radToServo(acos(d1/float(d2))-1.57, resolution)
        except:
            if self.debug:
                "LegIK FAILED"
            return [1024,1024,1024,0]

        if self.debug:
            print "LegIK:",ans
        return ans
Exemple #3
0
    def update_sound(self):
        """
        Used to update sound_sensor continuously depending on how far bird
        would need to turn to face sound_origin
        """
        a = math.radians(self.angle)
        self.facing_vector = [math.cos(a), math.sin(a)] # get unit facing vector
        if self.sound_timer > 0: # only perform calculations if bird has a
            # sound in memory
            self.sound_timer -= 1 # forget sound over time


            # now get a unit vector in the direction of the sound
            dist = ((self.x - self.sound_origin[0]) ** 2 + (
            self.y - self.sound_origin[1]) ** 2) ** .5
            self.sound_direction = [-(self.x - self.sound_origin[0]) / dist,
                                    -(self.y - self.sound_origin[1]) / dist]

            # use atan2 to get the difference between facing vector and
            # direction vector into radians
            a_dif = math.atan2(self.sound_direction[1],
                               self.sound_direction[0]) - math.atan2(
                self.facing_vector[1], self.facing_vector[0])

            self.sound_sensors = [max(0, a_dif), max(0, -a_dif)] # positive
            # means left, negative means right
        else:
            self.sound_sensors = [0, 0]

            self.sound_direction = [0, 0]
Exemple #4
0
def filter_stabilized():  # Analog zu filter_reference()
    global sens_stab
    offset = [0.0, 0.0]
    sens_stab = [0.0, 0.0]
    out = [0.0, 0.0]
    iterations = 0
    k = 0.025
    time.sleep(1)
    while True:
        out[0] = (
            offset[0]
            + 0.99 * (out[0] + stabilized_gyro_omega[0] * DT - offset[0])
            + 0.01
            * (360 / (2 * math.pi))
            * (math.atan2(stabilized_accelerometer_acc["y"], stabilized_accelerometer_acc["z"]) + math.pi)
        )
        out[1] = (
            offset[1]
            + 0.99 * (out[1] + stabilized_gyro_omega[1] * DT - offset[1])
            + 0.01
            * (360 / (2 * math.pi))
            * (math.atan2(stabilized_accelerometer_acc["z"], stabilized_accelerometer_acc["x"]) + math.pi)
        )
        iterations += 1
        time.sleep(k * DT)
        if iterations == 30 * (1 / DT):
            offset[0] = 0.0 - out[0]
            offset[1] = 0.0 - out[1]
            out[0] = 0
            out[1] = 0
            print "Sensor 2 calibrated"
            k = 1
        sens_stab[0] = int(out[0])
        sens_stab[1] = int(out[1])
Exemple #5
0
def matrix_to_euler(rotmat):
    '''Inverse of euler_to_matrix().'''
    if not isinstance(rotmat, np.matrixlib.defmatrix.matrix):
        # As this calculation relies on np.matrix algebra - convert array to
        # matrix
        rotmat = np.matrix(rotmat)

    def cvec(x, y, z):
        return np.matrix([[x, y, z]]).T
    ex = cvec(1., 0., 0.)
    ez = cvec(0., 0., 1.)
    exs = rotmat.T * ex
    ezs = rotmat.T * ez
    enodes = np.cross(ez.T, ezs.T).T
    if np.linalg.norm(enodes) < 1e-10:
        enodes = exs
    enodess = rotmat * enodes
    cos_alpha = float((ez.T*ezs))
    if cos_alpha > 1.:
        cos_alpha = 1.
    if cos_alpha < -1.:
        cos_alpha = -1.
    alpha = acos(cos_alpha)
    beta = np.mod(atan2(enodes[1, 0], enodes[0, 0]), pi * 2.)
    gamma = np.mod(-atan2(enodess[1, 0], enodess[0, 0]), pi*2.)
    return unique_euler(alpha, beta, gamma)
 def GetAngleOfLineBetweenTwoPoints(self, p1, p2, angle_unit="degrees"):
     xDiff = p2.x() - p1.x()
     yDiff = p2.y() - p1.y()
     if angle_unit == "radians":
         return atan2(yDiff, xDiff)
     else:
         return degrees(atan2(yDiff, xDiff))
Exemple #7
0
def isRectangle(pts, seuil=0.05):
    if len(pts) != 4:
        raise ValueError('Input must be 4 points')
    coords = pts.ravel().reshape(-1, 2)
    cx, cy = np.mean([coord[0] for coord in coords]), np.mean(
        [coord[1] for coord in coords])
    dist = [distance((cx, cy), coord) for coord in coords]
    res = 0
    # print coords
    for i in xrange(1, 4):
        res += abs(dist[0] - dist[i])
    bias = res / distance(coords[1], coords[2])
    logging.info('Regtangle bias: %.3f', res)
    logging.info('Ratio bias: %.3f', bias)
    if bias < seuil:
        line1 = coords[3] - coords[0]
        line2 = coords[2] - coords[1]
        mean_radian = - \
            (math.atan2(line1[1], line1[0]) +
             math.atan2(line2[1], line2[0])) / 2
        inclination = math.degrees(mean_radian)  # / np.pi * 90
        logging.info('Document rotation: %.3f degree', inclination)
        return True, mean_radian
    else:
        return False, None
Exemple #8
0
    def update_arrow(self, pt, size=10, angle=PI_OVER_FOUR):
        line = None
        ang = None
        x, y = pt

        if self.partial and not self.segments:
            (x1, y1), (x2, y2) = self.start, self.end
            dx, dy = x2 - x1, y2 - y1
            ang = math.atan2(dy, dx)

        elif self.partial and self.segments:
            (xd, yd), (x1, y1) = self.segments[-1]
            (x2, y2) = self.end
            dx, dy = x2 - x1, y2 - y1
            ang = math.atan2(dy, dx)

        elif self.segments:
            ang = self.segments[-1].get_angle()

        if ang is not None:
            ang0 = ang + PI - angle
            ang1 = ang + PI + angle
            x0, y0 = x + size * math.cos(ang0), y + size * math.sin(ang0)
            x1, y1 = x + size * math.cos(ang1), y + size * math.sin(ang1)
            line = (x0, y0), (x, y), (x1, y1)

        self.arrow = line
def pointObjectToTarget(obj, targetLoc):
    dx = targetLoc.x - obj.location.x;
    dy = targetLoc.y - obj.location.y;
    dz = targetLoc.z - obj.location.z;
    xRad = math.atan2(dz, math.sqrt(dy**2 + dx**2)) + math.pi/2;
    zRad = math.atan2(dy, dx) - math.pi/2;
    obj.rotation_euler = mathutils.Euler((xRad, 0, zRad), 'XYZ');
		def ecef_to_lla(ecef):
			#earths's radius in meters
			a = 6378137 
			#eccentricity
			e = 8.1819190842622e-2  
			asq = math.pow(a,2)
			esq = math.pow(e,2)
			x = ecef[0]
			y = ecef[1]
			z = ecef[2]
			b = math.sqrt( asq * (1-esq) )
			bsq = math.pow(b,2)
			ep = math.sqrt( (asq - bsq)/bsq)
			p = math.sqrt( math.pow(x,2) + math.pow(y,2) )
			th = math.atan2(a*z, b*p)
			lon = math.atan2(y,x)
			lat = math.atan2( (z + math.pow(ep,2)*b*math.pow(math.sin(th),3) ), (p - esq*a*math.pow(math.cos(th),3)) )
			N = a/( math.sqrt(1-esq*math.pow(math.sin(lat),2)) )
			alt = p / math.cos(lat) - N
			#mod lat to keep it between 0 and 2 pi
			lon = lon % (2*math.pi)
			#changing radians to degrees
			lat = math.degrees(lat)
			lon = math.degrees(lon)
			#normalizing angle
			lat = normalizeAngle(lat)
			lon = normalizeAngle(lon)
			ret = (lat, lon)
			return ret;
 def update_searchlight_from_orientation_(self):
   qw, qx, qy, qz = self.move.get_orientation()
   # Convert quaternion to euler-angle.
   # Formulas from:
   # http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
   # but note that all values are negated.
   azimuth = 0
   elevation = 0
   test = qx * qy + qz * qw
   roll = math.asin(2 * test)
   if test > 0.499:
     azimuth = -2 * math.atan2(qx, qw)
     elevation = math.pi / 2
   elif test < -0.499:
     azimuth = 2 * math.atan2(qx, qw)
     elevation = 0
   else:
     azimuth = -math.atan2(2 * qy * qw - 2 * qx * qz,
                           1 - 2 * qy * qy - 2 * qz * qz)
     elevation = math.atan2(2 * qx * qw - 2 * qy * qz ,
                            1 - 2 * qx * qx - 2 * qz * qz)
   # print 'az %.2f el %.2f roll %.2f' % (
   #     azimuth * 57.2957795, elevation * 57.2957795, roll * 57.2957795)
   min_elevation = -1
   for searchlight in self.searchlights:
     min_elevation = max(min_elevation, searchlight.config.elevation_lower_bound)
   min_elevation = clamp_and_scale(min_elevation, -1, 1, 0, 90 * DEGREES_TO_RADIANS)
   if elevation < min_elevation:
     elevation = min_elevation
   self.reactor.callFromThread(self.target_angle, azimuth, elevation)
 def fusion_get_orientation(self):
     '''
     Fuses data from accelerometer and magnetometer.  Same algorithm as the
     original Adafruit 10-DOF function except pitch = -roll and roll = pitch
     since this makes much more sense for the board layout.
     Returns a tuple of (pitch, roll, heading)
     '''
     # Calculate pitch based only on accel
     (accelX, accelY, accelZ) = self.accel_get_raw()
     pitch = -atan2(accelY, accelZ)
     
     # Calculate roll based on pitch and accel
     if accelY * sin(-pitch) + accelZ * cos(-pitch) == 0:
         roll = pi / 2 if accelX > 0 else -pi / 2
     else:
         roll = atan(-accelX / (accelY * sin(-pitch) + accelZ * cos(-pitch)))
         
     # Calculate heading based on pitch, roll, and mag
     (magX, magY, magZ) = self.mag_get_raw()
     heading = atan2(magZ * sin(-pitch) - magY * cos(-pitch), 
                     magX * cos(roll) +
                     magY * sin(roll) * sin(-pitch) +
                     magZ * sin(roll) * cos(-pitch))
     
     return (pitch * 180 / pi, roll * 180 / pi, heading * 180 / pi)
Exemple #13
0
    def getValues(self):        
        accx = self.twos_comp_combine(self.BUS.read_byte_data(self.LSM, self.ACC_X_MSB), self.BUS.read_byte_data(self.LSM, self.ACC_X_LSB))
        accy = self.twos_comp_combine(self.BUS.read_byte_data(self.LSM, self.ACC_Y_MSB), self.BUS.read_byte_data(self.LSM, self.ACC_Y_LSB))
        accz = self.twos_comp_combine(self.BUS.read_byte_data(self.LSM, self.ACC_Z_MSB), self.BUS.read_byte_data(self.LSM, self.ACC_Z_LSB))

        gyrox = self.twos_comp_combine(self.BUS.read_byte_data(self.GYRO, self.GYRO_X_MSB), self.BUS.read_byte_data(self.GYRO, self.GYRO_X_LSB))
        gyroy = self.twos_comp_combine(self.BUS.read_byte_data(self.GYRO, self.GYRO_Y_MSB), self.BUS.read_byte_data(self.GYRO, self.GYRO_Y_LSB))
        gyroz = self.twos_comp_combine(self.BUS.read_byte_data(self.GYRO, self.GYRO_Z_MSB), self.BUS.read_byte_data(self.GYRO, self.GYRO_Z_LSB))

        rate_gyrox = gyrox*self.GYRO_ADD
        rate_gyroy = gyroy*self.GYRO_ADD
        rate_gyroz = gyroz*self.GYRO_ADD

        if (not self.FIRST):
            self.FIRST = True
            self.gyroXangle = rate_gyrox*self.DT
            self.gyroYangle = rate_gyroy*self.DT
            self.gyroZangle = rate_gyroz*self.DT
        else:
            self.gyroXangle += rate_gyrox*self.DT
            self.gyroYangle += rate_gyroy*self.DT
            self.gyroZangle += rate_gyroz*self.DT

        roll = int(round(math.degrees(math.atan2(accx, accz))))
        pitch = int(round(math.degrees(math.atan2(accy, accz))))

        print "Przechylenie: ", int(round(roll,0)), " Pochylenie: ", int(round(pitch,0))

        self.FILTR_X = self.MULTIPLY*(roll)+(1-self.MULTIPLY)*self.gyroXangle
        self.FILTR_Y = self.MULTIPLY*(pitch)+(1-self.MULTIPLY)*self.gyroYangle

        print "Filtr przechylenie: ", int(round(self.FILTR_X,0)), " Filtr pochylenie: ", int(round(self.FILTR_Y,0))

        return str(roll)+';'+str(pitch)
Exemple #14
0
def matrix2angle(R):
    ''' compute three Euler angles from a Rotation Matrix. Ref: http://www.gregslabaugh.net/publications/euler.pdf
    Args:
        R: (3,3). rotation matrix
    Returns:
        x: yaw
        y: pitch
        z: roll
    '''
    # assert(isRotationMatrix(R))

    if R[2,0] !=1 or R[2,0] != -1:
        x = asin(R[2,0])
        y = atan2(R[2,1]/cos(x), R[2,2]/cos(x))
        z = atan2(R[1,0]/cos(x), R[0,0]/cos(x))
        
    else:# Gimbal lock
        z = 0 #can be anything
        if R[2,0] == -1:
            x = np.pi/2
            y = z + atan2(R[0,1], R[0,2])
        else:
            x = -np.pi/2
            y = -z + atan2(-R[0,1], -R[0,2])

    return x, y, z
Exemple #15
0
def draw_car(x,y,th,canv):    
    r = math.sqrt(math.pow(ROBOT_LENGTH,2) + math.pow(ROBOT_WIDTH,2))/2    
    x = x + 300.0/cm_to_pixels
    y = y + 300.0/cm_to_pixels
    
    # top left
    phi = th + math.pi/2+ math.atan2(ROBOT_LENGTH,ROBOT_WIDTH)
    topleft  = (x + r*math.cos(phi),y+r*math.sin(phi))
    
    #top right
    phi = th + math.atan2(ROBOT_WIDTH,ROBOT_LENGTH) 
    topright = (x + r*math.cos(phi),y+r*math.sin(phi))

    # bottom left
    phi = th + math.pi + math.atan2(ROBOT_WIDTH,ROBOT_LENGTH)
    bottomleft =  (x + r*math.cos(phi),y+r*math.sin(phi))
    
    # bottom right
    phi = th + 3*math.pi/2 + math.atan2(ROBOT_LENGTH,ROBOT_WIDTH)
    bottomright =  (x + r*math.cos(phi),y+r*math.sin(phi))
    
    canv.create_polygon(topleft[0]*cm_to_pixels,600 - topleft[1]*cm_to_pixels,
                        bottomleft[0]*cm_to_pixels,600 - bottomleft[1]*cm_to_pixels,
                        bottomright[0]*cm_to_pixels,600 - bottomright[1]*cm_to_pixels,
                        topright[0]*cm_to_pixels,600 - topright[1]*cm_to_pixels,
                        width = 1, outline = 'blue',fill = '')
    x1 = x*cm_to_pixels
    y1 = y*cm_to_pixels
    canv.create_oval(x1-1,600-(y1-1),x1+1,600-(y1+1),outline = 'green',fill = 'green')
Exemple #16
0
    def __getCameraMovementAngles(self):
        """
        @return: camera position as its movement angles (azimuth, elevation
                 and roll) in degres
        @rtype: (float, float, float)
        """
        camera = self.vtkapp.cameraPosition
        top = self.vtkapp.cameraViewUp

        if camera[0] != 0 or camera[2] != 0:
            a = math.atan2(camera[0], camera[2])
            camera = rotateY(-a, camera)
            top = rotateY(-a, top)

        b = math.atan2(camera[1], camera[2])
        camera = rotateX(-b, camera)
        top = rotateX(-b, top)

        c = math.atan2(top[0], top[1])
        #camera = rotateZ(c, camera)
        top = rotateZ(c, top)

        debugOutput("initial top: %s" % str(top))
        debugOutput("initial camera: %s" % str(camera))
        return tuple(x * 180 / math.pi for x in (a, b, c))
Exemple #17
0
    def to_geographic(self, x, y):

        # Retrieve the locals.
        A, E, PI4, PI2, P0, M0, X0, Y0, P1, P2, m1, m2, t1, t2, t0, n, F, rho0 = self._locals

        # Subtract the false northing/easting.
        x = x - X0
        y = y - Y0

        # Calculate the Longitude.
        lon = math.atan2(x, rho0 - y) / n + M0

        # Estimate the Latitude.
        rho = math.sqrt(math.pow(x, 2.0) + math.pow(rho0 - y, 2.0))
        t = math.pow(rho / (A * F), 1.0 / n)
        lat = PI2 - (2.0 * math.atan2(t, 1.0))

        # Substitute the estimate into the iterative calculation
        # that converges on the correct Latitude value.
        while True:
            lat1 = lat
            es = E * math.sin(lat1)
            lat = PI2 - 2.0 * math.atan2(t * math.pow((1.0 - es) / (1.0 + es), E / 2.0), 1.0)
            if math.fabs(lat - lat1) < 2.0e-9:
                break

        # Return lat/lon in degrees.
        return math.degrees(lat), math.degrees(lon)
Exemple #18
0
def coordangle_callback(data):
	x = data.x
	y = data.y
	z = data.z
	grip_angle = 0
	
	d = math.sqrt(x**2 + y**2)
	z_prime = z - shoulder_height
        d -= math.cos(grip_angle)*wrist_endpoint
        z_prime -= math.sin(grip_angle)*wrist_endpoint
        l1 = shoulder_elbow
        l2 = elbow_wrist
        numerator = d**2 + z_prime**2 - l1**2 - l2**2
        denominator = 2*l1*l2
        elbow_angle = math.atan2(math.sqrt(1 - (numerator/denominator)**2), numerator / denominator)
        if elbow_angle > 0:
        	elbow_angle = math.atan2(-math.sqrt(1 - (numerator / denominator)**2), numerator / denominator)
                k1 = l1 + l2 * math.cos(elbow_angle)
                k2 = l2 * math.sin(elbow_angle)
                shoulder_angle = math.atan2(z_prime, d) - math.atan2(k2, k1)
                wrist_angle = grip_angle - shoulder_angle - elbow_angle
                base_angle = math.atan2(y, x)

                next_angles.x = -base_angle + math.pi/2             #base angle
                next_angles.y = -math.pi/2 + shoulder_angle         #shoulder angle 
                next_angles.z = -elbow_angle - math.pi/2            #elbow angle
                next_angles.w = wrist_angle + 0.17444               #wrist angle
                
               	assert -1.5708 <= next_angles.x <= 1.5708, next_angles.x
		assert -0.8727 <= next_angles.y <= 0.8727, next_angles.y
		assert -1.5708 <= next_angles.z <= 1.1775, next_angles.z
		assert -0.7850 <= next_angles.w <= 0.7850, next_angles.w
		rospy.loginfo("Base angle = %f, shoulder_angle = %f, elbow_angle = %f, wrist_angle = %f",next_angles.x,next_angles.y,next_angles.z,next_angles.w)
		angle_pub.publish(next_angles)
Exemple #19
0
def inputs(actual_q, altitude, target_q, target_altitude):
    inpt = np.ones(4)*9.8/4 # TODO what range should these be in?

    q = quaternion.qmul(quaternion.qinv(target_q), actual_q)

    roll = math.atan2(2*(q[2]*q[3] + q[0]*q[1]), q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3])
    yaw = math.atan2(2*(q[1]*q[2] + q[0]*q[3]), q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3])
    pitch = math.asin(-2*(q[1]*q[3] - q[0]*q[2]))

    inpt[0] += 0.3*(target_altitude-altitude)
    inpt[1] += 0.3*(target_altitude-altitude)
    inpt[2] += 0.3*(target_altitude-altitude)
    inpt[3] += 0.3*(target_altitude-altitude)
    inpt = [max(0.3,x) for x in inpt]

    inpt[0] -= 0.3*yaw
    inpt[2] -= 0.3*yaw
    inpt[1] += 0.3*yaw
    inpt[3] += 0.3*yaw
    inpt = [max(0,x) for x in inpt]

    inpt[0] += 0.2*pitch
    inpt[2] -= 0.2*pitch
    inpt[1] += 0.2*roll
    inpt[3] -= 0.2*roll
    inpt = [max(0,x) for x in inpt]

    return inpt
Exemple #20
0
 def _calc_det_angles_given_det_or_naz_constraint(
         self, det_constraint, naz_constraint, theta, tau, alpha):
     
     assert det_constraint or naz_constraint
     
     if det_constraint:
         # One of the detector angles is given                 (Section 5.1)
         det_constraint_name, det_constraint = det_constraint.items()[0]
         delta, nu, qaz = self._calc_remaining_detector_angles(
                          det_constraint_name, det_constraint, theta)
         naz_qaz_angle = _calc_angle_between_naz_and_qaz(theta, alpha, tau)
         naz = qaz - naz_qaz_angle
         
     elif naz_constraint: # The 'detector' angle naz is given:
         det_constraint_name, det_constraint = naz_constraint.items()[0]
         naz_name, naz = det_constraint_name, det_constraint
         assert naz_name == 'naz'
         naz_qaz_angle = _calc_angle_between_naz_and_qaz(theta, alpha, tau)
         qaz = naz - naz_qaz_angle
         nu = atan2(sin(2 * theta) * cos(qaz), cos(2 * theta))
         delta = atan2(sin(qaz) * sin(nu), cos(qaz))
         
     delta_nu_pairs = self._generate_detector_solutions(
                      delta, nu, qaz, theta, det_constraint_name)
     if not delta_nu_pairs:
         raise DiffcalcException('No detector solutions were found,'
             'please unconstrain detector limits')
     delta, nu = self._choose_detector_solution(delta_nu_pairs)
     
     return qaz, naz, delta, nu
Exemple #21
0
def longest_lines(hull):
	l = len(hull)
	lines = [0] * l
	for n in xrange(l):
		x1, y1 = hull[n]
		x2, y2 = hull[(n+1) % l]
		lines[n] = {
			'c1': (x1, y1),
			'c2': (x2, y2),
			'len': ( (x2-x1)**2 + (y2-y1)**2 ) ** 0.5,
			'angle': math.atan2(y2 - y1, x2-x1),
		}
	#make straight-ish lines actually straight
	n = 0
	while n+1 < len(lines):
		l1 = lines[n]
		l2 = lines[(n+1) % len(lines)]
		if abs(l1['angle'] - l2['angle']) / (math.pi*2) < 0.0027:
			x1, y1 = c1 = l1['c1']
			x2, y2 = c2 = l2['c2']
			lines[n] = {
				'c1': c1,
				'c2': c2,
				'len': ( (x2-x1)**2 + (y2-y1)**2 ) ** 0.5,
				'angle': math.atan2(y2 - y1, x2-x1),
			}
			del lines[n+1]
		else:
			n += 1

	lines.sort(key = lambda l: -l['len'])
	return lines
Exemple #22
0
    def update_goals(self, dt):
        '''update the velocities to match the goals'''
        flagdist = collide.dist(self.pos, self.team.flag.pos)
        if self.flag and collide.rect2circle(self.team.base.rect, (self.pos, constants.TANKRADIUS)):
            self.team.map.scoreFlag(self.flag)

        if not self.flag and self.team.flag.tank is None and flagdist < config['puppy_guard_zone']:
            x,y = self.team.flag.pos
            rad = 1 # config['puppy_guard_zone']
            angle = math.atan2(self.pos[1]-y, self.pos[0]-x)
            npos = [math.cos(angle) * rad + self.pos[0], math.sin(angle) * rad + self.pos[1]]
            if not self.collision_at(npos, True):
                self.pos = npos
        

        ## return the flag once you get on "your side"
        # if self.flag and self.team.map.closest_base(self.pos) == self.team.base:
        #     self.team.map.scoreFlag(self.flag)

        self.hspeed += self.accelx
        self.vspeed += self.accely
        max = 30
        if collide.dist((0,0),(self.hspeed, self.vspeed)) > max:
            dr = math.atan2(self.vspeed, self.hspeed)
            self.hspeed = math.cos(dr) * max
            self.vspeed = math.sin(dr) * max
Exemple #23
0
    def getArcParameters(self, arc, version):
        xs = self.setUnit(arc['pos'][0], version)
        ys = self.setUnit(arc['pos'][1], version)
        
        x1 = self.setUnit(arc['ofsa'][0], version)
        y1 = self.setUnit(arc['ofsa'][1], version)
        
        x2 = self.setUnit(arc['ofsb'][0], version)
        y2 = self.setUnit(arc['ofsb'][1], version)
        
        angle = float("%.1f" % ((atan2(y2, x2) - atan2(y1, x1)) * 180 / 3.14))
        #
        x1 = self.setUnit(arc['ofsa'][0], version) + xs
        y1 = self.setUnit(arc['ofsa'][1], version) + ys
        
        [x2R, y2R] = self.obrocPunkt2([x1, y1], [xs, ys], angle)
        
        #FreeCAD.Console.PrintWarning(u"angle = {0}\n".format(angle))
        #FreeCAD.Console.PrintWarning(u"xs = {0}\n".format(xs))
        #FreeCAD.Console.PrintWarning(u"ys = {0}\n".format(ys))
        #FreeCAD.Console.PrintWarning(u"x1 = {0}\n".format(x1))
        #FreeCAD.Console.PrintWarning(u"y1 = {0}\n".format(y1))
        #FreeCAD.Console.PrintWarning(u"x2R = {0}\n".format(x2R))
        #FreeCAD.Console.PrintWarning(u"y2R = {0}\n\n".format(y2R))
        
        x2R = float("%.1f" % x2R)
        y2R = float("%.1f" % y2R)
        
        if angle < 0:
            angle = angle - 360
            
        width = self.setUnit(arc['width'], version)

        return [x1, y1 * -1, x2R, y2R * -1, angle, width]
Exemple #24
0
	def pid(self, data):
		#Function that does a z control over the heading for the ground vehicle
		#Uses z from the ar tag and tries to reach it
		
		#Calculate heading angle to goal
		u_x = data.x - self.x
		u_y = data.y - self.y 
		theta_g = math.atan2(u_y, u_x)
		
		#Calculate error in heading
		e_k = theta_g - self.theta
		e_k = math.atan2(math.sin(e_k), math.cos(e_k))

		#Calculate PID parameters
		e_P = e_k
		e_I = self.E_k + e_k
		e_D = self.e_k_1 - e_k

		#The controlled angular velocity
		self.w = self.Kp*e_P + self.Ki*e_I + self.Kd*e_D
		
		#Updates
		self.E_k = e_I
		self.e_k_1 = e_k

		#Print statements for debugging
		rospy.loginfo('PID called, w is: ')
		rospy.loginfo(self.w)

		goal = np.array((data.x, data.y))
		rospy.loginfo('distance from goal: ')
		rospy.loginfo(np.linalg.norm(self.pose-goal))
Exemple #25
0
def solve_2R_inverse_kinematics(x,y,L1=1,L2=1):
    """For a 2R arm centered at the origin, solves for the joint angles
    (q1,q2) that places the end effector at (x,y).

    The result is a list of up to 2 solutions, e.g. [(q1,q2),(q1',q2')].
    """
    D = vectorops.norm((x,y))
    thetades = math.atan2(y,x)
    if D == 0:
        raise ValueError("(x,y) at origin, infinite # of solutions")
    c2 = (D**2-L1**2-L2**2)/(2.0*L1*L2)
    q2s = []
    if c2 < -1:
        print "solve_2R_inverse_kinematics: (x,y) inside inner circle"
        return []
    elif c2 > 1:
        print "solve_2R_inverse_kinematics: (x,y) out of reach"
        return []
    else:
        if c2 == 1:
            q2s = [math.acos(c2)]
        else:
            q2s = [math.acos(c2),-math.acos(c2)]
    res = []
    for q2 in q2s:
        thetaactual = math.atan2(math.sin(q2),L1+L2*math.cos(q2))
        q1 = thetades - thetaactual
        res.append((q1,q2))
    return res
def speedsToReach( carrot, robotPose ) :
  """
  Compute robot commands for the next path point
  use 'follow the carrot' algorithm
  :param carrot: Point to reach
  :param robotPose: Robot collection storing robot position and orientation
  :return: speeds of the robots in a dictionnary
  """
  # robot angle
  angleRobot = atan2( getBearing()['Y'], getBearing()['X'] )
  # compute distance between the robot and the carrot
  distanceCarrot = computeDistance( carrot, robotPose['Position'] )
  # print "distance: ", distanceCarrot
  # compute the angle to the carrot
  angleCarrot = atan2( carrot['Y'] - robotPose['Position']['Y'], carrot['X'] - robotPose['Position']['X'] )
  angleToCarrot = angleCarrot - angleRobot
  if -pi > angleToCarrot :
    angleToCarrot = ( 2 * pi ) + angleToCarrot
  if pi < angleToCarrot :
      angleToCarrot = ( 2 * pi ) - angleToCarrot
  # print "angle: ", angleToCarrot
  # compute angular speed
  speeds = {}
  speeds['angular'] = ( angleToCarrot ) / radPerSec
  speeds['linear'] = ( speeds['angular'] * ( distanceCarrot / ( 2 * sin( angleToCarrot ) ) ) )
  
  if maxLinearSpeed < speeds['linear'] :
    speeds['linear'] = maxLinearSpeed
  if -maxLinearSpeed > speeds['linear'] :
    speeds['linear'] = -maxLinearSpeed
  return speeds
    def quat2eulerZYX (self,q):
        euler = Vector3()
        tol = self.quat2eulerZYX.tolerance

        qww, qxx, qyy, qzz = q.w*q.w, q.x*q.x, q.y*q.y, q.z*q.z
        qwx, qxy, qyz, qxz= q.w*q.x, q.x*q.y, q.y*q.z, q.x*q.z
        qwy, qwz = q.w*q.y, q.w*q.z

        test = -2.0 * (qxz - qwy)
        if test > +tol:
           euler.x = atan2 (-2.0*(qyz-qwx), qww-qxx+qyy-qzz)
           euler.y = +0.5 * pi
           euler.z = 0.0

           return euler

        elif test < -tol:
             euler.x = atan2 (-2.0*(qyz-qwx), qww-qxx+qyy-qzz)
             euler.y = -0.5 * pi
             euler.z = tol

             return euler

        else:
           euler.x = atan2 (2.0*(qyz+qwx), qww-qxx-qyy+qzz)
           euler.y = asin (test)
           euler.z = atan2 (2.0*(qxy+qwz), qww+qxx-qyy-qzz)

           return euler
Exemple #28
0
def on_mouse_press(x, y, button, modifiers):
    #print player1.x, player1.y
    #for n in range(0, player1.sides):
        #print n, " ::: ", player1.vertices[2 * n + 2], player1.vertices[2 * n + 3], " : ", player1.colors[4*n + 4:4*n+8]
    if button == mouse.MIDDLE:
        player1.drawTarget = True
    if button == mouse.LEFT and player1.sides > 3:
        a1 = math.atan2(float(y - player1.y), float(x - player1.x))
        side = 0
        closest = 3.1415
        a3 = 100
        for n in range(1, player1.sides + 1):
            vx = player1.vertices[2*n]
            vy = player1.vertices[2*n + 1]
            a2 = math.atan2((vy - player1.y), (vx - player1.x))
            if abs(a2 - a1) < closest:
                side = n
                closest = abs(a2 - a1)
                a3 = a2

        vtx, clr = player1.removeSide(side)
        #player1.x + math.cos(angle) * (player1.radius + 4), player1.y + math.sin(angle) * (player1.radius + 4)

        b = bullet(angle = a1, vtx=vtx, color = clr)
        game_objects.append(b)
Exemple #29
0
    def calc_ogp(self, params1, params2, freq):

        """
	   Calculates OGP values for both cores given best fit parameters
	   from a snap fit
	"""

	z_fact = -500.0/256.0
	true_zero = 0.0 * z_fact
	d_fact = 1e12/(2*np.pi*freq*1e6)

	z1 = z_fact * params1[0][0]
	sin1a = params1[0][1]
	cos1a = params1[0][2]
	amp1 = math.sqrt(sin1a**2 + cos1a**2)
	dly1 = d_fact*math.atan2(sin1a, cos1a)

	z2 = z_fact * params2[0][0]
	sin2a = params2[0][1]
	cos2a = params2[0][2]
	amp2 = math.sqrt(sin2a**2 + cos2a**2)
	dly2 = d_fact*math.atan2(sin2a, cos2a)

	avz = (z1+z2)/2.0
	avamp = (amp1+amp2)/2.0
	a1p = 100*(avamp-amp1)/avamp 
	a2p = 100*(avamp-amp2)/avamp 
	avdly = (dly1+dly2)/2.0

	ogp1 = (z1-true_zero, a1p, dly1-avdly)
	ogp2 = (z2-true_zero, a2p, dly2-avdly)

	return ogp1, ogp2
Exemple #30
0
	def angle (self, vector=None) :
		"""angle between two vectors
		to go from vector to self
		
		:warning: orientation in dimension 3 is not garanteed
		:param vector: the second vector
		:type vector: NVector
		:return: an angle in radian
		:rtype: float"""
		if self.dim()==2 :
			if vector is None : vector=axis(0,dim=2)
			else : vector=vector.get_normalized()
			y=vector.cross(self)#[2]
			x=self.dot(vector)
			return math.atan2(y,x)
		else : 
			if vector is None : vector=axis(0,self.dim())
			else : vector=vector.get_normalized()
			axis_normal=vector.cross(self)
			if axis_normal.norm()<1e-6 :
				if self.dot(vector)>0 : return 0.
				else : return math.pi
			
			axis_proj=axis_normal.cross(vector)
			axis_proj.normalize()
			y=self.dot(axis_proj)
			x=self.dot(vector)
			return math.atan2(y,x)
    def get_heading_in_degrees(self):
        theta_radians = math.atan2(self.leading_point.y - self.trailing_point.y,
                                   self.leading_point.x - self.trailing_point.x)

        theta_degrees = (theta_radians + math.pi) * 360.0 / (2.0 * math.pi)
        return theta_degrees
Exemple #32
0
def draw_decision_boundary(w, b, col):
    matplotlib.pyplot.axline(xy1=(0 - math.cos(math.atan2(w[1], w[0])) * b,
                                  0 - math.sin(math.atan2(w[1], w[0])) * b),
                             xy2=(-(w[1]) - math.cos(math.atan2(w[1], w[0])) * b,
                                  (w[0]) - math.sin(math.atan2(w[1], w[0])) * b), color=col)
Exemple #33
0
    def getInterpolatedArc(ptStart, ptArc, ptEnd, method, interValue):

        coords = []
        coords.append(ptStart)

        center = CircularArc.getArcCenter(ptStart, ptArc, ptEnd)

        if center == None:
            coords.append(ptEnd)
            g = QgsGeometry.fromPolyline(coords)
            return g

        cx = center.x()
        cy = center.y()

        px = ptArc.x()
        py = ptArc.y()
        r = ((cx - px) * (cx - px) + (cy - py) * (cy - py))**0.5

        ## If the method is "pitch" (=Pfeilhöhe) then
        ## we need to calculate the corresponding
        ## angle.
        if method == "pitch":
            myAlpha = 2.0 * math.acos(1.0 - (interValue / 1000) / r)
            arcIncr = myAlpha
#            print "myAlpha: " + str(myAlpha)
        else:
            arcIncr = interValue * math.pi / 180
#            print "arcIncr:  " + str(arcIncr)

        a1 = math.atan2(ptStart.y() - center.y(), ptStart.x() - center.x())
        a2 = math.atan2(ptArc.y() - center.y(), ptArc.x() - center.x())
        a3 = math.atan2(ptEnd.y() - center.y(), ptEnd.x() - center.x())

        # Clockwise
        if a1 > a2 and a2 > a3:
            sweep = a3 - a1

        # Counter-clockwise
        elif a1 < a2 and a2 < a3:
            sweep = a3 - a1

        # Clockwise, wrap
        elif (a1 < a2 and a1 > a3) or (a2 < a3 and a1 > a3):
            sweep = a3 - a1 + 2 * math.pi

        # Counter-clockwise, wrap
        elif (a1 > a2 and a1 < a3) or (a2 > a3 and a1 < a3):
            sweep = a3 - a1 - 2 * math.pi

        else:
            sweep = 0.0

        ptcount = int(math.ceil(math.fabs(sweep / arcIncr)))

        if sweep < 0:
            arcIncr *= -1.0

        angle = a1

        for i in range(0, ptcount - 1):
            angle += arcIncr

            if arcIncr > 0.0 and angle > math.pi:
                angle -= 2 * math.pi

            if arcIncr < 0.0 and angle < -1 * math.pi:
                angle -= 2 * math.pi

            x = cx + r * math.cos(angle)
            y = cy + r * math.sin(angle)

            point = QgsPoint(x, y)
            coords.append(point)
            #            print str(point.toString())

            if angle < a2 and (angle + arcIncr) > a2:
                coords.append(ptArc)

            if angle > a2 and (angle + arcIncr) < a2:
                coords.append(ptArc)

        coords.append(ptEnd)
        g = QgsGeometry.fromPolyline(coords)
        return g
Exemple #34
0
    h -= 2
    print px, py
    xmap = zeros([d * 2 + 1, d * 2 + 1])
    for x in range(0, d * 2 + 1):
        for y in range(0, d * 2 + 1):
            if ((x - d) % (w * 2) == 0 or (x - d) %
                (w * 2) == (w - px) * 2 - 1) and ((y - d) % (h * 2) == 0 or
                                                  (y - d) %
                                                  (h * 2) == (h - py) * 2 - 1):
                xmap[y][x] = 1.0

    print xmap
    prevAngles = []
    count = 0
    for x in range(0, d * 2 + 1):
        for y in range(0, d * 2 + 1):
            if x != d or y != d:
                dx = x - d
                dy = y - d
                distsquared = dx * dx + dy * dy
                if xmap[y][x] == 1.0 and distsquared <= d * d:
                    a = math.atan2(y - d, x - d)
                    if not a in prevAngles:
                        prevAngles.append(a)
                        count += 1
    print "Case #" + str(mastercount + 1) + ": " + str(count)
    fp2.write("Case #" + str(mastercount + 1) + ": " + str(count) + "\n")

fp.close()
fp2.close()
Exemple #35
0
 def Ellipse_Tangent(self, alpha=0):  #PointClass(0,0)
     #große Halbachse, kleine Halbachse, rotation der Ellipse (rad), Winkel des Punkts in der Ellipse (rad)
     phi = atan2(self.a * sin(alpha),
                 self.b * cos(alpha)) + self.rotation - pi / 2
     return phi
Exemple #36
0
 def Ellipse_Grundwerte(self):
     #Weitere Grundwerte der Ellipse, die nur einmal ausgerechnet werden müssen
     self.rotation = atan2(self.vector.y, self.vector.x)
     self.a = sqrt(self.vector.x**2 + self.vector.y**2)
     self.b = self.a * self.ratio
Exemple #37
0
    def execute(self,agent):
        agent.controller = calcController

        #getting the coordinates of the goalposts
        leftPost = Vector3([-sign(agent.team)*700 , 5100*-sign(agent.team), 200])
        rightPost = Vector3([sign(agent.team)*700, 5100*-sign(agent.team), 200])
        #center = Vector3([0, 5150*-sign(agent.team), 200])

        #time stuff that we don't worry about yet
        time_guess = 0 # this is set to zero, so its not really doing anything. Just assuming where the ball is right now
        bloc = future(agent.ball,time_guess)

        #vectors from the goalposts to the ball & to Gosling
        ball_left = angle2(bloc,leftPost)
        ball_right = angle2(bloc,rightPost)
        agent_left = angle2(agent.me,leftPost)
        agent_right = angle2(agent.me,rightPost)

        #determining if we are left/right/inside of cone
        if agent_left > ball_left and agent_right > ball_right:
            goal_target = rightPost
        elif agent_left > ball_left and agent_right < ball_right:
            goal_target = None
        elif agent_left < ball_left and agent_right < ball_right:
            goal_target = leftPost
        else:
            goal_target = None

        if goal_target != None:
            #if we are outside the cone, this is the same as Gosling's old code
            goal_to_ball = (agent.ball.location - goal_target).normalize()
            goal_to_agent = (agent.me.location - goal_target).normalize()
            difference = goal_to_ball - goal_to_agent
            error = cap(abs(difference.data[0])+ abs(difference.data[1]),1,10)
        else:
            #if we are inside the cone, our line to follow is a vector from the ball to us (although it's still named 'goal_to_ball')
            goal_to_ball = (agent.me.location - agent.ball.location).normalize()
            error = cap( distance2D(bloc,agent.me) /1000,0,1)

        #this is measuring how fast the ball is traveling away from us if we were stationary
        ball_dpp_skew = cap(abs(dpp(agent.ball.location, agent.ball.velocity, agent.me.location, [0,0,0]))/80, 1,1.5)

        #same as Gosling's old distance calculation, but now we consider dpp_skew which helps us handle when the ball is moving
        target_distance =cap( (40 + distance2D(agent.ball.location,agent.me)* (error**2))/1.8, 0,4000)
        target_location = agent.ball.location + Vector3([(goal_to_ball.data[0]*target_distance) * ball_dpp_skew, goal_to_ball.data[1]*target_distance,0])

       #this also adjusts the target location based on dpp
        ball_something = dpp(target_location,agent.ball.velocity, agent.me,[0,0,0])**2
        
        if ball_something > 100: #if we were stopped, and the ball is moving 100uu/s away from us
            ball_something = cap(ball_something,0,80)
            correction = agent.ball.velocity.normalize()
            correction = Vector3([correction.data[0]*ball_something,correction.data[1]*ball_something,correction.data[2]*ball_something])
            target_location += correction #we're adding some component of the ball's velocity to the target position so that we are able to hit a faster moving ball better
            #it's important that this only happens when the ball is moving away from us.

        
        #another target adjustment that applies if the ball is close to the wall
        extra = 4120 - abs(target_location.data[0])
        if extra < 0:
            # we prevent our target from going outside the wall, and extend it so that Gosling gets closer to the wall before taking a shot, makes things more reliable
            target_location.data[0] = cap(target_location.data[0],-4120,4120) 
            target_location.data[1] = target_location.data[1] + (-sign(agent.team)*cap(extra,-500,500))

        #getting speed, this would be a good place to modify because it's not very good
        target_local = toLocal(agent.ball.location,agent.me)
        angle_to_target = cap(math.atan2(target_local.data[1], target_local.data[0]),-3,3)
        #distance_to_target = distance2D(agent.me, target_location)
        speed=  2000 - (100*(1+angle_to_target)**2)

        #picking our rendered target color based on the speed we want to go
        colorRed = cap(int( (speed/2300) * 255),0,255)
        colorBlue =cap(255-colorRed,0,255)

        #see the rendering tutorial on github about this, just drawing lines from the posts to the ball and one from the ball to the target
        agent.renderer.begin_rendering()
        agent.renderer.draw_line_3d(bloc.data, leftPost.data, agent.renderer.create_color(255,255,0,0))
        agent.renderer.draw_line_3d(bloc.data, rightPost.data, agent.renderer.create_color(255,0,255,0))

        agent.renderer.draw_line_3d(agent.ball.location.data,target_location.data, agent.renderer.create_color(255,colorRed,0,colorBlue))
        agent.renderer.draw_rect_3d(target_location.data, 10,10, True, agent.renderer.create_color(255,colorRed,0,colorBlue))
        agent.renderer.end_rendering()

        if  ballReady(agent) == False or abs(agent.ball.location.data[1]) > 5050:
            self.expired = True

        return agent.controller(agent,target_location,speed)
def calc_perp(x,y,theta,pt1,pt2):
    #this also needs to tell me if the xt,yt point has gone beyond pt2
    skip = False
    ld = 0.85
    INF_ERROR = False
    # ld = 0.85 #this worked best
    # ld = 5
    #find the equation of the line
    x1,y1 = pt1
    x2,y2 = pt2
    try:
        slope = (y2-y1)/(x2-x1)
    except:
        INF_ERROR = True
    if not INF_ERROR:
        print("[NO ERROR]")
        phi = m.atan2((y2-y1),(x2-x1))
        slope = (y2-y1)/(x2-x1)
        intercept = y1 - slope*x1
        xp = (x + slope*y - slope*intercept)/(1+slope**2)
        yp = intercept + slope*xp
        xt = xp + m.cos(phi)*ld
        yt = yp + m.sin(phi)*ld
        #checking if it has crossed pt2
        check_angle = m.atan2((yt-y2),(xt-x2))
        if check_angle == phi:
            skip = True
            xt = x2
            yt = y2
        #checking if the point is behind pt1
        """
        MEASURES TO PREVENT WEIRD BEHAVIOUR
        """
        check_angle2 = m.atan2((y1-yt),(x1-xt))
        if check_angle2 == phi:
            xt = x1 + m.cos(phi)*0.1
            yt = y1 + m.sin(phi)*0.1
        # elif abs(wrapToPi(phi-theta))>m.radians(80):  #this means that the target point has cleared the pt1
        #     xt = x1 + m.cos(phi)*4
        #     yt = y1 + m.sin(phi)*4
        # else:
        #     pass

    else:
        phi = m.atan2((y2-y1),(x2-x1))
        print("[ZERO DIV ERROR]")
        xp = x1
        yp = y
        sign = (y2-y1)/abs(y2-y1)
        xt = xp
        yt = yp + sign*1
        #here also we need to check the crossing thresh
        if sign>0:
            if yt>y2:
                skip = True
            if yt<y1:
                yt = y1
                xt = x2
                yt = y2
        elif yt<y2:
            skip = True
            if yt>y1:
                yt = y1
            xt = x2
            yt = y2
        """
        MEASURES TO PREVENT WEIRD BEHAVIOUR
        """
        check_angle2 = m.atan2((y1-yt),(x1-xt))
        if check_angle2 == phi:
            xt = x1 + m.cos(phi)*0.1
            yt = y1 + m.sin(phi)*0.1

    return(xp,yp,xt,yt,skip)
def path_track(path):
    xstart, ystart = path[0]
    start = [xstart,ystart,0]
    goal_points = path
    dummy_gp = copy.deepcopy(goal_points)
    #need to calculate goal theta last two points
    last_pt = dummy_gp[-1]
    second_last_pt = dummy_gp[-2]
    theta_g = m.atan2((last_pt[1]-second_last_pt[1]),(last_pt[0]-second_last_pt[0]))
    goalx,goaly = path[-1]
    goal = [goalx,goaly,theta_g]
    # goal_points = [[2,2]]
    x,y,theta = start
    v = 1
    s = 0
    gp_array = np.array(goal_points)
    x_traj = []
    y_traj = []


    skip = False
    while len(dummy_gp) >1:
        #first step would be to find the target point
        target,proximity,skip = calc_target(x,y,theta,dummy_gp)
        xt,yt = target
        if proximity<0.1 or skip==True:
            dummy_gp.pop(0)
        if skip==True:
            print(skip)
        plt.cla()
        plt.axis('scaled')
        plt.xlim(-10,15)
        plt.ylim(-10,15)
        plt.plot(gp_array[:,0],gp_array[:,1],'--')
        plt.plot(start[0],start[1],'co')
        plt.plot(xt,yt,'ro')

        if DRAW_DIFF:
            draw_robot(x,y,theta)
        if DRAW_PALLET:
            dpj(x,y,theta,s)

        x_traj.append(x)
        y_traj.append(y)
        plt.plot(x_traj,y_traj,'r')
        x,y,theta,s = seek_one([x,y,theta],[xt,yt],goal)
        # print(m.degrees(s))
        plt.pause(0.0001)
    if ALIGN_TO_GOAL_LINE:
        pt1 = goal_points[-2]
        pt2 = goal_points[-1]
        while wrapToPi(abs(theta - goal[2]))>0.1:
            _,_,xt,yt,_ = calc_perp(x,y,pt1,pt2)
            plt.cla()
            plt.axis('scaled')
            plt.xlim(-5,15)
            plt.ylim(-5,15)
            plt.plot(gp_array[:,0],gp_array[:,1],'--')
            plt.plot(start[0],start[1],'co')
            plt.plot(xt,yt,'ro')

            if DRAW_DIFF:
                draw_robot(x,y,theta)
            if DRAW_PALLET:
                dpj(x,y,theta,s)

            x_traj.append(x)
            y_traj.append(y)
            plt.plot(x_traj,y_traj,'r')
            x,y,theta,s = seek_one([x,y,theta],[xt,yt],goal)
            # print(m.degrees(s))
            plt.pause(0.0001)
def path_track3(path,thetas):
    # thetas = 0 #cancelling user defined theta
    """
    SAMPLING STARTS HERE
    """
    final_path = []
    x,y = path[0]
    sample_rate = 2 #best was 2
    final_path.append([x,y])
    for x,y in path:
        xf,yf = final_path[-1]
        if m.sqrt((xf-x)**2 + (yf-y)**2)>sample_rate:
            final_path.append([x,y])
        else:
            continue

    if path[-1] not in final_path:
        final_path.append(path[-1])


    """
    SAMPLING FINISHES HERE
    """

    prev_path = path
    path = final_path
    prev_path_array = np.array(prev_path)

    tic = time.time()
    xstart, ystart = path[0]
    start = [xstart,ystart,thetas]
    goal_points = path

    dummy_gp = copy.deepcopy(goal_points)


    #need to calculate goal theta last two points
    last_pt = dummy_gp[-1]
    second_last_pt = dummy_gp[-2]
    theta_g = -m.atan2((last_pt[1]-second_last_pt[1]),(last_pt[0]-second_last_pt[0]))
    goalx,goaly = goal_points[-1]
    goal = [goalx,goaly,theta_g]

    x,y,theta = start
    v = 1
    s = 0
    gp_array = np.array(goal_points)
    x_traj = []
    y_traj = []

    skip = False
    while m.sqrt((x - goal[0])**2 + (y - goal[1])**2)>0.2:
        #first step would be to find the target point
        target,proximity,skip = calc_target(x,y,theta,dummy_gp)
        xt,yt = target
        if (proximity<0.1 or skip==True) and len(dummy_gp)>2:
            dummy_gp.pop(0)
        if len(dummy_gp)==2:
            print(dummy_gp)
            angle = theta_g
            xt = last_pt[0] + 0*m.cos(theta_g)
            yt = last_pt[1] + 0*m.sin(-theta_g)
            plt.plot(xt,yt,'ko')

        if skip==True:
            #need to set the value of target to something
            target, proximity, skip = calc_target(x,y,theta,dummy_gp)
            print(skip)
        plt.cla()
        plt.axis('scaled')
        plt.xlim(-10,15)
        plt.ylim(-10,15)
        plt.plot(gp_array[:,0],gp_array[:,1],'m',label="Sampled-Target-path")
        plt.plot(prev_path_array[:,0],prev_path_array[:,1],'c--',label="Actual-Target-path")
        plt.plot(start[0],start[1],'co')
        plt.plot(xt,yt,'ro')

        if DRAW_DIFF:
            draw_robot(x,y,theta)
        if DRAW_PALLET:
            dpj(x,y,theta,s)

        x_traj.append(x)
        y_traj.append(y)
        plt.plot(x_traj,y_traj,'r',label="Actual-Path-taken")
        x,y,theta,s = seek_one([x,y,theta],[xt,yt],goal)
        # print(m.degrees(s))
        plt.pause(0.0001)
    if ALIGN_TO_GOAL_LINE:
        pt1 = goal_points[-2]
        pt2 = goal_points[-1]
        while wrapToPi(abs(theta - goal[2]))>0.1:
            _,_,xt,yt,_ = calc_perp(x,y,theta,pt1,pt2)
            plt.cla()
            plt.axis('scaled')
            plt.xlim(-10,15)
            plt.ylim(-10,15)
            plt.plot(gp_array[:,0],gp_array[:,1],'m',label="Sampled-Target-path")
            plt.plot(prev_path_array[:,0],prev_path_array[:,1],'c--',label="Actual-Target-path")
            plt.plot(start[0],start[1],'co')
            plt.plot(xt,yt,'ro')

            if DRAW_DIFF:
                draw_robot(x,y,theta)
            if DRAW_PALLET:
                dpj(x,y,theta,s)

            x_traj.append(x)
            y_traj.append(y)
            plt.plot(x_traj,y_traj,'r',label="Actual-Path-taken")
            x,y,theta,s = seek_one([x,y,theta],[xt,yt],goal)
            # print(m.degrees(s))
            plt.pause(0.0001)
    print("Time taken: {} s".format(time.time()-tic))
    plt.title('PID BASED CONSTANT SPEED PATH TRACKING OF A PALLET JACK')
    plt.legend()
    # plt.show()
    return theta
Exemple #41
0
    def straighten(self):
        print("Straight {}x at {:.2f} length ".format(len(self.edges),
                                                      self.length))

        # Get edge angles in UV space
        angles = {}
        for edge in self.edges:
            uv1 = self.vert_to_uv[edge.verts[0]][0].uv
            uv2 = self.vert_to_uv[edge.verts[1]][0].uv
            delta = uv2 - uv1
            angle = math.atan2(delta.y, delta.x) % (math.pi / 2)
            if angle >= (math.pi / 4):
                angle = angle - (math.pi / 2)
            angles[edge] = abs(angle)
            # print("Angle {:.2f} degr".format(angle * 180 / math.pi))

        # Pick edge with least rotation offset to U or V axis
        edge_main = sorted(angles.items(), key=operator.itemgetter(1))[0][0]

        print("Main edge: {} at {:.2f} degr".format(
            edge_main.index, angles[edge_main] * 180 / math.pi))

        # Rotate main edge to closest axis
        uvs = [uv for v in edge_main.verts for uv in self.vert_to_uv[v]]
        bpy.ops.uv.select_all(action='DESELECT')
        for uv in uvs:
            uv.select = True
        uv1 = self.vert_to_uv[edge_main.verts[0]][0].uv
        uv2 = self.vert_to_uv[edge_main.verts[1]][0].uv
        diff = uv2 - uv1
        angle = math.atan2(diff.y, diff.x) % (math.pi / 2)
        if angle >= (math.pi / 4):
            angle = angle - (math.pi / 2)
        bpy.ops.uv.cursor_set(location=uv1 + diff / 2)
        bpy.ops.transform.rotate(value=angle,
                                 orient_axis='Z',
                                 constraint_axis=(False, False, False),
                                 orient_type='GLOBAL',
                                 mirror=False,
                                 use_proportional_edit=False)

        # Expand edges and straighten
        count = len(self.edges)
        processed = [edge_main]
        for i in range(count):
            if (len(processed) < len(self.edges)):
                verts = set([v for e in processed for v in e.verts])
                edges_expand = [
                    e for e in self.edges if e not in processed and (
                        e.verts[0] in verts or e.verts[1] in verts)
                ]
                verts_ends = [
                    v for e in edges_expand for v in e.verts if v in verts
                ]

                print("Step, proc {} exp: {}".format(
                    [e.index for e in processed],
                    [e.index for e in edges_expand]))

                if len(edges_expand) == 0:
                    continue

                for edge in edges_expand:
                    # if edge.verts[0] in verts_ends and edge.verts[1] in verts_ends:
                    #     print("Cancel at edge {}".format(edge.index))
                    #     return

                    print("  E {} verts {} verts end: {}".format(
                        edge.index, [v.index for v in edge.verts],
                        [v.index for v in verts_ends]))
                    v1 = [v for v in edge.verts if v in verts_ends][0]
                    v2 = [v for v in edge.verts if v not in verts_ends][0]
                    # direction
                    previous_edge = [
                        e for e in processed
                        if e.verts[0] in edge.verts or e.verts[1] in edge.verts
                    ][0]
                    prev_v1 = [v for v in previous_edge.verts if v != v1][0]
                    prev_v2 = [v for v in previous_edge.verts if v == v1][0]
                    direction = (self.vert_to_uv[prev_v2][0].uv -
                                 self.vert_to_uv[prev_v1][0].uv).normalized()

                    for uv in self.vert_to_uv[v2]:
                        uv.uv = self.vert_to_uv[v1][0].uv + \
                            direction * self.edge_length[edge]

                print("Procesed {}x Expand {}x".format(len(processed),
                                                       len(edges_expand)))
                print("verts_ends: {}x".format(len(verts_ends)))
                processed.extend(edges_expand)

        # Select edges
        uvs = list(
            set([
                uv for e in self.edges for v in e.verts
                for uv in self.vert_to_uv[v]
            ]))
        bpy.ops.uv.select_all(action='DESELECT')
        for uv in uvs:
            uv.select = True

        # Pin UV's
        bpy.ops.uv.pin()
        bpy.ops.uv.unwrap(method='ANGLE_BASED', margin=0.001)
        bpy.ops.uv.pin(clear=True)
def wrapToPi(theta):
        return m.atan2(m.sin(theta),m.cos(theta))
def angle(point1, point2):
    from math import sin, cos, sqrt, atan2, radians, degrees
    delx = point2[0] - point1[0]
    dely = point2[1] - point1[1]
    return math.degrees(atan2(radians(delx), radians(dely)))
Exemple #44
0
 def calculateTrueNorthAngle(self, x, y):
     return round(math.degrees(math.atan2(y, x)) - 90, 6)
Exemple #45
0
 def getArmBaseAng(self, finalPose):
     # return self.getBestBaseAng(finalPose, math.atan2(finalPose.y, finalPose.x))
     return math.atan2(finalPose.y, finalPose.x)
    vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)

    setMotorSpeed(clientID, motor_handle, Vx)
    setMotorPosition(clientID, steer_handle, 0)

    vrep.simxSynchronousTrigger(clientID)

    e = np.zeros((4,1))

    while True:
        _, vehicle_lin, vehicle_ang = vrep.simxGetObjectVelocity(clientID,vehicle_handle,vrep.simx_opmode_streaming)
        _, vehicle_pos = vrep.simxGetObjectPosition(clientID,vehicle_handle,-1,vrep.simx_opmode_streaming)
        _, vehicle_ori = vrep.simxGetObjectOrientation(clientID,vehicle_handle,-1,vrep.simx_opmode_streaming)

        atan = math.atan2(vehicle_pos[1], vehicle_pos[0])

        y = vehicle_pos[1]
        psi = vehicle_ori[2] # in radians
        ydot = (-math.sin(atan) * vehicle_lin[0] + math.cos(atan) * vehicle_lin[1])
        psidot = vehicle_ang[2]

        y_des = 0
        psi_des =0
        psidot_des = 0

        e[0] = y - y_des
        e[1] = ydot + Vx*(psi - psi_des)
        e[2] = psi - psi_des
        e[3] = psidot - psidot_des
Exemple #47
0
def angle(x, y):
    a = math.atan2(x, y)
    return a if a >= 0 else a + 2 * math.pi
Exemple #48
0
def tangentbug():
	global goalVisible
	global scan
	goalfromlaser = rospy.Publisher('robot_1/robot2goal',Twist,queue_size=10)
	robotController = rospy.Publisher('robot_1/cmd_vel',Twist,queue_size=10)

	listener = tf.TransformListener()
	rate = rospy.Rate(1.0)
	while not rospy.is_shutdown():
		try:
			# trans is a list of 3 elements x,y,z of the transform. rot is a list of
			# 4 elements of the quaternion for the transform
			(trans,rot) = listener.lookupTransform('robot1/trueOdom','robot1/goal',rospy.Time(0))
			twi = Twist()
			twi.linear.x = trans[0]
			twi.linear.y = trans[1]
			twi.linear.z = trans[2]
			#ignore angular orientations since we only care about getting to the goal position
			twi.angular = Vector3(0.0,0.0,0.0)
			
			control = Twist()
			control.linear = Vector3(0.0,0.0,0.0)
			control.angular = Vector3(0.0,0.0,0.0)
			angle2goal = math.atan2(trans[1],trans[0])

			sensorIndex = int((angle2goal-scan.angle_min)/scan.angle_increment)

			# check if we can see the goal directly. Move towards it if we can
			if (scan.ranges[sensorIndex] >= maxRange):
				goalVisible = 1
				if (angle2goal > 0):
					control.linear.x = 0.3
					control.angular.z = 0.2
				elif (angle2goal < 0):
					control.linear.x = 0.3
					control.angular.z = -0.2
				else:
					control.linear.x = 0.3
					control.angular.z = 0.0
			else:
				goalVisible = 0

			# if we can't see the goal directly, check for the best direction of travel
			if goalVisible == 0:
				bestAngle = 0.0
				besti = 0
				bestDist = 10000.0
				realAngle = 0.0

				for i in range(len(scan.ranges)):
					# check for discontinuties within a specified threshold
					if (i>0) and (abs(scan.ranges[i]-scan.ranges[i-1]) > discThresh):
						# output the index for the discontinuity and the angle value and the distance to that discontinuity
						discDist = scan.ranges[i]
						if discDist==float('Inf'):
							discDist = scan.range_max
						dAng = scan.angle_min + i * scan.angle_increment
						xDist = discDist * math.sin(dAng)
						yDist = discDist * math.cos(dAng)
						heurDist = math.sqrt((twi.linear.x-xDist) ** 2 + (twi.linear.y-yDist) ** 2)
						if ((heurDist + discDist) < bestDist):
							bestDist = heurDist + discDist
							bestAngle = dAng
							besti = i

				# drive towards the best heuristic or turn towards it if we're not facing it already
				if ((bestAngle) > 0):
					control.linear.x = 0.2
					control.angular.z = 0.3
				elif ((bestAngle) < 0):
					control.linear.x = 0.2
					control.angular.z = -0.3
				else:
					control.linear.x = 0.2
					control.angular.z = 0.0

				# prioritize avoiding obstacles
				if (besti > 90) and (besti < (len(scan.ranges)-90)):
					if scan.ranges[besti+20] < 2.0:
						control.linear.x = 0.2
						control.angular.z = 0.5
					elif scan.ranges[besti-20] < 2.0:
						control.linear.x = 0.2
						control.angular.z = -0.5
				elif (besti > 90) and (besti < (len(scan.ranges)-90)):
					if scan.ranges[besti+30] < 2.0:
						control.linear.x = 0.2
						control.angular.z = 0.5
					elif scan.ranges[besti-30] < 2.0:
						control.linear.x = 0.2
						control.angular.z = -0.5

			# if obstacles are too close to the robot, prioritize avoiding them
			j = int(len(scan.ranges)/2) - 70
			m = int(len(scan.ranges)/2) - 10
			k = int(len(scan.ranges)/2) + 70
			n = int(len(scan.ranges)/2) + 10

			for i in range(j,m):
				if (scan.ranges[i] < 2.5):
					control.linear.x = 0.0
					control.angular.z = 0.5
			for i in range(n,k):
				if (scan.ranges[i] < 2.5):
					control.linear.x = 0.0
					control.angular.z = -0.5

			# stop moving if we're close enough to the goal
			if math.sqrt((twi.linear.x) ** 2 + (twi.linear.y) ** 2) < 0.5:
				control.linear.x = 0.0
				control.linear.y = 0.0

			robotController.publish(control)
			goalfromlaser.publish(twi)
		except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
			continue

	rate.sleep()
Exemple #49
0
def corner_to_center_box3d(boxes_corner, coordinate='camera', calib_mat=None):
    # (N, 8, 3) -> (N, 7) x,y,z,h,w,l,ry/z
    if coordinate == 'lidar':
        for idx in range(len(boxes_corner)):
            boxes_corner[idx] = lidar_to_camera_point(boxes_corner[idx], calib_mat=calib_mat)
    ret = []
    for roi in boxes_corner:
        if cfg.CORNER2CENTER_AVG:  # average version
            roi = np.array(roi)
            h = abs(np.sum(roi[:4, 1] - roi[4:, 1]) / 4)
            w = np.sum(
                np.sqrt(np.sum((roi[0, [0, 2]] - roi[3, [0, 2]])**2)) +
                np.sqrt(np.sum((roi[1, [0, 2]] - roi[2, [0, 2]])**2)) +
                np.sqrt(np.sum((roi[4, [0, 2]] - roi[7, [0, 2]])**2)) +
                np.sqrt(np.sum((roi[5, [0, 2]] - roi[6, [0, 2]])**2))
            ) / 4
            l = np.sum(
                np.sqrt(np.sum((roi[0, [0, 2]] - roi[1, [0, 2]])**2)) +
                np.sqrt(np.sum((roi[2, [0, 2]] - roi[3, [0, 2]])**2)) +
                np.sqrt(np.sum((roi[4, [0, 2]] - roi[5, [0, 2]])**2)) +
                np.sqrt(np.sum((roi[6, [0, 2]] - roi[7, [0, 2]])**2))
            ) / 4
            x, y, z = np.sum(roi, axis=0) / 8
            y = y + h/2
            # warn("x {} y {} z {}".format(x, y, z))


            ry = np.sum(
                math.atan2(roi[2, 0] - roi[1, 0], roi[2, 2] - roi[1, 2]) +
                math.atan2(roi[6, 0] - roi[5, 0], roi[6, 2] - roi[5, 2]) +
                math.atan2(roi[3, 0] - roi[0, 0], roi[3, 2] - roi[0, 2]) +
                math.atan2(roi[7, 0] - roi[4, 0], roi[7, 2] - roi[4, 2]) +
                math.atan2(roi[0, 2] - roi[1, 2], roi[1, 0] - roi[0, 0]) +
                math.atan2(roi[4, 2] - roi[5, 2], roi[5, 0] - roi[4, 0]) +
                math.atan2(roi[3, 2] - roi[2, 2], roi[2, 0] - roi[3, 0]) +
                math.atan2(roi[7, 2] - roi[6, 2], roi[6, 0] - roi[7, 0])
            ) / 8
            if w > l:
                w, l = l, w
                ry = angle_in_limit(ry + np.pi / 2)
        else:  # max version
            h = max(abs(roi[:4, 1] - roi[4:, 1]))
            w = np.max(
                np.sqrt(np.sum((roi[0, [0, 2]] - roi[3, [0, 2]])**2)) +
                np.sqrt(np.sum((roi[1, [0, 2]] - roi[2, [0, 2]])**2)) +
                np.sqrt(np.sum((roi[4, [0, 2]] - roi[7, [0, 2]])**2)) +
                np.sqrt(np.sum((roi[5, [0, 2]] - roi[6, [0, 2]])**2))
            )
            l = np.max(
                np.sqrt(np.sum((roi[0, [0, 2]] - roi[1, [0, 2]])**2)) +
                np.sqrt(np.sum((roi[2, [0, 2]] - roi[3, [0, 2]])**2)) +
                np.sqrt(np.sum((roi[4, [0, 2]] - roi[5, [0, 2]])**2)) +
                np.sqrt(np.sum((roi[6, [0, 2]] - roi[7, [0, 2]])**2))
            )
            x, y, z = np.sum(roi, axis=0) / 8
            y = y + h/2
            ry = np.sum(
                math.atan2(roi[2, 0] - roi[1, 0], roi[2, 2] - roi[1, 2]) +
                math.atan2(roi[6, 0] - roi[5, 0], roi[6, 2] - roi[5, 2]) +
                math.atan2(roi[3, 0] - roi[0, 0], roi[3, 2] - roi[0, 2]) +
                math.atan2(roi[7, 0] - roi[4, 0], roi[7, 2] - roi[4, 2]) +
                math.atan2(roi[0, 2] - roi[1, 2], roi[1, 0] - roi[0, 0]) +
                math.atan2(roi[4, 2] - roi[5, 2], roi[5, 0] - roi[4, 0]) +
                math.atan2(roi[3, 2] - roi[2, 2], roi[2, 0] - roi[3, 0]) +
                math.atan2(roi[7, 2] - roi[6, 2], roi[6, 0] - roi[7, 0])
            ) / 8
            if w > l:
                w, l = l, w
                ry = angle_in_limit(ry + np.pi / 2)
        ret.append([x, y, z, h, w, l, ry])
        # warn("z: {}".format(x))
    if coordinate == 'lidar':
        ret = camera_to_lidar_box(np.array(ret), calib_mat=calib_mat)

    return np.array(ret)
def observation_model(xVeh, iFeature, Map):
    Delta = Map[0:2, iFeature:(iFeature+1)]-xVeh[0:2]
    z = np.array([[np.linalg.norm(Delta)], [
        atan2(Delta[1], Delta[0]) - xVeh[2, 0]]])
    z[1, 0] = angle_wrap(z[1, 0])
    return z
Exemple #51
0
    def _is_hit_constrain(self, q_now, q_near):
        result = 0
        counter = 1
        theta = atan2(q_near[1] - q_now[1], q_near[0] - q_now[0])
        _c = cos(theta)
        _s = sin(theta)
        t = np.zeros(16)
        s = np.zeros(16)
        while counter <= self.obstacle_list.size:
            for obs_index in range(1, self.obstacle_list.size):
                bound = [
                    self.obstacle_list[obs_index].x -
                    self.obstacle_list[obs_index].r / 2,
                    self.obstacle_list[obs_index].x +
                    self.obstacle_list[obs_index].r / 2,
                    self.obstacle_list[obs_index].y -
                    self.obstacle_list[obs_index].r / 2,
                    self.obstacle_list[obs_index].y +
                    self.obstacle_list[obs_index].r / 2
                ]

                t[0] = (q_near[0] - bound[0] + self.car_length / 2 * _c) / _s
                s[0] = (q_near[1] - bound[2] + t[0] * _c +
                        self.car_length / 2 * _s) / (bound[3] - bound[2])
                t[1] = (q_near[0] - bound[1] + self.car_length / 2 * _c) / _s
                s[1] = (q_near[1] - bound[2] + t[1] * _c +
                        self.car_length / 2 * _s) / (bound[3] - bound[2])
                t[2] = (bound[2] - q_near[1] - self.car_length / 2 * _s) / _c
                s[2] = (q_near[0] - bound[2] - t[2] * _s +
                        self.car_length / 2 * _c) / (bound[1] - bound[0])
                t[3] = (bound[3] - q_near[1] - self.car_length / 2 * _s) / _c
                s[3] = (q_near[0] - bound[0] - t[3] * _s +
                        self.car_length / 2 * _c) / (bound[1] - bound[0])

                t[4] = (q_near[0] - bound[0] - self.car_length / 2 * _c) / _s
                s[4] = (q_near[1] - bound[2] + t[4] * _c -
                        self.car_length / 2 * _s) / (bound[3] - bound[2])
                t[5] = (q_near[0] - bound[1] - self.car_length / 2 * _c) / _s
                s[5] = (q_near[1] - bound[2] + t[5] * _c * self.car_length /
                        2 * _s) / (bound[3] - bound[2])
                t[6] = (bound[2] - q_near[1] + self.car_length / 2 * _s) / _c
                s[6] = (q_near[0] - bound[0] - t[6] * _s -
                        self.car_length / 2 * _c) / (bound[1] - bound[0])
                t[7] = (bound[3] - q_near[1] + self.car_length / 2 * _s) / _c
                s[7] = (q_near[0] - bound[0] - t[7] * _s -
                        self.car_length / 2 * _c) / (bound[1] - bound[0])

                t[8] = (bound[0] - q_near[0] + self.car_length / 2 * _s) / _c
                s[8] = (q_near[1] - bound[2] + t[8] * _s +
                        self.car_length / 2 * _c) / (bound[3] - bound[2])
                t[9] = (bound[1] - q_near[0] + self.car_length / 2 * _s) / _c
                s[9] = (q_near[1] - bound[2] + t[9] * _s -
                        self.car_length / 2 * _c) / (bound[3] - bound[2])
                t[10] = (bound[2] - q_near[1] - self.car_length / 2 * _c) / _s
                s[10] = (q_near[0] - bound[0] + t[10] * _c -
                         self.car_length / 2 * _s) / (bound[1] - bound[0])
                t[11] = (bound[3] - q_near[1] - self.car_length / 2 * _c) / _s
                s[11] = (q_near[0] - bound[0] + t[11] * _c -
                         self.car_length / 2 * _s) / (bound[1] - bound[0])

                t[12] = (bound[0] - q_near[0] - self.car_length / 2 * _s) / _c
                s[12] = (q_near[1] - bound[2] + t[12] * _s -
                         self.car_length / 2 * _c) / (bound[3] - bound[2])
                t[13] = (bound[1] - q_near[0] - self.car_length / 2 * _s) / _c
                s[13] = (q_near[1] - bound[2] + t[13] * _s -
                         self.car_length / 2 * _c) / (bound[3] - bound[2])
                t[14] = (bound[2] - q_near[1] + self.car_length / 2 * _c) / _s
                s[14] = (q_near[0] - bound[0] + t[14] * _c -
                         self.car_length / 2 * _s) / (bound[1] - bound[0])
                t[15] = (bound[3] - q_near[1] + self.car_length / 2 * _c) / _s
                s[15] = (q_near[0] - bound[0] + t[15] * _c +
                         self.car_length / 2 * _s) / (bound[1] - bound[0])

                for parameter_checker in range(0, 15):
                    if (t[parameter_checker] >= -self.car_length / 2
                            and t[parameter_checker] <= self.car_length / 2
                            and s[parameter_checker] >= 0
                            and s[parameter_checker] <= 1):
                        result = 1
                    # end if
                # end for (parameter_checker)
            # end for (obs_index)
            counter = counter + 1
        #end while (counter)
        return result
def get_path_position_orientation(pose, my_path, timestamp, sync):
    if pose:
        # Print some of the pose data to the terminal
        data = pose.get_pose_data()

        w_r = data.rotation.w
        x_r = -data.rotation.z
        y_r = data.rotation.x
        z_r = -data.rotation.y

        pitch =  -math.asin(2.0 * (x_r*z_r - w_r*y_r)) * 180.0 / math.pi;
        roll  =  math.atan2(2.0 * (w_r*x_r + y_r*z_r), w_r*w_r - x_r*x_r - y_r*y_r + z_r*z_r) * 180.0 / math.pi
        yaw   =  math.atan2(2.0 * (w_r*z_r + x_r*y_r), w_r*w_r + x_r*x_r - y_r*y_r - z_r*z_r) * 180.0 / math.pi

        #create path
        pose = PoseStamped()
        pose.pose.position.x = -data.translation.z
        pose.pose.position.z = data.translation.y
        pose.pose.position.y = -data.translation.x

        pose.pose.orientation.x = pitch
        pose.pose.orientation.z = -yaw
        pose.pose.orientation.y = roll
        pose.pose.orientation.w = 1

        pose.header.frame_id = 'camera'

        if sync:
            #print("trajectory timestamp: ",timestamp)
            t1 = (timestamp / 100000000)
            t2 = (t1 - int(t1)) * 100000

            #t1 = timestamp / 1000.0
            time = rospy.Time(secs=int(t2), nsecs = int((t2 - int(t2))*100))
            #print("trajectory time: ",time)

        my_path.poses.append(pose)
        position = [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z]
        orientation = [pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z]

        odom = Odometry()
        odom.header.frame_id = 'camera'

        if sync:
            odom.header.stamp = time

        odom.pose.pose.position.x = data.translation.x
        odom.pose.pose.position.z = data.translation.y
        odom.pose.pose.position.y = data.translation.z

        odom.pose.pose.orientation.x = -x_r
        odom.pose.pose.orientation.z = z_r
        odom.pose.pose.orientation.y = y_r
        odom.pose.pose.orientation.w = w_r

        odom.pose.covariance = np.diag([1e-1, 1e-1, 1e-1, 1e-2, 1e-2, 1e-2]).ravel()

        odom.twist.twist.linear.x = data.velocity.x
        odom.twist.twist.linear.z = data.velocity.y
        odom.twist.twist.linear.y = -data.velocity.z

        odom.twist.twist.angular.x = data.acceleration.x
        odom.twist.twist.angular.y = data.acceleration.y
        odom.twist.twist.angular.z = data.acceleration.z

        odom.twist.covariance = np.diag([1e-1, 1e-1, 1e-1, 1e-2, 1e-2, 1e-2]).ravel()

    return my_path, position, orientation, odom
    def plot(self):
        """Opens a window, draws the graph into the window.
           Requires Tk, and of course a windowing system.
        """
        import Tkinter as tk
        import math
        window = tk.Tk()
        canvas_size = 400
        drawing = tk.Canvas(window, height=canvas_size, width=canvas_size, background="white")
        n_nodes = self.n_nodes
        radius = 150
        node_radius = 10
        
        drawing.create_text(200, 10, text="Network:" + str(id(self)))

        list_of_coordinates = [(radius * math.sin(2 * math.pi * n / n_nodes) + canvas_size / 2, radius * math.cos(2 * math.pi * n / n_nodes) + canvas_size / 2) for n in range(n_nodes)]
        
        for linksto, node in enumerate(self.adjacency):
            for linksfrom, link in enumerate(node):
                
                if linksto == linksfrom and link == True:
                    angle = math.atan2(list_of_coordinates[linksto][1] - 200,
                                      list_of_coordinates[linksto][0] - 200)
                    
                    drawing.create_line(list_of_coordinates[linksto][0] + node_radius * math.cos(angle),
                                        list_of_coordinates[linksto][1] + node_radius * math.sin(angle),
                                        list_of_coordinates[linksto][0] + node_radius * 2 * (math.cos(angle + 20)),
                                        list_of_coordinates[linksto][1] + node_radius * 2 * math.sin(angle + 20),
                                        list_of_coordinates[linksto][0] + node_radius * 4 * (math.cos(angle)),
                                        list_of_coordinates[linksto][1] + node_radius * 4 * math.sin(angle),
                                        list_of_coordinates[linksto][0] + node_radius * 2 * math.cos(angle - 20),
                                        list_of_coordinates[linksto][1] + node_radius * 2 * (math.sin(angle - 20)),
                                        list_of_coordinates[linksto][0] + node_radius * math.cos(angle),
                                        list_of_coordinates[linksto][1] + node_radius * math.sin(angle),
                                        smooth=True, joinstyle="round", fill="black", width=2, arrow="last"
                                        )
                
                elif link == True: 
                    angle = math.atan2(list_of_coordinates[linksto][1] - list_of_coordinates[linksfrom][1],
                                   list_of_coordinates[linksto][0] - list_of_coordinates[linksfrom][0])
                
                    drawing.create_line(list_of_coordinates[linksfrom][0] + node_radius * math.cos(angle),
                                        list_of_coordinates[linksfrom][1] + node_radius * math.sin(angle),
                                        list_of_coordinates[linksto][0] - node_radius * math.cos(angle),
                                        list_of_coordinates[linksto][1] - node_radius * math.sin(angle),
                                        fill="black", width=2, arrow="last")
        
        for node_ctr, (x, y) in enumerate(list_of_coordinates):

            if type(self) != Network:
                node_color = "white"
                text_color = "black"
            elif self.state == num.array([[]]):
                node_color = "white"
                text_color = "black"
            else:
                if self.state[-1][node_ctr] == True: 
                    node_color = "black"
                    text_color = "white"
                else:                
                    node_color = "white"
                    text_color = "black"
                    
            drawing.create_oval(x - node_radius, y - node_radius, x + node_radius, y + node_radius, width=2, fill=node_color)
            drawing.create_text(x, y, text=str(node_ctr), fill=text_color, font="Arial")
            

        
        drawing.pack()
        window.mainloop()
Exemple #54
0
        T_B_L_door = velma.getTf("B", "right_door")
        ldX,ldY,ldZ = T_B_L_door.p
        ldYaw,ldPitch,ldRoll = T_B_L_door.M.GetEulerZYX()
        Rot = PyKDL.Rotation.RotZ(ldYaw + math.pi) 
        tempX = ldX + math.sin(ldYaw) * 0.2835 + math.cos(ldYaw) * (0.3 + step)
        tempY = ldY - math.cos(ldYaw) * 0.2835 + math.sin(ldYaw) * (0.3 + step)
        Trans = PyKDL.Vector(tempX,tempY,ldZ + 0.7)
        dest_cab = PyKDL.Frame(Rot,Trans)
    
    # VELMA IS READY FOR MOVING 
    

    print("Preparing velma for the task (torso and right hand)")
    T_B_Cab = velma.getTf("B", "cabinet")   
    cabX,cabY,cabZ = T_B_Cab.p
    cab_angle = math.atan2(cabY,cabX)
    if cab_angle < math.pi/2 and cab_angle > -math.pi/2 :
        q_map_start['torso_0_joint'] = cab_angle
    elif cab_angle > math.pi/2 :
        q_map_start['torso_0_joint'] = math.pi/2 - 0.03
    else :
        q_map_start['torso_0_joint'] = -math.pi/2 + 0.03 
    planAndExecute(q_map_start)

    

    # UNTESTED NEEDS MORE TESTS
    
    print("moving hand to cabinet")
    T_B_L_door = velma.getTf("B", "right_door")
    ldX,ldY,ldZ = T_B_L_door.p
        # Step 2.2 Calculate Exponential factor from intrinsic impedance
        ej = cmath.exp(-2*thickness*dj);                     
    
        # Step 2.3 Calculate reflection coeficient using current layer
        #          intrinsic impedance and the below layer impedance
        belowImpedance = impedances[j + 1];
        rj = (wj - belowImpedance)/(wj + belowImpedance);
        re = rj*ej; 
        Zj = wj * ((1 - re)/(1 + re));
        impedances[j] = Zj;    

    # Step 3. Compute apparent resistivity from top layer impedance
    Z = impedances[0];
    absZ = abs(Z);
    apparentResistivity.append((absZ * absZ)/(mu * w))
    phase.append(math.atan2(Z.imag, Z.real))
    
print('');
print('time taken = ', time.clock() - start, 's');

# Plot the results in 2 axes. Phase (degree) and apparent resistivity (ohm meter)

fig, ax = plt.subplots(2, 1, figsize=(6, 4))

ax[0].loglog(frequencies,apparentResistivity,'r-o'), 
ax[0].set_ylabel("$\\rho_a \ (\Omega m)$", fontsize=12)

ax[1].semilogx(frequencies,np.array(phase)*180/np.pi,'b-o'),
ax[1].set_ylabel("$\phi \ (^{\circ})$", fontsize=12)
ax[1].set_ylim([0., 90.])
  def __getitem__(self, index):
    """
    Get the pixels of an image, and a random synthetic scene graph for that
    image constructed on-the-fly from its COCO object annotations. We assume
    that the image will have height H, width W, C channels; there will be O
    object annotations, each of which will have both a bounding box and a
    segmentation mask of shape (M, M). There will be T triples in the scene
    graph.

    Returns a tuple of:
    - image: FloatTensor of shape (C, H, W)
    - objs: LongTensor of shape (O,)
    - boxes: FloatTensor of shape (O, 4) giving boxes for objects in
      (x0, y0, x1, y1) format, in a [0, 1] coordinate system
    - masks: LongTensor of shape (O, M, M) giving segmentation masks for
      objects, where 0 is background and 1 is object.
    - triples: LongTensor of shape (T, 3) where triples[t] = [i, p, j]
      means that (objs[i], p, objs[j]) is a triple.
    """
    image_id = self.image_ids[index]
    abs_image = [] 
    filename = self.image_id_to_filename[image_id]
    image_path = os.path.join(self.image_dir, filename)
    with open(image_path, 'rb') as f:
      with PIL.Image.open(f) as image:
        # unscaled image
        abs_image = np.array(image)
        WW, HH = image.size
        image = self.transform(image.convert('RGB'))

    H, W = self.image_size
    objs, boxes, masks = [], [], []
    # absolute masks (unscaled)
    # can later convert masks into self.image_size 
    # scale if scaling factor is known (s = self.image_size/HH, etc)
    abs_boxes, abs_masks = [], []
    obj_contours = []

    for object_data in self.image_id_to_objects[image_id]:
      objs.append(object_data['category_id'])
      x, y, w, h = object_data['bbox']
      # Normalized coordinates, preserves aspect ratio
      x0 = x / WW
      y0 = y / HH
      x1 = (x + w) / WW
      y1 = (y + h) / HH
      boxes.append(torch.FloatTensor([x0, y0, x1, y1]))
      # unscaled boxes in true image coords  (e.g side len = 4 => coords [0,3]
      abs_boxes.append(torch.FloatTensor([x, y, x + w - 1, y + h - 1]))
 
      # This will give a numpy array of shape (HH, WW)
      mask = seg_to_mask(object_data['segmentation'], WW, HH)

      # Crop the mask according to the bounding box, being careful to
      # ensure that we don't crop a zero-area region
      mx0, mx1 = int(round(x)), int(round(x + w))
      my0, my1 = int(round(y)), int(round(y + h))
      mx1 = max(mx0 + 1, mx1)
      my1 = max(my0 + 1, my1)
      mask = mask[my0:my1, mx0:mx1]
      # unscaled mask
      abs_mask = mask

      # masks resized to 64x64
      mask = imresize(255.0 * mask, (self.mask_size, self.mask_size),
                      mode='constant', anti_aliasing=True)
      mask = torch.from_numpy((mask > 128).astype(np.int64))
      # unscaled/resized mask 
      abs_mask = torch.from_numpy((abs_mask > 0).astype(np.int64))

      masks.append(mask)
      # add unscaled/resized mask
      abs_masks.append(abs_mask)
 
     # Extract contours for each scaled mask
      max_contour_pts = 25
      pad = 1
      obj_contour = None
      # Add padding to capture edge contours
      mask_pad = np.pad(mask, ((pad,pad),(pad,pad)), 'constant')
      obj_contour = measure.find_contours(mask_pad, 0.99)
      obj_contour = np.concatenate(obj_contour, axis=0) - pad # remove padding
      subsample = int(np.ceil(len(obj_contour)/max_contour_pts))
      obj_contour = obj_contour[0::subsample] # min val = 1

      if len(obj_contour) < max_contour_pts:
        num_pad = max_contour_pts - len(obj_contour)
        obj_contour = np.concatenate([obj_contour, np.zeros((num_pad, 2))], axis=0)

      # points visualized in (y,x) order
      orig_contour = np.copy(obj_contour)
      # contour points are in (y,x) order
      obj_contour[:,[0, 1]] = obj_contour[:,[1, 0]]
      # normalize to size of object mask (which is self.mask_size)
      obj_contour[:,0] = obj_contour[:,0]/self.mask_size # x
      obj_contour[:,1] = obj_contour[:,1]/self.mask_size # y
      obj_contours.append(torch.FloatTensor(obj_contour.flatten()))

      import matplotlib.pyplot as plt
      fig, ax = plt.subplots()
      ax.imshow(mask)
      print(h, w, triplet_mask.shape)
      ax.scatter(orig_contour[:,1],orig_contour[:,0], linewidth=0.5)
      plt.show()
      pdb.set_trace()

    # Add dummy __image__ object
    objs.append(self.vocab['object_name_to_idx']['__image__'])
    boxes.append(torch.FloatTensor([0, 0, 1, 1]))
    masks.append(torch.ones(self.mask_size, self.mask_size).long())
    # unscaled box/mask for singleton case: [x0, y0, x1, y1]
    abs_boxes.append(torch.FloatTensor([0, 0, WW, HH]))
    abs_masks.append(torch.ones(HH, WW).long())
    # add empty contours with all zeros
    obj_contours.append(torch.FloatTensor(np.zeros((max_contour_pts, 2)).flatten()))

    objs = torch.LongTensor(objs)
    # merge a list of Tensors into one Tensor
    boxes = torch.stack(boxes, dim=0)
    masks = torch.stack(masks, dim=0)
    abs_boxes = torch.stack(abs_boxes, dim=0)
    obj_contours = torch.stack(obj_contours, dim=0)

    box_areas = (boxes[:, 2] - boxes[:, 0]) * (boxes[:, 3] - boxes[:, 1])

    # Compute centers of all objects
    obj_centers = []
    _, MH, MW = masks.size()
    for i, obj_idx in enumerate(objs):
      x0, y0, x1, y1 = boxes[i]
      mask = (masks[i] == 1)
      xs = torch.linspace(x0, x1, MW).view(1, MW).expand(MH, MW)
      ys = torch.linspace(y0, y1, MH).view(MH, 1).expand(MH, MW)
      if mask.sum() == 0:
        mean_x = 0.5 * (x0 + x1)
        mean_y = 0.5 * (y0 + y1)
      else:
        mean_x = xs[mask].mean()
        mean_y = ys[mask].mean()
      obj_centers.append([mean_x, mean_y])
    obj_centers = torch.FloatTensor(obj_centers)

    # Add triples
    triples = []
    num_objs = objs.size(0)
    __image__ = self.vocab['object_name_to_idx']['__image__']
    real_objs = []
    if num_objs > 1:
      real_objs = (objs != __image__).nonzero().squeeze(1)
    for cur in real_objs:
      choices = [obj for obj in real_objs if obj != cur]
      if len(choices) == 0 or not self.include_relationships:
        break

      # by default, randomize
      ###############
      if self.seed != 0:  
        random.seed(self.seed)  
      #################
  
      other = random.choice(choices)

      ##################
      if self.seed != 0:  
        random.seed(self.seed) 
      ##################  
      
      if random.random() > 0.5:
        s, o = cur, other
      else:
        s, o = other, cur

      # Check for inside / surrounding
      sx0, sy0, sx1, sy1 = boxes[s]
      ox0, oy0, ox1, oy1 = boxes[o]
      d = obj_centers[s] - obj_centers[o]
      theta = math.atan2(d[1], d[0])

      if sx0 < ox0 and sx1 > ox1 and sy0 < oy0 and sy1 > oy1:
        p = 'surrounding'
      elif sx0 > ox0 and sx1 < ox1 and sy0 > oy0 and sy1 < oy1:
        p = 'inside'
      elif theta >= 3 * math.pi / 4 or theta <= -3 * math.pi / 4:
        p = 'left of'
      elif -3 * math.pi / 4 <= theta < -math.pi / 4:
        p = 'above'
      elif -math.pi / 4 <= theta < math.pi / 4:
        p = 'right of'
      elif math.pi / 4 <= theta < 3 * math.pi / 4:
        p = 'below'
      # add heuristics here
      p = self.vocab['pred_name_to_idx'][p]
      triples.append([s, p, o])

      #########################################################
      # front_back_flag = False
      # s_obj_type = self.vocab['object_idx_to_name'][s]  
      # o_obj_type = self.vocab['object_idx_to_name'][o]  
      # if s_obj_type in self.instance_whitelist and o_obj_type in self.instance_whitelist: 
      #   if sy1 > oy1 and oy1 > sy0 and ox1 > sx0 and ox0 < sx1:
      #     q = 'infront of'
      #     front_back_flag = True
      #   elif sy1 < oy1 and oy1 < sy0 and ox1 < sx0 and ox0 > sx1:
      #     q = 'behind'
      #     front_back_flag = True
        
      # if front_back_flag:
      #   q = self.vocab['pred_name_to_idx'][q]
      #   triples.append([s, q, o])
      #########################################################

    # Add __in_image__ triples
    O = objs.size(0)
    in_image = self.vocab['pred_name_to_idx']['__in_image__']
    for i in range(O - 1):
      triples.append([i, in_image, O - 1])
  
    triples = torch.LongTensor(triples)

    # get bounding boxes for triples
    num_triples = triples.size(0)
    s, p, o = triples.chunk(3, dim=1)
    s_boxes, o_boxes = abs_boxes[s], abs_boxes[o]
    triplet_boxes = torch.cat([torch.squeeze(s_boxes), torch.squeeze(o_boxes)], dim=1)
    triplet_masks = []
    triplet_contours = []
    superboxes = []

    #print(triples) 
   
    # masks will all be resized to fixed size (32x32) 
    for n in range(0, num_triples):
      s_idx, o_idx = s[n], o[n] # index into object-related arrays
      s_mask = abs_masks[s_idx] # unscaled subject mask 
      o_mask = abs_masks[o_idx] # unscaled object mask 
      # find dimensions of encapsulating triplet superbox 
      min_x = np.min([triplet_boxes[n][0], triplet_boxes[n][4]])
      min_y = np.min([triplet_boxes[n][1], triplet_boxes[n][5]])
      max_x = np.max([triplet_boxes[n][2], triplet_boxes[n][6]])
      max_y = np.max([triplet_boxes[n][3], triplet_boxes[n][7]]) 
      superboxes.append([min_x, min_y, max_x, max_y])
      
      #print('----------------------------------')
      #print(superboxes[n])
    
      min_x, min_y = int(round(min_x)), int(round(min_y))
      max_x, max_y = int(round(max_x)), int(round(max_y))
      h = max_y - min_y + 1    
      w = max_x - min_x + 1
       
      #print([min_x, min_y, max_x, max_y])
      #print('[', 0, 0, w-1, h-1, ']')
    
       
      # create empty mask the size of superbox
      self.triplet_mask_size = 32
      triplet_mask = np.zeros((h, w))
      # superbox shift offset
      dx, dy = min_x, min_y
      
      #print('dx, dy: ', dx, dy)
     
      # indices must be integers 
      bbs = np.array(np.round(triplet_boxes[n])).astype(int) 
      
      #print("subj:", bbs[:4], "[", bbs[0] - dx, bbs[1] - dy, bbs[2] - dx, bbs[3] - dy, "]") # 
      #print("obj:", bbs[4:], "[", bbs[4] - dx, bbs[5] - dy, bbs[6] - dx, bbs[7] - dy, "]") # 

      #print('size: ', s_mask.shape)
      # python array indexing: (0,640) is RHS non-inclusive
      # subject mask
      mask_h, mask_w  = s_mask.shape[0], s_mask.shape[1]
      x0, y0 = bbs[0] - dx, bbs[1] - dy
      x0, y0 = max(x0, 0), max(y0, 0) 
      x1, y1 = x0 + mask_w, y0 + mask_h # this should be ok: w = 5, [0:5]
      x1, y1 = min(x1, w), min(y1, h)
      assert triplet_mask[y0:y1, x0:x1].shape == s_mask[0:y1 - y0, 0:x1 - x0].shape, print('s_mask mismatch shape: ', triplet_mask[y0:y1, x0:x1].shape, s_mask[0:y1 - y0, 0:x1 - x0].shape)
      
      # implicite: label subject points as value 1
      triplet_mask[y0:y1, x0:x1] = s_mask[0:y1 - y0, 0:x1 - x0]
      # resize
      triplet_mask = imresize(255.0 * triplet_mask, (self.triplet_mask_size, self.triplet_mask_size),
                      mode='constant', anti_aliasing=True)
      triplet_mask = (triplet_mask > 128).astype(np.int64) 

      #pdb.set_trace() 
      # contour points for subject 
      max_contour_pts = 50  
      pad = 10
      s_contours = None
      s_triplet_mask_pad = np.pad(triplet_mask, ((pad,pad),(pad,pad)), 'constant')
      # problem: sometimes small contours are downsampled away
      s_contours = measure.find_contours(s_triplet_mask_pad, 0.99)   
      # in rare case object gets subsampled away
      if len(s_contours) < 1:
        s_contours = np.zeros((max_contour_pts, 2)) # if this is concatenated, becomes 1x100D
      else:
        s_contours = np.concatenate(s_contours, axis=0) - pad # remove padding 
      subsample = int(np.ceil(len(s_contours)/max_contour_pts))
      s_contours = s_contours[0::subsample] # min val = 1 
      if len(s_contours) < max_contour_pts:
        num_pad = max_contour_pts - len(s_contours)
        s_contours = np.concatenate([s_contours, np.zeros((num_pad, 2))], axis=0) 

      #s_ch = ConvexHull(s_contours)
      #pdb.set_trace() 
      # need to deal with case for singleton triples
      #if(self.vocab['object_idx_to_name'][objs[o[n]]] == '__image__'):
        #import matplotlib.pyplot as plt
        #print('--------------------------------')
        #pdb.set_trace()
        #plt.imshow(triplet_mask)
        #plt.imshow(s_mask)
        #plt.show()
        #plt.imshow(triplet_image)
        #plt.show()
      #else:

      o_contours = None
      if(self.vocab['object_idx_to_name'][objs[o[n]]] != '__image__'):
        # object mask 
        mask_h, mask_w  = o_mask.shape[0], o_mask.shape[1]
        x0, y0 = bbs[4] - dx, bbs[5] - dy
        x0, y0 = max(x0, 0), max(y0, 0) 
        x1, y1 = x0 + mask_w, y0 + mask_h 
        x1, y1 = min(x1, w), min(y1, h)
        #assert triplet_mask[y0:y1, x0:x1].shape == o_mask[0:y1 - y0, 0:x1 - x0].shape, print('mismatch shape: ', triplet_mask[y0:y1, x0:x1].shape, o_mask[0:y1 - y0, 0:x1 - x0].shape) 
        o_triplet_mask = np.zeros((h, w))
        o_triplet_mask[y0:y1, x0:x1] = o_mask[0:y1 - y0, 0:x1 - x0]
        o_triplet_mask = imresize(255.0 * o_triplet_mask, (self.triplet_mask_size, self.triplet_mask_size),
                         mode='constant', anti_aliasing=True)
        o_triplet_mask = (o_triplet_mask > 128).astype(np.int64)
        # OR triplet masks to deal with areas of overlap
        triplet_mask = np.logical_or(triplet_mask, o_triplet_mask).astype(np.int64)
        # label object pixel value 2
        triplet_mask += o_triplet_mask
        
        # contour points for object
        #o_triplet_mask = np.zeros((h, w))
        #o_triplet_mask[y0:y1, x0:x1] = o_mask[0:y1 - y0, 0:x1 - x0]
        o_triplet_mask_pad = np.pad(o_triplet_mask, ((pad,pad),(pad,pad)), 'constant')
        # get contours
        o_contours = measure.find_contours(o_triplet_mask_pad, 0.99)  
        # in rare case object gets subsampled away
        if len(o_contours) < 1:
          o_contours = np.zeros((max_contour_pts, 2))
        else:
          o_contours = np.concatenate(o_contours, axis=0) - pad # removed padding
        subsample = int(np.ceil(len(o_contours)/max_contour_pts))
        o_contours = o_contours[0::subsample] # min val = 1
        if len(o_contours) < max_contour_pts:
          num_pad = max_contour_pts - len(o_contours)
          o_contours = np.concatenate([o_contours, np.zeros((num_pad, 2))], axis=0)
      
      if s_contours is not None and  o_contours is not None:
        assert s_contours.shape  == o_contours.shape, pdb.set_trace()
        contours = np.concatenate([s_contours, o_contours], axis=0)
      elif s_contours is not None: # singleton (o_contours is None)
        assert s_contours.shape  == np.zeros((max_contour_pts, 2)).shape, pdb.set_trace()
        contours = np.concatenate([s_contours, np.zeros((max_contour_pts, 2))], axis=0)

      # just in case
      if len(contours) > 2*max_contour_pts:
        contours = contours[0:2*max_contour_pts]
      elif len(contours) < 2*max_contour_pts:
        num_pad = max_contour_pts - len(contours)
        contours = np.concatenate([contours, np.zeros((num_pad, 2))], axis=0)
    
      # points visualized in (y,x) order 
      orig_contours = np.copy(contours) 
      # contour points are (y,x) 
      contours[:,[0, 1]] = contours[:,[1, 0]]
      # normalize to size of padded triplet mask 
      contours[:,0] = contours[:,0]/self.triplet_mask_size # w, x
      contours[:,1] = contours[:,1]/self.triplet_mask_size # h, y
      triplet_contours.append(torch.FloatTensor(contours.flatten())) 

      triplet_mask = torch.from_numpy(triplet_mask)
      triplet_masks.append(triplet_mask)    
    
      #import matplotlib.pyplot as plt
      #fig, ax = plt.subplots()
      #ax.imshow(triplet_mask)
      #print(h, w, triplet_mask.shape)
      #ax.scatter(orig_contours[:,1],orig_contours[:,0], linewidth=0.5)
      #ax.scatter(s_contours[:,1],s_contours[:,0], linewidth=0.5)
      #if o_contours is not None:
      #  ax.scatter(o_contours[:,1],o_contours[:,0], linewidth=0.5)
      #plt.show()

    # merge multiple tensors into one
    triplet_masks = torch.stack(triplet_masks, dim=0)
    triplet_contours = torch.stack(triplet_contours, dim=0)
    #print('----- end processing of image --------') 
    # this gets passed to coco_collate
    return image, objs, boxes, masks, triples, triplet_masks, triplet_contours
Exemple #57
0
def curve3_recursive_bezier( points, x1, y1, x2, y2, x3, y3, level = 0 ):
    if level > curve_recursion_limit:
        return

    # Calculate all the mid-points of the line segments
    # -------------------------------------------------
    x12  = (x1 + x2) / 2.
    y12  = (y1 + y2) / 2.
    x23  = (x2 + x3) / 2.
    y23  = (y2 + y3) / 2.
    x123 = (x12 + x23) / 2.
    y123 = (y12 + y23) / 2.

    dx = x3 - x1
    dy = y3 - y1
    d = math.fabs((x2-x3)*dy - (y2-y3)*dx)

    if d > curve_collinearity_epsilon:
        # Regular case
        # ------------
        if d*d <= m_distance_tolerance_square * (dx*dx + dy*dy):
            # If the curvature doesn't exceed the distance_tolerance value
            # we tend to finish subdivisions.
            if m_angle_tolerance < curve_angle_tolerance_epsilon:
                points.append( (x123,y123) )
                return

            # Angle & Cusp Condition
            da = math.fabs(math.atan2(y3 - y2, x3 - x2) - math.atan2(y2 - y1, x2 - x1))
            if da >= math.pi:
                da = 2*math.pi - da

            if da < m_angle_tolerance:
                # Finally we can stop the recursion
                points.append( (x123,y123) )
                return
    else:
        # Collinear case
        # --------------
        da = dx*dx + dy*dy
        if da == 0:
            d = calc_sq_distance(x1, y1, x2, y2)
        else:
            d = ((x2 - x1)*dx + (y2 - y1)*dy) / da
            if d > 0 and d < 1:
                # Simple collinear case, 1---2---3, we can leave just two endpoints
                return
            if(d <= 0):
                d = calc_sq_distance(x2, y2, x1, y1)
            elif d >= 1:
                d = calc_sq_distance(x2, y2, x3, y3)
            else:
               d = calc_sq_distance(x2, y2, x1 + d*dx, y1 + d*dy)

        if d < m_distance_tolerance_square:
            points.append( (x2,y2) )
            return

    # Continue subdivision
    # --------------------
    curve3_recursive_bezier( points, x1, y1, x12, y12, x123, y123, level + 1 )
    curve3_recursive_bezier( points, x123, y123, x23, y23, x3, y3, level + 1 )
def angle_of_vector(point1, point2):
    rad = math.atan2(point2[1] - point1[1], point2[0] - point1[0])
    return math.degrees(rad)
Exemple #59
0
def curve4_recursive_bezier( points, x1, y1, x2, y2, x3, y3, x4, y4, level=0):
    if level > curve_recursion_limit: 
        return

    # Calculate all the mid-points of the line segments
    # -------------------------------------------------
    x12   = (x1 + x2) / 2.
    y12   = (y1 + y2) / 2.
    x23   = (x2 + x3) / 2.
    y23   = (y2 + y3) / 2.
    x34   = (x3 + x4) / 2.
    y34   = (y3 + y4) / 2.
    x123  = (x12 + x23) / 2.
    y123  = (y12 + y23) / 2.
    x234  = (x23 + x34) / 2.
    y234  = (y23 + y34) / 2.
    x1234 = (x123 + x234) / 2.
    y1234 = (y123 + y234) / 2.


    # Try to approximate the full cubic curve by a single straight line
    # -----------------------------------------------------------------
    dx = x4 - x1
    dy = y4 - y1
    d2 = math.fabs(((x2 - x4) * dy - (y2 - y4) * dx))
    d3 = math.fabs(((x3 - x4) * dy - (y3 - y4) * dx))

    s =  int((d2 > curve_collinearity_epsilon) << 1) + int(d3 > curve_collinearity_epsilon)

    if s == 0:
        # All collinear OR p1==p4
        # ----------------------
        k = dx*dx + dy*dy
        if k == 0:
            d2 = calc_sq_distance(x1, y1, x2, y2)
            d3 = calc_sq_distance(x4, y4, x3, y3)

        else:
            k   = 1. / k
            da1 = x2 - x1
            da2 = y2 - y1
            d2  = k * (da1*dx + da2*dy)
            da1 = x3 - x1
            da2 = y3 - y1
            d3  = k * (da1*dx + da2*dy)
            if d2 > 0 and d2 < 1 and d3 > 0 and d3 < 1:
                # Simple collinear case, 1---2---3---4
                # We can leave just two endpoints
                return
             
            if d2 <= 0:
                d2 = calc_sq_distance(x2, y2, x1, y1)
            elif d2 >= 1:
                d2 = calc_sq_distance(x2, y2, x4, y4)
            else:
                d2 = calc_sq_distance(x2, y2, x1 + d2*dx, y1 + d2*dy)

            if d3 <= 0:
                d3 = calc_sq_distance(x3, y3, x1, y1)
            elif d3 >= 1:
                d3 = calc_sq_distance(x3, y3, x4, y4)
            else:
                d3 = calc_sq_distance(x3, y3, x1 + d3*dx, y1 + d3*dy)

        if d2 > d3:
            if d2 < m_distance_tolerance_square:
                points.append( (x2, y2) )
                return
        else:
            if d3 < m_distance_tolerance_square:
                points.append( (x3, y3) )
                return

    elif s == 1:
        # p1,p2,p4 are collinear, p3 is significant
        # -----------------------------------------
        if d3 * d3 <= m_distance_tolerance_square * (dx*dx + dy*dy):
            if m_angle_tolerance < curve_angle_tolerance_epsilon:
                points.append((x23, y23) )
                return
            
            # Angle Condition
            # ---------------
            da1 = math.fabs(math.atan2(y4 - y3, x4 - x3) - math.atan2(y3 - y2, x3 - x2))
            if da1 >= math.pi:
                da1 = 2*math.pi - da1
            
            if da1 < m_angle_tolerance:
                points.extend( [(x2, y2),(x3, y3)] )
                return

            if m_cusp_limit != 0.0:
                if da1 > m_cusp_limit:
                    points.append( (x3, y3) )
                    return

    elif s == 2:
        # p1,p3,p4 are collinear, p2 is significant
        # -----------------------------------------
        if d2 * d2 <= m_distance_tolerance_square * (dx*dx + dy*dy):
            if m_angle_tolerance < curve_angle_tolerance_epsilon:
                points.append( (x23, y23) )
                return
            
            # Angle Condition
            # ---------------
            da1 = math.fabs(math.atan2(y3 - y2, x3 - x2) - math.atan2(y2 - y1, x2 - x1))
            if da1 >= math.pi:
                da1 = 2*math.pi - da1
            
            if da1 < m_angle_tolerance:
                points.extend( [(x2, y2),(x3, y3)] )
                return
            
            if m_cusp_limit != 0.0:
                if da1 > m_cusp_limit:
                    points.append( (x2, y2) )
                    return
        
    elif s == 3:
        # Regular case
        # ------------
        if (d2 + d3)*(d2 + d3) <= m_distance_tolerance_square * (dx*dx + dy*dy):
            # If the curvature doesn't exceed the distance_tolerance value
            # we tend to finish subdivisions.

            if m_angle_tolerance < curve_angle_tolerance_epsilon:
                points.append( (x23, y23) )
                return
            
            # Angle & Cusp Condition
            # ----------------------
            k   = math.atan2(y3 - y2, x3 - x2)
            da1 = math.fabs(k - math.atan2(y2 - y1, x2 - x1))
            da2 = math.fabs(math.atan2(y4 - y3, x4 - x3) - k)
            if da1 >= math.pi:
                da1 = 2*math.pi - da1
            if da2 >= math.pi:
                da2 = 2*math.pi - da2

            if da1 + da2 < m_angle_tolerance:
                # Finally we can stop the recursion
                # ---------------------------------
                points.append( (x23, y23) )
                return
            
            if m_cusp_limit != 0.0:
                if da1 > m_cusp_limit:
                    points.append( (x2, y2) )
                    return
                
                if da2 > m_cusp_limit:
                    points.append( (x3, y3) )
                    return
    
    # Continue subdivision
    # --------------------
    curve4_recursive_bezier( points, x1, y1, x12, y12, x123, y123, x1234, y1234, level + 1 )
    curve4_recursive_bezier( points, x1234, y1234, x234, y234, x34, y34, x4, y4, level + 1 )
Exemple #60
0
def ecliptic_to_angle(ecliptic):
    return math.atan2(ecliptic[1], ecliptic[0])