def retrace_path(self, remaining_steps, offset, remaining_orbs):
retraced_frames = offset
out = []
rot_matrix = utils.rotation_matrix(self.angle * delay,
not self.clockwise)
i = 0
while True:
if i >= remaining_orbs:
return out
if retraced_frames >= self.frame_counter:
previous_step = remaining_steps.pop()
return out + previous_step.retrace_path(
remaining_steps, retraced_frames - self.frame_counter,
remaining_orbs - i)
if i == 0:
p2 = utils.rotate_p_around_p(
self.pos, self.orbit_centre,
utils.rotation_matrix(
(self.frame_counter - offset) * self.angle,
self.clockwise))
else:
p2 = utils.rotate_p_around_p(out[i - 1], self.orbit_centre,
rot_matrix)
retraced_frames += delay
out.append(p2)
i += 1
def setrotationmatrix(self):
rotthetaaxis = np.array([0, 1, 0])
rotphiaxis = np.array([0, 0, 1])
rottheta = self.antennageo[0]
rotphi = self.antennageo[1]
rotthetaangle = utils.degtorad(rottheta)
rotphiangle = utils.degtorad(rotphi)
rotthetamatrix = utils.rotation_matrix(rotthetaaxis, -rotthetaangle)
rotphimatrix = utils.rotation_matrix(rotphiaxis, -rotphiangle)
rottot = np.dot(rotthetamatrix, rotphimatrix)
self.rotationmatrix = rottot
def compute_error(self, cur_pose):
# Find the first element of the plan that is in front of the robot, and remove any elements that are behind the robot. To do this:
# Loop over the plan (starting at the beginning) For each configuration in the plan
# If the configuration is behind the robot, remove it from the plan
# Will want to perform a coordinate transformation to determine if the configuration is in front or behind the robot
# If the configuration is in front of the robot, break out of the loop
while len(self.plan) > 0:
# YOUR CODE HERE
rot = utils.rotation_matrix((np.pi / 2) - cur_pose[2])
trans_v = [[self.plan[0][0] - cur_pose[0]],
[self.plan[0][1] - cur_pose[1]]]
zg_configr = np.array(np.matmul(rot, trans_v))
if zg_configr[1] < 0:
self.plan.pop(0)
else:
break
pass
# Check if the plan is empty. If so, return (False, 0.0)
# YOUR CODE HERE
if len(self.plan) == 0:
return False, 0.0
# At this point, we have removed configurations from the plan that are behind
# the robot. Therefore, element 0 is the first configuration in the plan that is in
# front of the robot. To allow the robot to have some amount of 'look ahead',
# we choose to have the robot head towards the configuration at index 0 + self.plan_lookahead
# We call this index the goal_index
goal_idx = int(min(0 + self.plan_lookahead, len(self.plan) - 1))
# Compute the translation error between the robot and the configuration at goal_idx in the plan
# YOUR CODE HERE
rota = utils.rotation_matrix((np.pi / 2) - cur_pose[2])
trans_ve = [[self.plan[goal_idx][0] - cur_pose[0]],
[self.plan[goal_idx][1] - cur_pose[1]]]
lg_configr = np.array(np.matmul(rota, trans_ve))
translation_error = -lg_configr[0]
# Compute the total error
# Translation error was computed above
# Rotation error is the difference in yaw between the robot and goal configuration
# Be carefull about the sign of the rotation error
# YOUR CODE HERE
rotation_error = self.plan[goal_idx][2] - cur_pose[2]
error = (self.translation_weight *
translation_error) + (self.rotation_weight * rotation_error)
return True, error
def odomCB(self, msg):
'''
Store deltas between consecutive odometry messages in the coordinate space of the car.
Odometry data is accumulated via dead reckoning, so it is very inaccurate on its own.
'''
position = np.array([
msg.pose.pose.position.x,
msg.pose.pose.position.y])
orientation = Utils.quaternion_to_angle(msg.pose.pose.orientation)
pose = np.array([position[0], position[1], orientation])
if isinstance(self.last_pose, np.ndarray):
# changes in x,y,theta in local coordinate system of the car
rot = Utils.rotation_matrix(-self.last_pose[2])
delta = np.array([position - self.last_pose[0:2]]).transpose()
local_delta = (rot*delta).transpose()
self.odometry_data = np.array([local_delta[0,0], local_delta[0,1], orientation - self.last_pose[2]])
self.last_pose = pose
self.last_stamp = msg.header.stamp
self.odom_initialized = True
else:
print "...Received first Odometry message"
self.last_pose = pose
# this topic is slower than lidar, so update every time we receive a message
self.update()
def update(self, i):
for key in self.quads:
state = self.quad.get_state(key)
self.quads[key]['position'] = state[0:3]
self.quads[key]['orientation'] = state[3:6]
R = utils.rotation_matrix(self.quads[key]['orientation'])
L = self.quads[key]['L']
points = R @ np.array([[-L, 0, 0], [L, 0, 0], [0, -L, 0],
[0, L, 0], [0, 0, 0], [0, 0, 0]]).T
points[0, :] += self.quads[key]['position'][0]
points[1, :] += self.quads[key]['position'][1]
points[2, :] += self.quads[key]['position'][2]
self.quads[key]['l1'].set_data_3d(points[0, 0:2], points[1, 0:2],
points[2, 0:2])
self.quads[key]['l2'].set_data_3d(points[0, 2:4], points[1, 2:4],
points[2, 2:4])
self.quads[key]['hub'].set_data_3d(points[0, 5], points[1, 5],
points[2, 5])
if self.plot_quad_trail:
quad_x, quad_y, quad_z = self.quads[key]['position']
if not (quad_x > 6 or quad_x < -2 or quad_y > 6 or quad_y < -2
or quad_z > 7 or quad_z < -2):
self.x.append(quad_x)
self.y.append(quad_y)
self.z.append(quad_z)
self.trail.set_data_3d(self.x, self.y, self.z)
if self.plot_sim_trail:
sim = np.array(self.quad.last_sim_state)
sim_x = sim[:, 0]
sim_y = sim[:, 1]
sim_z = sim[:, 2]
self.sim_trail.set_data_3d(sim_x, sim_y, sim_z)
'''bounds = self.quad.get_bounds(self.t, 0.01)
def compute_motion(self, frame):
positions = {}
def sanitize(vector):
vector = vector.flatten()
return np.array([vector[0, 2], vector[0, 0], vector[0, 1]])
def dfs(position, node, stack):
angles = np.zeros(3)
direction = np.zeros(3)
if "direction" in node:
direction = node["direction"] * node["length"]
if "axis" in node and node["name"] in frame:
dofs = ["rx", "ry", "rz"]
for d in range(len(node["dof"])):
dof = node["dof"][d]
angles[dofs.index(dof)] = frame[node["name"]][d]
L = rotation_matrix(node, angles[0], angles[1], angles[2])
stack.append(np.dot(L, stack[-1]))
end_position = position + np.dot(direction, stack[-1])
positions[node["name"]] = sanitize(end_position.copy())
if "children" in node:
for child in node["children"].values():
dfs(end_position, child, stack)
stack.pop()
root_matrix = rotation_matrix(self.hierarchy["root"], frame["root"][3],
frame["root"][4], frame["root"][5])
root_pos = frame["root"][:3]
dfs(root_pos, self.hierarchy["root"], [root_matrix])
return positions
def odomCB(self, msg):
'''
Store deltas between consecutive odometry messages in the coordinate space of the car.
Odometry data is accumulated via dead reckoning, so it is very inaccurate on its own.
'''
position = np.array([
msg.pose.pose.position.x,
msg.pose.pose.position.y])
orientation = Utils.quaternion_to_angle(msg.pose.pose.orientation)
pose = np.array([position[0], position[1], orientation])
if isinstance(self.last_pose, np.ndarray):
# changes in x,y,theta in local coordinate system of the car
rot = Utils.rotation_matrix(-self.last_pose[2])
delta = np.array([position - self.last_pose[0:2]]).transpose()
local_delta = (rot*delta).transpose()
self.odometry_data = np.array([local_delta[0,0], local_delta[0,1], orientation - self.last_pose[2]])
self.last_pose = pose
self.last_stamp = msg.header.stamp
self.odom_initialized = True
else:
print "...Received first Odometry message"
self.last_pose = pose
# this topic is slower than lidar, so update every time we receive a message
self.update()
def update_quiver(num, hQ, hQ2, slope, grid, flow, idxes):
#print('-------- phi: %5.3f ---------'%phi_arr[i])
phi = (num + 180) % 360 - 180
R = rotation_matrix([1, slope, 0], phi, degree=True)
flow_rot = np.einsum('jk,...k', R, flow)
flow_sum = (flow_rot**2).sum(1).sum(0)
grid_rot = np.einsum('jk,...k', R, grid)
dims = grid.shape
hQ2.set_offsets(grid_rot[:, :, :2].reshape(dims[0] * dims[1], 2))
hQ2.set_UVC(flow_rot[::idxes, ::idxes, 0], flow_rot[::idxes, ::idxes, 1])
dim = grid.shape
grid_plot = grid_rot.reshape(dim[0] * dim[1], dim[2])
flow_plot = flow_rot[::idxes, ::idxes, :].reshape(dim[0] * dim[1], dim[2])
segs = [[[x[0], x[1], x[2]], [x[0] + f[0], x[1] + f[1], x[2] + f[2]]]
for (x, f) in zip(grid_plot, flow_plot)]
hQ.set_segments(segs)
plt.title('phi = %4.2f' % phi)
return hQ
def __init__(self, **kwargs):
self.head = kwargs['head']
self.orbit_centre = kwargs['orbit_centre']
v_centre = self.orbit_centre - self.head.pos
self.clockwise = np.cross(self.head.dir, v_centre) < 0
self.angle = self.head.speed / utils.distance_p_p(self.head.pos, self.orbit_centre)
self.rot_matrix = utils.rotation_matrix(self.angle, self.clockwise)
self.head.tracer.add_trace(tracer.Orbit(self.head.pos, self.orbit_centre, self.clockwise, self.angle))
def transform_to_car(self, cur_pose, index):
theta = cur_pose[2] - np.pi / 2
rot_mat = utils.rotation_matrix(theta)
rot_mat[0, 1] *= -1
rot_mat[1, 0] *= -1
plan_coords = np.array([[self.plan[index][0]], [self.plan[index][1]]])
trans_mat = np.array([[cur_pose[0] * -1], [cur_pose[1] * -1]])
plan_from_car = np.matmul(rot_mat, plan_coords + trans_mat)
return plan_from_car
def compute_error(self, cur_pose):
while len(self.plan) > 0:
rot = np.array(
np.matmul(utils.rotation_matrix(np.pi / 2 - cur_pose[2]),
[[self.plan[0][0] - cur_pose[0]],
[self.plan[0][1] - cur_pose[1]]]))
if (self.plan[0][0] - cur_pose[0]) < 0:
if math.sqrt((self.plan[1][0] - cur_pose[0])**2 +
(self.plan[1][1] - cur_pose[1])**2) < math.sqrt(
(self.plan[0][0] - cur_pose[0])**2 +
(self.plan[0][1] - cur_pose[1])**2):
self.plan.pop(0)
else:
break
else:
if 0 > rot[1]:
self.plan.pop(0)
else:
break
goal_idx = min(0 + self.plan_lookahead, len(self.plan) - 1)
if len(self.plan) <= 0 or goal_idx < 0:
self.report_mindist_car2waypoints()
return (False, 0.0)
translation_error = -np.array(
np.matmul(
utils.rotation_matrix(np.pi / 2 - cur_pose[2]),
np.array([[self.plan[int(goal_idx)][0] - cur_pose[0]],
[self.plan[int(goal_idx)][1] - cur_pose[1]]])))[0]
if self.plan[int(goal_idx)][2] < 0 and cur_pose[2] > 0:
rotation_error = (np.pi - abs(self.plan[int(goal_idx)][2])) + (
np.pi - cur_pose[2])
elif self.plan[int(goal_idx)][2] > 0 and cur_pose[2] < 0:
rotation_error = -1 * ((np.pi - abs(cur_pose[2])) +
(np.pi - self.plan[int(goal_idx)][2]))
else:
rotation_error = self.plan[int(goal_idx)][2] - cur_pose[2]
return True, (self.translation_weight * translation_error +
self.rotation_weight * rotation_error)
def determine_steering_angle(self, pose, lookahead_point):
''' Given a robot pose, and a lookahead point, determine the open loop control
necessary to navigate to that lookahead point. Uses Ackermann steering geometry.
'''
# get the lookahead point in the coordinate frame of the car
rot = utils.rotation_matrix(-pose[2])
delta = np.array([lookahead_point - pose[0:2]]).transpose()
local_delta = (rot * delta).transpose()
local_delta = np.array([local_delta[0, 0], local_delta[0, 1]])
# use the ackermann model
steering_angle = self.model.steering_angle(local_delta)
return steering_angle
def new_init_pose_cb(self, msg):
if len(self.plan) > 0:
rot_mat = utils.rotation_matrix(-1 * self.curr_pose[2])
while len(self.plan) > 0:
distance = np.sqrt(
np.square(self.curr_pose[0] - self.plan[0][0]) +
np.square(self.curr_pose[1] - self.plan[0][1]))
# Figure out if self.plan[0] is in front or behind car
offset = rot_mat * (
(self.plan[0][0:2] - self.curr_pose[0:2]).reshape(2, 1))
offset.flatten()
if offset[0] > 0.0 or distance > 1.0:
break
self.plan.pop(0)
def test_invariance(self, atol=1e-6):
# Test invariance with respect to an arbitray rotation and translation
# Reference values
r, _, _ = distances_and_pair_types(self.xyzs,
self.types,
2,
cutoff=2.6)
# Construct random rotation matrix
R = tf.constant(rotation_matrix(np.random.randn(3),
np.random.randn(1)[0]),
dtype=tf.float32)
# Apply random translation before rotation
xyzs2 = tf.matmul(self.xyzs + np.random.randn(3), R)
r2, _, _ = distances_and_pair_types(xyzs2, self.types, 2, cutoff=2.6)
np.testing.assert_allclose(r.to_tensor().numpy(),
r2.to_tensor().numpy(),
atol=atol)
def motion_cb(self, msg):
self.state_lock.acquire()
# Compute the control from the msg and last_pose
position = np.array([msg.pose.pose.position.x,
msg.pose.pose.position.y])
orientation = Utils.quaternion_to_angle(msg.pose.pose.orientation)
pose = np.array([position[0], position[1], orientation])
if isinstance(self.last_pose, np.ndarray):
rot = Utils.rotation_matrix(-self.last_pose[2])
delta = np.array([position - self.last_pose[0:2]]).transpose()
local_delta = (rot*delta).transpose()
# changes in x,y,theta in local coordinate system of the car
control = np.array([local_delta[0,0], local_delta[0,1], orientation - self.last_pose[2]])
self.apply_motion_model(self.particles, control)
self.last_pose = pose
self.state_lock.release()
def dfs(position, node, stack):
angles = np.zeros(3)
direction = np.zeros(3)
if "direction" in node:
direction = node["direction"] * node["length"]
if "axis" in node and node["name"] in frame:
dofs = ["rx", "ry", "rz"]
for d in range(len(node["dof"])):
dof = node["dof"][d]
angles[dofs.index(dof)] = frame[node["name"]][d]
L = rotation_matrix(node, angles[0], angles[1], angles[2])
stack.append(np.dot(L, stack[-1]))
end_position = position + np.dot(direction, stack[-1])
positions[node["name"]] = sanitize(end_position.copy())
if "children" in node:
for child in node["children"].values():
dfs(end_position, child, stack)
stack.pop()
def viz_sub_cb(self, msg):
# Create the PoseArray to publish. Will contain N poses, where the n-th pose
# represents the last pose in the n-th trajectory
# PP - get current pose from mgs
cur_pose = np.array([
msg.pose.position.x, msg.pose.position.y,
utils.quaternion_to_angle(msg.pose.orientation)
])
pa = PoseArray()
pa.header.frame_id = '/map'
pa.header.stamp = rospy.Time.now()
# Transform the last pose of each trajectory to be w.r.t the world and insert into
for i in range(self.rollouts.shape[0]):
# PP - below code is similar to code in line_follower for transformation
trans_mat = [cur_pose[0], cur_pose[1]]
rot_mat = utils.rotation_matrix(cur_pose[2])
arrow_pose = (self.rollouts[i][-1][0], self.rollouts[i][-1][1])
arrow_pose = np.reshape(arrow_pose, (2, 1))
trans_mat = np.reshape(trans_mat, (2, 1))
arrow_wrt_cur_pose = rot_mat * arrow_pose + trans_mat # co ordinate transformation
# create Pose to add in PoseArray
pose = Pose()
pose.position.x = arrow_wrt_cur_pose[0]
pose.position.y = arrow_wrt_cur_pose[1]
pose.position.z = 0
pose.orientation = utils.angle_to_quaternion(
self.rollouts[i][-1][2] +
cur_pose[2]) # PP - is ths correct? verify with Patrick
#print last_pose_in_traj
pa.poses.append(pose)
self.viz_pub.publish(pa)
def setUp(self):
self.A = file_to_np(
os.path.join(os.getcwd(), 'data/Stanford_bunny.txt'))
# self.A = file_to_np(os.path.join(os.getcwd(), 'data/little.txt'))
self.B = np.copy(self.A)
# Translate
self.t = np.random.rand(DIM) * TRANSLATION
# print('before', self.B)
self.B += self.t
# print('Translate', self.t)
# print('after', self.B)
# Rotate
self.R = rotation_matrix(np.random.rand(DIM),
np.random.rand() * ROTATION)
self.B = np.dot(self.R, self.B.T).T
# print('Rotate', self.R)
# Add noise
# print('noise', np.random.randn(self.A.shape[0], DIM))
self.B += np.random.randn(self.A.shape[0], DIM) * NOISE_SIGMA
def viz_sub_cb(self, msg):
pa = PoseArray()
pa.header.frame_id = '/map'
pa.header.stamp = rospy.Time.now()
trans = np.array([msg.pose.position.x, msg.pose.position.y]).reshape(
(2, 1))
car_yaw = utils.quaternion_to_angle(msg.pose.orientation)
rot_mat = utils.rotation_matrix(car_yaw)
for i in xrange(self.rollouts.shape[0]):
robot_config = self.rollouts[i, -1, 0:2].reshape(2, 1)
map_config = rot_mat * robot_config + trans
map_config.flatten()
pose = Pose()
pose.position.x = map_config[0]
pose.position.y = map_config[1]
pose.position.z = 0.0
pose.orientation = utils.angle_to_quaternion(self.rollouts[i, -1,
2] +
car_yaw)
pa.poses.append(pose)
self.viz_pub.publish(pa)
def step(self, action):
speed, phi = action
self.action = action
# kinematic model
# states: position x, position y, and heading angle
x, y, theta = self.state
dxdt = speed * np.cos(theta)
dydt = speed * np.sin(theta)
dthetadt = speed * np.tan(phi) / self.vehicle_mid_length
dstatedt = np.array([dxdt, dydt, dthetadt])
self.state += dstatedt * self.dt
observation = self.state
# tmp: contain xy coordinate of 4 points of the vehicle rectangle
# array 0: x axis, array 1: y axis
tmp = self.vehicle_body()
x, y, theta = self.state
# rotate
tmp = np.dot(tmp, rotation_matrix(theta))
# translate
tmp += np.array([[x, y]])
self.tmpstep = tmp
# calculate reward
reward = 1. / (np.sqrt(self.state[0] ** 2 + self.state[1] ** 2) + np.sqrt(self.dt*dxdt ** 2 + self.dt*dydt ** 2))
# done condition: pass within parking line or hit the boundaries, wall, obstacles
done = (self._border_fcn(tmp[:, 0]) > tmp[:, 1]).any() \
or (tmp[:, 0] < self.xlim[0]).any() \
or (tmp[:, 0] > self.xlim[1]).any() \
or (tmp[:, 1] < self.ylim[0]).any() \
or (tmp[:, 1] > self.ylim[1]).any() \
or ((tmp[:, 0] > min(self.ox1)).any() and (tmp[:, 0] < max(self.ox1)).any() and (tmp[:, 1] > min(self.oy1)).any() and (tmp[:, 1] < max(self.oy1)).any()) \
or ((tmp[:, 0] > min(self.ox2)).any() and (tmp[:, 0] < max(self.ox2)).any() and (tmp[:, 1] > min(self.oy2)).any() and (tmp[:, 1] < max(self.oy2)).any())
return observation, reward, done
def compute_error(self, cur_pose):
rot_mat = utils.rotation_matrix(-1*cur_pose[2])
while len(self.plan) > 0:
# Figure out if self.plan[0] is in front or behind car
offset = rot_mat * ((self.plan[0][0:2]-cur_pose[0:2]).reshape(2,1))
offset.flatten()
if offset[0] > 0.0:
break
self.plan.pop(0)
if len(self.plan) <= 0:
return False, 0.0
# Get the idx of point that we are heading towards
goal_idx = min(self.plan_lookahead, len(self.plan)-1)
# Compute the offset of goal point
goal_offset = rot_mat * ((self.plan[goal_idx][0:2]-cur_pose[0:2]).reshape(2,1))
goal_offset.flatten()
# Compute error
error = (self.translation_weight*goal_offset[1] +
self.rotation_weight * (self.plan[goal_idx][2]-cur_pose[2]))
return True, error
def test_determinant(self):
R = rotation_matrix(np.random.randn(3), np.random.randn(1)[0])
np.testing.assert_allclose(np.linalg.det(R), 1)
def state_dot(self, t, state, key, method="Zero_Dynamic_Stabilization"):
"""
Calculates d/dt of the state
From Francesco Sabatino's thesis: Quadrotor control: modeling, nonlinear control design, and simulation
:param t: Current time during integration
:param state: Current state of the quadcopter
:param key: String key for accessing the quadcopter information dictionary
:param sim: Whether the function is being to used to simulate over a time horizon (True) or to simulate in real
time (False)
:param method:
Feedback_Linearization:
(Quadrotor control: modeling, nonlinear control design, and simulation by Francesco Sabatino)
[x, y, z, psi, theta, phi, x_dot, y_dot, z_dot, zeta, xi, p, q, r]
Zero_Dynamic_Stabilization:
(Quadrotor control: modeling, nonlinear control design, and simulation by Francesco Sabatino)
[x, y, z, psi, theta, phi, x_dot, y_dot, z_dot, 0, 0, p, q, r]
Regular_Dynamics:
(Quadcopter Dynamics, Simulation, and Control by Andrew Gibiansky)
[x y z x_dot y_dot z_dot theta phi gamma theta_dot phi_dot gamma_dot]
:return: d/dt of state
"""
if method not in ["Feedback_Linearization", "Zero_Dynamic_Stabilization", "Regular_Dynamics"]:
raise SyntaxError("parameter 'method' must be one of "
+ "\"Feedback_Linearization\", "
+ "\"Zero_Dynamic_Stabilization\", or"
"\"Regular_Dynamics\"")
m = self.quads[key]['weight']
Ix, Iy, Iz = self.quads[key]['I'].diagonal()
if method.lower() == "Feedback_Linearization".lower():
if np.array(state).shape[0] != 14:
raise ValueError("State should be 14-dimensional!")
x, y, z, psi, theta, phi, x_dot, y_dot, z_dot, zeta, xi, p, q, r = state
cs, ct, cp = np.cos(state[3:6])
ss, st, sp = np.sin(state[3:6])
ts, tt, tp = np.tan(state[3:6])
state_dot = np.zeros(14)
u1, u2, u3, u4 = self.control(t, state, key, "Feedback_Linearization")
# Linear velocities
state_dot[0:3] = state[6:9]
# Angular velocities
state_dot[3] = q * sp / ct + r * cp / ct
state_dot[4] = q * cp - r * sp
state_dot[5] = p + q * sp * tt + r * cp * tt
# Linear Accelerations
state_dot[6] = - (sp * ss + cp * cs * st) * zeta / m
state_dot[7] = - (cs * sp - cp * ss * st) * zeta / m
state_dot[8] = - (cp * ct) * zeta / m
# zeta_dot/xi_dot
state_dot[9] = xi
state_dot[10] = 0 + u1
# Angular Accelerations
state_dot[11] = q * r * (Iy - Iz) / Ix + u2 / Ix
state_dot[12] = p * r * (Iz - Ix) / Iy + u3 / Iy
state_dot[13] = 0 + u4 / Iz
return state_dot
elif method.lower() == "Zero_Dynamic_Stabilization".lower():
if np.array(state).shape[0] != 12:
raise ValueError("State should be 12-dimensional!")
a = 1
x_desireds = np.array([1 * np.cos(a * t) + 2,
-1 * a * np.sin(a * t),
-1 * a ** 2 * np.cos(a * t),
1 * a ** 3 * np.sin(a * t),
1 * a ** 4 * np.cos(a * t)])
y_desireds = np.array([1 * np.sin(a * t) + 2,
1 * a * np.cos(a * t),
-1 * a ** 2 * np.sin(a * t),
-1 * a ** 3 * np.cos(a * t),
1 * a ** 4 * np.sin(a * t)])
z_desireds = np.array([2, 0, 0])
psi_desireds = np.array([0, 0, 0])
u1, u2, u3, u4 = self.control(t, state, key, self.quads[key]['method'],
x_desireds=x_desireds, y_desireds=y_desireds,
z_desireds=z_desireds, psi_desireds=psi_desireds)
self.u1_prev = u1
cs, ct, cp = np.cos(state[3:6])
ss, st, sp = np.sin(state[3:6])
ts, tt, tp = np.tan(state[3:6])
p, q, r = state[9:12]
state_dot = np.zeros(12)
# Linear velocities
state_dot[0:3] = state[6:9]
# Angular velocities
state_dot[3] = q * sp / ct + r * cp / ct
state_dot[4] = q * cp - r * sp
state_dot[5] = p + q * sp * tt + r * cp * tt
# Linear Accelerations
state_dot[6] = - u1 * (sp * ss + cp * cs * st) / m
state_dot[7] = - u1 * (cs * sp - cp * ss * st) / m
state_dot[8] = self.g - u1 * cp * ct / m
# Angular Accelerations
state_dot[9] = q * r * (Iy - Iz) / Ix + u2 / Ix
state_dot[10] = p * r * (Iz - Ix) / Iy + u3 / Iy
state_dot[11] = p * q * (Ix - Iy) / Iz + u4 / Iz
return state_dot
elif method.lower() == "Regular_Dynamics".lower():
if np.array(state).shape[0] != 12:
raise ValueError("State should be 12-dimensional!")
# Define motor thrusts, based on whether function is used to simulate future or present trajectory
control_input = self.control(state)
m1, m2, m3, m4 = utils.thrust(control_input, self.quads[key]['prop_size'][1],
self.quads[key]['prop_size'][0])
state_dot = np.zeros(12)
# The velocities(t+1 x_dots equal the t x_dots)
state_dot[0:3] = state[3:6]
# The acceleration (x_dotdot)
x_dotdot = np.array([0, 0, -self.quads[key]['weight'] * self.g]) + \
(utils.rotation_matrix(state[6:9]) @ np.array([0, 0, (m1 + m2 + m3 + m4)]) - self.k_d * state[
3:6]) \
/ self.quads[key]['weight']
state_dot[3:6] = x_dotdot
# The angular rates(t+1 theta_dots equal the t theta_dots)
state_dot[6:9] = state[9:12]
# The angular accelerations (omega_dot)
omega = state[9:12]
tau = np.array([self.quads[key]['L'] * (m1 - m3),
self.quads[key]['L'] * (m2 - m4),
self.b * (m1 - m2 + m3 - m4)])
omega_dot = self.quads[key]['invI'] @ (tau - np.cross(omega, (self.quads[key]['I'] @ omega)))
state_dot[9:12] = omega_dot
return state_dot
def test_orthogonality(self, atol=1e-10):
R = rotation_matrix(np.random.randn(3), np.random.randn(1)[0])
# R * R.T == R.T * R
np.testing.assert_allclose(R.dot(R.T), R.T.dot(R), atol=atol)
# since R.T == R^-1 -> R * R.T == I
np.testing.assert_allclose(R.dot(R.T), np.eye(3), atol=atol)
# from base_icp import register_by_SVD, icp
from dis_icp import dis_icp
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
# 匹配前
# Constants
TESTS_NUM = 100 # number of test iterations
DIM = 3 # number of dimensions of the points
NOISE_SIGMA = .01 # standard deviation error to be added
TRANSLATION = .1 # max translation of the test set
ROTATION = .1 # max rotation (radians) of the test set
A = file_to_np(os.path.join(os.getcwd(), 'data/Stanford_bunny.txt'))
B = np.copy(A)
# Translate
t = np.random.rand(DIM) * TRANSLATION
print('before', B)
B += t
# print('Translate', self.t)
# print('after', self.B
# Rotate
R = rotation_matrix(np.random.rand(DIM), np.random.rand() * ROTATION)
B = np.dot(R, B.T).T
# print('Rotate', self.R
# Add noise
# print('noise', np.random.randn(self.A.shape[0], DIM))
B += np.random.randn(A.shape[0], DIM) * NOISE_SIGMA
view_numpy_data(A, B)
def _front_wheel(self): wheel = self._rear_wheel() s, phi = self.action wheel = np.dot(wheel, rotation_matrix(phi)) return wheel
def rotate(self, theta: float, orig: bool = False):
R = rotation_matrix(theta)
edge_set = self.orig_edge_set if orig else self.edge_set_dict
self.edge_set_dict = {k: v @ R.T for k, v in edge_set.items()}
def compute_error(self, cur_pose):
"""
Find the first element of the plan that is in front of the robot, and remove
any elements that are behind the robot. To do this:
Loop over the plan (starting at the beginning) For each configuration in the plan
If the configuration is behind the robot, remove it from the plan
Will want to perform a coordinate transformation to determine if
the configuration is in front or behind the robot
If the configuration is in front of the robot, break out of the loop
"""
print "Computing error..."
# check the leftmost pose in the plan pose-array and if it is behind the car then delete it
if len(self.plan) > 0:
left_edge = (cur_pose[2] + np.pi / 2) * 180 / 3.14 # deg
right_edge = (cur_pose[2] - np.pi / 2) * 180 / 3.14 # deg
angle_robot_path_point = math.atan2(
cur_pose[1] - self.plan[0][1],
cur_pose[0] - self.plan[0][0]) * 180 / 3.14 # deg
# for troubleshooting if path points are not deleted correctly
# converted angles from rad to deg for easier troubleshooting
# print("robot position: ", cur_pose)
# print("path point position: ", self.plan[0])
# print("left_edge: ", left_edge)
# print("right_edge: ", right_edge)
# print("path point to robot vector: ",cur_pose[1] - self.plan[0][1], cur_pose[0] - self.plan[0][0])
# print("angle of path point to robot vector",angle_robot_path_point)
# print("path_point yaw",self.plan[0][2] * 180 / 3.14)
behind = (angle_robot_path_point > right_edge
and angle_robot_path_point < left_edge
) # is path point behind robot?
path_pose_similar_direction = (
self.plan[0][2] > right_edge and self.plan[0][2] < left_edge
) # is path point in similar direction as robot?
if behind and path_pose_similar_direction and len(
self.plan
) > 0: # delete point if behind robot, similar direction, and not last point in path
print "delete element: ", len(
self.plan
) # for troubleshooting, show path points before deleting
self.plan.pop(
0
) # delete the first element in the path, since that point is behind robot and it's direction is similar to robot
print "element deleted? : ", len(
self.plan
) # for troubleshooting, show path points after deleting
if len(self.plan) > 0:
PS = PoseStamped() # create a PoseStamped() msg
PS.header.stamp = rospy.Time.now(
) # set header timestamp value
PS.header.frame_id = "map" # set header frame id value
goal_idx = min(
0 + self.plan_lookahead,
len(self.plan) - 1
) # get goal index for looking ahead this many indices in the path
PS.pose.position.x = self.plan[goal_idx][
0] # set msg x position to value of the x position in the look ahead pose from the path
PS.pose.position.y = self.plan[goal_idx][
1] # set msg y position to value of the y position in the look ahead pose from the path
PS.pose.position.z = 0 # set msg z position to 0 since robot is on the ground
PS.pose.orientation = utils.angle_to_quaternion(
self.plan[goal_idx][2]
) # set msg orientation to [converted to queternion] value of the yaw angle in the look ahead pose from the path
self.goal_pub.publish(
PS
) # publish look ahead follower, now you can add a Pose with topic of PUB_TOPIC_2 value in rviz
# Check if the plan is empty. If so, return (False, 0.0)
# YOUR CODE HERE
if len(self.plan) == 0:
return False, 0.0
# At this point, we have removed configurations from the plan that are behind
# the robot. Therefore, element 0 is the first configuration in the plan that is in
# front of the robot. To allow the robot to have some amount of 'look ahead',
# we choose to have the robot head towards the configuration at index 0 + self.plan_lookahead
# We call this index the goal_index
goal_idx = min(0 + self.plan_lookahead, len(self.plan) - 1)
# Compute the translation error between the robot and the configuration at goal_idx in the plan
# YOUR CODE HERE
print "cur_pose: ", cur_pose
print "lookahead pose: ", self.plan[goal_idx]
look_ahead_position = np.array(
[self.plan[goal_idx][0], self.plan[goal_idx][1]]).reshape([2, 1])
translation_robot_to_origin = np.array([-cur_pose[0],
-cur_pose[1]]).reshape([2, 1])
look_ahead_position_translated = look_ahead_position + translation_robot_to_origin
rotation_matrix_robot_to_x_axis = utils.rotation_matrix(-cur_pose[2])
look_ahead_position_translated_and_rotated = rotation_matrix_robot_to_x_axis * look_ahead_position_translated
print "look_ahead_position_translated_and_rotated: ", look_ahead_position_translated_and_rotated
x_error = float(
look_ahead_position_translated_and_rotated[0][0]
) # This is the distance that the robot is behind the lookahead point parallel to the path
y_error = float(
look_ahead_position_translated_and_rotated[1][0]
) # This is the distance away from the path, perpendicular from the path to the robot
translation_error = -y_error # math.tan(y_error / x_error) * math.pi / 180 # angle in rad to drive along hypotenuse toward the look ahead point
# translation_error *= 10 #float(y_error/x_error) # make the robot turn more sharply if far away from path
# translation_error = np.sqrt(np.square(cur_pose[0] - self.plan[goal_idx][0]) + np.square(cur_pose[1] - self.plan[goal_idx][1]))
print "Translation error: ", translation_error
# Compute the total error
# Translation error was computed above
# Rotation error is the difference in yaw between the robot and goal configuration
# Be careful about the sign of the rotation error
# YOUR CODE HERE
rotation_error = cur_pose[2] - self.plan[goal_idx][2]
print "Rotation error: ", rotation_error
error = self.translation_weight * translation_error + self.rotation_weight * rotation_error
print "Overall error: ", error
self.total_error_list.append(error)
return True, error
def viz_sub_cb(self, msg):
# Create the PoseArray to publish. Will contain N poses, where the n-th pose
# represents the last pose in the n-th trajectory
# print "inside viz_sub_cb"
cur_pose = np.array([
msg.pose.position.x, msg.pose.position.y,
utils.quaternion_to_angle(msg.pose.orientation)
]) # current car pose
if self.show_all_poses: # all poses in each rollout
pose_range = range(0, self.rollouts.shape[1])
else: # only final pose in each rollout
pose_range = range(self.rollouts.shape[1] - 2,
self.rollouts.shape[1])
# Read http://docs.ros.org/jade/api/geometry_msgs/html/msg/PoseArray.html
PA = PoseArray() # create a PoseArray() msg
PA.header.stamp = rospy.Time.now() # set header timestamp value
PA.header.frame_id = "map" # set header frame id value
PA.poses = []
for i in range(0, self.rollouts.shape[0]): # for each [7] rollouts
for j in pose_range: # for pose in range(0, 300) to show all, or range(299,300) to show only final pose
P = Pose()
pose = self.rollouts[
i,
j, :] # self.rollouts[i, 299, :] will be an array of [x, y, theta] for the final pose (last j index) for rollout i
# This is in car frame, so assumes straight is x axis
# P.position.x = pose[0]
# P.position.y = pose[1]
# P.position.z = 0
# P.orientation = utils.angle_to_quaternion(pose[2])
# Transform pose from car frame to map frame
# Method 1: Map Frame to Robot Frame
# Rotation must be done before translation with this method, but it's ok to do both in one step
# First find translation matrix [2, 1] from map origin to robot position
# Second find rotation matrix [2, 2] from map x axis to robot x axis
# Third rotate the rollout position about the origin of the robot axis [a.k.a. robot position] with the same angle as from map x axis to robot x axis
# Fourth translate the rollout position with the same translation matrix as from the map origin to the robot position
translation_map_to_robot = np.array([[cur_pose[0]],
[cur_pose[1]]
]).reshape([2, 1])
rotation_matrix_map_to_robot = utils.rotation_matrix(
cur_pose[2])
rollout_position = np.array([[pose[0]],
[pose[1]]]).reshape([2, 1])
map_position = rotation_matrix_map_to_robot * rollout_position + translation_map_to_robot
P.position.x = map_position[0]
P.position.y = map_position[1]
P.position.z = 0
P.orientation = utils.angle_to_quaternion(
pose[2] + cur_pose[2]
) # car's yaw angle + rollout pose's angle from car
PA.poses.append(P)
# print "Publishing Rollout Vizualization"
self.viz_pub.publish(PA)
mask = np.abs(pose_dot[:, 1]) < 0.5
pose_dot = pose_dot[mask]
poses = poses[mask]
mask = np.abs(pose_dot[:, 2]) < 1
pose_dot = pose_dot[mask]
poses = poses[mask]
CAR_FRAME = True
if CAR_FRAME == True:
# Car FRAME
print("GIMMA DAT CAR FRAM")
car_frame = np.zeros((pose_dot.shape[0], 3))
for i in range(0, pose_dot.shape[0]):
rot = Utils.rotation_matrix(-poses[i, 2])
delta = pose_dot[i, 0:2] #.transpose()
local_delta = rot * delta[:, np.newaxis] #).transpose()
car_frame[i, 0] = local_delta[0]
car_frame[i, 1] = local_delta[1]
car_frame[i, 2] = pose_dot[i, 2]
x_datas[0:car_frame.shape[0] - 1,
0:3] = car_frame[0:car_frame.shape[0] - 1, :]
y_datas[0:car_frame.shape[0] - 1, :] = car_frame[1:car_frame.shape[0], :]
else:
print("Da WRULD IS URZ")
# WORLD FRAME
x_datas[0:pose_dot.shape[0] - 1,
0:3] = pose_dot[0:pose_dot.shape[0] - 1, :]
y_datas[0:pose_dot.shape[0] - 1, :] = pose_dot[1:pose_dot.shape[0], :]
def render(self):
if self.fig is None:
assert self.ax is None
self.fig, self.ax = plt.subplots()
# plotting border and dot on vehicle and walls
if not self.ax.lines:
self.ax.plot([], [], "C0", linewidth=3)
for _ in range(4):
self.ax.plot([], [], "C1", linewidth=3)
self.ax.plot([], [], "C2o", markersize=6)
x = np.linspace(-15, 15, 1000)
y = self._border_fcn(x)
self.ax.plot(x, y, "C3", linewidth=3) # plot red wall
self.ax.plot([0], [0], "C3o", markersize=6) # plot red dot on parking
self.ax.grid()
self.ax.set_xlim(self.xlim)
self.ax.set_aspect("equal")
self.ax.set_ylim(self.ylim)
self.ax.plot(self.ox1, self.oy1)
self.ax.plot(self.ox2, self.oy2)
bbox, lfw, rfw, lrw, rrw, center = self.ax.lines[:6]
# plotting vehicle's parts
tmp = self.vehicle_body()
x, y, theta = self.state
# rotate
tmp = np.dot(tmp, rotation_matrix(theta))
# translate
tmp += np.array([[x, y]])
# repeat to close the drawed object
tmp = np.concatenate([tmp, tmp[[0]]])
bbox.set_data(tmp.T)
# left front wheel
tmp = self._front_wheel()
tmp += np.array([[self.vehicle_mid_length, self.front_wheel_bar / 2.]])
# rotate
tmp = np.dot(tmp, rotation_matrix(theta))
# translate
tmp += np.array([[x, y]])
lfw.set_data(tmp.T)
# right front wheel
tmp = self._front_wheel()
tmp += np.array([[self.vehicle_mid_length, -self.front_wheel_bar / 2.]])
# rotate
tmp = np.dot(tmp, rotation_matrix(theta))
# translate
tmp += np.array([[x, y]])
rfw.set_data(tmp.T)
# left rear wheel
tmp = self._rear_wheel()
tmp += np.array([[0., self.rear_wheel_bar / 2.]])
# rotate
tmp = np.dot(tmp, rotation_matrix(theta))
# translate
tmp += np.array([[x, y]])
lrw.set_data(tmp.T)
# right rear wheel
tmp = self._rear_wheel()
tmp += np.array([[0., -self.rear_wheel_bar / 2.]])
# rotate
tmp = np.dot(tmp, rotation_matrix(theta))
# translate
tmp += np.array([[x, y]])
rrw.set_data(tmp.T)
center.set_data([x], [y]) #
self.ax.relim() #
self.ax.autoscale_view()
plt.draw()
# process delay
plt.pause(1e-07)
def rotate(self, axis, angle):
return Vec3.from_matrix(utils.rotation_matrix(axis, angle)*self.as_matrix())
