def handle(parsed):
for x in parsed:
print(x)
if x[0] == 'N':
pos.add(Vec2(0, x[1]))
elif x[0] == 'S':
pos.add(Vec2(0, -x[1]))
elif x[0] == 'E':
pos.add(Vec2(x[1], 0))
elif x[0] == 'W':
pos.add(Vec2(-x[1], 0))
elif x[0] == 'F':
pos.add(Vec2(facing.x * x[1], facing.y * x[1]))
elif x[0] == 'R':
facing.rotate(-deg2rad(x[1]))
facing.x = int(facing.x)
facing.y = int(facing.y)
elif x[0] == 'L':
facing.rotate(deg2rad(x[1]))
facing.x = int(facing.x)
facing.y = int(facing.y)
else:
raise Exception("Invalid key")
print(pos)
print(facing)
def handle(parsed):
for x in parsed:
print(x)
if x[0] == 'N':
waypoint.add(Vec2(0, x[1]))
elif x[0] == 'S':
waypoint.add(Vec2(0, -x[1]))
elif x[0] == 'E':
waypoint.add(Vec2(x[1], 0))
elif x[0] == 'W':
waypoint.add(Vec2(-x[1], 0))
elif x[0] == 'F':
pos.add(Vec2(waypoint.x * x[1], waypoint.y * x[1]))
elif x[0] == 'R':
waypoint.rotate(-deg2rad(x[1]))
waypoint.x = round(waypoint.x)
waypoint.y = round(waypoint.y)
elif x[0] == 'L':
waypoint.rotate(deg2rad(x[1]))
waypoint.x = round(waypoint.x)
waypoint.y = round(waypoint.y)
else:
raise Exception("Invalid key")
print(pos)
print(waypoint)
def test_deg2rad():
"""Unit tests for deg2rad()."""
# Note: python's math package includes a radians function that
# converts degrees to radians. This function could be eliminated
# to reduce custom code.
assert deg2rad(57.2958) == 1.0000003575641672
assert deg2rad(1) == 0.017453292519943295
assert deg2rad(-1) == -0.017453292519943295
def turn_zumy(self, angle):
# angle in degrees
# returns a Twist that is published and turns zumy
if self.delay_type is None and not self.is_turning:
print 'delay type none in turn_zumy is none'
self.stop_and_think_time = time.time()
self.delay_type = "stop_and_think"
elif self.delay_type == 'done thinking':
self.delay_type = None
self.is_turning = True
omega = np.array([0, 0, 1]) # rotate about z
translation = np.array([0, 0, 0])
theta = utils.deg2rad(angle)
omega = omega * theta
rbt = utils.create_rbt(omega, theta, translation)
v = utils.find_v(omega, theta, translation)
linear = Vector3(0.003 * SPEED_CONST, v[1] / 3,
SPEED_CONST * v[2] / 3)
angular = Vector3(omega[0] / 3, omega[1] / 3,
SPEED_CONST * omega[2] / 12)
twist = Twist(linear, angular)
print 'STARTED TURNING'
self.turn_start_direction = self.zumy_direction
self.zumy_vel.publish(twist)
def get_P(FOV=40, n=10., f=1000., aspect_ratio=4 / 3.):
# Perspective projection matrix
# assume symmetrical view volume
t = torch.tan(torch.tensor([utils.deg2rad(FOV / 2)])) * n
r = t * aspect_ratio
return torch.tensor([[n / r, 0, 0, 0], [0, n / t, 0, 0],
[0, 0, -(f + n) / (f - n), -2 * f * n / (f - n)],
[0, 0, -1, 0]])
def random_rotation(image, deg=20, seed=None):
rotation_theta = utils.deg2rad(deg)
random_deg = tf.random.uniform(shape=[1],
minval=-rotation_theta,
maxval=rotation_theta,
seed=seed)
image = tfa.image.rotate(image, random_deg, interpolation='BILINEAR')
return image
def telemetry(sid, data):
if data:
# The current steering angle of the car
steering_angle = float(data["steering_angle"])
# The current throttle of the car
throttle = float(data["throttle"])
# The current speed of the car
speed = float(data["speed"])
# The current image from the center camera of the car
image = Image.open(BytesIO(base64.b64decode(data["image"])))
# save frame
if args.image_folder != '':
timestamp = datetime.utcnow().strftime('%Y_%m_%d_%H_%M_%S_%f')[:-3]
image_filename = os.path.join(args.image_folder, timestamp)
image.save('{}.jpg'.format(image_filename))
try:
image = np.asarray(image) # from PIL image to numpy array
image = utils.preprocess(image) # apply the preprocessing
image = np.array([image]) # the model expects 4D array
# predict the steering angle for the image
steering_angle = float(model.predict(image, batch_size=1))
# lower the throttle as the speed increases
# if the speed is above the current speed limit, we are on a downhill.
# make sure we slow down first and then go back to the original max speed.
global speed_limit
if speed > speed_limit:
speed_limit = MIN_SPEED # slow down
else:
speed_limit = MAX_SPEED
throttle = 1.0 - utils.deg2rad(steering_angle)**2 - (
speed / speed_limit)**2
print('{} {} {}'.format(steering_angle, throttle, speed))
send_control(steering_angle, throttle)
except Exception as e:
print(e)
else:
# NOTE: DON'T EDIT THIS.
sio.emit('manual', data={}, skip_sid=True)
def check_turn_completion(self):
if abs(self.zumy_direction -
self.turn_start_direction) >= utils.deg2rad(
self.max_turn_degree):
# print 'zumy dirn', self.zumy_direction
difference = self.zumy_direction - self.turn_start_direction
if difference >= 0:
# print('pos dir')
linear = Vector3(0, 0, 0)
angular = Vector3(0, 0, -1.35 * SPEED_CONST)
opp_twist = Twist(linear, angular)
self.zumy_vel.publish(opp_twist)
elif difference < 0:
# print('neg dir')
linear = Vector3(0, 0, 0)
angular = Vector3(0, 0, 1.35 * SPEED_CONST)
opp_twist = Twist(linear, angular)
self.zumy_vel.publish(opp_twist)
self.stop_zumy()
self.done_turning_time = time.time()
def get_R(angles_deg):
# Rotation matrix
# Input: list containing x,y,z Euler angles (degrees)
angles_rad = utils.deg2rad(angles_deg)
sin = torch.sin(angles_rad).view(-1, 1, 1)
cos = torch.cos(angles_rad).view(-1, 1, 1)
# ! using the following three lines will cause grads to disappear
# Rx = torch.tensor([[1, 0, 0], [0, cos[0], -sin[0]], [0, sin[0], cos[0]]])
# Ry = torch.tensor([[cos[1], 0, sin[1]], [0, 1, 0], [-sin[1], 0, cos[1]]])
# Rz = torch.tensor([[cos[2], -sin[2], 0], [sin[2], cos[2], 0], [0, 0, 1]])
Rx = torch.cat(
(torch.tensor([[1., 0., 0.]]),
torch.cat((torch.zeros(1, 1), cos[0], -sin[0]),
1), torch.cat((torch.zeros(1, 1), sin[0], cos[0]), 1)))
Ry = torch.cat((torch.cat((cos[1], torch.zeros(1, 1), sin[1]),
1), torch.tensor([[0., 1., 0.]]),
torch.cat((-sin[1], torch.zeros(1, 1), cos[1]), 1)))
Rz = torch.cat(
(torch.cat((cos[2], -sin[2], torch.zeros(1, 1)),
1), torch.cat((sin[2], cos[2], torch.zeros(1, 1)),
1), torch.tensor([[0., 0., 1.]])))
return Rz @ Ry @ Rx
def assign_metadata_result_classes_from_groundtruth(metadata_file_name, rs_folder_path, gt_folder_path, time_th, dist_th):
# Load files
res_list = []
with open(os.path.join(rs_folder_path, metadata_file_name), 'r') as f:
reader = csv.reader(f, delimiter=',')
res_list = list(reader)[1:]
gt_list = []
with open(os.path.join(gt_folder_path, metadata_file_name), 'r') as f:
reader = csv.reader(f, delimiter=',')
gt_list = list(reader)[1:]
# Find the event from gt which is parallel to the one in res
res_dict = {}
used_gt_event_idx = []
for res_e_idx, res_e in enumerate(res_list):
res_start_time = float(res_e[1])
res_end_time = float(res_e[2])
res_ele = float(res_e[3])
res_azi = float(res_e[4])
min_diff = 1000
min_diff_idx = -1
if len(gt_list) == 0:
break
else:
for gt_e_idx, gt_e in enumerate(gt_list):
if gt_e_idx not in used_gt_event_idx:
gt_start_time = float(gt_e[1])
gt_end_time = float(gt_e[2])
gt_ele = float(gt_e[3])
gt_azi = float(gt_e[4])
start_diff = abs(gt_start_time - res_start_time)
end_diff = abs(gt_end_time - res_end_time)
dist_diff = distance_between_spherical_coordinates_rad(deg2rad(gt_azi),
deg2rad(gt_ele),
deg2rad(res_azi),
deg2rad(res_ele))
all_diff = start_diff + end_diff + dist_diff
if start_diff <= time_th and end_diff <= time_th and dist_diff <= dist_th:
if all_diff < min_diff:
min_diff = all_diff
min_diff_idx = gt_e_idx
if min_diff_idx != -1:
used_gt_event_idx.append(min_diff_idx)
res_dict[res_e_idx] = min_diff_idx
output_res_list = copy(res_list)
last_rs_e_idx = len(res_list)
for rs_e_idx in range(last_rs_e_idx):
if rs_e_idx in res_dict.keys():
gt_e_idx = res_dict[rs_e_idx]
class_id = gt_list[gt_e_idx][0]
output_res_list[rs_e_idx][0] = class_id
write_metadata_result_file(output_res_list, os.path.join(rs_folder_path, metadata_file_name))
def read_dxl(self):
""" Read dynamixel position, velocity, current value and publish through ROS."""
self.groupBulkReadPosition.txRxPacket()
self.dxl_present_position[0] = self.groupBulkReadPosition.getData(
self.cfg["DXL1_ID"],
self.cfg["ADDR_PRESENT_POSITION"],
self.cfg["LEN_PRESENT_POSITION"],
)
self.dxl_present_position[1] = self.groupBulkReadPosition.getData(
self.cfg["DXL2_ID"],
self.cfg["ADDR_PRESENT_POSITION"],
self.cfg["LEN_PRESENT_POSITION"],
)
self.dxl_present_position[2] = self.groupBulkReadPosition.getData(
self.cfg["DXL3_ID"],
self.cfg["ADDR_PRESENT_POSITION"],
self.cfg["LEN_PRESENT_POSITION"],
)
self.dxl_present_position[3] = self.groupBulkReadPosition.getData(
self.cfg["DXL4_ID"],
self.cfg["ADDR_PRESENT_POSITION"],
self.cfg["LEN_PRESENT_POSITION"],
)
self.groupBulkReadVelocity.txRxPacket()
self.dxl_present_velocity[0] = self.groupBulkReadVelocity.getData(
self.cfg["DXL1_ID"],
self.cfg["ADDR_PRESENT_VELOCITY"],
self.cfg["LEN_PRESENT_VELOCITY"],
)
self.dxl_present_velocity[1] = self.groupBulkReadVelocity.getData(
self.cfg["DXL2_ID"],
self.cfg["ADDR_PRESENT_VELOCITY"],
self.cfg["LEN_PRESENT_VELOCITY"],
)
self.dxl_present_velocity[2] = self.groupBulkReadVelocity.getData(
self.cfg["DXL3_ID"],
self.cfg["ADDR_PRESENT_VELOCITY"],
self.cfg["LEN_PRESENT_VELOCITY"],
)
self.dxl_present_velocity[3] = self.groupBulkReadVelocity.getData(
self.cfg["DXL4_ID"],
self.cfg["ADDR_PRESENT_VELOCITY"],
self.cfg["LEN_PRESENT_VELOCITY"],
)
self.groupBulkReadCurrent.txRxPacket()
self.dxl_present_current[0] = self.groupBulkReadVelocity.getData(
self.cfg["DXL1_ID"],
self.cfg["ADDR_PRESENT_CURRENT"],
self.cfg["LEN_PRESENT_CURRENT"],
)
self.dxl_present_current[1] = self.groupBulkReadVelocity.getData(
self.cfg["DXL2_ID"],
self.cfg["ADDR_PRESENT_CURRENT"],
self.cfg["LEN_PRESENT_CURRENT"],
)
self.dxl_present_current[2] = self.groupBulkReadVelocity.getData(
self.cfg["DXL3_ID"],
self.cfg["ADDR_PRESENT_CURRENT"],
self.cfg["LEN_PRESENT_CURRENT"],
)
self.dxl_present_current[3] = self.groupBulkReadVelocity.getData(
self.cfg["DXL4_ID"],
self.cfg["ADDR_PRESENT_CURRENT"],
self.cfg["LEN_PRESENT_CURRENT"],
)
for i in range(4):
if self.dxl_present_velocity[i] > 2**(
8 * self.cfg["ADDR_PRESENT_VELOCITY"] / 2):
self.dxl_present_velocity[i] = self.dxl_present_velocity[
i] - 2**(8 * self.cfg["ADDR_PRESENT_VELOCITY"])
if self.dxl_present_current[i] > 2**(
8 * self.cfg["LEN_PRESENT_CURRENT"] / 2):
self.dxl_present_current[i] = self.dxl_present_current[
i] - 2**(8 * self.cfg["LEN_PRESENT_CURRENT"])
q_current = [
0.0,
0.0,
deg2rad(
(self.dxl_present_position[0] - self.cfg["DXL_POS_OFFSET"]) *
self.cfg["DXL_RESOLUTION"]),
deg2rad(
(self.dxl_present_position[1] - self.cfg["DXL_POS_OFFSET"]) *
self.cfg["DXL_RESOLUTION"]),
deg2rad(
(self.dxl_present_position[2] - self.cfg["DXL_POS_OFFSET"]) *
self.cfg["DXL_RESOLUTION"]),
deg2rad(
(self.dxl_present_position[3] - self.cfg["DXL_POS_OFFSET"]) *
self.cfg["DXL_RESOLUTION"]),
]
qdot_current = [
0.0,
0.0,
rpm2rad(self.dxl_present_velocity[0] *
self.cfg["DXL_VELOCITY_RESOLUTION"]),
rpm2rad(self.dxl_present_velocity[1] *
self.cfg["DXL_VELOCITY_RESOLUTION"]),
rpm2rad(self.dxl_present_velocity[2] *
self.cfg["DXL_VELOCITY_RESOLUTION"]),
rpm2rad(self.dxl_present_velocity[3] *
self.cfg["DXL_VELOCITY_RESOLUTION"]),
]
motor_current = [
0.0,
0.0,
self.dxl_present_current[0] * self.cfg["DXL_TO_CURRENT"],
self.dxl_present_current[1] * self.cfg["DXL_TO_CURRENT"],
self.dxl_present_current[2] * self.cfg["DXL_TO_CURRENT"],
self.dxl_present_current[3] * self.cfg["DXL_TO_CURRENT"],
]
self.joint_states.position = q_current
self.joint_states.velocity = qdot_current
self.joint_states.effort = motor_current
self.joint_states_pub.publish(self.joint_states)
def make_points(self):
cut, par, wall, angle = self.cut[:], self.par, self.wall, self.angle
points = []
# We use a coordinate system in which the diameter of the parent tube
# is always 1.0 (or starts at 1.0 in the case of a taper) R is then the
# radii of the inside of the cut tube.
R = [0, 0]
for i in range(2):
cut[i] -= wall * 2
R[i] = cut[i] / par
offset = self.offset / par
# The angle is the angle of the tube axes, but we want the orientation
# of the parent tube relative to the cut tube's cross-section, so just
# add 90 degrees.
angle = fmod(angle + 90, 360)
slope = tan(deg2rad(angle))
twist = deg2rad(self.twist)
# Radius as opposed to diameter
par_r = par / 2
# How much the radius changes by
taper_r = self.taper / 2
min_y = 0
for theta in angles():
# (a, b) is the point on the inside circumference of the cut tube
# at theta
a = R[0] * cos(theta + twist)
b = R[1] * sin(theta + twist)
a += offset
# Now we're projecting the point onto the parent tube
if a > 1 or a < -1:
# In this case we miss the parent tube altogether, so no
# cutting here.
y = 1
else:
# phi is the angle of this point around the parent tube.
phi = asin(a)
r = 1.0 + b * taper_r
y = 1 - r * cos(phi) - b * slope
# We map the angle back onto the outside diameter, although the
# actual curve is on the inside diameter. This implies your cut is
# perpendicular to the tube wall.
point = [self.ellipse.circum_distance(theta), par_r * y]
min_y = min(min_y, point[1])
points.append(point)
# Now push everthing up to a couple of inches above 0 just to have a
# bigger piece of paper to wrap
headroom = 25.4 * 2
if min_y < 0:
headroom -= min_y
for p in points:
p[1] += headroom
# Track the infimum and supremum of the full set of points
for i in range(2):
self.inf[i] = min(self.inf[i], p[i])
self.sup[i] = max(self.sup[i], p[i])
self.points = points
DIMS = (600, 600)
data_dir = './data/'
# load 4 cat images
img1 = img2array(data_dir + 'cat1.jpg', DIMS, expand=True) # , view=True)
img2 = img2array(data_dir + 'cat2.jpg', DIMS, expand=True)
img3 = img2array(data_dir + 'cat3.jpg', DIMS, expand=True)
img4 = img2array(data_dir + 'cat4.jpg', DIMS, expand=True)
input_img = np.concatenate([img1, img2, img3, img4], axis=0)
B, H, W, C = input_img.shape
print("Input Img Shape: {}".format(input_img.shape))
# initialize affine transform tensor `theta`
degree = 45
theta = np.array([[np.cos(deg2rad(degree)), -np.sin(deg2rad(degree)), 0],
[np.sin(deg2rad(degree)),
np.cos(deg2rad(degree)), 0]])
# inverse affine transform
inv_degree = -45
inv_theta = np.array(
[[np.cos(deg2rad(inv_degree)), -np.sin(deg2rad(inv_degree)), 0],
[np.sin(deg2rad(inv_degree)),
np.cos(deg2rad(inv_degree)), 0]])
x = tf.placeholder(tf.float32, [None, H, W, C])
with tf.variable_scope('spatial_transformer'):
theta = theta.astype('float32')
theta = theta.flatten()
def testDegConversion(self):
"""Make sure conversion from degree to radian works
"""
pi = 3.14159
self.assertAlmostEqual(utils.deg2rad(180), pi, places=4)