コード例 #1
0
def arclen_ori(A, B, r, ori):
    if ori > 0:
        ret = normalize(B - A, 0, 360) * (2 * np.pi * r / 360.0)
    elif ori < 0:
        ret = normalize(B - A, -360, 0) * (2 * np.pi * r / 360.0)
    else:
        raise Exception("ori must be -1 or 1")
    return abs(ret)
コード例 #2
0
def arclen_ori(A, B, r, ori):
    if ori > 0:
        ret = normalize(B-A, 0, 360)*(2*np.pi*r/360.0)
    elif ori < 0:
        ret = normalize(B-A, -360, 0)*(2*np.pi*r/360.0)
    else:
        raise Exception("ori must be -1 or 1")
    return abs(ret)
コード例 #3
0
def circle_right_direction(center, radius, sense, point, angle):
    """
    :param center: Circle center, np array
    :param sense: Clockwise=-1, Counterclockwise=1
    :param point: Point in circunference, np array
    :param angle: Degrees, direction starting from 'point'
    :return: Bool
    """
    A = angle_to(center, point)
    right = move_by_radius(center, radius, A + sense)
    wrong = move_by_radius(center, radius, A - sense)
    right_dist = abs(normalize(angle - angle_to(point, right)))
    wrong_dist = abs(normalize(angle - angle_to(point, wrong)))
    return right_dist < wrong_dist
コード例 #4
0
def circle_right_direction(center, radius, sense, point, angle):
    """
    :param center: Circle center, np array
    :param sense: Clockwise=-1, Counterclockwise=1
    :param point: Point in circunference, np array
    :param angle: Degrees, direction starting from 'point'
    :return: Bool
    """
    A = angle_to(center, point)
    right = move_by_radius(center, radius, A+sense)
    wrong = move_by_radius(center, radius, A-sense)
    right_dist = abs(normalize(angle-angle_to(point, right)))
    wrong_dist = abs(normalize(angle-angle_to(point, wrong)))
    return right_dist < wrong_dist
コード例 #5
0
def arctan2(co, ca):
    """
    :param co: Number, cateto opuesto
    :param ca: Number, cateto adyacente
    :return: Degrees
    """
    ang = np.arctan2(co, ca)
    return normalize(np.degrees(ang))
コード例 #6
0
def arctan2(co, ca):
    """
    :param co: Number, cateto opuesto
    :param ca: Number, cateto adyacente
    :return: Degrees
    """
    ang = np.arctan2(co, ca)
    return normalize(np.degrees(ang))
コード例 #7
0
    def act(self):

        while not rospy.is_shutdown():
            self._rate.sleep()

            if self._human_joints is None:
                rospy.logwarn("No human joints received, waiting...")
                continue

            #left arm
            pr2_joints_left = [0] * 7

            pr2_joints_left[0] = normalize(self._human_joints[2], -pi, pi)
            pr2_joints_left[1] = normalize(-(self._human_joints[4] - pi/2), -pi, pi)
            pr2_joints_left[2] = normalize(-(self._human_joints[6] - pi/2), -pi, pi)
            pr2_joints_left[3] = normalize(self._human_joints[0], -pi, pi)

            #right_arm
            pr2_joints_right = [0] * 7
            pr2_joints_right[0] = normalize(self._human_joints[3], -pi, pi)
            pr2_joints_right[1] = normalize(-(self._human_joints[5] - pi/2), -pi, pi)
            pr2_joints_right[2] = normalize((self._human_joints[7] - pi/2), -pi, pi)
            pr2_joints_right[3] = normalize(self._human_joints[1], -pi, pi)

            #get left fk pose
            l_execute = True
            l_pose = self._ik.get_left_arm_fk_pose(pr2_joints_left,
                                                   self._pr2box.frame_id)[0]
            rospy.loginfo("testing pose: %s", l_pose)
            if (l_pose[1] < 0.2):
                rospy.logwarn("Left arm too close to center, not executing"
                              )
                l_execute = False
            if self._pr2box.point_inside(l_pose):
                rospy.logwarn("Warning, desired pose: %s is dangerous for left arm,"
                              "not executing!", l_pose)
                l_execute = False
            if (l_execute):
                self._pr2.set_arm_state(pr2_joints_left, "left", wait=False)

            #get right ik pose
            r_pose = self._ik.get_right_arm_fk_pose(pr2_joints_right,
                                                    self._pr2box.frame_id)[0]
            r_execute = True
            if (r_pose[1] > -0.2):
                rospy.logwarn("Right arm too close to center, not executing"
                              )
                r_execute = False
            if (self._pr2box.point_inside(r_pose)):
                rospy.logwarn("Warning, desired pose: %s is dangerous for right arm,"
                              "not executing!", r_pose)
                r_execute = False
            if (r_execute):
                self._pr2.set_arm_state(pr2_joints_right, "right", wait=False)
