def get_ctrl_output(self):
# use self.x self.y and self.theta to
# compute the right control input here
### YOUR CODE HERE ###
x = self.x
y = self.y
th = self.theta
xg = 3
yg = 3
thg = -np.pi/2
rho = ((xg-x)**2+(yg-y)**2)**0.5
m = np.arctan2((yg-y),(xg-x)) #angle between rho vector and x-axis
al = wrapToPi(m-th)
dl = wrapToPi(m-thg)
k1 = 1
k2 = 1
k3 = 1
#control input
V = k1*rho*np.cos(al)
w = k2*al+k1*np.sinc(al/np.pi)*np.cos(al)*(al+k3*dl)
V_max = 0.5
om_max = 1
V = min(abs(V),V_max)*np.sign(V) #saturate V
om = min(abs(w),om_max)*np.sign(w) #saturate om
#ctrl = np.array([V,om])
cmd_x_dot = V
cmd_theta_dot = om
### END OF YOUR CODE ###
cmd = Twist()
cmd.linear.x = cmd_x_dot
cmd.angular.z = cmd_theta_dot
return cmd
def compute_control(self, x, y, th, t):
"""
Inputs:
x,y,th: Current state
t: Current time (you shouldn't need to use this)
Outputs:
V, om: Control actions
Hints: You'll need to use the wrapToPi function. The np.sinc function
may also be useful, look up its documentation
"""
pho = ((x - self.x_g)**2 + (y - self.y_g)**2)**(0.5)
alpha = wrapToPi(np.arctan2((self.y_g-y),(self.x_g - x)) - th)
sigma = wrapToPi(alpha + th - self.th_g)
V = self.k1*pho*np.cos(alpha)
om = self.k2*alpha + self.k1*np.sinc(alpha/np.pi)*np.cos(alpha)*(alpha + self.k3*sigma)
# apply control limits
V = np.clip(V, -self.V_max, self.V_max)
om = np.clip(om, -self.om_max, self.om_max)
return V, om
def ctrl_pose(x,y,th,x_g,y_g,th_g):
#(x,y,th): current state
#(x_g,y_g,th_g): desired final state
#Code pose controller
rho = math.sqrt((x-x_g)**2 + (y-y_g)**2);
alpha = wrapToPi(math.atan2((y-y_g), (x-x_g)) - (th) + math.pi);
delta = wrapToPi(alpha + (th-th_g));
#Define control inputs (V,om) - without saturation constraints
k1 = 0.3
k2 = 0.3
k3 = 0.3
V = k1*rho*math.cos(alpha)
om = k2*alpha + k1*math.cos(alpha)*np.sinc(alpha/math.pi)*(alpha + k3*delta)
# Apply saturation limits
V = np.sign(V)*min(0.5, np.abs(V))
om = np.sign(om)*min(1, np.abs(om))
return np.array([V, om])
def NEESes(
cls,
x: np.ndarray,
P: np.ndarray,
x_gt: np.ndarray,
) -> np.ndarray:
"""Calculates the total NEES and the NEES for the substates
Args:
x (np.ndarray): The estimate
P (np.ndarray): The state covariance
x_gt (np.ndarray): The ground truth
Raises:
AssertionError: If any input is of the wrong shape, and if debug mode is on, certain numeric properties
Returns:
np.ndarray: NEES for [all, position, heading], shape (3,)
"""
assert x.shape == (3, ), f"EKFSLAM.NEES: x shape incorrect {x.shape}"
assert P.shape == (3, 3), f"EKFSLAM.NEES: P shape incorrect {P.shape}"
assert x_gt.shape == (
3, ), f"EKFSLAM.NEES: x_gt shape incorrect {x_gt.shape}"
d_x = x - x_gt
d_x[2] = utils.wrapToPi(d_x[2])
assert (-np.pi <= d_x[2] <=
np.pi), "EKFSLAM.NEES: error heading must be between (-pi, pi)"
d_p = d_x[0:2]
P_p = P[0:2, 0:2]
assert d_p.shape == (2, ), "EKFSLAM.NEES: d_p must be 2 long"
d_heading = d_x[2] # Note: scalar
assert np.ndim(
d_heading) == 0, "EKFSLAM.NEES: d_heading must be scalar"
P_heading = P[2, 2] # Note: scalar
assert np.ndim(
P_heading) == 0, "EKFSLAM.NEES: P_heading must be scalar"
# NB: Needs to handle both vectors and scalars! Additionally, must handle division by zero
NEES_all = d_x @ (np.linalg.solve(P, d_x))
NEES_pos = d_p @ (np.linalg.solve(P_p, d_p))
try:
NEES_heading = d_heading**2 / P_heading
except ZeroDivisionError:
NEES_heading = 1.0 # TODO: beware
NEESes = np.array([NEES_all, NEES_pos, NEES_heading])
NEESes[np.isnan(NEESes)] = 1.0 # We may divide by zero, # TODO: beware
assert np.all(NEESes >= 0), "ESKF.NEES: one or more negative NEESes"
return NEESes
def compute_control(self, x, y, th, t):
"""
Inputs:
x,y,th: Current state
t: Current time (you shouldn't need to use this)
Outputs:
V, om: Control actions
Hints: You'll need to use the wrapToPi function. The np.sinc function
may also be useful, look up its documentation
"""
########## Code starts here ##########
temp = (np.arctan2(self.y_g - y, self.x_g - x))
alpha = wrapToPi(temp - th)
delta = wrapToPi(temp - self.th_g)
rho = np.sqrt((self.x_g - x)**2 + (self.y_g - y)**2)
V = self.k1 * rho * np.cos(alpha)
om = self.k2 * alpha + self.k1 * \
(np.sinc(alpha / np.pi) * np.cos(alpha)) * (alpha + self.k3 * delta)
my_message1 = Float32()
my_message1.data = alpha
self.pub1.publish(my_message1)
my_message2 = Float32()
my_message2.data = delta
self.pub2.publish(my_message2)
my_message3 = Float32()
my_message3.data = rho
self.pub3.publish(my_message3)
########## Code ends here ##########
# apply control limits
V = np.clip(V, -self.V_max, self.V_max)
om = np.clip(om, -self.om_max, self.om_max)
return V, om
def compute_control(self, x, y, th, t):
"""
Inputs:
x,y,th: Current state
t: Current time (you shouldn't need to use this)
Outputs:
V, om: Control actions
Hints: You'll need to use the wrapToPi function. The np.sinc function
may also be useful, look up its documentation
"""
########## Code starts here ##########
#calculate change of variables from cartesion
rho = np.sqrt((x - self.x_g)**2 + (y - self.y_g)**2) #rho
alpha = wrapToPi(np.arctan2(y - self.y_g, x - self.x_g) - th +
np.pi) #alpha
delta = wrapToPi(alpha + th - self.th_g) #delta
"""
NOTE: to get the final pose, we can "rotate" the space by the goal heading by simply
subtracting off theta goal in the delta calculation
"""
#calulate control values
V = self.k1 * rho * np.cos(alpha)
om = self.k2 * alpha + self.k1 * (
np.sinc(alpha / np.pi) * np.cos(alpha)) * (alpha + self.k3 * delta)
########## Code ends here ##########
# apply control limits
V = np.clip(V, -self.V_max, self.V_max)
om = np.clip(om, -self.om_max, self.om_max)
##### PUBLISHING ####
self.alpha_pub.publish(alpha)
self.rho_pub.publish(rho)
self.delta_pub.publish(delta)
return V, om
def compute_control(self, x, y, th, t):
"""
Inputs:
x,y,th: Current state
t: Current time (you shouldn't need to use this)
Outputs:
V, om: Control actions
Hints: You'll need to use the wrapToPi function. The np.sinc function
may also be useful, look up its documentation
"""
########## Code starts here ##########
x_delta = x - self.x_g
y_delta = y - self.y_g
th_delta = th - self.th_g
# need rotation
Rotation = np.array([[np.cos(self.th_g), -np.sin(self.th_g)],
[np.sin(self.th_g),
np.cos(self.th_g)]])
new_x, new_y = np.dot(Rotation.T, np.array([x_delta, y_delta]))
rho = np.sqrt(new_x**2 + new_y**2)
alpha = wrapToPi(np.arctan2(new_y, new_x) - th_delta + np.pi)
delta = wrapToPi(alpha + th_delta)
V = self.k1 * rho * np.cos(alpha)
om = self.k2 * alpha + self.k1 * np.sinc(
2 * alpha / np.pi) * (alpha + self.k3 * delta)
########## Code ends here ##########
# apply control limits
V = np.clip(V, -self.V_max, self.V_max)
om = np.clip(om, -self.om_max, self.om_max)
return V, om
def compute_control(self, x, y, th, t):
"""
Inputs:
x,y,th: Current state
t: Current time (you shouldn't need to use this)
Outputs:
V, om: Control actions
Hints: You'll need to use the wrapToPi function. The np.sinc function
may also be useful, look up its documentation
"""
########## Code starts here ##########
# get differences in position
x_diff = self.x_g - x
y_diff = self.y_g - y
# calculate polar coordinates
rho = np.sqrt(x_diff**2 + y_diff**2)
alpha = wrapToPi(np.arctan2(y_diff, x_diff) - th)
delta = wrapToPi(alpha + th - self.th_g)
# calculate the controls (keeping in mind thresholds)
if rho < RHO_THRES and alpha < ALPHA_THRES and delta < DELTA_THRES:
V = 0
om = 0
else:
V = self.k1 * rho * np.cos(alpha)
om = self.k2 * alpha + self.k1 * np.sinc(
alpha / np.pi) * np.cos(alpha) * (alpha + self.k3 * delta)
########## Code ends here ##########
# apply control limits
V = np.clip(V, -self.V_max, self.V_max)
om = np.clip(om, -self.om_max, self.om_max)
return V, om
def compute_control(self, x, y, th, t):
"""
Inputs:
x,y,th: Current state
t: Current time (you shouldn't need to use this)
Outputs:
V, om: Control actions
Hints: You'll need to use the wrapToPi function. The np.sinc function
may also be useful, look up its documentation
"""
########## Code starts here ##########
delta_x = self.x_g - x
delta_y = self.y_g - y
rho = np.sqrt(delta_x**2 + delta_y**2)
alpha = wrapToPi(np.arctan2(delta_y, delta_x) - th)
delta = wrapToPi(th + alpha - self.th_g)
if np.abs(rho) < RHO_THRES or np.abs(alpha) < ALPHA_THRES or np.abs(
delta) < DELTA_THRES:
#V = 0 what's up with this?
V = self.k1 * rho * np.cos(alpha)
om = self.k2 * alpha + self.k1 * np.sinc(
alpha / np.pi) * np.cos(alpha) * (alpha + self.k3 * delta)
else:
V = self.k1 * rho * np.cos(alpha)
om = self.k2 * alpha + self.k1 * np.sin(alpha) / alpha * np.cos(
alpha) * (alpha + self.k3 * delta)
########## Code ends here ##########
# apply control limits
V = np.clip(V, -self.V_max, self.V_max)
om = np.clip(om, -self.om_max, self.om_max)
return V, om
def get_goal_pose(self, target_name, distance):
"""
Given the target and distance from the vendor, computes the
position the robot should travel to.
"""
if target_name not in self.target_names:
return
marker_id = self.target_names.index(target_name)
theta = self.heading[marker_id]
if theta is None:
return
x, y = self.marker_locations[marker_id]
x += np.cos(theta) * distance
y += np.sin(theta) * distance
robot_heading = wrapToPi(theta + np.pi)
return (x, y, robot_heading)
def f(self, x: np.ndarray, u: np.ndarray) -> np.ndarray:
"""Add the odometry u to the robot state x.
Parameters
x : np.ndarray, shape=(3,) the robot state
u : np.ndarray, shape=(3,) the odometry
Returns:
np.ndarray, shape = (3,) the predicted state
"""
# eq (11.7). Should wrap heading angle between (-pi, pi), see utils.wrapToPi
xpred = np.array([
x[0] + u[0] * np.cos(x[2]) - u[1] * np.sin(x[2]),
x[1] + u[0] * np.sin(x[2]) + u[1] * np.cos(x[2]),
utils.wrapToPi(x[2] + u[2])
])
#assert xpred.shape == (3,), "EKFSLAM.f: wrong shape for xpred"
return xpred
def modelStep(state_km1, odom):
L = GP.wheelbase
Ts = GP.Ts
x_km1 = state_km1[0]
y_km1 = state_km1[1]
theta_km1 = state_km1[2]
v = odom[0]
gamma = odom[1]
x_k = x_km1 + v * np.cos(theta_km1) * Ts
y_k = y_km1 + v * np.sin(theta_km1) * Ts
theta_k = theta_km1 + (v / L) * np.tan(gamma) * Ts
theta_k = wrapToPi(theta_k)
return np.array([x_k, y_k, theta_k])
def Fu(self, x: np.ndarray, u: np.ndarray) -> np.ndarray:
"""Calculate the Jacobian of f with respect to u.
Parameters
----------
x : np.ndarray, shape=(3,)
the robot state
u : np.ndarray, shape=(3,)
the odometry
Returns
-------
np.ndarray
The Jacobian of f wrt. u.
"""
psi = utils.wrapToPi(x[2])
Fu = np.array([[np.cos(psi), -np.sin(psi), 0],
[np.sin(psi), np.cos(psi), 0], [0, 0, 1]]) # eq (11.14)
assert Fu.shape == (3, 3), "EKFSLAM.Fu: wrong shape"
return Fu
def run_pose_controller(self):
""" runs a simple feedback pose controller """
try:
(translation, rotation) = self.trans_listener.lookupTransform(
'/map', '/base_footprint', rospy.Time(0))
self.x = translation[0]
self.y = translation[1]
euler = tf.transformations.euler_from_quaternion(rotation)
self.theta = euler[2]
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
pass
rel_coords = np.array([self.x - self.x_g, self.y - self.y_g])
R = np.array([[np.cos(self.theta_g),
np.sin(self.theta_g)],
[-np.sin(self.theta_g),
np.cos(self.theta_g)]])
rel_coords_rot = np.dot(R, rel_coords)
th_rot = self.theta - self.theta_g
rho = linalg.norm(rel_coords)
ang = np.arctan2(rel_coords_rot[1], rel_coords_rot[0]) + np.pi
angs = wrapToPi(np.array([ang - th_rot, ang]))
alpha = angs[0]
delta = angs[1]
V = K1 * rho * np.cos(alpha)
om = K2 * alpha + K1 * np.sinc(
2 * alpha / np.pi) * (alpha + K3 * delta)
# Apply saturation limits
cmd_x_dot = np.sign(V) * min(V_MAX, np.abs(V))
cmd_theta_dot = np.sign(om) * min(W_MAX, np.abs(om))
cmd_msg = Twist()
cmd_msg.linear.x = cmd_x_dot
cmd_msg.angular.z = cmd_theta_dot
self.cmd_vel_publisher.publish(cmd_msg)
def ctrl_pose(x, y, th, x_g, y_g, th_g):
#(x,y,th): current state
#(x_g,y_g,th_g): desired final state
# Relative pos in global frame
rel_pos_global = np.array([x - x_g, y - y_g])
# Rotation matrix from relative to global
R = np.array([[np.cos(th_g), -np.sin(th_g)], [np.sin(th_g), np.cos(th_g)]])
# Relative pos in relative frame
rel_pos = R.T.dot(rel_pos_global)
# New state variables
theta = th - th_g
rho = np.linalg.norm(rel_pos)
delta = np.arctan2(rel_pos[1], rel_pos[0]) + np.pi
alpha = delta - theta
# Wrap angles to [-pi, pi]
delta, alpha = wrapToPi(np.array([delta, alpha]))
# Gains
k1 = 0.5
k2 = 0.8
k3 = 0.8
# Define control inputs (V,om) - without saturation constraints
thresh = 1e-5
if (np.array([rho, delta, alpha]) <= np.array([thresh] * 3)).all():
V, om = 0.0, 0.0
else:
V = k1 * rho * np.cos(alpha)
om = k2 * alpha + k1 * np.sinc(
2 * alpha / np.pi) * (alpha + k3 * delta)
# Apply saturation limits
V = np.sign(V) * min(0.5, np.abs(V))
om = np.sign(om) * min(1, np.abs(om))
return np.array([V, om])
def Fx(self, x: np.ndarray, u: np.ndarray) -> np.ndarray:
"""Calculate the Jacobian of f with respect to x.
Parameters
----------
x : np.ndarray, shape=(3,)
the robot state
u : np.ndarray, shape=(3,)
the odometry
Returns
-------
np.ndarray
The Jacobian of f wrt. x.
"""
psi = utils.wrapToPi(x[2])
Fx = np.array([[1, 0, -u[0] * np.sin(psi) - u[1] * np.cos(psi)],
[0, 1, u[0] * np.cos(psi) - u[1] * np.sin(psi)],
[0, 0, 1]]) # eq (11.13)
assert Fx.shape == (3, 3), "EKFSLAM.Fx: wrong shape"
return Fx
def f(self, x: np.ndarray, u: np.ndarray) -> np.ndarray:
"""Add the odometry u to the robot state x.
Parameters
----------
x : np.ndarray, shape=(3,)
the robot state
u : np.ndarray, shape=(3,)
the odometry
Returns
-------
np.ndarray, shape = (3,)
the predicted state
"""
pos = x[:2] + rotmat2d(x[2]) @ u[:2]
yaw_angle = utils.wrapToPi(x[2] + u[2]) # Wrap heading angle between (-pi, pi)
xpred = np.array([*pos, yaw_angle])
assert xpred.shape == (3,), "EKFSLAM.f: wrong shape for xpred"
return xpred
def add_food_to_list(self, msg, label):
'''
Description:Add the food item to the data matrix
Arguments:msg from the topic, food item label
Returns:False if nothing was added, true if added
'''
# get the angle of the frame wrt the world
theta_food = 0.5 * wrapToPi(msg.thetaleft -
msg.thetaright) + self.theta
# find the x, y, of the food using the angle
x_food = self.x #+(msg.distance - self.chunky_radius)*np.cos(theta_food)
y_food = self.y #+(msg.distance - self.chunky_radius)*np.sin(theta_food)
# check to see if the food was added or we have a better distance, but only when we are exploring
if self.exploring and (
self.food_found[label] is 0
or msg.distance < self.food_data[label, 3]
): #np.abs(theta_food) < self.food_data[label,2]:
# popluate the array at the correct row
self.food_data[
label] = x_food, y_food, theta_food, msg.distance, msg.confidence
# indicate that we found the food
self.food_found[label] = 1
# add marker to location of food
#self.broadcast_tf(x_food,y_food,0)
self.add_marker(x_food, y_food, label)
# return true to indicate successful addition
return True
# else return false
else:
return False
# Apply control and simulate
Trj = [init_state.copy()]
Vs = []
t = 0
x = init_state
while t < 2000:
x_torch = numpy2torch(x.copy())
with torch.no_grad():
_, u, V = net(x_torch.unsqueeze(0))
u = torch.clamp(u, -10., 10.) # Limit the amount of control input
u = torch2numpy(u[0])
times = np.arange(2) * dt
x_orig = x.copy()
x_orig[:n_link] = wrapToPi(x_orig[:n_link] + target)
y = odeint(system.gradient, x_orig, times, args=(u, ))
y = y[1]
y[:n_link] = wrapToPi(y[:n_link] - target)
x = y
Trj.append(x.copy())
Vs.append(V.item())
print('t = {}, V = {}'.format(t, V.item()))
t += 1
Trj = np.array(Trj[:-1])
def update(self,
eta: np.ndarray,
P: np.ndarray,
z: np.ndarray,
gps=None) -> Tuple[np.ndarray, np.ndarray, float, np.ndarray]:
"""Update eta and P with z, associating landmarks and adding new ones.
Parameters
----------
eta : np.ndarray
[description]
P : np.ndarray
[description]
z : np.ndarray, shape=(#detections, 2)
[description]
Returns
-------
Tuple[np.ndarray, np.ndarray, float, np.ndarray]
[description]
"""
numLmk = (eta.size - 3) // 2
assert (len(eta) -
3) % 2 == 0, "EKFSLAM.update: landmark lenght not even"
if numLmk > 0:
# Prediction and innovation covariance
zpred = self.h(eta) # TODO
H = self.H(eta) # TODO
# Here you can use simply np.kron (a bit slow) to form the big (very big in VP after a while) R,
# or be smart with indexing and broadcasting (3d indexing into 2d mat) realizing you are adding the same R on all diagonals
S = H @ P @ H.T + block_diag_einsum(self.R, numLmk)
assert (S.shape == zpred.shape *
2), "EKFSLAM.update: wrong shape on either S or zpred"
z = z.ravel() # 2D -> flat
# Perform data association
za, zpred, Ha, Sa, a = self.associate(z, zpred, H, S)
# No association could be made, so skip update
if za.shape[0] == 0:
etaupd = eta # TODO
Pupd = P # TODO
NIS = 1 # TODO: beware this one when analysing consistency.
else:
# Create the associated innovation
v = za.ravel() - zpred # za: 2D -> flat
v[1::2] = utils.wrapToPi(v[1::2])
# Kalman mean update
#S_cho_factors = la.cho_factor(Sa) # Optional, used in places for S^-1, see scipy.linalg.cho_factor and scipy.linalg.cho_solve
W = P @ Ha.T @ la.inv(
Sa) # TODO, Kalman gain, can use S_cho_factors
#W = la.cho_solve(S_cho_factors, Ha @ P).T
etaupd = eta + W @ v # TODO, Kalman update
# Kalman cov update: use Joseph form for stability
jo = -W @ Ha
jo[np.diag_indices(
jo.shape[0])] += 1 # same as adding Identity mat
Pupd = jo @ P # @ jo.T + W @ np.kron(np.eye(za.size//2), self.R)@W.T
# TODO, Kalman update. This is the main workload on VP after speedups
# calculate NIS, can use S_cho_factors
NIS = (v.T @ la.inv(Sa) @ v
) # - CI[0]) / (CI[1] - CI[0]) # TODO
#NIS = v.T @ la.cho_solve(S_cho_factors, v)
# When tested, remove for speed
assert np.allclose(
Pupd, Pupd.T), "EKFSLAM.update: Pupd not symmetric"
assert np.all(np.linalg.eigvals(Pupd) > 0
), "EKFSLAM.update: Pupd not positive definite"
else: # All measurements are new landmarks,
a = np.full(z.shape[0], -1)
z = z.flatten()
NIS = 0 # TODO: beware this one, you can change the value to for instance 1
etaupd = eta
Pupd = P
# Create new landmarks if any is available
if self.do_asso:
is_new_lmk = a == -1
if np.any(is_new_lmk):
z_new_inds = np.empty_like(z, dtype=bool)
z_new_inds[::2] = is_new_lmk
z_new_inds[1::2] = is_new_lmk
z_new = z[z_new_inds]
etaupd, Pupd = self.add_landmarks(
etaupd, Pupd, z_new) # TODO, add new landmarks.
assert np.allclose(Pupd,
Pupd.T), "EKFSLAM.update: Pupd must be symmetric"
assert np.all(
np.linalg.eigvals(Pupd) >= 0), "EKFSLAM.update: Pupd must be PSD"
return etaupd, Pupd, NIS, a
def at_goal(self):
"""
returns whether the robot has reached the goal position with enough
accuracy to return to idle state
"""
return (linalg.norm(np.array([self.x-self.x_g, self.y-self.y_g])) < self.at_thresh and abs(wrapToPi(self.theta - self.theta_g)) < self.at_thresh_theta)
def replan(self):
"""
loads goal into pose controller
runs planner based on current pose
if plan long enough to track:
smooths resulting traj, loads it into traj_controller
sets self.current_plan_start_time
sets mode to ALIGN
else:
sets mode to PARK
"""
# Make sure we have a map
if not self.occupancy:
rospy.loginfo(
"Navigator: replanning canceled, waiting for occupancy map.")
self.switch_mode(Mode.IDLE)
return
# Attempt to plan a path
state_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
state_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
x_init = self.snap_to_grid((self.x, self.y))
self.plan_start = x_init
x_goal = self.snap_to_grid((self.x_g, self.y_g))
problem = AStar(state_min, state_max, x_init, x_goal, self.occupancy,
self.plan_resolution)
rospy.loginfo("Navigator: computing navigation plan")
success = problem.solve()
if not success:
rospy.loginfo("Planning failed")
return
rospy.loginfo("Planning Succeeded")
planned_path = problem.path
# Check whether path is too short
if len(planned_path) < 4:
rospy.loginfo("Path too short to track")
self.switch_mode(Mode.PARK)
return
# Smooth and generate a trajectory
traj_new, t_new = compute_smoothed_traj(planned_path, self.v_des,
self.spline_alpha,
self.traj_dt)
# If currently tracking a trajectory, check whether new trajectory will take more time to follow
if self.mode == Mode.TRACK:
t_remaining_curr = self.current_plan_duration - self.get_current_plan_time(
)
# Estimate duration of new trajectory
th_init_new = traj_new[0, 2]
th_err = wrapToPi(th_init_new - self.theta)
t_init_align = abs(th_err / self.om_max)
t_remaining_new = t_init_align + t_new[-1]
if t_remaining_new > t_remaining_curr:
rospy.loginfo(
"New plan rejected (longer duration than current plan)")
self.publish_smoothed_path(traj_new,
self.nav_smoothed_path_rej_pub)
return
# Otherwise follow the new plan
self.publish_planned_path(planned_path, self.nav_planned_path_pub)
self.publish_smoothed_path(traj_new, self.nav_smoothed_path_pub)
self.pose_controller.load_goal(self.x_g, self.y_g, self.theta_g)
self.traj_controller.load_traj(t_new, traj_new)
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_duration = t_new[-1]
self.th_init = traj_new[0, 2]
self.heading_controller.load_goal(self.th_init)
if not self.aligned():
rospy.loginfo("Not aligned with start direction")
self.switch_mode(Mode.ALIGN)
return
rospy.loginfo("Ready to track")
self.switch_mode(Mode.TRACK)
def replan(self, force=False):
"""
loads goal into pose controller
runs planner based on current pose
if plan long enough to track:
smooths resulting traj, loads it into traj_controller
sets self.current_plan_start_time
sets mode to ALIGN
else:
sets mode to PARK
"""
# Make sure we have a map
if not self.occupancy:
rospy.loginfo(
"Navigator: replanning canceled, waiting for occupancy map.")
return
# Check if the goal is free
if not self.x_g or not self.y_g or not self.theta_g:
rospy.loginfo("No goal.")
return
X_g = (self.x_g, self.y_g)
# Before planning a path, load goal pose
self.pose_controller.load_goal(X_g[0], X_g[1], self.theta_g)
# Attempt to plan a path
state_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
state_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
x_init = nearest_free((self.x, self.y), self.occupancy_strict,
self.plan_resolution)
self.plan_start = x_init
problem = AStar(state_min, state_max, x_init, X_g,
self.occupancy_strict, self.occupancy,
self.plan_resolution)
success = problem.solve()
if not success:
if self.prev_goal != X_g:
self.error_count = 0
self.prev_goal = X_g
self.error_count += 1
return
planned_path = problem.path
# Check whether path is too short
if len(planned_path) < 4:
self.switch_mode(Mode.PARK)
return
# Smooth and generate a trajectory
traj_new, t_new = compute_smoothed_traj(planned_path, self.v_des,
self.spline_alpha,
self.traj_dt)
# If currently tracking a trajectory, check whether new trajectory will take more time to follow
if self.mode == Mode.TRACK:
t_remaining_curr = self.current_plan_duration - self.get_current_plan_time(
)
# Estimate duration of new trajectory
th_init_new = traj_new[0, 2]
th_err = wrapToPi(th_init_new - self.theta)
t_init_align = abs(th_err / self.om_max)
t_remaining_new = t_init_align + t_new[-1]
if t_remaining_new > t_remaining_curr and not (self.new_goal
or force):
self._publish_smoothed_path(traj_new,
self.nav_smoothed_path_rej_pub)
return
# Otherwise follow the new plan
self.publish_planned_path(planned_path, self.nav_planned_path_pub)
self._publish_smoothed_path(traj_new, self.nav_smoothed_path_pub)
self.traj_controller.load_traj(t_new, traj_new)
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_duration = t_new[-1]
self.new_goal = False
self.th_init = traj_new[0, 2]
self.heading_controller.load_goal(self.th_init)
if not self.aligned():
self.switch_mode(Mode.ALIGN)
return
self.switch_mode(Mode.TRACK)
CI_ANEES = np.array(chi2.interval(alpha, df*N)) / N
print(f"CI ANEES {tag}: {CI_ANEES}")
print(f"ANEES {tag}: {NEES.mean()}")
fig4.tight_layout()
# %% RMSE
ylabels = ['m', 'deg']
scalings = np.array([1, 180/np.pi])
fig5, ax5 = plt.subplots(nrows=2, ncols=1, figsize=(7, 5), num=5, clear=True, sharex=True)
pos_err = np.linalg.norm(pose_est[:N,:2] - poseGT[:N,:2], axis=1)
heading_err = np.abs(utils.wrapToPi(pose_est[:N,2] - poseGT[:N,2]))
errs = np.vstack((pos_err, heading_err))
for ax, err, tag, ylabel, scaling in zip(ax5, errs, tags[1:], ylabels, scalings):
ax.plot(err*scaling)
ax.set_title(f"{tag}: RMSE {np.sqrt((err**2).mean())*scaling} {ylabel}")
ax.set_ylabel(f"[{ylabel}]")
ax.grid()
fig5.tight_layout()
# %% Movie time
if playMovie:
try:
def get_ctrl_output(self):
""" runs a simple feedback pose controller """
if (rospy.get_rostime().to_sec()-self.cmd_pose_time.to_sec()) < TIMEOUT:
# if you are not using gazebo, your need to use a TF look-up to find robot's states
# relevant for hw 3+
if not use_gazebo:
try:
origin_frame = "/map" if mapping else "/odom"
(translation,rotation) = self.trans_listener.lookupTransform(origin_frame, '/base_footprint', rospy.Time(0))
self.x = translation[0]
self.y = translation[1]
euler = tf.transformations.euler_from_quaternion(rotation)
self.theta = euler[2]
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
pass
######### YOUR CODE HERE ############
# robot's state is self.x, self.y, self.theta
# robot's desired state is self.x_g, self.y_g, self.theta_g
# fill out cmd_x_dot = ... cmd_theta_dot = ...
x = self.x
y = self.y
th = self.theta
xg = self.x_g
yg = self.y_g
thg = self.theta_g
rho = ((xg-x)**2+(yg-y)**2)**0.5
m = np.arctan2((yg-y),(xg-x)) #angle between rho vector and x-axis
al = wrapToPi(m-th)
dl = wrapToPi(m-thg)
k1 = 1
k2 = 1
k3 = 1
#control input
V = k1*rho*np.cos(al)
w = k2*al+k1*np.sinc(al/np.pi)*np.cos(al)*(al+k3*dl)
V_max = 0.5
om_max = 1
V = min(abs(V),V_max)*np.sign(V) #saturate V
om = min(abs(w),om_max)*np.sign(w) #saturate om
cmd_x_dot = V
cmd_theta_dot = om
######### END OF YOUR CODE ##########
else:
# haven't received a command in a while so stop
rospy.loginfo("Pose controller TIMEOUT: commanding zero controls")
cmd_x_dot = 0
cmd_theta_dot = 0
cmd = Twist()
cmd.linear.x = cmd_x_dot
cmd.angular.z = cmd_theta_dot
return cmd
def f(self, x, u):
xpred = np.hstack(
(utils.rot_mat(x[2]) @ u, utils.wrapToPi(x[2] + u[2])))
return xpred
def aligned(self):
"""
returns whether robot is aligned with starting direction of path
(enough to switch to tracking controller)
"""
return (abs(wrapToPi(self.theta - self.th_init)) < self.theta_start_thresh)
def update(
self, eta: np.ndarray, P: np.ndarray,
z: np.ndarray) -> Tuple[np.ndarray, np.ndarray, float, np.ndarray]:
"""Update eta and P with z, associating landmarks and adding new ones.
Parameters
eta : np.ndarray
P : np.ndarray
z : np.ndarray, shape=(#detections, 2)
Returns
Tuple[np.ndarray, np.ndarray, float, np.ndarray]
[description]
"""
numLmk = (eta.size - 3) // 2
assert (len(eta) -
3) % 2 == 0, "EKFSLAM.update: landmark lenght not even"
print(f"numLmk={numLmk}")
if numLmk > 0:
# Prediction and innovation covariance
zpred = self.h(eta)
H = self.H(eta)
#R = np.kron(np.eye(numLmk), self.R)
R = np.diag(np.repeat([self.R[0, 0], self.R[1, 1]], numLmk))
S = H @ P @ H.T + R
assert (S.shape == zpred.shape *
2), f"EKFSLAM.update: wrong shape on either S or zpred"
z = z.ravel() # 2D -> flat
# Perform data association
za, zpred, Ha, Sa, a = self.associate(z, zpred, H, S)
print(
f"numLmk={numLmk} S: {S.shape}, Sa: {Sa.shape}, R: {R.shape}")
# No association could be made, so skip update
if za.shape[0] == 0:
etaupd = eta
Pupd = P
NIS = 1 # TODO: beware this one when analysing consistency.
else:
# Create the associated innovation
v = za.ravel() - zpred # za: 2D -> flat
v[1::2] = utils.wrapToPi(v[1::2])
# Kalman mean update
S_cho_factors = la.cho_factor(Sa)
Sa_inv = la.cho_solve(S_cho_factors, np.eye(Sa.shape[0]))
W = P @ Ha.T @ Sa_inv
etaupd = eta + W @ v # Kalman update
# Kalman cov update: use Joseph form for stability
jo = -W @ Ha
jo[np.diag_indices(
jo.shape[0])] += 1 # same as adding Identity mat
Pupd = jo @ P @ jo.T
Pupd += W @ R[:Sa.shape[0], :Sa.shape[
0]] @ W.T # TODO, Kalman update. This is the main workload on VP after speedups
#Pupd = P - W @ Ha @ P
#EKF: P_upd = (I - W @ H) @ P @ (I - W @ H).T + W @ self.sensor_model.R(x) @ W.T
# calculate NIS, can use S_cho_factors
NIS = self.NIS(Sa_inv, v)
# When tested, remove for speed
assert np.allclose(
Pupd, Pupd.T), "EKFSLAM.update: Pupd not symmetric"
assert np.all(np.linalg.eigvals(Pupd) > 0
), "EKFSLAM.update: Pupd not positive definite"
else: # All measurements are new landmarks,
a = np.full(z.shape[0], -1)
z = z.flatten()
NIS = 1 # TODO: beware this one, you can change the value to for instance 1
etaupd = eta
Pupd = P
# Create new landmarks if any is available
if self.do_asso:
is_new_lmk = a == -1
if np.any(is_new_lmk):
z_new_inds = np.empty_like(z, dtype=bool)
z_new_inds[::2] = is_new_lmk
z_new_inds[1::2] = is_new_lmk
z_new = z[z_new_inds]
# TODO, add new landmarks:
etaupd, Pupd = self.add_landmarks(etaupd, Pupd,
z_new) #(eta, P, z_new)
assert np.allclose(Pupd,
Pupd.T), "EKFSLAM.update: Pupd must be symmetric"
assert np.all(
np.linalg.eigvals(Pupd) >= 0), "EKFSLAM.update: Pupd must be PSD"
return etaupd, Pupd, NIS, a
def replan(self):
"""
loads goal into pose controller
runs planner based on current pose
if plan long enough to track:
smooths resulting traj, loads it into traj_controller
sets self.current_plan_start_time
sets mode to ALIGN
else:
sets mode to PARK
"""
# Make sure we have a map
if not self.occupancy:
rospy.loginfo(
"Navigator: replanning canceled, waiting for occupancy map.")
self.switch_mode(Mode.IDLE)
return
# Attempt to plan a path
state_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
state_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
x_init = self.snap_to_grid((self.x, self.y))
self.plan_start = x_init
x_goal = self.snap_to_grid((self.x_g, self.y_g))
problem = AStar(state_min, state_max, x_init, x_goal, self.occupancy,
self.plan_resolution)
rospy.loginfo("Navigator: computing navigation plan")
success = problem.solve()
if not success:
rospy.loginfo("Planning failed")
#tell squirtle we could not find a path
msg = String()
msg.data = "no_path"
self.publish_squirtle.publish(msg)
if self.mode == Mode.BACKING_UP:
self.deflate_map()
self.switch_mode(Mode.IDLE)
return
rospy.loginfo("Planning Succeeded")
planned_path = problem.path
# Check whether path is too short
if self.at_goal():
#check the stop flag and don't post 'at_goal' if set (or else Squirtle will get confused at stop signs)
if not self.stop_flag:
rospy.loginfo("Path already at goal pose")
#tell squirtle we are at the goal
msg = String()
msg.data = "at_goal"
self.publish_squirtle.publish(msg)
self.switch_mode(Mode.IDLE)
return
elif len(planned_path) < 4:
rospy.loginfo("Path too short to track")
self.switch_mode(Mode.PARK)
return
# Smooth and generate a trajectory
traj_new, t_new = compute_smoothed_traj(planned_path, self.v_des,
self.spline_alpha,
self.traj_dt)
# If currently tracking a trajectory, check whether new trajectory will take more time to follow
if self.mode == Mode.TRACK:
t_remaining_curr = self.current_plan_duration - self.get_current_plan_time(
)
# Estimate duration of new trajectory
th_init_new = traj_new[0, 2]
th_err = wrapToPi(th_init_new - self.theta)
t_init_align = abs(th_err / self.om_max)
t_remaining_new = t_init_align + t_new[-1]
if t_remaining_new > t_remaining_curr:
rospy.loginfo(
"New plan rejected (longer duration than current plan)")
self.publish_smoothed_path(traj_new,
self.nav_smoothed_path_rej_pub)
return
# Otherwise follow the new plan
self.publish_planned_path(planned_path, self.nav_planned_path_pub)
self.publish_smoothed_path(traj_new, self.nav_smoothed_path_pub)
self.pose_controller.load_goal(self.x_g, self.y_g, self.theta_g)
self.traj_controller.load_traj(t_new, traj_new)
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_duration = t_new[-1]
self.th_init = traj_new[0, 2]
self.heading_controller.load_goal(self.th_init)
if not self.aligned():
rospy.loginfo("Not aligned with start direction")
if self.mode == Mode.BACKING_UP:
self.switch_mode(Mode.INFLATE_ALIGN)
else:
self.switch_mode(Mode.ALIGN)
return
if self.mode == Mode.BACKING_UP: #what happens if we want to nav to new goal here...?
self.start_timer(INFLATE_TIME)
self.switch_mode(Mode.INFLATE)
else:
rospy.loginfo("Ready to track")
self.switch_mode(Mode.TRACK)
torch.set_default_tensor_type('torch.FloatTensor')
n_link = 2
dt = 0.01
# Target
target = [np.pi, np.pi]
target = np.array(target)[np.newaxis, :]
# Training data
train_data = np.load(os.path.join(args.dataset, 'train.npy'),
allow_pickle=True).item()
train_X = train_data['states']
train_X = train_X.reshape(-1, 2 * n_link)
train_X[:, :n_link] = wrapToPi(train_X[:, :n_link] - target)
train_y = train_data['grads']
train_y = train_y.reshape(-1, 2 * n_link)
train_y_forced = train_data['grads_forced']
train_y_forced = train_y_forced.reshape(-1, 2 * n_link)
train_u = train_data['forces']
train_u = train_u.reshape(-1, n_link)
Xscale = np.max(np.abs(train_X), axis=0, keepdims=True)
yscale = np.max(np.abs(train_y), axis=0, keepdims=True)
yfscale = np.max(np.abs(train_y_forced), axis=0, keepdims=True)
yscale = np.maximum(yscale, yfscale)
