Exemplo n.º 1
0
def error(geoprint, radians=False):
    """Returns the error of a given geoprint.

    If radians is true, return the error in radians, otherwise
    degrees.
    """
    e = 90.0 * pow(2, neg(len(geoprint) - 1))
    if radians:
        return rads(e)
    return e
Exemplo n.º 2
0
def decode(geoprint, radians=False, box=False):
    """Decode a geoprint, returning the latitude and longitude.  These
    coordinates should approximate the input coordinates within a
    degree of error returned by 'error()'

    >>> c = (7.0625, -95.677068)
    >>> h = encode(*c)
    >>> c2 = decode(h)
    >>> e = error(h)
    >>> abs(c[0] - c2[0]) <= e
    True
    >>> abs(c[1] - c2[1]) <= e
    True

    If radians is True, results are in radians instead of degrees.

    >>> c2 = decode(h, radians=True)
    >>> e = error(h, radians=True)
    >>> abs(rads(c[0]) - c2[0]) <= e
    True
    >>> abs(rads(c[1]) - c2[1]) <= e
    True
    """
    lati = interval(-90.0, 90.0)
    first = geoprint[0]
    if first == 'w':
        loni = interval(-180.0, 0.0)
    elif first == 'e':
        loni = interval(0.0, 180.0)

    geoprint = geoprint[1:]

    for c in geoprint:
        cd = decodemap[c]
        if cd & 2:
            loni = interval((loni.min + loni.max) / 2, loni.max)
        else:
            loni = interval(loni.min, (loni.min + loni.max) / 2)
        if cd & 1:
            lati = interval((lati.min + lati.max) / 2, lati.max)
        else:
            lati = interval(lati.min, (lati.min + lati.max) / 2)
    lat = (lati.min + lati.max) / 2
    lon = (loni.min + loni.max) / 2
    if radians:
        lati = interval(rads(lati.min), rads(lati.max))
        loni = interval(rads(loni.min), rads(loni.max))
        lat, lon = rads(lat), rads(lon)
    if box:
        return (geoprint, (lati[0], loni[0]), (lati[1], loni[1]))
    return lat, lon
Exemplo n.º 3
0
def main():
    parser = ArgumentParser()
    parser.add_argument(
        "-p",
        "--p2d",
        help=
        "Add a 2D pose in meters and degrees as '[x,y,theta]'. Example: '[4.5,-2.0,45]'",
        type=str,
        action="append")
    parser.add_argument(
        "-f",
        "--tf",
        help="The angle theta of the vehicle's front legs in degrees",
        type=float,
        default=0)
    parser.add_argument(
        "-b",
        "--tb",
        help="The angle theta of the vehicle's back legs in degrees",
        type=float,
        default=0)
    parser.add_argument(
        "-o",
        "--odot",
        help="The peak velocity of the vehicle's wheels in degrees/sec",
        type=float,
        default=180.0)
    parser.add_argument("-x",
                        "--ppx",
                        help="The x position of the sample / pick point",
                        type=float,
                        default=0)
    parser.add_argument("-y",
                        "--ppy",
                        help="The y position of the sample / pick point",
                        type=float,
                        default=0)
    parser.add_argument(
        "-t",
        "--ppt",
        help="The orientation theta of the sample / pick point",
        type=float,
        default=0)
    parser.add_argument("-g",
                        "--gamma",
                        help="The orientation for the pick",
                        type=float,
                        default=180)

    args = parser.parse_args()
    if args.p2d is None:
        print "Error: nothing to do with no waypoints"
        return

    waypoints = []
    for p2d_str in args.p2d:
        p2d_str = p2d_str[p2d_str.index("[") + 1:p2d_str.index("]")]
        p2d_parts = [p.strip() for p in p2d_str.split(",")]
        if len(p2d_parts) != 3:
            print "Error: could not get pose components from {0}".format(
                p2d_str)
            return
        waypoints.append(
            Pose2D(x=float(p2d_parts[0]),
                   y=float(p2d_parts[1]),
                   theta=rads(float(p2d_parts[2]))))

        pick_point = Pose2D(x=args.ppx, y=args.ppx, theta=rads(args.ppt))

    caller = CombinedKinCaller(NODE_NAME, CLI_CALC_COMBINED_KIN)
    response = caller.call(rads(args.tf),
                           rads(args.tb),
                           rads(args.odot),
                           waypoints,
                           pick_point,
                           rads(args.gamma),
                           waiti_s=0.25)

    print "CALCULATION RESPONSE:"
    print "  {0}".format(str(response).replace("\n", "\n  "))