コード例 #8
0
ファイル: test_angles.py プロジェクト: phn/angles
def test_normalize_b_false_outside_left_limit():
    """Values to the left of the left limit must be handled properly."""
    L, U = 0, 360
    assert normalize(L-1, L, U) == U - 1
    assert normalize(-360, L, U) == 0
    assert normalize(-361, L, U) == 359
    assert normalize(-721, L, U) == 359

    L, U = -90, 90
    assert normalize(L-1, L, U) == U - 1
    assert normalize(-180, L, U) == 0
    assert normalize(-181, L, U) == -1
    assert normalize(-361, L, U) == -1
コード例 #9
0
ファイル: test_angles.py プロジェクト: lhy26/angles
def test_normalize_b_false_outside_left_limit():
    """Values to the left of the left limit must be handled properly."""
    L, U = 0, 360
    assert normalize(L - 1, L, U) == U - 1
    assert normalize(-360, L, U) == 0
    assert normalize(-361, L, U) == 359
    assert normalize(-721, L, U) == 359

    L, U = -90, 90
    assert normalize(L - 1, L, U) == U - 1
    assert normalize(-180, L, U) == 0
    assert normalize(-181, L, U) == -1
    assert normalize(-361, L, U) == -1
コード例 #10
0
ファイル: test_angles.py プロジェクト: lhy26/angles
def test_normalize_proper_input_range():
    """Normalize must be called with proper range limits must be"""
    with pytest.raises(ValueError):
        normalize(10, 0, 360, b=True)

    with pytest.raises(ValueError):
        normalize(10, -89, 90, b=True)

    with pytest.raises(ValueError):
        normalize(10, 1, 90)

    # both shouldn't raise any exception
    assert normalize(10, 0, 360) == 10
    assert normalize(10, -90, 90) == 10
コード例 #11
0
ファイル: test_angles.py プロジェクト: phn/angles
def test_normalize_proper_input_range():
    """Normalize must be called with proper range limits must be"""
    with pytest.raises(ValueError):
        normalize(10, 0, 360, b=True)

    with pytest.raises(ValueError):
        normalize(10, -89, 90, b=True)

    with pytest.raises(ValueError):
        normalize(10, 1, 90)

    # both shouldn't raise any exception
    assert normalize(10, 0, 360) == 10
    assert normalize(10, -90, 90) == 10
コード例 #12
0
ファイル: carode.py プロジェクト: lorenzoriano/sbpl
    def set_control(self, vel, steer):
        norm_steer = angles.normalize(steer, -2*pi, 2*pi)

        self.control_vel = vel
        if self.control_vel > self.max_vel:
            self.control_vel = self.max_vel
        if self.control_vel > self.max_vel:
            self.control_vel = self.min_vel

        self.control_angle = norm_steer
        if self.control_angle > self.max_steer:
            self.control_angle = self.max_steer
        if self.control_angle > self.max_steer:
            self.control_angle = self.min_steer
コード例 #13
0
ファイル: utils.py プロジェクト: Kelvinrr/mgs_tes_data
def mgs84_norm_lat(angle):
    """
    Noralize latitude to the MGS84 datum. For storing data where only MGS84 is supported
    (e.g. MongoDB)

    Parameters
    ----------
    angle : double
            The angle to normalize

    Returns
    -------
    : double
      The new Angle normalized between [-90, 90]
    """
    return normalize(angle, lower=-90, upper=90)
