def feedforward_control(self, current_x, vel, rx, rv, ra):
A = np.asmatrix([[
-3.40636316e+00, 1.72084569e-15, -2.22044605e-16, 3.33066907e-16,
9.49457230e-10
],
[
1.06332511e-15, -3.40636316e+00, -4.89821158e-15,
-9.49457230e-10, 5.97084703e-15
],
[
8.05604854e-15, -3.39407337e-16, -1.18314673e+01,
1.29496126e-16, -4.45369336e-15
]])
B = np.asmatrix([[-0.34751847, -0.34751847, 0.34751847, 0.34751847],
[0.34751847, -0.34751847, -0.34751847, 0.34751847],
[10.04133891, 10.04133891, 10.04133891, 10.04133891]])
constantMatrix = np.matrix([[-2.88657986e-15], [-2.66501017e-14],
[1.58923465e-14]])
rv_body = np.linalg.inv(util.rotation_matrix(current_x[2, 0])) * rv
vel_body = np.linalg.inv(util.rotation_matrix(current_x[2, 0])) * vel
xDot = np.linalg.inv(util.rotation_matrix(current_x[2, 0])) * ra
x = np.matrix([[rv_body[0, 0]], [rv_body[1, 0]], [rv_body[2, 0]],
[rv_body[0, 0] * rv_body[2, 0]],
[rv_body[1, 0] * rv_body[2, 0]]])
feedforward = np.linalg.pinv(B) * (xDot - A * x - constantMatrix)
return feedforward
def draw_points(state, waypoint, pts):
delta_global = waypoint.position - state.position
point_body = rotation_matrix(-state.heading) @ delta_global
print(f"Waypoint in body frame: {point_body}")
for x, y in pts:
yield from move_arm(state, point_body + [x / 1000.0, y / 1000.0])
yield from move_arm(state, None)
def actuate(self, magnitude, state):
super(RotateBehavior, self).actuate(magnitude, state)
direction = 1 if self.cw else -1
theta = magnitude * direction * (self.world.MAX_ROT_VELOCITY) # Rad, CCW
z_axis = [0, 0, 1]
rm = util.rotation_matrix(z_axis, theta)
dir_3d = np.append(state.direction, [0])
state.direction = np.dot(rm, dir_3d)[:2] # Back to 2D
def circle_proba(self):
cov = self.obstacle.pos_cov
eig_value = np.linalg.eigvals(cov)
rot_matrix = rotation_matrix(self.obstacle.angle)
inv_rot = np.linalg.inv(rot_matrix)
cov = inv_rot@cov@inv_rot.T
# print(cov)
# tt()
var = np.max(eig_value)
ans = compute_proba(self.robot.corner, np.array([self.obstacle.cx, self.obstacle.cy]), var=cov[0, 0])
return ans
def control(self, x, v, rx, rv, ra):
gRb = util.rotation_matrix(x[2, 0])
gRp = util.rotation_matrix(rx[2, 0])
error_path_relative = gRp * (rx - x)
self.integral += self.dt * error_path_relative
G = self.model.geom
GTinv = np.linalg.pinv(G.T * 0.029)
error = gRb.T * (rx - x)
error[2, 0] *= 0.1
derivative = gRb.T * (rv - v)
derivative[2, 0] *= 0.01
goal_acceleration = ra + self.Mp * (rx - x) + self.Md * (
rv - v) + self.Mi * gRp.T * self.integral
return self.model.inverse_dynamics_world(x, rv, goal_acceleration)
uff = self.model.inverse_dynamics_world(x, rv, ra)
up = self.Kp * GTinv * error
ui = self.Ki * GTinv * np.diag([1, 1, 0]) * gRb * gRp.T * self.integral
ud = self.Kd * GTinv * derivative
return uff + up + ui + ud
def forward_dynamics_world(self, pose, velocity, voltage):
"""
Perform the forward dynamics calculation in world space.
params:
pose: world-space pose vector [x; y; theta]
velocity: world-space velocity vector [x; y; theta]
voltage: wheel voltage vector [v1; v2; v3; v4]
"""
coriolis = np.asmatrix([[0, -1, 0], [1, 0, 0], [0, 0, 0]
]) * velocity[2, 0]
rotation_matrix = util.rotation_matrix(pose[2, 0])
velocity_body = rotation_matrix.T * velocity
acceleration_body = self.forward_dynamics_body(velocity_body, voltage)
return coriolis * velocity + rotation_matrix * acceleration_body
def generate(self, theta=None):
if theta is None:
theta = self.angle
x, y = self.cx, self.cy
min_x, max_x = - self.ratio * self.length, (1 - self.ratio) * self.length
min_y, max_y = - 0.5 * self.width, 0.5 * self.width
rot_matrix = rotation_matrix(theta)
pts = np.array([[min_x, max_y],
[max_x, max_y],
[max_x, min_y],
[min_x, min_y]])
pts = np.matmul(rot_matrix, pts.T).T
pts += np.array([x, y])
return pts
def compute_convex_hull(self, center_point, vector):
point_list = []
#1. find all faces that have same coordinates
counter = 0
face_list = []
for face, normal in zip(self.faces, self.normals):
point_projected_to_face, dist = util.project_point2plane(center_point, normal, face)
if abs(dist) < 0.001:
for i in range(3):
T = util.rotation_matrix(vector, np.array([0,0,-1]))
rotated_face = np.matmul(T[0:3, 0:3], face[i, :])
point_list.append(rotated_face[0:2])
face_list.append(counter)
counter += 1
q = ConvexHull(point_list)
q_points = []
for index in q.vertices:
q_points.append(q._points[index])
q_points.sort(key=helper.angle_2d)
return q_points
def update(self):
dt = 1. / 15
velocity = self.state.drive_speed * np.array(
[np.cos(self.heading), np.sin(self.heading)])
self.position = self.position + velocity * dt
self.heading = normalize_angle(self.heading +
self.state.drive_angle * dt)
self.time += dt
self.state.time = self.time
self.state.sim_position = self.position
self.state.sim_heading = self.heading
self.state.marvelmind_position = self.position + np.random.normal(
0, 0.015, 2)
self.state.realsense_position = self.rs_scale * rotation_matrix(
-self.initial_heading) @ (self.position - self.initial_position)
self.state.realsense_heading = normalize_angle(self.heading -
self.initial_heading)
self.state.rs_tracker_confidence = 2.0
self.state.rs_mapper_confidence = 2.0
def trajectories(self, N=100, dt=0.02):
perimeter = self.params['perimeter']
T = self.params["T"]
n = int(T / dt)
mu, sigma, b = [
self.params[i]
for i in ["mean_rotation", "std_dev_rotation", "std_dev_forward"]
]
rotation_velocities = torch.tensor(
np.random.normal(mu, sigma, size=(n, N))).float()
forward_velocities = torch.tensor(np.random.rayleigh(
b, size=(n, N))).float()
positions = ntorch.zeros((n, 2, N), names=("t", "ax", "sample"))
vs = torch.zeros((n, N))
angles = rotation_velocities
directions = torch.zeros((n, 2, N))
vs[0] = self.params["v0"]
theta = torch.rand(N) * 2 * np.pi
directions[0] = unit_vector(theta)
positions[{
"t": 0
}] = ntorch.tensor(self.scene.random(N), names=("sample", "ax"))
for i in range(1, n):
dist, phi = self.scene.closestWall(positions[{
"t": i - 1
}].values, directions[i - 1])
wall = (dist < perimeter) & (phi.abs() < np.pi / 2)
angle_correction = torch.where(
wall,
phi.sign() * (np.pi / 2 - phi.abs()), torch.zeros_like(phi))
angles[i] += angle_correction
vs[i] = torch.where(
wall,
(1 - self.params["velocity_reduction"]) * (vs[i - 1]),
forward_velocities[i],
)
positions[{
"t": i
}] = (positions[{
"t": i - 1
}] + directions[i - 1] * vs[i] * dt)
mat = rotation_matrix(angles[i] * dt)
directions[i] = torch.einsum("ijk,jk->ik", mat, directions[i - 1])
idx = np.round(np.linspace(
0, n - 2, self.params["trajectory_length"])).astype(int)
# idx = np.array(sorted(np.random.choice(np.arange(n), size=self.params["trajectory_length"], replace=False)))
dphis = ntorch.tensor(angles[idx] * dt, names=("t", "sample"))
velocities = ntorch.tensor(vs[idx], names=("t", "sample"))
vel = ntorch.stack((velocities, dphis.cos(), dphis.sin()), "input")
xs = ntorch.tensor(positions.values[idx], names=("t", "ax", "sample"))
# xs0 = positions[{'t': 0}]
xs0 = ntorch.tensor(self.scene.random(n=N), names=("sample", "ax"))
hd = torch.atan2(directions[:, 1], directions[:, 0])
hd0 = ntorch.tensor(hd[0][None], names=("hd", "sample"))
hd = ntorch.tensor(hd[idx + 1][None], names=("hd", "t", "sample"))
xs = xs.transpose('sample', 't', 'ax')
hd = hd.transpose('sample', 't', 'hd')
vel = vel.transpose('sample', 't', 'input')
xs0 = xs0.transpose('sample', 'ax')
hd0 = hd0.transpose('sample', 'hd')
return xs, hd, vel, xs0, hd0
def Petunin_ellipsoid(points):
"""
Строит эллипсоид Петунина
:param points: shape = (n_points, dim)
:return: c, radii, rot, alphas
c - центр эллипсоида
R - массив относительных расстояний до каждой точки входного массива (не отсортирован)
coef - коеффициенты для получения полуосей эллипсоидов; Полуоси эллипсоида, отвечающего i-ой
входной точке, являются R[i] * coef
rot - матрица поворота
alphas - shape = (dim-1,), углы поворотов, где alphas[i] - угол поворота в плоскости x_{i}, x_{i+1};
применять от 0 до dim-1
"""
x = np.asarray(points)
x = np.copy(x)
d = x.shape[1]
hull = ConvexHull(x)
v = hull.vertices
vl = v.shape[0]
dists = np.array([[np.linalg.norm(x[v[i]] - x[v[j]]), (v[i],v[j])]
for i in range(vl)
for j in range(i)])
#x[k] и x[l] лежат на диаметре
k,l = dists[np.argmax(dists[:,0]),1]
#выбираем точку с меньшей первой координатой, она перейдёт в начало координат
if x[k][0] <= x[l][0]:
x0 = x[k]
x1 = x[l]
else:
x0 = x[l]
x1 = x[k]
new_x = x - x0
direction = x1 - x0
#поворачиваем direction так, чтобы вектор лежал на первой координатной оси (с положительной стороны)
rot = np.eye(d)
alphas = np.zeros((d-1,))
for i in range(d - 1, 0, -1):
if direction[i] == 0:
continue
alpha = -np.arctan2(direction[i], direction[i-1])
alphas[i-1] = alpha
cur_rot = util.rotation_matrix(d,i-1, i, alpha)
direction = cur_rot.dot(direction)
rot = cur_rot @ rot
new_x = rot.dot(new_x.T).T
#строим параллелепипед наименьшего объема; ищем нижние и верхние границы области по всем координатам
lower = np.min(new_x[v],axis=0)
upper = np.max(new_x[v],axis=0)
#размеры сторон параллелепипеда
size = upper - lower
#смещаем параллелепипед и растягиваем его до гиперкуба со стороной size[0]
new_x -= lower
coef = size[0] / size
new_x *= coef
center = np.sum(new_x, axis=0) / new_x.shape[0]
dists_center = np.linalg.norm(new_x - center, axis = 1)
#dtype = [("dist", float), ("index", int)]
#to_sort = np.array([(dist, k) for k, dist in enumerate(dists_center)], dtype=dtype)
#sorted_dists = np.sort(to_sort, order="dist")
#order = sorted_dists["index"]
#R = sorted_dists["dist"]
R = dists_center
#R = np.max(dists_center)
#обратные превращения
c = center / coef
c += lower
c = rot.T.dot(c)
c += x0
#radii = R[-1] / coef
return c, R, 1 / coef, rot.T, -alphas
def convertToBody(self, x, vec):
return np.linalg.inv(util.rotation_matrix(x[2, 0])) * vec
def main():
lqr.init()
clock = pygame.time.Clock()
pos = np.asmatrix([0., 1., 0.]).T
vel = np.asmatrix([3., 0., 3.]).T
visualizer = vis.Visualizer()
fps = 60
dt = 1.0 / fps
t = 0.0
i = 0
maxon_motor = motor.Motor(
resistance=1.03,
torque_constant=0.0335,
speed_constant=0.0335,
rotor_inertia=135*1e-3*(1e-2**2))
gearbox = motor.Gearbox(gear_ratio=20.0 / 60.0, gear_inertia=0)
gear_motor = motor.GearMotor(maxon_motor, gearbox)
our_robot = robot.Robot(
robot_mass=6.5,
robot_radius=0.085,
gear_motor=gear_motor,
robot_inertia=6.5*0.085*0.085*0.5,
wheel_radius=0.029,
wheel_inertia=2.4e-5,
wheel_angles = np.deg2rad([45, 135, -135, -45]))
# wheel_angles=np.deg2rad([60, 129, -129, -60]))
controller = Controller(dt, our_robot)
while not visualizer.close:
visualizer.update_events()
v = 3
rx = np.asmatrix([np.sin(v * t), np.cos(v * t / 3), v * np.sin(t)]).T
rv = v * np.asmatrix([np.cos(v * t), -np.sin(v * t / 3) / 3, np.cos(t)]).T
ra = v ** 2 * np.asmatrix([-np.sin(v * t), -np.cos(v * t / 3) / 9, -np.sin(t) / v]).T
vel_b = np.linalg.inv(rotation_matrix(pos[2,0])) * vel
rv_b = np.linalg.inv(rotation_matrix(pos[2,0])) * rv
rx_b = np.linalg.inv(rotation_matrix(pos[2,0])) * rx
pos_b = np.linalg.inv(rotation_matrix(pos[2,0])) * pos
# u = controller.feedforward_control(pos, vel, rx, rv, ra)
state_vector = np.vstack((rx_b-pos_b,rv_b-vel_b))
u = lqr.controlLQR(state_vector) # +=
u = u[2:,0]
# u = controller.control(pos, vel, rx, rv, ra)
vdot_b = our_robot.forward_dynamics_body(vel_b, u)
vdot = our_robot.forward_dynamics_world(pos, vel, u)
# print(u - our_robot.inverse_dynamics_body(vel_b, vdot_b))
robot.sysID(u[0,0], u[1,0], u[2,0], u[3,0], vel_b[0,0], vel_b[1,0], vel_b[2,0], vdot_b[0,0], vdot_b[1,0], vdot_b[2,0])
visualizer.draw(pos, rx)
pos += dt * vel + 0.5 * vdot * dt ** 2
vel += dt * vdot
# robot_data_line = [vdot_b[0,0], vdot_b[1,0], vdot_b[2,0], vel_b[0,0], vel_b[1,0], vel_b[2,0], u[0,0], u[1,0], u[2,0], u[3,0]]
# robot_data.append(robot_data_line)
clock.tick(60)
t += dt
i += 1
if t > 20:
print("t = 20")
t = 0.0
i = 0
pos = np.asmatrix([0, 1, 0.]).T
vel = np.asmatrix([3, 0, 3.]).T
controller.reset()
robot.main()
def transformed_cb(self, plot=False, ax1=None, ax2=None):
"""
:param plot:
:param ax1:
:param ax2:
:return: the rotated bounding box of the combined body and the transformed center of the obstacle
"""
W = self.obstacle.W
# min_x, min_y, width, height
bbx = min(self.cb_box[:,0]), min(self.cb_box[:,1]), max(self.cb_box[:,0]) - min(self.cb_box[:,0]), \
max(self.cb_box[:,1]) - min(self.cb_box[:,1])
bounding_box_corner = np.array([[bbx[0], bbx[1]],
[bbx[0], bbx[1] + bbx[3]],
[bbx[0] + bbx[2], bbx[1] + bbx[3]],
[bbx[0] + bbx[2], bbx[1]]
])
self.cb_box_transformed = self.cb_box @ W.T
cb_box_t = np.concatenate([self.cb_box_transformed, self.cb_box_transformed[0].reshape(1, 2)], axis=0)
pg = Polygon(xy=cb_box_t, closed=True, facecolor="None", edgecolor='b', fill=False, linestyle='--')
robot_plot = self.robot_corner @ W.T
trans_robot = robot_plot
robot_plot = np.concatenate([robot_plot, robot_plot[0].reshape(1, 2)], axis=0)
robot_plot = Polygon(xy=robot_plot, closed=True, facecolor="blue", edgecolor='b', fill=True,
alpha=0.3, linestyle='-')
demo_center = np.array([self.demo_obst.cx, self.demo_obst.cy]) @ W.T
demo_obst = Obstacle(demo_center[0], demo_center[1],
width=self.obstacle.width, length=self.obstacle.length,
angle=self.obstacle.angle * 180 / np.pi,
pos_cov=W @ self.obstacle.pos_cov @ W.T)
transformed_bbx = bounding_box_corner @ W.T
plot_transformed_bbx = Polygon(xy=transformed_bbx, closed=True, facecolor="None", edgecolor='b',
linestyle='--')
# compute angle
line = np.array([1, 0]).reshape(1, 2)
line = line @ W.T
theta = np.arctan(line[0, 1] / line[0, 0])
rot_matrix = rotation_matrix(- theta)
# re-rotated combined body and its bounding box
rot_transform_cb = cb_box_t @ rot_matrix.T
rot_pg = Polygon(xy=rot_transform_cb, closed=True, facecolor="None", edgecolor='b', fill=False, linestyle='--')
rot_bbx = min(rot_transform_cb[:,0]), min(rot_transform_cb[:,1]), \
max(rot_transform_cb[:,0]) - min(rot_transform_cb[:,0]), \
max(rot_transform_cb[:,1]) - min(rot_transform_cb[:,1])
bounding_box_corner = np.array([[rot_bbx[0], rot_bbx[1]],
[rot_bbx[0], rot_bbx[1] + rot_bbx[3]],
[rot_bbx[0] + rot_bbx[2], rot_bbx[1] + rot_bbx[3]],
[rot_bbx[0] + rot_bbx[2], rot_bbx[1]]
])
plot_rotated_bbx = Polygon(xy=bounding_box_corner, closed=True, facecolor="None", edgecolor='orange',
linestyle='--')
demo_center = np.array([self.obstacle.cx, self.obstacle.cy]) @ W.T @ rot_matrix.T
demo_obst_rot = Obstacle(demo_center[0], demo_center[1],
width=self.obstacle.width, length=self.obstacle.length,
angle=self.obstacle.angle * 180 / np.pi,
pos_cov=W @ self.obstacle.pos_cov @ W.T)
# TODO: bouding box for the shape
if plot:
ax1.add_artist(pg)
demo_obst.plot(ax1, transformer=W)
ax1.add_artist(robot_plot)
# ax.add_artist(plot_transformed_bbx)
ax1.legend([robot_plot, pg], ["transformed robot", 'transformed cb polygon'])
ax2.add_artist(plot_rotated_bbx)
ax2.add_artist(rot_pg)
demo_obst_rot.plot(ax2, transformer=W)
ax2.legend([plot_rotated_bbx, rot_pg], ["bounding box", 'rotated cb polygon'])
# print("Collision probability upper bound: {}".format(compute_proba(bounding_box_corner, demo_center)))
return bounding_box_corner, demo_center