Exemplo n.º 4
0
"""
import math
from math import tan, sin, cos, pi
cot = lambda x: 1 / tan(x)

from math import degrees as degs
from math import radians as rads

import numpy as np
from scipy.optimize import fsolve

# ---------------------------------------------------------------
# Calibration parameters for imaging model
# ---------------------------------------------------------------

aph0 = rads(70)  # degree between camera x-axis and second knuckle
theta4 = rads(120)  # degree of the fingertip of thumb

d0 = 19.4  # length from point A0 to A1 (mm)
d1 = 25.0  # length of first knuckle of thumb (mm)
d2 = 35.0  # length of second knuckle of thumb (mm)
e = 7.5  # half length of tangent plane (mm)
l = 28.7  # distance to point A0

a0_x, a0_y = -l, 0
pt_a0 = [a0_x, a0_y]  # coord of point A0
pt_a1 = [a0_x + d0 * cos(aph0), a0_y + d0 * sin(aph0)]  # coord of point A1
pt_a2 = [a0_x - (d2 - d0) * cos(aph0),
         a0_y - (d2 - d0) * sin(aph0)]  # coord of point A2

av = rads(39.2)  # vertical field of view of camera
Exemplo n.º 5
0
def main():
    parser = ArgumentParser(description=APPLICATION_HELP)
    parser.add_argument(
        "--hz",
        help="the number of calls to the service to make every second",
        type=int,
        default=-1)
    parser.add_argument(
        "--jr",
        help=
        """Usage: '--jr VehicleJointName JointPosDegrees (the name of the joint on the vehicle and the decimal position
 (in degrees) to set it to, can be repeated as many times as desired / as many joints are available to be set)""",
        action="append",
        nargs="*")
    parser.add_argument(
        "--ja",
        help=
        """Usage: '--ja ArmJointName JointPosDegrees (the name of the joint on the arm and the decimal position
 (in degrees) to set it to, can be repeated as many times as desired / as many joints are available to be set)""",
        action="append",
        nargs="*")
    parser.add_argument(
        "--mx",
        help=
        """The x position to set the model to. Options: (1) a decimal position relative to the world frame, (2) \"init\"
 (without the quotes) to set it back to the initial position, (3) \"current\" (without the quotes) to not change it (this is
 the default value)""",
        default="current")
    parser.add_argument(
        "--my",
        help=
        "The y position to set the model to. The options for --mx apply here too.",
        default="current")
    parser.add_argument(
        "--mz",
        help=
        "The z position to set the model to. The options for --mx apply here too.",
        default="current")
    parser.add_argument(
        "--maa",
        help=
        """The axis-angle rotation to set the model to. Options: (1) \"th,x,y,z\" (without the quotes, separated by commas,
 no spaces), where th is the rotation about the axis with components x, y, and z, (2) \"init\" (without the quotes) to set it
 back to the initial position, (3) \"current\" (without the quotes) to not change it (this is the default value)""",
        default="current")

    args = parser.parse_args()
    vehicle_joint_names = []
    vehicle_joint_positions = []
    arm_joint_names = []
    arm_joint_positions = []
    if args.jr is not None:
        for j in args.jr:
            if len(j) != 2:
                print "Each value for --jr must be a joint name and a position (in degrees) separated by a space. Exiting."
                return
            vehicle_joint_names.append(j[0])
            vehicle_joint_positions.append(rads(float(j[1])))
    if args.ja is not None:
        for j in args.ja:
            if len(j) != 2:
                print "Each value for --ja must be a joint name and a position (in degrees) separated by a space. Exiting."
                return
            arm_joint_names.append(j[0])
            arm_joint_positions.append(rads(float(j[1])))

    wrapper = ROSWrapper(args.hz, vehicle_joint_names, vehicle_joint_positions,
                         arm_joint_names, arm_joint_positions, args.mx,
                         args.my, args.mz, args.maa)

    if not wrapper.get_model_name():
        print "Model name is not set in global rosparam server."
        return

    try:
        while not wrapper.is_shutdown():
            if wrapper.objective_model_state:
                srv_response = wrapper.call_cli_set_model_state()
                if not srv_response.success:
                    print "Exiting, as ROS service call to {0} failed: {1}".format(
                        CLI_SET_MODEL_STATE_TOPIC, srv_response.status_message)
                    break
            else:
                print "WARNING: Objective model state not set. Skipping."
            srv_response = wrapper.call_cli_set_model_configuration()
            if not srv_response.success:
                print "Exiting, as ROS service call to {0} failed: {1}".format(
                    CLI_SET_MODEL_CONFIGURATION_TOPIC,
                    srv_response.status_message)
                break
            if not wrapper.has_call_rate():
                print "Task finished successfully. Exiting."
                break
            else:
                wrapper.call_rate.sleep()
    except rospy.service.ServiceException as e:
        print "Caught a service exception: {0}".format(e)
    except rospy.exceptions.ROSInterruptException:
        print "Exiting."