コード例 #14
0
ファイル: test_angles.py プロジェクト: phn/angles
def test_normalize_b_true_sanity_check():
    """Values at extremes and in the middle must be handled."""
    L, U = -180, 180
    assert normalize(10, L, U, b=True) == 10
    assert normalize(L, L, U, b=True) == L
    assert normalize(U, L, U, b=True) == U

    L, U = -90, 90
    assert normalize(10, L, U, b=True) == 10
    assert normalize(L, L, U, b=True) == L
    assert normalize(U, L, U, b=True) == U
コード例 #15
0
ファイル: test_angles.py プロジェクト: phn/angles
def test_normalize_b_false_outside_right_limit():
    """Values to the right of the right limit must be handled properly."""
    L, U = 0, 360
    assert normalize(U+1, L, U) == L + 1
    assert normalize(U*2+1, L, U) == L + 1
    assert normalize(U*100+(U-L)/2.0, L, U) == L + (U-L)/2.0

    L, U = -90, 90
    assert normalize(U+1, L, U) == L + 1
    assert normalize(181, L, U) == 1
    assert normalize(361, L, U) == 1
コード例 #16
0
ファイル: test_angles.py プロジェクト: lhy26/angles
def test_normalize_b_true_outside_left_limit():
    """Values to the left of the left limit must be handled properly."""
    L, U = -180, 180
    assert normalize(-181, L, U, b=True) == -179
    assert normalize(-361, L, U, b=True) == 1
    assert normalize(-721, L, U, b=True) == -1

    L, U = -90, 90
    assert normalize(-91, L, U, b=True) == -89
    assert normalize(-181, L, U, b=True) == 1
    assert normalize(-361, L, U, b=True) == -1
コード例 #17
0
ファイル: test_angles.py プロジェクト: phn/angles
def test_normalize_b_true_outside_right_limit():
    """Values to the right of the right limit must be handled properly."""
    L, U = -180, 180
    assert normalize(U+1, L, U, b=True) == U - 1
    assert normalize(181, L, U, b=True) == U - 1
    assert normalize(361, L, U, b=True) == -1

    L, U = -90, 90
    assert normalize(U+1, L, U, b=True) == U - 1
    assert normalize(181, L, U, b=True) == -1
    assert normalize(361, L, U, b=True) == 1
コード例 #18
0
ファイル: test_angles.py プロジェクト: lhy26/angles
def test_normalize_b_false_outside_right_limit():
    """Values to the right of the right limit must be handled properly."""
    L, U = 0, 360
    assert normalize(U + 1, L, U) == L + 1
    assert normalize(U * 2 + 1, L, U) == L + 1
    assert normalize(U * 100 + (U - L) / 2.0, L, U) == L + (U - L) / 2.0

    L, U = -90, 90
    assert normalize(U + 1, L, U) == L + 1
    assert normalize(181, L, U) == 1
    assert normalize(361, L, U) == 1
コード例 #19
0
ファイル: test_angles.py プロジェクト: lhy26/angles
def test_normalize_b_true_sanity_check():
    """Values at extremes and in the middle must be handled."""
    L, U = -180, 180
    assert normalize(10, L, U, b=True) == 10
    assert normalize(L, L, U, b=True) == L
    assert normalize(U, L, U, b=True) == U

    L, U = -90, 90
    assert normalize(10, L, U, b=True) == 10
    assert normalize(L, L, U, b=True) == L
    assert normalize(U, L, U, b=True) == U
コード例 #20
0
ファイル: test_angles.py プロジェクト: lhy26/angles
def test_normalize_b_false_sanity_check():
    """Values at extremes and in the middle must be handled."""
    L, U = 0, 360
    assert normalize(10, L, U) == 10
    assert normalize(L, L, U) == L
    assert normalize(U, L, U) == L

    L, U = -90, 90
    assert normalize(10, L, U) == 10
    assert normalize(L, L, U) == L
    assert normalize(U, L, U) == L
