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