Exemplo n.º 6
0
def load_image_meta(src, sm):
    # params given in equatorial J2000 coordinates, details:
    # https://pds.nasa.gov/ds-view/pds/viewProfile.jsp
    #                                   ?dsid=RO-C-NAVCAM-2-ESC3-MTP021-V1.0

    with open(src, 'r') as f:
        config_data = f.read()

    config_data = '[meta]\n' + config_data
    config_data = re.sub(r'^/\*', '#', config_data, flags=re.M)
    config_data = re.sub(r'^\^', '', config_data, flags=re.M)
    config_data = re.sub(r'^(\w+):(\w+)', r'\1__\2', config_data, flags=re.M)
    config_data = re.sub(r'^END\s*$', '', config_data, flags=re.M)
    config_data = re.sub(r'^NOTE\s*=\s*"[^"]*"', '', config_data, flags=re.M)
    config_data = re.sub(r'^OBJECT\s*=\s*.*?END_OBJECT\s*=\s*\w+',
                         '',
                         config_data,
                         flags=re.M | re.S)
    config_data = re.sub(r' <(DEGREE|SECOND|KILOMETER)>', '', config_data)

    config = ConfigParser(converters={'tuple': literal_eval})
    config.read_string(config_data)

    image_time = config.get('meta', 'START_TIME')

    # spacecraft orientation, equatorial J2000
    sc_rot_ra = config.getfloat('meta', 'RIGHT_ASCENSION')
    sc_rot_dec = config.getfloat('meta', 'DECLINATION')
    sc_rot_cnca = config.getfloat('meta', 'CELESTIAL_NORTH_CLOCK_ANGLE')
    sc_igrf_q = tools.ypr_to_q(
        rads(sc_rot_dec), rads(sc_rot_ra),
        -rads(sc_rot_cnca))  # same with rosetta lbls also

    # from asteroid to spacecraft, asteroid body fixed coordinates
    # TODO: figure out why FOR SOME REASON distance is given ~30x too close
    ast_sc_dist = config.getfloat('meta', 'TARGET_CENTER_DISTANCE') * 30
    ast_sc_lat = config.getfloat('meta', 'SUB_SPACECRAFT_LATITUDE')
    ast_sc_lon = config.getfloat('meta', 'SUB_SPACECRAFT_LONGITUDE')
    ast_sc_bff_r = tools.spherical2cartesian(rads(ast_sc_lat),
                                             rads(ast_sc_lon), ast_sc_dist)

    ast_axis_img_clk_ang = config.getfloat('meta', 'BODY_POLE_CLOCK_ANGLE')
    ast_axis_img_plane_ang = config.getfloat(
        'meta', 'HAY__BODY_POLE_ASPECT_ANGLE')  # what is the use?

    # from sun to spacecraft, equatorial J2000
    ast_sun_dist = config.getfloat('meta', 'TARGET_HELIOCENTRIC_DISTANCE')
    ast_sun_lat = config.getfloat('meta', 'SUB_SOLAR_LATITUDE')
    ast_sun_lon = config.getfloat('meta', 'SUB_SOLAR_LONGITUDE')
    sun_ast_bff_r = -tools.spherical2cartesian(rads(ast_sun_lat),
                                               rads(ast_sun_lon), ast_sun_dist)
    sun_sc_bff_r = sun_ast_bff_r + ast_sc_bff_r

    ast_axis_sun_ang = config.getfloat('meta', 'HAY__BODY_POLE_SUN_ANGLE')
    a = config.getfloat('meta', 'SUB_SOLAR_AZIMUTH')  # what is this!?

    # TODO: continue here
    ast_axis_scf_q = tools.ypr_to_q(-rads(ast_sc_lat), -rads(ast_sc_lon), 0)
    # TODO: figure out: how to get roll as some ast_axis_img_clk_ang come from ast_sc_lat?
    ast_rot_scf_q = tools.ypr_to_q(0, 0, -rads(ast_axis_img_clk_ang))
    ast_scf_q = ast_axis_scf_q  #* ast_rot_scf_q

    dec = 90 - ast_sc_lat
    ra = -ast_sc_lon
    if dec > 90:
        dec = 90 + ast_sc_lat
        ra = tools.wrap_degs(ra + 180)

    print('ra: %f, dec: %f, zlra: %f' % (ra, dec, ast_axis_img_clk_ang))

    ast_igrf_q = ast_scf_q * sc_igrf_q
    sun_ast_igrf_r = tools.q_times_v(ast_igrf_q, sun_ast_bff_r)
    ast_sc_igrf_r = tools.q_times_v(ast_igrf_q, ast_sc_bff_r)
    sun_sc_igrf_r = tools.q_times_v(ast_igrf_q, sun_sc_bff_r)

    z_axis = np.array([0, 0, 1])
    x_axis = np.array([1, 0, 0])
    ast_axis_u = tools.q_times_v(ast_igrf_q, z_axis)
    ast_zlon_u = tools.q_times_v(ast_igrf_q, x_axis)
    ast_axis_dec, ast_axis_ra, _ = tools.cartesian2spherical(*ast_axis_u)
    ast_zlon_proj = tools.vector_rejection(ast_zlon_u, z_axis)
    ast_zlon_ra = tools.angle_between_v(ast_zlon_proj, x_axis)
    ast_zlon_ra *= 1 if np.cross(x_axis, ast_zlon_proj).dot(z_axis) > 0 else -1

    # frame where ast zero lat and lon point towards the sun?
    # ast_axis_ra = -ast_sun_lon
    # ast_axis_dec = 90 - ast_sun_lat
    # ast_axis_zero_lon_ra = 0

    arr2str = lambda arr: '[%s]' % ', '.join(['%f' % v for v in arr])

    print('sun_ast_bff_r: %s' % arr2str(sun_ast_bff_r * 1e3))
    print('sun_sc_bff_r: %s' % arr2str(sun_sc_bff_r * 1e3))
    print('ast_sc_bff_r: %s' % arr2str(ast_sc_bff_r * 1e3))
    # TODO: even the light is wrong, should be right based on the sun_ast and sun_sc vectors!!

    print('sun_ast_igrf_r: %s' % arr2str(sun_ast_igrf_r * 1e3))
    print('sun_sc_igrf_r: %s' % arr2str(sun_sc_igrf_r * 1e3))
    print('ast_sc_igrf_r: %s' % arr2str(ast_sc_igrf_r * 1e3))
    print('ast_axis_ra: %f' % degs(ast_axis_ra))
    print('ast_axis_dec: %f' % degs(ast_axis_dec))
    print('ast_zlon_ra: %f' % degs(ast_zlon_ra))

    aa = quaternion.as_rotation_vector(sc_igrf_q)
    angle = np.linalg.norm(aa)
    sc_angleaxis = [angle] + list(aa / angle)
    print('sc_angleaxis [rad]: %s' % arr2str(sc_angleaxis))
