def sigmoid(self):
rb = -(self.src.shape[1] * np.tan(np.deg2rad(100 / 2))) / (
np.tan(np.deg2rad(self.fov / 2)) * np.log(99e-8))
ra = rb * np.log(99)
return np.repeat(
np.power(
1 +
np.exp(-(np.linalg.norm(self.uniform_grid, axis=2) - ra) / rb),
-1), 2).reshape(self.uniform_grid.shape)
def score_tan(G):
"""
Takes advantage of tangent function.
"""
sum_score = 0
for n in G.nodes():
sum_score += np.tan(n)
for nbr in G.neighbors(n):
sum_score += np.tan(nbr)
return sum_score
def score_tan(G):
"""
Takes advantage of tangent function.
"""
sum_score = 0
for n in G.nodes():
sum_score += np.tan(n)
for nbr in G.neighbors(n):
sum_score += np.tan(nbr)
return sum_score
def build_roatation_matrix_3D(pitch, roll, yaw):
base = np.sqrt(np.tan(-pitch) ** 2 + np.tan(-roll) ** 2 + 1)
rotation_matrix_p_r = np.array([[1 / base, np.tan(-roll) / base, np.tan(-roll) * np.tan(-pitch) * base],
[-np.tan(-roll) / base, 1 / base, np.tan(-pitch) / base],
[np.tan(-roll) * np.tan(-pitch) / base, -np.tan(-pitch) / base,
1 / base]])
rotation_matrix_y = np.array([[np.cos(yaw), 0, -np.sin(yaw)], [0, 1, 0],
[np.sin(yaw), 0, np.cos(yaw)]])
return rotation_matrix_p_r, rotation_matrix_y
def Hapke_vect(SZA, VZA, DPHI, W, R, BB, CC, HH, B0, variant="2002"):
# En radian
theta0r = SZA * np.pi / 180
thetar = VZA * np.pi / 180
phir = DPHI * np.pi / 180
R = R * np.pi / 180
CTHETA = np.cos(theta0r) * np.cos(thetar) + np.sin(thetar) * np.sin(
theta0r) * np.cos(phir)
THETA = np.arccos(CTHETA)
MUP, MU, S = roughness(theta0r, thetar, phir, R)
P = (1 - CC) * (1 - BB**2) / ((1 + 2 * BB * CTHETA + BB**2)**(3 / 2))
P = P + CC * (1 - BB**2) / ((1 - 2 * BB * CTHETA + BB**2)**(3 / 2))
B = B0 * HH / (HH + np.tan(THETA / 2))
gamma = np.sqrt(1 - W)
# H0 = (1 + 2 * MUP) / (1 + 2 * MUP * gamma)
# H = (1 + 2 * MU) / (1 + 2 * MU * gamma)
if variant == "1993":
H0 = H_1993(MUP, gamma)
H = H_1993(MU, gamma)
else:
H0 = H_2002(MUP, gamma)
H = H_2002(MU, gamma)
REFF = W / 4 / (MU + MUP) * ((1 + B) * P + H0 * H - 1)
REFF = S * REFF * MUP / np.cos(theta0r)
return REFF
def dynamics(self, x, u):
# x, y, th, v
xn = x + self._dt * np.array([
x[3] * np.cos(x[2]), x[3] * np.sin(x[2]),
x[3] * np.tan(u[1]) / self._l, u[0]
])
return xn
def test_function_overloading():
a = pe.pseudo_Obs(17, 2.9, 'e1')
b = pe.pseudo_Obs(4, 0.8, 'e1')
fs = [
lambda x: x[0] + x[1], lambda x: x[1] + x[0], lambda x: x[0] - x[1],
lambda x: x[1] - x[0], lambda x: x[0] * x[1], lambda x: x[1] * x[0],
lambda x: x[0] / x[1], lambda x: x[1] / x[0], lambda x: np.exp(x[0]),
lambda x: np.sin(x[0]), lambda x: np.cos(x[0]), lambda x: np.tan(x[0]),
lambda x: np.log(x[0]), lambda x: np.sqrt(np.abs(x[0])),
lambda x: np.sinh(x[0]), lambda x: np.cosh(x[0]),
lambda x: np.tanh(x[0])
]
for i, f in enumerate(fs):
t1 = f([a, b])
t2 = pe.derived_observable(f, [a, b])
c = t2 - t1
assert c.is_zero()
assert np.log(np.exp(b)) == b
assert np.exp(np.log(b)) == b
assert np.sqrt(b**2) == b
assert np.sqrt(b)**2 == b
np.arcsin(1 / b)
np.arccos(1 / b)
np.arctan(1 / b)
np.arctanh(1 / b)
np.sinc(1 / b)
def sample(self, params, n_samples):
m, s = self.unpack_params(params)
unif_samples = agnp.random.uniform(0,
1,
size=(n_samples, self.n_params))
samples = m + agnp.exp(s) * agnp.tan(agnp.pi * (unif_samples - 0.5))
return samples
def vjp(g):
a = m * 1/np.tan(th)*ans
if abs(m+1) <= n:
b = np.exp(-1j*ph) * np.sqrt((n+1)*n - (m+1)*m) * sph_harm(m+1, n, ph, th)
else:
b = 0
return g * (a + b)
def dynamics(self, x, u):
_u = np.clip(u, -self.ulim, self.ulim)
# x, y, th, v
xn = x + self._dt * np.array([x[3] * np.cos(x[2]),
x[3] * np.sin(x[2]),
x[3] * np.tan(_u[1]) / self._l,
_u[0]])
xn = np.clip(xn, -self._xmax, self._xmax)
return xn
def visit_Function(self, node):
f = node.value
if f == EXP:
return np.exp(self.visit(node.expr))
if (f == LOG) or (f == LN):
return np.log(self.visit(node.expr))
if f == LOG10:
return np.log10(self.visit(node.expr))
if f == SQRT:
return np.sqrt(self.visit(node.expr))
if f == ABS:
return np.abs(self.visit(node.expr))
if f == SIGN:
return np.sign(self.visit(node.expr))
if f == SIN:
return np.sin(self.visit(node.expr))
if f == COS:
return np.cos(self.visit(node.expr))
if f == TAN:
return np.tan(self.visit(node.expr))
if f == ASIN:
return np.arcsin(self.visit(node.expr))
if f == ACOS:
return np.arccos(self.visit(node.expr))
if f == ATAN:
return np.arctan(self.visit(node.expr))
if f == MAX:
raise NotImplementedError(MAX)
if f == MIN:
raise NotImplementedError(MIN)
if f == NORMCDF:
raise NotImplementedError(NORMCDF)
if f == NORMPDF:
raise NotImplementedError(NORMPDF)
if f == ERF:
return erf(self.visit(node.expr))
def roughness(theta0, theta, phi, R):
xidz = 1 / np.sqrt(1 + np.pi * (np.tan(R)**2))
cose = np.cos(theta)
sine = np.sin(theta)
cosi = np.cos(theta0)
sini = np.sin(theta0)
mu_b = xidz * (cose + sine * np.tan(R) * e2(R, theta) / (2 - e1(R, theta)))
mu0_b = xidz * (cosi + sini * np.tan(R) * e2(R, theta0) /
(2 - e1(R, theta0)))
f = np.exp(-2 * np.tan(phi / 2))
mask1 = theta0 <= theta
mask2 = ~mask1
mu0_e, mu_e, S = np.empty(theta.shape), np.empty(theta.shape), np.empty(
theta.shape)
# Case 1
mu0_e1 = xidz * (cosi + sini * np.tan(R) *
(np.cos(phi) * e2(R, theta) +
(np.sin(phi / 2)**2) * e2(R, theta0)) /
(2 - e1(R, theta) - (phi / np.pi) * e1(R, theta0)))
mu_e1 = xidz * (cose + sine * np.tan(R) *
(e2(R, theta) - (np.sin(phi / 2)**2) * e2(R, theta0)) /
(2 - e1(R, theta) - (phi / np.pi) * e1(R, theta0)))
S1 = mu_e1 * cosi * xidz / mu_b / mu0_b / (1 - f + f * xidz * cosi / mu0_b)
# Case 2
mu0_e2 = xidz * (cosi + sini * np.tan(R) *
(e2(R, theta0) - np.sin(phi / 2)**2 * e2(R, theta)) /
(2 - e1(R, theta0) - (phi / np.pi) * e1(R, theta)))
mu_e2 = xidz * (
cose + sine * np.tan(R) *
(np.cos(phi) * e2(R, theta0) + np.sin(phi / 2)**2 * e2(R, theta)) /
(2 - e1(R, theta0) - (phi / np.pi) * e1(R, theta)))
S2 = mu_e2 * cosi * xidz / mu_b / mu0_b / (1 - f + f * xidz * cose / mu_b)
# mu0_e[mask1] = mu0_e1[mask1]
# mu0_e[mask2] = mu0_e2[mask2]
# mu_e[mask1] = mu_e1[mask1]
# mu_e[mask2] = mu_e2[mask2]
# S[mask1] = S1[mask1]
# S[mask2] = S2[mask2]
mu0_e = mask1 * mu0_e1 + mask2 * mu0_e2
mu_e = mask1 * mu_e1 + mask2 * mu_e2
S = mask1 * S1 + mask2 * S2
return mu0_e, mu_e, S
def reparameterize(x, prior=None):
if prior == None:
return x
elif prior == 'horseshoe':
eta = np.tan(.5 * np.pi * norm.cdf(x))
return eta**2 * x
elif prior == 'exp':
return -np.log(x)
elif prior == 'laplace':
return -np.sign(x) * np.log(1 - 2 * x)
elif prior == 'lognorm':
return np.exp(x)
elif prior == 'IG':
return 1 / x
elif prior == 'dropout':
return npr.binomial(1, .8, size=x.shape) * x
def test_man_grad():
a = pe.pseudo_Obs(17, 2.9, 'e1')
b = pe.pseudo_Obs(4, 0.8, 'e1')
fs = [
lambda x: x[0] + x[1], lambda x: x[1] + x[0], lambda x: x[0] - x[1],
lambda x: x[1] - x[0], lambda x: x[0] * x[1], lambda x: x[1] * x[0],
lambda x: x[0] / x[1], lambda x: x[1] / x[0], lambda x: np.exp(x[0]),
lambda x: np.sin(x[0]), lambda x: np.cos(x[0]), lambda x: np.tan(x[0]),
lambda x: np.log(x[0]), lambda x: np.sqrt(x[0]),
lambda x: np.sinh(x[0]), lambda x: np.cosh(x[0]),
lambda x: np.tanh(x[0])
]
for i, f in enumerate(fs):
t1 = f([a, b])
t2 = pe.derived_observable(f, [a, b])
c = t2 - t1
assert c.value == 0.0, str(i)
assert np.all(np.abs(c.deltas['e1']) < 1e-14), str(i)
def value(self, x, u):
# Controls
w = u[0] # Front wheel angle
a = u[1] # Front wheel acceleration
o = x[2] # Car angle
v = x[3] # Velocity
# Change in car angle
d_angle = v / CAR_AXLES_DISTANCE * np.tan(w)
xdot = np.array([v * np.cos(o),
v * np.sin(o),
d_angle,
a],
dtype=np.float32)
if self.Q is not None:
# Add system noise
xdot += np.random.multivariate_normal(np.zeros_like(x), self.Q)
return xdot
def g4(self,angles4):
# output = np.ones((4,3))
# determine distances between points
# angles411 = np.arccos(angles4[1, 1])
# angles410 = np.arccos(angles4[1, 0])
# angles421 = np.arccos(angles4[2, 1])
# angles431 = np.arccos(angles4[3, 1])
# angles420 = np.arccos(angles4[2, 0])
# angles430 = np.arccos(angles4[3, 0])
# angles400 = np.arccos(angles4[0, 0])
# angles401 = np.arccos(angles4[0, 1])
angles411 = (angles4[1, 1])
angles410 = (angles4[1, 0])
angles421 = (angles4[2, 1])
angles431 = (angles4[3, 1])
angles420 = (angles4[2, 0])
angles430 = (angles4[3, 0])
angles400 = (angles4[0, 0])
#angles401 = (angles4[0, 1])
c2d2 = 1 / np.sin(angles411)
c1d2 = 1 / np.sin(angles410)
d2_d1c2 = c2d2 * np.sin(angles421)
d2_c1d1 = np.sin(angles431) * c1d2
d1c2 = (d2_d1c2 / np.tan(angles420)) + (d2_d1c2 / np.tan(angles421))
d1c1 = (d2_c1d1 / np.tan(angles430)) + (d2_c1d1 / np.tan(angles431))
d1d2 = d2_d1c2 / np.sin(angles420)
#d1_c1c2 = np.sin(angles401) * d1c1
# law of cosines
#c1c2 = (d1c1 ** 2 + d1c2 ** 2 - 2 * d1c2 * d1c1 * np.cos(angles400)) ** 0.5
result = []
result.append(0.0)
result.append(0.0)
result.append(0.0)
result.append(d1c1)
result.append(0.0)
result.append(0.0)
result.append(d1c2 * np.cos(angles400))
result.append(d1c2 * np.sin(angles400))
result.append(0.0)
s = c1d2 * np.cos(angles410) / (c2d2 * np.cos(angles411) + c1d2 * np.cos(angles410))
asdf = [(1 - s) * d1c1 + s * d1c2 * np.cos(angles400), s * d1c2 * np.sin(angles400), 0.]
slope = (result[3] - result[6]) / (result[7])
x = d1d2 * np.cos(angles430)
result.append(x)
# rise = slope * (x - d1c1)
rise = slope * (x - asdf[0])
y = asdf[1] + rise
result.append(y)
z = (d1d2 ** 2 - (x ** 2 + y ** 2)) ** 0.5
result.append(z)
# return(asdf[2])
# print(result)
result = np.asarray(result)
# return(result[11])
d1 = result[0:3]
# return(d1[])
c1 = result[3:6]
c2 = result[6:9]
d2 = result[9:12]
cc = (c2 - c1)
ip = np.inner((d1 - c1), (c2 - c1)) / (np.sum((c2 - c1) ** 2))
# return(output)
tilded1 = [d1[0] - ip * cc[0], d1[1] - ip * cc[1], d1[2] - ip * cc[2]]
iq = np.inner((d2 - c2), (c1 - c2)) / (np.sum((c1 - c2) ** 2))
cc2 = (c1 - c2)
tilded2 = [d2[0] - iq * cc2[0], d2[1] - iq * cc2[1], d2[2] - iq * cc2[2]]
tilded2star = [tilded2[0] + cc2[0], tilded2[1] + cc2[1], tilded2[2] + cc2[2]]
ab = np.sqrt((tilded2star[0] - c1[0]) ** 2 + (tilded2star[1] - c1[1]) ** 2 + (tilded2star[2] - c1[2]) ** 2)
bc = np.sqrt((tilded2star[0] - tilded1[0]) ** 2 + (tilded2star[1] - tilded1[1]) ** 2 + (tilded2star[2] - tilded1[2]) ** 2)
ca = np.sqrt((tilded1[0] - c1[0]) ** 2 + (tilded1[1] - c1[1]) ** 2 + (tilded1[2] - c1[2]) ** 2)
output = (ab ** 2 - bc ** 2 + ca ** 2) / (2 * ab * ca)
return (output)
def complicated_fun(a,b,c,d,e,f=1.1, g=9.0):
return a + np.sin(b) + np.cosh(c) + np.cos(d) + np.tan(e) + f + g
def test_tan():
fun = lambda x: 3.0 * np.tan(x)
check_grads(fun)(npr.randn())
def f(x, u):
# x, y, th, v
x, y, th, v = x
a, phi = u
return np.array(
[v * np.cos(th), v * np.sin(th), v * np.tan(phi) / self.l, a])
# x_slice is progressed for one step every iterations
x_slice = matrix[:, i:i + 1]
w_batch_hist = gradient_descent(least_square,
w[i:i + 1],
alpha,
max_its,
beta=0)
w[i:i + 1] = w_batch_hist[-1]
b = model(matrix[:, :i + 1], w[:i + 1])
w_hist.append(w_batch_hist[-1])
return w
sin = lambda w: np.sin(w)
cos = lambda w: np.cos(w)
tan = lambda w: np.tan(w)
def function_vector(a, b, c):
result = np.array((sin(a), cos(b), tan(c)))
return result.reshape(-1, 1)
def matrix_function(w, function_list):
for i in range(w.shape[1]):
w[:, i] = np.array(list(map(eval(function_list[i]), w[:, i])))
return w
if __name__ == "__main__":
matrix = np.random.randn(50, 5)
def tan(a: Numeric):
return anp.tan(a)
def f(x, u):
return np.hstack([
x[3] * np.cos(x[2]), x[3] * np.sin(x[2]),
x[3] * np.tan(u[1]) / self.l, u[0]
])
def complicated_fun(a, b, c, d, e, f=1.1, g=9.0):
return a + np.sin(b) + np.cosh(c) + np.cos(d) + np.tan(e) + f + g
def e2(R, x):
arg = (np.tan(R)**2 * np.tan(x)**2 * np.pi)
mask = np.logical_not(np.isclose(arg, 0))
y = np.where(mask, np.exp(-1 / arg[mask]), 0)
return y
def __init__(self):
#=============#
# ROS Objects
#=============#
# ROS Parameters
rospy.init_node("maneuver_path")
self.approach_pose_offset = rospy.get_param(
"~approach_pose_offset", 8.0
) # distance in meters used to offset the maneuver starting pose from the roll, this is to give a good initial value for the optimization
self.roll_radius = rospy.get_param("/roll/radius", 0.20)
self.resolution = rospy.get_param(
"~maneuver_resolution", 0.10
) # resolution for determining number of waypoints in the maneuver paths
self.rescale_factor = rospy.get_param(
"~rescale_factor", 0.5
) # Decreases the resolution of the image along each axis by this fraction
self.max_angle = rospy.get_param("/forklift/steering/max_angle",
65 * (np.pi / 180.)) # deg
# Optimization Parameters
print("Namespace: %s" % rospy.get_name())
self.optimization_method = rospy.get_param(
"~optimization_method",
0) # 0 = use hardcoded, 1 = use scipy, 2 = use pyipopt
self.start_x_s = rospy.get_param("~start_x_s", 11.5)
self.start_y_s = rospy.get_param("~start_y_s", -6.8)
self.start_theta_s = rospy.get_param("~start_theta_s", 0.2)
self.start_r_1 = rospy.get_param("~start_r_1", -0.4)
self.start_alpha_1 = rospy.get_param("~start_alpha_1", -1.6)
self.start_r_2 = rospy.get_param("~start_r_2", 1.0)
self.start_alpha_2 = rospy.get_param("~start_alpha_2", 1.4)
self.target_x = None
self.target_y = None
self.target_approach_angle = None
self.obstacles = None
self.maneuver_path = PathWithGear()
self.maneuver_path.path.header.frame_id = "/odom"
self.optimization_success = False
self.update_obstacle_end_pose = False
self.current_pose = None
self.rate = rospy.Rate(30)
# Forklift dimensions
self.base_to_clamp = rospy.get_param("/forklift/body/base_to_clamp",
1.4658) # {m}
self.base_to_back = rospy.get_param("/forklift/body/base_to_back",
1.9472) # {m}
self.width = rospy.get_param("/forklift/body/width", 1.3599) # {m}
self.total_length = rospy.get_param("/forklift/body/total",
3.5659) # {m}
self.buffer = 0.5 # {m} gap between edge of forklift and the bounding box
'''
Forklift Bounding Box
L_3
|--------------|
___ ___
| buffer |
_|_ |
/ |
\ / |
\ / | L_1
----------------- |
|__ ^ __| |
| | | | | |
|__| <--o |__|buffer---
| |-----| |
| | |
| | |
| | | L_2
----------------- |
| buffer |
_|_ _|_
'''
self.L_1 = self.base_to_clamp + self.buffer # length from base to back end of box
self.L_2 = self.base_to_back + self.buffer # length from base to front end of box
self.L_3 = (self.width / 2.) + self.buffer # length from base to side
# Max turning radius
self.axle_distance = 1.7249
self.min_radius = 1.5 * self.axle_distance / np.tan(
self.max_angle
) # add a scaling factor to give more space to make the turn
# ROS Publishers and Subscribers
self.path1_pub = rospy.Publisher("~path1",
Path,
queue_size=3,
latch=True)
self.path2_pub = rospy.Publisher("~path2",
Path,
queue_size=3,
latch=True)
self.path1_gear_pub = rospy.Publisher("~path1_gear",
PathWithGear,
queue_size=3,
latch=True)
self.path2_gear_pub = rospy.Publisher("~path2_gear",
PathWithGear,
queue_size=3,
latch=True)
self.approach_pose_pub = rospy.Publisher("/forklift/approach_pose",
PoseStamped,
queue_size=3)
self.occupancy_grid_sub = rospy.Subscriber("/map",
OccupancyGrid,
self.occupancyGridCallback,
queue_size=1)
self.roll_pose_sub = rospy.Subscriber("/roll/pose",
PoseStamped,
self.rollCallback,
queue_size=3)
self.odom_sub = rospy.Subscriber("/odom",
Odometry,
self.odomCallback,
queue_size=1)
self.obstacle_path_sub = rospy.Subscriber("/obstacle_avoidance_path",
Path,
self.obstaclePathCallback,
queue_size=1)
# indicates whether the optimzation completed successfully or not, to know whether the path is usable
self.optimize_maneuver_srv = rospy.Service("~optimize_maneuver",
OptimizeManeuver,
self.maneuverService)
def test_tan():
fun = lambda x : 3.0 * np.tan(x)
d_fun = grad(fun)
check_grads(fun, npr.randn())
check_grads(d_fun, npr.randn())
def test_tan():
fun = lambda x : 3.0 * np.tan(x)
check_grads(fun)(npr.randn())
def test_tan():
fun = lambda x : 3.0 * np.tan(x)
d_fun = grad(fun)
check_grads(fun, npr.randn())
check_grads(d_fun, npr.randn())
def phi_z_0_0(Cphi, Dphi, nc, nd, psi_dot, psi):
term1 = (Cphi / psi_dot * (np.sqrt(nc) / (nc - 1)) * np.arctan(
(1 - np.sqrt(nc)) * np.tan(psi) / (1 + np.sqrt(nc) * np.tan(psi)**2)))
term2 = (Dphi / psi_dot * (np.sqrt(nd) / (nd - 1)) * np.arctan(
(1 - np.sqrt(nd)) * np.tan(psi) / (1 + np.sqrt(nd) * np.tan(psi)**2)))
return term1 + term2