def __str__(self):
return_str = ""
if self.name is not None:
return_str += "Name: '" + self.name + "'\n"
if self.reference is not None:
if self.reference.name is not None:
ref_name = self.reference.name
else:
ref_name = ""
return_str += "Reference: '" + ref_name + "'\n"
if self.t is not None:
return_str += "Time: " + self.val2str(self.t) + "\n"
state_dot = self.state_dot
return_str += "PWM: " + self.val2str(self.pwm) + "\n"
return_str += "Force (quad frame): " + self.val2str(self.force.xyz) + "\n"
return_str += "Force (local frame): " + self.val2str(self.force_local.xyz) + "\n"
return_str += "Torque (quad frame): " + self.val2str(self.torque.xyz) + "\n"
return_str += "Torque (Euler): " + self.val2str(tuple(np.squeeze(mechanics.ang_torque_to_att_torque(self.attitude, self.torque.array)))) + "\n"
return_str += "Position: " + self.val2str(self.xyz) + "\n"
return_str += "Attitude: " + self.val2str(self.attitude) + "\n"
return_str += "Position Vel: " + self.val2str(self.linear_vel.xyz) + "\n"
return_str += "Attitude Vel: " + self.val2str(self.attitude_vel) + "\n"
return_str += "Position Acc: " + self.val2str(tuple(np.squeeze(state_dot[6:9]))) + "\n"
return_str += "Attitude Acc: " + self.val2str(tuple(np.squeeze(state_dot[9:12]))) + "\n"
return return_str
def __str__(self):
return_str = ""
if self.name is not None:
return_str += "Name: '" + self.name + "'\n"
if self.reference is not None:
if self.reference.name is not None:
ref_name = self.reference.name
else:
ref_name = ""
return_str += "Reference: '" + ref_name + "'\n"
if self.t is not None:
return_str += "Time: " + self.val2str(self.t) + "\n"
state_dot = self.state_dot
return_str += "PWM: " + self.val2str(self.pwm) + "\n"
return_str += "Force (quad frame): " + self.val2str(
self.force.xyz) + "\n"
return_str += "Force (local frame): " + self.val2str(
self.force_local.xyz) + "\n"
return_str += "Torque (quad frame): " + self.val2str(
self.torque.xyz) + "\n"
return_str += "Torque (Euler): " + self.val2str(
tuple(
np.squeeze(
mechanics.ang_torque_to_att_torque(
self.attitude, self.torque.array)))) + "\n"
return_str += "Position: " + self.val2str(self.xyz) + "\n"
return_str += "Attitude: " + self.val2str(self.attitude) + "\n"
return_str += "Position Vel: " + self.val2str(
self.linear_vel.xyz) + "\n"
return_str += "Attitude Vel: " + self.val2str(self.attitude_vel) + "\n"
return_str += "Position Acc: " + self.val2str(
tuple(np.squeeze(state_dot[6:9]))) + "\n"
return_str += "Attitude Acc: " + self.val2str(
tuple(np.squeeze(state_dot[9:12]))) + "\n"
return return_str
def dynamics(self,
attitude=None,
linear_vel=None,
attitude_vel=None,
pwm=None,
force=None,
torque=None,
torque_attitude=None):
if attitude is None:
attitude = self.attitude
if linear_vel is None:
linear_vel = self.linear_vel
if attitude_vel is None:
attitude_vel = self.attitude_vel
if pwm is None:
if force is None:
force = self.force
if torque is None and torque_attitude is None:
torque_attitude = np.array([self.torque_attitude]).T
elif torque is not None and torque_attitude is None:
torque_attitude = mechanics.ang_torque_to_att_torque(
self.attitude, torque.array)
elif torque is not None and torque_attitude is not None:
raise Exception(
"Invalid argument. Either use torque or torque_attitude, but not both."
)
elif pwm is not None and force is None and torque is None:
(force, torque) = pwm_to_force_torque(np.array(pwm), self.L,
self.C_F, self.C_T)
# force_local = self.rotation * rigid.Vector(x=0, y=0, z=force, reference=self)
force = rigid.Vector(x=0, y=0, z=-force, reference=self)
torque = rigid.Vector(*torque, reference=self)
torque_attitude = mechanics.ang_torque_to_att_torque(
self.attitude, torque.array)
else:
raise Exception(
"Invalid arguments. Either use pwm or (force and torque), but not both."
)
(x_dot_trans, x_dot_rot, A_trans, B_trans, A_rot, B_rot) = \
quad_model.quad_dynamics(self.G, self.M,
self.I[0, 0], self.I[1, 1], self.I[2, 2],
linear_vel.x, linear_vel.y, linear_vel.z,
attitude[0], attitude[1], attitude[2],
attitude_vel[0], attitude_vel[1], attitude_vel[2],
-force.z,
torque_attitude[0, 0], torque_attitude[1, 0], torque_attitude[2, 0])
# Rearrange the ordering of the states from
# [pos pos_vel att att_vel] to [pos, att, pos_vel, att_vel]
i3 = np.identity(3)
z3 = np.zeros((3, 3))
# Note: For this similarity transform T_inv = T.T = T
T_inv = np.vstack([
np.hstack([i3, z3, z3, z3]),
np.hstack([z3, z3, i3, z3]),
np.hstack([z3, i3, z3, z3]),
np.hstack([z3, z3, z3, i3])
])
f_bar = np.vstack([x_dot_trans, x_dot_rot])
x_dot = np.dot(T_inv, f_bar)
A_left = np.vstack([A_trans, np.zeros((6, 6))])
A_right = np.vstack([np.zeros((6, 6)), A_rot])
A_bar = np.hstack([A_left, A_right])
A = np.dot(T_inv, np.dot(A_bar, T_inv))
B_top = np.hstack([B_trans, np.zeros((6, 3))])
B_bot = np.hstack([np.zeros((6, 1)), B_rot])
B_bar = np.vstack([B_top, B_bot])
B = np.dot(T_inv, B_bar)
# Temporary
# x_dot[0:3] = np.zeros((3,1))
if self.finite_state[0] == 'LANDED':
x_dot[2, 0] = max(x_dot[2, 0], 0)
return (x_dot, A, B, A_trans, B_trans, A_rot, B_rot)
def torque_attitude(self):
return tuple(
np.squeeze(
mechanics.ang_torque_to_att_torque(self.attitude,
self.torque.array)))
def dynamics(self,
attitude=None,
linear_vel=None,
attitude_vel=None,
pwm=None,
force=None,
torque=None,
torque_attitude=None):
if attitude is None:
attitude = self.attitude
if linear_vel is None:
linear_vel = self.linear_vel
if attitude_vel is None:
attitude_vel = self.attitude_vel
if pwm is None:
if force is None:
force = self.force
if torque is None and torque_attitude is None:
torque_attitude = np.array([self.torque_attitude]).T
elif torque is not None and torque_attitude is None:
torque_attitude = mechanics.ang_torque_to_att_torque(self.attitude, torque.array)
elif torque is not None and torque_attitude is not None:
raise Exception("Invalid argument. Either use torque or torque_attitude, but not both.")
elif pwm is not None and force is None and torque is None:
(force, torque) = pwm_to_force_torque(np.array(pwm),
self.L,
self.C_F,
self.C_T)
# force_local = self.rotation * rigid.Vector(x=0, y=0, z=force, reference=self)
force = rigid.Vector(x=0, y=0, z=-force, reference=self)
torque = rigid.Vector(*torque, reference=self)
torque_attitude = mechanics.ang_torque_to_att_torque(self.attitude, torque.array)
else:
raise Exception("Invalid arguments. Either use pwm or (force and torque), but not both.")
(x_dot_trans, x_dot_rot, A_trans, B_trans, A_rot, B_rot) = \
quad_model.quad_dynamics(self.G, self.M,
self.I[0, 0], self.I[1, 1], self.I[2, 2],
linear_vel.x, linear_vel.y, linear_vel.z,
attitude[0], attitude[1], attitude[2],
attitude_vel[0], attitude_vel[1], attitude_vel[2],
-force.z,
torque_attitude[0, 0], torque_attitude[1, 0], torque_attitude[2, 0])
# Rearrange the ordering of the states from
# [pos pos_vel att att_vel] to [pos, att, pos_vel, att_vel]
i3 = np.identity(3)
z3 = np.zeros((3, 3))
# Note: For this similarity transform T_inv = T.T = T
T_inv = np.vstack([np.hstack([i3, z3, z3, z3]),
np.hstack([z3, z3, i3, z3]),
np.hstack([z3, i3, z3, z3]),
np.hstack([z3, z3, z3, i3])])
f_bar = np.vstack([x_dot_trans, x_dot_rot])
x_dot = np.dot(T_inv, f_bar)
A_left = np.vstack([A_trans, np.zeros((6, 6))])
A_right = np.vstack([np.zeros((6, 6)), A_rot])
A_bar = np.hstack([A_left, A_right])
A = np.dot(T_inv, np.dot(A_bar, T_inv))
B_top = np.hstack([B_trans, np.zeros((6, 3))])
B_bot = np.hstack([np.zeros((6, 1)), B_rot])
B_bar = np.vstack([B_top, B_bot])
B = np.dot(T_inv, B_bar)
# Temporary
# x_dot[0:3] = np.zeros((3,1))
if self.finite_state[0] == 'LANDED':
x_dot[2, 0] = max(x_dot[2, 0], 0)
return (x_dot, A, B, A_trans, B_trans, A_rot, B_rot)
def torque_attitude(self):
return tuple(np.squeeze(mechanics.ang_torque_to_att_torque(self.attitude, self.torque.array)))
