def control(v, t, x):
goal = (6, 6)
goal_heading = atan2(goal[1] - x[1], goal[0] - x[0])
d_heading = base.angdiff(goal_heading, x[2])
v.stopif(base.norm(x[0:2] - goal) < 0.1)
return (1, d_heading)
def demand(self):
"""
Compute speed and heading for random waypoint
%
% [SPEED,STEER] = R.demand() is the speed and steer angle to
% drive the vehicle toward the next waypoint. When the vehicle is
% within R.dtresh a new waypoint is chosen.
%
% See also Vehicle."""
if self._goal is None:
self._new_goal()
# if nearly at goal point, choose the next one
d = np.linalg.norm(self._veh._x[0:2] - self._goal)
if d < self._dthresh:
self._new_goal()
# elif d > 2 * self._d_prev:
# self.choose_goal()
# self._d_prev = d
speed = self._speed
goal_heading = atan2(self._goal[1] - self._veh._x[1],
self._goal[0] - self._veh._x[0])
delta_heading = base.angdiff(goal_heading, self._veh._x[2])
return np.r_[speed, self._headinggain * delta_heading]
def h(self, xv, jf=None):
"""
Landmark range and bearing
%
# Z = self.h(X, K) is a sensor observation (1x2), range and bearing, from vehicle at
# pose X (1x3) to the K'th landmark.
%
# Z = self.h(X, P) as above but compute range and bearing to a landmark at coordinate P.
%
# Z = self.h(X) as above but computes range and bearing to all
# map featureself. Z has one row per landmark.
%
# Notes::
# - Noise with covariance W (propertyW) is added to each row of Z.
# - Supports vectorized operation where XV (Nx3) and Z (Nx2).
# - The landmark is assumed visible, field of view and range liits are not
# applied.
%
# See also RangeBearingSensor.reading, RangeBearingSensor.Hx, RangeBearingSensor.Hw, RangeBearingSensor.Hp.
"""
# get the landmarks, one per row
if jf is None:
# self.h(XV) all landmarks
dx = self.map.x - xv[0]
dy = self.map.y - xv[1]
elif base.isinteger(jf):
# self.h(XV, JF)
xlm = self.map.landmark(jf)
dx = xlm[0] - xv[0]
dy = xlm[1] - xv[1]
else:
# self.h(XV, XF)
xlm = base.getvector(jf, 2)
dx = xlm[0] - xv[0]
dy = xlm[1] - xv[1]
# compute range and bearing (Vectorized code)
z = np.c_[np.sqrt(dx**2 + dy**2),
base.angdiff(np.arctan2(dy, dx),
xv[2])] # range & bearing as columns
# add noise with covariance W
z += self._rand.normal(size=z.shape) @ sp.linalg.sqrtm(self._W)
if z.shape[0] == 1:
return z[0]
else:
return z
def ik3(robot, T, config='lun'):
config = self.config_validate(config, ('lr', 'ud', 'nf'))
# solve for the first three joints
a2 = robot.links[1].a
a3 = robot.links[2].a
d1 = robot.links[0].d
d3 = robot.links[2].d
d4 = robot.links[3].d
# The following parameters are extracted from the Homogeneous
# Transformation as defined in equation 1, p. 34
Px, Py, Pz = T.t
Pz -= d1 # offset the pedestal height
theta = np.zeros((3, ))
# Solve for theta[0]
# r is defined in equation 38, p. 39.
# theta[0] uses equations 40 and 41, p.39,
# based on the configuration parameter n1
r = np.sqrt(Px**2 + Py**2)
if 'r' in config:
theta[0] = np.arctan2(Py, Px) + np.arcsin(d3 / r)
elif 'l' in config:
theta[0] = np.arctan2(Py, Px) + np.pi - np.arcsin(d3 / r)
else:
raise ValueError('bad configuration string')
# Solve for theta[1]
# V114 is defined in equation 43, p.39.
# r is defined in equation 47, p.39.
# Psi is defined in equation 49, p.40.
# theta[1] uses equations 50 and 51, p.40, based on the
# configuration parameter n2
if 'u' in config:
n2 = 1
elif 'd' in config:
n2 = -1
else:
raise ValueError('bad configuration string')
if 'l' in config:
n2 = -n2
V114 = Px * np.cos(theta[0]) + Py * np.sin(theta[0])
r = np.sqrt(V114**2 + Pz**2)
Psi = np.arccos(
(a2**2 - d4**2 - a3**2 + V114**2 + Pz**2) / (2.0 * a2 * r))
if np.isnan(Psi):
theta = None # pragma nocover
else:
theta[1] = np.arctan2(Pz, V114) + n2 * Psi
# Solve for theta[2]
# theta[2] uses equation 57, p. 40.
num = np.cos(theta[1]) * V114 + np.sin(theta[1]) * Pz - a2
den = np.cos(theta[1]) * Pz - np.sin(theta[1]) * V114
theta[2] = np.arctan2(a3, d4) - np.arctan2(num, den)
theta = base.angdiff(theta)
return theta
def linearize_and_solve(self):
t0 = time.time()
print('solving')
# H and b are respectively the system matrix and the system vector
H = np.zeros((self.n * 3, self.n * 3))
b = zeros(g.n * 3, 1)
# this loop constructs the global system by accumulating in H and b the contributions
# of all edges (see lecture)
fprintf('.')
etotal = 0
for edge in self.edges:
e, A, B = self.linear_factors(edge)
omega = g.edata(edge).info
# compute the blocks of H^k
# not quite sure whey SE3 is being transposed, what does that mean?
b_i = -A.T @ omega @ e
b_j = -B.T @ omega @ e
H_ii = A.T @ omega @ A
H_ij = A.T @ omega @ B
H_jj = B.T @ omega @ B
v = g.vertices(edge)
i = v[0]
j = v[1]
islice = slice(i * 3, (i + 1) * 3)
jslice = slice(j * 3, (j + 1) * 3)
# accumulate the blocks in H and b
H[islice, islice] += Hii
H[jslice, jslice] += Hjj
H[islice, jslice] += Hij
H[jslice, islice] += Hij.T
b[islice, 0] += b_i
b[jslice, 0] += b_j
etotal = etotal + np.inner(e, e)
printf('.', end=None)
# %note that the system (H b) is obtained only from
# %relative constraints. H is not full rank.
# %we solve the problem by anchoring the position of
# %the the first vertex.
# %this can be expressed by adding the equation
# deltax(1:3,1) = 0
# which is equivalent to the following
H[0:3, 0:3] += np.eye(3)
SH = sp.bsr_sparse(H)
print('.', end=None)
deltax = sp.spsolve(SH, b) # SH \ b
print('.', end=None)
# split the increments in nice 3x1 vectors and sum them up to the original matrix
newmeans = self.coord() + np.reshape(deltax, (3, self.n))
# normalize the angles between -PI and PI
# for (i = 1:size(newmeans,2))
# s = sin(newmeans(3,i))
# c = cos(newmeans(3,i))
# newmeans(3,i) = atan2(s,c)
newmeans[3, :] = base.angdiff(newmeans[3, :])
dt = time.time() - t0
print(f"done in {dt:0.2f} sec. Total cost {etotal}")
return newmeans, energy