Exemplo n.º 7
0
    def build_goal_model_state(self):
        self.objective_model_state = None

        msg = None
        try:
            msg = rospy.wait_for_message(SUB_MODEL_STATES_TOPIC,
                                         ModelStates,
                                         timeout=2)
        except rospy.exceptions.ROSException:
            print "WARNING: Did not hear model state on {0}.".format(
                SUB_MODEL_STATES_TOPIC)
            return

        model_name = self.get_model_name()
        for i in range(len(msg.name)):
            if msg.name[i] == model_name:
                self.objective_model_state = ModelState(model_name=model_name,
                                                        pose=msg.pose[i],
                                                        twist=msg.twist[i],
                                                        reference_frame="map")
                break

        if self.objective_model_state:
            cx = self.objective_model_state.pose.position.x
            cy = self.objective_model_state.pose.position.y
            cz = self.objective_model_state.pose.position.z
            cR, cP, cY = tft.euler_from_quaternion([
                self.objective_model_state.pose.orientation.x,
                self.objective_model_state.pose.orientation.y,
                self.objective_model_state.pose.orientation.z,
                self.objective_model_state.pose.orientation.w
            ])
            ix = None
            iy = None
            iz = None
            iR = None
            iP = None
            iY = None
            if self.init_model_pose:
                try:
                    vals = {}
                    for val in self.init_model_pose.split("-"):
                        val = val.strip()
                        if len(val) > 0:
                            space = val.index(" ")
                            vals[val[0:space].strip()] = float(val[space +
                                                                   1:].strip())
                    ix = vals["x"]
                    iy = vals["y"]
                    iz = vals["z"]
                    iR = rads(vals["R"])
                    iP = rads(vals["P"])
                    iY = rads(vals["Y"])
                except:
                    print "Error parsing init pose \"{0}\", assuming no initial pose provided.".format(
                        self.init_model_pose)

            if self.modelX == "current":
                self.objective_model_state.pose.position.x = cx
            elif self.modelX == "init":
                if ix is None:
                    print "Could not get initial X position from global rosparam server, defaulting to current."
                    self.objective_model_state.pose.position.x = cx
                else:
                    self.objective_model_state.pose.position.x = ix
            else:
                try:
                    self.objective_model_state.pose.position.x = float(
                        self.modelX)
                except ValueError as e:
                    print "Could not set initial X position from {0}, defaulting to current.".format(
                        self.modelX)
                    self.objective_model_state.pose.position.x = cx

            if self.modelY == "current":
                self.objective_model_state.pose.position.y = cy
            elif self.modelY == "init":
                if iy is None:
                    print "Could not get initial Y position from global rosparam server, defaulting to current."
                    self.objective_model_state.pose.position.y = cy
                else:
                    self.objective_model_state.pose.position.y = iy
            else:
                try:
                    self.objective_model_state.pose.position.y = float(
                        self.modelY)
                except ValueError as e:
                    print "Could not set inital Y position from {0}, defaulting to current.".format(
                        self.modelY)
                    self.objective_model_state.pose.position.y = cy

            if self.modelZ == "current":
                self.objective_model_state.pose.position.z = cz
            elif self.modelZ == "init":
                if iz is None:
                    print "Could not get initial Z position from global rosparam server, defaulting to current."
                    self.objective_model_state.pose.position.z = cz
                else:
                    self.objective_model_state.pose.position.z = iz
            else:
                try:
                    self.objective_model_state.pose.position.z = float(
                        self.modelZ)
                except ValueError as e:
                    print "Could not set initial Z position from {0}, defaulting to current.".format(
                        self.modelZ)
                    self.objective_model_state.pose.position.z = cz

            oR = None
            oP = None
            oY = None
            if self.modelAxisAngle == "current":
                oR = cR
                oP = cP
                oY = cY
            elif self.modelAxisAngle == "init":
                if iz is None:
                    print "Could not get inital rotation from global rosparam server, defaulting to current."
                    oR = cR
                    oP = cP
                    oY = cY
                else:
                    oR = iR
                    oP = iP
                    oY = iY
            else:
                try:
                    aT, aX, aY, aZ = [
                        float(o) for o in self.modelAxisAngle.split(",")
                    ]
                    aT = rads(aT)
                    oR, oP, oY = tft.euler_from_matrix(
                        tft.rotation_matrix(aT, [aX, aY, aZ]))
                except (ValueError, IndexError), e:
                    print "Could not set inital rotation from {0}, defaulting to current.".format(
                        self.modelAxisAngle)
                    oR = cR
                    oP = cP
                    oY = cY

            self.objective_model_state.pose.orientation.x, \
            self.objective_model_state.pose.orientation.y, \
            self.objective_model_state.pose.orientation.z, \
            self.objective_model_state.pose.orientation.w = tft.quaternion_from_euler(oR, oP, oY)