コード例 #21
0
ファイル: test_angles.py プロジェクト: lhy26/angles
def test_normalize_b_true_outside_right_limit():
    """Values to the right of the right limit must be handled properly."""
    L, U = -180, 180
    assert normalize(U + 1, L, U, b=True) == U - 1
    assert normalize(181, L, U, b=True) == U - 1
    assert normalize(361, L, U, b=True) == -1

    L, U = -90, 90
    assert normalize(U + 1, L, U, b=True) == U - 1
    assert normalize(181, L, U, b=True) == -1
    assert normalize(361, L, U, b=True) == 1
コード例 #22
0
ファイル: test_angles.py プロジェクト: phn/angles
def test_normalize_b_true_outside_left_limit():
    """Values to the left of the left limit must be handled properly."""
    L, U = -180, 180
    assert normalize(-181, L, U, b=True) == -179
    assert normalize(-361, L, U, b=True) == 1
    assert normalize(-721, L, U, b=True) == -1

    L, U = -90, 90
    assert normalize(-91, L, U, b=True) == -89
    assert normalize(-181, L, U, b=True) == 1
    assert normalize(-361, L, U, b=True) == -1
コード例 #23
0
ファイル: test_angles.py プロジェクト: phn/angles
def test_normalize_b_false_sanity_check():
    """Values at extremes and in the middle must be handled."""
    L, U = 0, 360
    assert normalize(10, L, U) == 10
    assert normalize(L, L, U) == L
    assert normalize(U, L, U) == L

    L, U = -90, 90
    assert normalize(10, L, U) == 10
    assert normalize(L, L, U) == L
    assert normalize(U, L, U) == L
コード例 #24
0
 def process(self):
     self.output.twist.linear.x = self.pid.x.update(
         self.input_.pose.x, self.state.position[0], self.state.twist.linear[0], self.dt
     )
     self.output.twist.linear.y = self.pid.y.update(
         self.input_.pose.y, self.state.position[1], self.state.twist.linear[1], self.dt
     )
     self.output.twist.linear.z = self.pid.z.update(
         self.input_.pose.z, self.state.position[2], self.state.twist.linear[2], self.dt
     )
     yaw_command = angles.normalize(
         self.input_.pose.yaw, self.state.euler[2] - math.pi, self.state.euler[2] + math.pi
     )
     self.output.twist.angular.z = self.pid.yaw.update(
         yaw_command, self.state.euler[2], self.state.twist.angular[2], self.dt
     )
     if self.verbose:
         utils.pv("self.state.euler[2]", "yaw_command")
コード例 #25
0
ファイル: controller.py プロジェクト: adityag6994/quad_arrt
 def process(self):
     self.output.twist.linear.x = self.pid.x.update(
         self.input_.pose.x, self.state.position[0],
         self.state.twist.linear[0], self.dt)
     self.output.twist.linear.y = self.pid.y.update(
         self.input_.pose.y, self.state.position[1],
         self.state.twist.linear[1], self.dt)
     self.output.twist.linear.z = self.pid.z.update(
         self.input_.pose.z, self.state.position[2],
         self.state.twist.linear[2], self.dt)
     yaw_command = angles.normalize(self.input_.pose.yaw,
                                    self.state.euler[2] - math.pi,
                                    self.state.euler[2] + math.pi)
     self.output.twist.angular.z = self.pid.yaw.update(
         yaw_command, self.state.euler[2], self.state.twist.angular[2],
         self.dt)
     if self.verbose:
         utils.pv('self.state.euler[2]', 'yaw_command')
コード例 #26
0
    def _step_cog(self):
        """Calculate the course between two position points"""

        def course_utm(df):
            df.reset_index(inplace=True)
            df["Step_Azimuth"] = azimuth_utm(
                df["geometry"].shift(), df.loc[1:, "geometry"]
            )
            return df.set_index("index")

        # Calculate and normalize course between successive points
        self.gdf = self.grouped_trip.apply(course_utm)
        self.gdf["Step_Azimuth"].fillna(method="bfill", inplace=True)
        self.gdf["Step_Azimuth"] = round(
            self.gdf["Step_Azimuth"].apply(lambda x: angles.normalize(x, 0, 360))
        )

        # Caclulate error
        self.gdf["Error_COG"] = 180 - abs(
            abs(self.gdf["COG"] - self.gdf["Step_Azimuth"]) - 180
        )
        self.gdf["Error_Heading"] = 180 - abs(
            abs(self.gdf["Heading"] - self.gdf["Step_Azimuth"]) - 180
        )
