def to_tree(cls, node: Rotation, ctx): """ Convert an instance of the 'Dimension' type into YAML representations. Parameters ---------- node : Instance of the 'Dimension' type to be serialized. ctx : An instance of the 'AsdfFile' object that is being written out. Returns ------- A basic YAML type ('dict', 'list', 'str', 'int', 'float', or 'complex') representing the properties of the 'Dimension' type to be serialized. """ tree = {} if not hasattr(node, "wx_meta"): # default to quaternion representation tree["quaternions"] = node.as_quat() elif node.wx_meta["constructor"] == "from_quat": tree["quaternions"] = node.as_quat() elif node.wx_meta["constructor"] == "from_matrix": tree["matrix"] = node.as_matrix() elif node.wx_meta["constructor"] == "from_rotvec": tree["rotvec"] = node.as_rotvec() elif node.wx_meta["constructor"] == "from_euler": seq_str = node.wx_meta["seq"] if not len(seq_str) == 3: if all([c in "xyz" for c in seq_str]): seq_str = seq_str + "".join( [c for c in "xyz" if c not in seq_str]) elif all([c in "XYZ" for c in seq_str]): seq_str = seq_str + "".join( [c for c in "XYZ" if c not in seq_str]) else: # pragma: no cover raise ValueError( "Mix of intrinsic and extrinsic euler angles.") angles = node.as_euler(seq_str, degrees=node.wx_meta["degrees"]) angles = np.squeeze(angles[..., :len(node.wx_meta["seq"])]) if node.wx_meta["degrees"]: angles = Q_(angles, "degree") else: angles = Q_(angles, "rad") tree["sequence"] = node.wx_meta["seq"] tree["angles"] = angles else: # pragma: no cover raise NotImplementedError("unknown or missing constructor") return tree
def from_rotation(rotation: Rotation, frame: Frame) -> Orientation: """ :param rotation: Scipy Rotation object :param frame: Frame of orientation :return: Orientation object """ return Orientation(*rotation.as_quat(), frame=frame) # type: ignore
def scipy_rot_to_ros_odom(rotation: Rotation): odom_out = Odometry() q_rotation = rotation.as_quat() odom_out.pose.pose.orientation.x = q_rotation[0] odom_out.pose.pose.orientation.y = q_rotation[1] odom_out.pose.pose.orientation.z = q_rotation[2] odom_out.pose.pose.orientation.w = q_rotation[3] return odom_out
def dt(cls, V: V3, dVdt: V3, theta: R, omega: V3, domegadt: V3) \ -> np.ndarray: o1 = omega[0] o2 = omega[1] o3 = omega[2] mat = np.array([[0, -o1, -o2, -o3], [o1, 0, o3, -o2], [o2, -o3, 0, o1], [o3, o2, -o1, 0]]) dXdt = V dthetadt = np.dot(mat, theta.as_quat()) return np.concatenate([dXdt, dVdt, dthetadt, domegadt])
def to_yaml_tree(self, obj: Rotation, tag: str, ctx) -> dict: """Convert to python dict.""" tree = {} if not hasattr(obj, ROT_META): # default to quaternion representation tree["quaternions"] = obj.as_quat() elif getattr(obj, ROT_META)["constructor"] == "from_quat": tree["quaternions"] = obj.as_quat() elif getattr(obj, ROT_META)["constructor"] == "from_matrix": tree["matrix"] = obj.as_matrix() elif getattr(obj, ROT_META)["constructor"] == "from_rotvec": tree["rotvec"] = obj.as_rotvec() elif getattr(obj, ROT_META)["constructor"] == "from_euler": seq_str = getattr(obj, ROT_META)["seq"] if not len(seq_str) == 3: if all(c in "xyz" for c in seq_str): seq_str = seq_str + "".join( [c for c in "xyz" if c not in seq_str]) elif all(c in "XYZ" for c in seq_str): seq_str = seq_str + "".join( [c for c in "XYZ" if c not in seq_str]) else: # pragma: no cover raise ValueError( "Mix of intrinsic and extrinsic euler angles.") angles = obj.as_euler(seq_str, degrees=getattr(obj, ROT_META)["degrees"]) angles = np.squeeze( angles[..., :len(getattr(obj, ROT_META)["seq"])]) if getattr(obj, ROT_META)["degrees"]: angles = Q_(angles, "degree") else: angles = Q_(angles, "rad") tree["sequence"] = getattr(obj, ROT_META)["seq"] tree["angles"] = angles else: # pragma: no cover raise NotImplementedError("unknown or missing constructor") return tree
def state_deriv(t, x): c = self.a * self.alpha ang_vel_mat = np.array([ [ 0, x[6], -x[5], x[4]], [-x[6], 0, x[4], x[5]], [ x[5], -x[4], 0, x[6]], [-x[4], -x[5], -x[6], 0] ]) orientation = Rotation(x[7:11]) net_force = orientation.apply([0, 0, self.alpha * np.sum(x[0:4])]) return np.array([ 0, 0, 0, 0, # Motor torque derivatives (x[1] - x[3])*c/self.principal_moments[0], # Angular accel x (x[2] - x[0])*c/self.principal_moments[1], # Angular accel y (x[0] + x[2] - x[1] - x[3])/self.principal_moments[2] # Angular accel z ] + list(0.5 * ang_vel_mat @ orientation.as_quat()) # Quaternion derivative + list([0, 0, -9.8] + net_force/self.mass) # Acceleration )
def quaternionFromScipy(quaternion : Rot, quaternionpb = Quaterniond_pb2.Quaterniond()): return quaternionFromNumpy(quaternion.as_quat(), quaternionpb = quaternionpb)
for p in label_tag.other_agent_trajectories[-1].poses ], dtype=np.float32) quatsreconstructed = np.array([[ p.rotation.x, p.rotation.y, p.rotation.z, p.rotation.w ] for p in label_tag.other_agent_trajectories[-1].poses], dtype=np.float32) velreconstructed = np.array( [[v.vector.x, v.vector.y, v.vector.z] for v in label_tag.other_agent_trajectories[-1]. linear_velocities], dtype=np.float32) assert (np.allclose(positions_samp.astype(np.float32), posreconstructed, atol=1E-6)) assert (np.allclose(Rot.as_quat(rotations_samp).astype( np.float32), quatsreconstructed, atol=1E-6)) assert (np.allclose(velocities_samp.astype(np.float32), velreconstructed, atol=1E-6)) if debug and (not time_trial) and (not len( label_tag.other_agent_trajectories) == 0) and idx % 30 == 0: image_file = "image_%d.jpg" % idx print("Found a match for %s. Metadata:" % (image_file, )) print("Match positions: " + str(match_positions)) print("Closest packet index: " + str(istart)) print("Car indices: " + str(label_tag.trajectory_car_indices)) # imagein = cv2.imread(os.path.join(image_folder,image_file)) # cv2.imshow("Image",imagein) # cv2.waitKey(0)
def set_orientation(self, rotation: Rotation): self.uniforms["orientation"][:] = rotation.as_quat() return self
def update(self, t, state, flat_output): """ This function receives the current time, true state, and desired flat outputs. It returns the command inputs. Inputs: t, present time in seconds state, a dict describing the present state with keys # State x, position, m v, linear velocity, m/s q, quaternion [i,j,k,w] w, angular velocity, rad/s flat_output, a dict describing the present desired flat outputs with keys # Desired x, position, m x_dot, velocity, m/s x_ddot, acceleration, m/s**2 x_dddot, jerk, m/s**3 x_ddddot, snap, m/s**4 yaw, yaw angle, rad yaw_dot, yaw rate, rad/s Outputs: control_input, a dict describing the present computed control inputs with keys # Output cmd_motor_speeds, rad/s cmd_thrust, N (for debugging and laboratory; not used by simulator) cmd_moment, N*m (for debugging; not used by simulator) cmd_q, quaternion [i,j,k,w] (for laboratory; not used by simulator) """ cmd_motor_speeds = np.zeros((4,)) cmd_thrust = 0 cmd_moment = np.zeros((3,)) cmd_q = np.zeros((4,)) # STUDENT CODE HERE -------------------------------------------------------------------------------------------- # define error error_pos = state.get('x') - flat_output.get('x') error_vel = state.get('v') - flat_output.get('x_dot') error_vel = np.array(error_vel).reshape(3, 1) error_pos = np.array(error_pos).reshape(3, 1) # Equation 26 rdd_des = np.array(flat_output.get('x_ddot')).reshape(3, 1) - np.matmul(self.Kd, error_vel) - np.matmul(self.Kp, error_pos) # Equation 28 F_des = (self.mass * rdd_des) + np.array([0, 0, self.mass * self.g]).reshape(3, 1) # (3 * 1) # Find Rotation matrix R = Rotation.as_matrix(Rotation.from_quat(state.get('q'))) # Quaternions to Rotation Matrix # print(R.shape) # Equation 29, Find u1 b3 = R[0:3, 2:3] # print(b3) u1 = np.matmul(b3.T, F_des) # u1[0,0] to access value # print(np.transpose(b3)) # ----------------------- the following is to find u2 --------------------------------------------------------- # Equation 30 b3_des = F_des / np.linalg.norm(F_des) # 3 * 1 a_Psi = np.array([np.cos(flat_output.get('yaw')), np.sin(flat_output.get('yaw')), 0]).reshape(3, 1) # 3 * 1 b2_des = np.cross(b3_des, a_Psi, axis=0) / np.linalg.norm(np.cross(b3_des, a_Psi, axis=0)) b1_des = np.cross(b2_des, b3_des, axis=0) # Equation 33 R_des = np.hstack((b1_des, b2_des, b3_des)) # print(R_des) # Equation 34 # R_temp = 0.5 * (np.matmul(np.transpose(R_des), R) - np.matmul(np.transpose(R), R_des)) temp = R_des.T @ R - R.T @ R_des R_temp = 0.5 * temp # orientation error vector e_R = 0.5 * np.array([-R_temp[1, 2], R_temp[0, 2], -R_temp[0, 1]]).reshape(3, 1) # Equation 35 u2 = self.inertia @ (-self.K_R @ e_R - self.K_w @ (np.array(state.get('w')).reshape(3, 1))) gama = self.k_drag / self.k_thrust Len = self.arm_length cof_temp = np.array( [1, 1, 1, 1, 0, Len, 0, -Len, -Len, 0, Len, 0, gama, -gama, gama, -gama]).reshape(4, 4) u = np.vstack((u1, u2)) F_i = np.matmul(np.linalg.inv(cof_temp), u) for i in range(4): if F_i[i, 0] < 0: F_i[i, 0] = 0 cmd_motor_speeds[i] = self.rotor_speed_max cmd_motor_speeds[i] = np.sqrt(F_i[i, 0] / self.k_thrust) if cmd_motor_speeds[i] > self.rotor_speed_max: cmd_motor_speeds[i] = self.rotor_speed_max cmd_thrust = u1[0, 0] cmd_moment[0] = u2[0, 0] cmd_moment[1] = u2[1, 0] cmd_moment[2] = u2[2, 0] cmd_q = Rotation.as_quat(Rotation.from_matrix(R_des)) control_input = {'cmd_motor_speeds': cmd_motor_speeds, 'cmd_thrust': cmd_thrust, 'cmd_moment': cmd_moment, 'cmd_q': cmd_q} return control_input
def canonicalize_rotation(self, rotation: Rotation): q_rotation = rotation.as_quat() for i in q_rotation: if i < 0: return Rotation.from_quat(-1 * q_rotation) return rotation
def rotation_2_quaternion(rot): r = R.from_matrix(rot) q_array = R.as_quat(r) return q_array # retrun numpy array
fx = interpolate.interp1d(time_array, data[:,1], fill_value="extrapolate") fy = interpolate.interp1d(time_array, data[:,2], fill_value="extrapolate") fz = interpolate.interp1d(time_array, data[:,3], fill_value="extrapolate") pos_new = np.zeros((t_new.shape[0], 3)) pos_new[:,0] = fx(t_new) pos_new[:,1] = fy(t_new) pos_new[:,2] = fz(t_new) att_array = np.empty((0,4)) for i in range(data.shape[0]): att_array = np.append(att_array, quat_wx2xw(Euler2quat(data[i,7:10]))[np.newaxis,:], axis=0) key_rots = R.from_quat(att_array) slerp = Slerp(time_array, key_rots) interp_rots = slerp(t_new) att_new_array = R.as_quat(interp_rots) att_new = np.zeros_like(att_new_array) for i in range(t_new.shape[0]): att_new[i,:] = quat_xw2wx(att_new_array[i,:]) # Progress Bar data_len = t_new.shape[0] print("data length: {}".format(data_len)) bar_iter = 0 bar_max = data_len bar = Bar('Processing Video', max=bar_max, suffix='%(percent)d%%') bar_step = np.around(data_len/bar_max) # Request Image and Save for i in range(data_len): filename = "{}/{}.png".format(save_path,i)