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)