コード例 #27
0
def angle_to(a, b):
    v = vector_to(a, b)
    return normalize(arctan2(v[1], v[0]))
コード例 #28
0
def min_angular_distance(A, B):
    return abs(normalize(A - B))
コード例 #29
0
def arclen(A, B, r):
    return normalize(B - A, 0, 360) * (2 * np.pi * r / 360.0)
コード例 #30
0
ファイル: point.py プロジェクト: easyNav/easyNav-pi-nav
	def normalizeRad(self, value):
		deg = angles.r2d(float(value))
		return angles.d2r(angles.normalize(deg, -180, 180))
コード例 #31
0
def arccos(x):
    return normalize(np.degrees(np.arccos(x)))
コード例 #32
0
 def _normalize_angles(self):
     """Normalize COG to an angle between [0, 360)."""
     self.df["COG"] = self.df["COG"].apply(lambda x: angles.normalize(x, 0, 360))
     self.df["Heading"] = self.df["Heading"].apply(
         lambda x: angles.normalize(x, 0, 360)
     )
コード例 #33
0
ファイル: local_flocking.py プロジェクト: rdarnold/usma_swarm
    def step_autonomy(self, t, dt):
        super(LocalFlocking, self).step_autonomy(t, dt)

        # If outside of battlebox, head towards own base!"
        if not self._battlebox.contains(self._own_pose.pose.pose.position.lat,
                                        self._own_pose.pose.pose.position.lon,
                                        self._own_pose.pose.pose.position.alt):
            self._wp = np.array(
                [self._own_base[0], self._own_base[1], self._last_ap_wp[2]])
            return True

        # Compute distance to each blue uav, tuple: (id, distance)
        # Don't include ourself
        blue_dists = [
            (id, np.linalg.norm(self.state.pos[:2] - state.pos[:2]))
            for id, state in self.local_blues.iteritems() if id != self._id
            and self.state.in_field_of_view(state.pos, math.pi, math.pi)
        ]

        # Experimental: only use neighbors in front of own uav
        #

        # Sort blue distances
        blue_dists.sort(key=lambda x: x[1])

        # return nearest neighbors
        neighbors = blue_dists[:self._max_neighbors]

        # Separation: steer to avoid crowding local flockmates
        sep_vec_sum = np.array([0, 0])

        # Alignment: steer towards the average heading of local flockmates
        yaw_sum = 0

        # Cohesion: steer to move toward the average position (centroid) of
        # local flockmates
        centroid_sum = np.array([0, 0])

        # Set gains for each behavior
        sep_gain = 0.05
        yaw_gain = 1
        coh_gain = 0.05
        tgt_gain = self._target_gain
        box_gain = 10

        # calculate running sum for each neighbor-based behavior
        for id, state in neighbors:
            # Calculate vector pointing from neighbor to own position
            vec_diff = (self.state.pos[:2] - self.local_blues[id].pos[:2])

            # Running sum of vectors pointing from neighbor to own position for
            # separation vector calculation
            sep_vec_sum = sep_vec_sum + vec_diff / np.linalg.norm(vec_diff)

            # Running sum of neighbor headings for heading alignment
            # calculation
            yaw_sum = yaw_sum + self.local_blues[id].yaw()

            # Running sum of neighbor positions for centroid calculation
            centroid_sum = centroid_sum + self.local_blues[id].pos[:2]

        # Initialize vectors to zero
        vec_result = np.array([0, 0])
        sep_vec = np.array([0, 0])
        yaw_vec = np.array([0, 0])
        coh_vec = np.array([0, 0])
        tgt_vec = np.array([0, 0])
        box_vec = np.array([0, 0])

        # Normalize / average neighbor-based behaviors
        if len(neighbors) > 0:
            sep_vec = sep_vec_sum / len(neighbors)  # Normalize separation vec
            avg_yaw = yaw_sum / len(neighbors)  # Average yaw calc
            centroid = centroid_sum / len(neighbors)  # Centroid of neighbors

            # Convert desired centroid to velocity pointing from own position
            # to centroid
            coh_vec = centroid - self.state.pos[:2]

            # Convert average yaw to vector pointing in direction for own uav
            # to achieve average yaw
            yaw_vec = np.array([math.cos(avg_yaw), math.sin(avg_yaw)])

        # Compute distance to each red uav, tuple: (id, distance)
        red_dists = [ (id, np.linalg.norm(self.state.pos[:2]-state.pos[:2])) \
                      for id, state in self.local_reds.iteritems() ]

        # Determine nearest enemy
        try:
            closest_enemy_id = min(red_dists, key=lambda t: t[1])[0]
            tgt_dir = self.local_reds[
                closest_enemy_id].pos[:2] - self.state.pos[:2]
            tgt_vec = tgt_dir / np.linalg.norm(tgt_dir)
        except ValueError:
            # No enemies exist, zero out target vector
            tgt_vec = np.array([0, 0])

        # Get time-to-breach closest battle cube wall.
        wall_time, wall_angle = self.closest_wall_time_angle()

        # If the time-to-breach closest wall is less than 10 seconds, calculate
        # avoidance velocity vector
        if wall_time < self._min_wall_time:
            box_yaw = 0
            if wall_angle > 0:
                # Turn hard left (increase angle) if the wall_angle is greater
                # than 0
                box_yaw = normalize(self.state.yaw() + math.pi - 0.1, -math.pi,
                                    math.pi)
            else:
                # Turn hard right (decrease angle) if the wall_angle is greater
                # than 0
                box_yaw = normalize(self.state.yaw() - math.pi - 0.1, -math.pi,
                                    math.pi)

            # Convert desired yaw to velocity vector
            box_vec = np.array([math.cos(box_yaw), math.sin(box_yaw)])

        # Perform linear combination of each flocking behavior
        vec_result = sep_gain * sep_vec + \
                     yaw_gain * yaw_vec + \
                     coh_gain * coh_vec + \
                     tgt_gain * tgt_vec + \
                     box_gain * box_vec

        # If the vector sum of all flocking behaviors is very small, set the
        # velocity to the current velocity
        if np.linalg.norm(vec_result) < 0.001:
            vec_result = self.state.vel[:2]

        # Convert local desired velocity vector to a GPS waypoint
        self._wp = self.vector_2_waypoint(vec_result, alt_deconflict=True)

        # Determine if any enemy aircraft can be tagged
        for uav_id in self.local_reds:
            if self.hitable(uav_id):
                self._action = ["Fire", uav_id]
                break

        return True