Exemplo n.º 8
0
def draw(w, c):
    # Draw the backgrounds

    c.set_line_width(50)
    c.set_source_rgba(*desktop.rgb(bg, bga))


    c.arc(0, w.height, 150, rads(270), rads(0))
    c.stroke()

    c.arc(320, w.height, 100, rads(180), rads(0))
    c.stroke()

    c.arc(570, w.height, 80, rads(180), rads(0))
    c.stroke()

    c.arc(w.width-570, w.height, 80, rads(180), rads(0))
    c.stroke()

    c.arc(w.width-320, w.height, 100, rads(180), rads(0))
    c.stroke()

    c.arc(w.width, w.height, 150, rads(0), rads(270))
    c.stroke()

    # End of background section


    c.set_line_width(25)
    c.set_source_rgba(*desktop.rgb(fg[0],fga))


    # Cpu
    cpu = desktop.cpu()

    c.arc(0, w.height, 150, rads(270), rads(270+(cpu/100*90)))
    c.stroke()


    # Net up

    c.set_source_rgba(*desktop.rgb(fg[1],fga))
    desktop.graph_arc('net_up', c, 320, w.height, 100, 50, 180, 0, desktop.net_up, border=False) 
    
    # Battery

    c.set_source_rgba(*desktop.rgb(fg[2], fga))
    battery = desktop.battery_current_full_max('BAT1')
    m = 180+(battery['full']) / battery['max'] * 180
    
    c.set_line_width(35)
    c.arc(570, w.height, 80, rads(m), rads(0))
    c.stroke()
    c.set_line_width(25)

    percent = 180+battery['current']/battery['full']*(m-180)
    c.arc(570, w.height, 80, rads(180), rads(percent))
    c.stroke()

    # Temp

    c.set_source_rgba(*desktop.rgb(fg[3], fga))
    temp = desktop.temp()

    c.arc_negative(w.width-570, w.height, 80, rads(360), rads(360-(temp/100*180)))
    c.stroke()

    # Net down

    c.set_source_rgba(*desktop.rgb(fg[4], fga))
    desktop.graph_arc('net_down', c, w.width-320, w.height, 100, 50, 180, 360, desktop.net_down, negative=True, border=False) 
    
    # Ram

    c.set_source_rgba(*desktop.rgb(fg[5], fga))
    ram = desktop.ram()

    c.arc_negative(w.width, w.height, 150, rads(270), rads(270-(ram/100*90)))
    c.stroke()


    # Text

    c.select_font_face('Terminal Bold', 0, 1)
    c.set_font_size(25)


    dtime = datetime.datetime.now()
    
    # Day name

    c.set_source_rgba(*desktop.rgb(tc[0], fga))
    c.move_to(15, w.height-50)
    c.show_text(dtime.strftime('%a'))

    # Day of month

    c.set_font_size(20)
    c.set_source_rgba(*desktop.rgb(tc[1], fga))
    c.move_to(15, w.height-30)
    c.show_text(str(dtime.day))

    # Day suffix

    c.set_font_size(10)
    c.move_to(45, w.height-30)
    c.show_text(str(date_suffix(dtime.day)))


    # AM / PM

    c.set_font_size(14)
    c.move_to(w.width-36, w.height-30)
    c.show_text(dtime.strftime('%p'))

    # Time

    c.set_source_rgba(*desktop.rgb(tc[0], fga))
    c.set_font_size(25)
    c.move_to(w.width-90, w.height-50)
    c.show_text(dtime.strftime('%I:%M'))

    return None
Exemplo n.º 9
0
 def angle(self, value):
     self._angle = value
     self.collider.angle = self.angle
     self.vx = math.cos(rads(self._angle)) * self.speed
     self.vy = math.sin(rads(self._angle)) * self.speed
Exemplo n.º 10
0
 def speed(self, value):
     self.angle = degs(math.atan2(self.vy,
                                  self.vx)) + (180 if self.vx < 0 else 0)
     self.vx = math.cos(rads(self._angle)) * value
     self.vy = math.sin(rads(self._angle)) * value
Exemplo n.º 11
0
 def speed(self, value):
     self.vx = math.cos(rads(self._angle)) * value
     self.vy = math.sin(rads(self._angle)) * value