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
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
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 "))
"""
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
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."
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))
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)
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
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
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
def speed(self, value):
self.vx = math.cos(rads(self._angle)) * value
self.vy = math.sin(rads(self._angle)) * value
