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 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())