コード例 #34
0
 def _move_on_torsion(self, residue_number, move_pose, get_angle,
                      set_angle):
     new_torsion_position = normalize(
         get_angle(residue_number) + move_pose, -180, 180)
     set_angle(residue_number, new_torsion_position)
コード例 #35
0
ファイル: carode.py プロジェクト: lorenzoriano/sbpl
def norm_angle(x):
    return angles.normalize(x, 0, 2*pi)
コード例 #36
0
def min_angular_distance(A, B):
    return abs(normalize(A-B))
コード例 #37
0
def angle_to(a, b):
    v = vector_to(a, b)
    return normalize(arctan2(v[1], v[0]))
コード例 #38
0
def arccos(x):
    return normalize(np.degrees(np.arccos(x)))
コード例 #39
0
def arclen(A, B, r):
    return normalize(B-A, 0, 360)*(2*np.pi*r/360.0)
def normalizeAngles(angleList, angle_range):
    return np.array([angles.normalize(i, angle_range[0], angle_range[1])
                     for i in angleList])
コード例 #41
0
ファイル: preprocessing.py プロジェクト: vr308/HiggsProject
    def normalize_angles(cls, radian_vector):

        return np.asarray(map(lambda x: angles.d2r(angles.normalize(angles.r2d(x), -180, +180)), radian_vector))