def imageDataCallback(self, msg):
trans_msg = msg.relative_pose.position # NED translation
rot_msg = msg.relative_pose.orientation # orientation in quaternion
# these come in as body to body, need camera to camera
translation = np.array([trans_msg.x, trans_msg.y, trans_msg.z])
rotation = Quaternion(
np.array([rot_msg.w, rot_msg.x, rot_msg.y, rot_msg.z]))
# convert from body to camera frame
translation = self.R_bc @ translation
features_next = np.array(msg.features_next).reshape(-1, 2).T
features_prev = np.array(msg.features_prev).reshape(-1, 2).T
#dt = msg.dt
if np.linalg.norm(translation) > 0.05:
# points in the next camera frame
p_next = vt.triangulate_points(features_prev, features_next,
translation, rotation,
self.triang_method, self.Kc_inv)
p_inertial = self.current_att.R.T @ self.R_bc.T @ p_next + self.current_pos.reshape(
(3, 1))
self.publish_pointCloud(p_inertial.T)
def stateCallback(self, msg):
position = msg.pose.pose.position # NED position in i frame
orient = msg.pose.pose.orientation # orientation in quaternion
self.current_pos = np.array([position.x, position.y, position.z])
self.current_att = Quaternion(
np.array([orient.w, orient.x, orient.y, orient.z]))
def world_frame(self):
""" position returns a 3x6 matrix
where row is [x, y, z] column is m1 m2 m3 m4 origin h
"""
origin = self.state[0:3]
quat = Quaternion(self.state[6:10])
rot = quat.as_rotation_matrix()
wHb = np.r_[np.c_[rot, origin], np.array([[0, 0, 0, 1]])]
quadBodyFrame = params.body_frame.T
quadWorldFrame = wHb.dot(quadBodyFrame)
world_frame = quadWorldFrame[0:3]
return world_frame
def state_dot(self, state, t, F, M, Fi_s):
x, y, z, xdot, ydot, zdot, qw, qx, qy, qz, p, q, r = state
quat = np.array([qw,qx,qy,qz])
bRw = Quaternion(quat).as_rotation_matrix() # world to body rotation matrix
wRb = bRw.T # orthogonal matrix inverse = transpose
# acceleration - Newton's second law of motion
#accel = (1.0 / params.mass) * (wRb.dot(np.array([[0, 0, F]]).T) - np.array([[0, 0, params.mass * params.g]]).T)
rotor_drag = True
if(rotor_drag):
v = np.array([[xdot],[ydot],[zdot]])
vw = np.array([[0],[0],[0]]) # wind velocity
omega = np.array([[p],[q],[r]])
Fa = self.total_rotor_drag(v, vw, omega, Fi_s, wRb) # total rotor drag in body frame
else:
Fa = np.array([[0],[0],[0]])
accel = -params.g*params.e3 + (F/params.mass)*np.dot(wRb,params.e3) + np.dot(wRb, Fa)
# angular velocity - using quternion
# http://www.euclideanspace.com/physics/kinematics/angularvelocity/
K_quat = 2.0; # this enforces the magnitude 1 constraint for the quaternion
quaterror = 1.0 - (qw**2 + qx**2 + qy**2 + qz**2)
qdot = (-1.0/2) * np.array([[0, -p, -q, -r],
[p, 0, -r, q],
[q, r, 0, -p],
[r, -q, p, 0]]).dot(quat) + K_quat * quaterror * quat;
# angular acceleration - Euler's equation of motion
# https://en.wikipedia.org/wiki/Euler%27s_equations_(rigid_body_dynamics)
omega = np.array([p,q,r])
pqrdot = params.invI.dot( M.flatten() - np.cross(omega, params.I.dot(omega)) )
state_dot = np.zeros(13)
state_dot[0] = xdot
state_dot[1] = ydot
state_dot[2] = zdot
state_dot[3] = accel[0][0]
state_dot[4] = accel[1][0]
state_dot[5] = accel[2][0]
state_dot[6] = qdot[0]
state_dot[7] = qdot[1]
state_dot[8] = qdot[2]
state_dot[9] = qdot[3]
state_dot[10] = pqrdot[0]
state_dot[11] = pqrdot[1]
state_dot[12] = pqrdot[2]
return state_dot
def update_imu(self, gyroscope, accelerometer):
"""
Perform one update step with data from a IMU sensor array
:param gyroscope: A three-element array containing the gyroscope data in radians per second.
:param accelerometer: A three-element array containing the accelerometer data. Can be any unit since a normalized value is used.
"""
q = self.quaternion
gyroscope = np.array(gyroscope, dtype=float).flatten()
accelerometer = np.array(accelerometer, dtype=float).flatten()
# Normalise accelerometer measurement
if norm(accelerometer) is 0:
warnings.warn("accelerometer is zero")
return
accelerometer /= norm(accelerometer)
# Gradient descent algorithm corrective step
f = np.array([
2 * (q[1] * q[3] - q[0] * q[2]) - accelerometer[0],
2 * (q[0] * q[1] + q[2] * q[3]) - accelerometer[1],
2 * (0.5 - q[1]**2 - q[2]**2) - accelerometer[2]
])
j = np.array([[-2 * q[2], 2 * q[3], -2 * q[0], 2 * q[1]],
[2 * q[1], 2 * q[0], 2 * q[3], 2 * q[2]],
[0, -4 * q[1], -4 * q[2], 0]])
step = j.T.dot(f)
step /= norm(step) # normalise step magnitude
# Compute rate of change of quaternion
qdot = (q * Quaternion(0, gyroscope[0], gyroscope[1],
gyroscope[2])) * 0.5 - self.beta * step.T
# Integrate to yield quaternion
q += qdot * self.samplePeriod
self.quaternion = Quaternion(q / norm(q)) # normalise quaternion
def __init__(self, info):
"""
Inputs:
info json item load from character file
"""
self._pos = np.array(
[info["AttachX"], info["AttachY"], info["AttachZ"]])
self._name = info["Name"]
self._parent_id = info["Parent"]
self._parent = None
self._type_name = info['Type']
self._type = NAME2TYPE[info['Type']]
self._dof = Dof[self._type]
self._expdof = ExpDof[self._type]
self._w = info["DiffWeight"]
self._is_end_effector = info["IsEndEffector"]
if self._type is JointType.REVOLUTE:
self._torque_lim = [info["TorqueLim"]]
elif self._type is JointType.SPHERE:
if "TorqueLimX" not in info:
self._torque_lim = [info["TorqueLim"]] * 4
else:
self._torque_lim = [
info["TorqueLimX"], info["TorqueLimY"], info["TorqueLimZ"],
0
]
else:
self._torque_lim = [0] * 4
if self._type is JointType.REVOLUTE:
self.limlow = np.array([info["LimLow0"]])
self.limhigh = np.array([info["LimHigh0"]])
elif self._type is JointType.SPHERE:
self.limlow = -3.14 * np.ones(3)
self.limhigh = 3.14 * np.ones(3)
else:
self.limlow = np.array([])
self.limhigh = np.array([])
self.pose = np.zeros(self._dof)
self.pose2quat = POSE2QUAT[self._type]
self.quat = Quaternion(1, 0, 0, 0)
self.vel = np.zeros(self._dof)
self.vel2omg = VEL2OMG[self._type]
self.omg = np.zeros(3)
def state_dot(self, state, t, F, M):
x, y, z, xdot, ydot, zdot, qw, qx, qy, qz, p, q, r = state
quat = np.array([qw, qx, qy, qz])
bRw = Quaternion(
quat).as_rotation_matrix() # world to body rotation matrix
self.R = bRw
wRb = bRw.T # orthogonal matrix inverse = transpose
# acceleration - Newton's second law of motion
accel = 1.0 / params.mass * (wRb.dot(np.array(
[[0, 0, F]]).T) - np.array([[0, 0, params.mass * params.g]]).T)
# angular velocity - using quternion
# http://www.euclideanspace.com/physics/kinematics/angularvelocity/
K_quat = 2.0
# this enforces the magnitude 1 constraint for the quaternion
quaterror = 1.0 - (qw**2 + qx**2 + qy**2 + qz**2)
qdot = (-1.0 / 2) * np.array([[0, -p, -q, -r], [p, 0, -r, q],
[q, r, 0, -p], [r, -q, p, 0]
]).dot(quat) + K_quat * quaterror * quat
# angular acceleration - Euler's equation of motion
# https://en.wikipedia.org/wiki/Euler%27s_equations_(rigid_body_dynamics)
omega = np.array([p, q, r])
pqrdot = params.invI.dot(M.flatten() -
np.cross(omega, params.I.dot(omega)))
state_dot = np.zeros(13)
state_dot[0] = xdot
state_dot[1] = ydot
state_dot[2] = zdot
state_dot[3] = accel[0]
state_dot[4] = accel[1]
state_dot[5] = accel[2]
state_dot[6] = qdot[0]
state_dot[7] = qdot[1]
state_dot[8] = qdot[2]
state_dot[9] = qdot[3]
state_dot[10] = pqrdot[0]
state_dot[11] = pqrdot[1]
state_dot[12] = pqrdot[2]
return state_dot
class MadgwickAHRS:
samplePeriod = 0.01
quaternion = Quaternion(1, 0, 0, 0)
beta = 1
def __init__(self, sampleperiod=None, quaternion=None, beta=None):
"""
Initialize the class with the given parameters.
:param sampleperiod: The sample period
:param quaternion: Initial quaternion
:param beta: Algorithm gain beta
:return:
"""
if sampleperiod is not None:
self.samplePeriod = sampleperiod
if quaternion is not None:
self.quaternion = quaternion
if beta is not None:
self.beta = beta
def update(self, gyroscope, accelerometer, magnetometer):
"""
Perform one update step with data from a AHRS sensor array
:param gyroscope: A three-element array containing the gyroscope data in radians per second.
:param accelerometer: A three-element array containing the accelerometer data. Can be any unit since a normalized value is used.
:param magnetometer: A three-element array containing the magnetometer data. Can be any unit since a normalized value is used.
:return:
"""
q = self.quaternion
gyroscope = np.array(gyroscope, dtype=float).flatten()
accelerometer = np.array(accelerometer, dtype=float).flatten()
magnetometer = np.array(magnetometer, dtype=float).flatten()
# Normalise accelerometer measurement
if norm(accelerometer) is 0:
warnings.warn("accelerometer is zero")
return
accelerometer /= norm(accelerometer)
# Normalise magnetometer measurement
if norm(magnetometer) is 0:
warnings.warn("magnetometer is zero")
return
magnetometer /= norm(magnetometer)
h = q * (Quaternion(0, magnetometer[0], magnetometer[1],
magnetometer[2]) * q.conj())
b = np.array([0, norm(h[1:3]), 0, h[3]])
# Gradient descent algorithm corrective step
f = np.array([
2 * (q[1] * q[3] - q[0] * q[2]) - accelerometer[0],
2 * (q[0] * q[1] + q[2] * q[3]) - accelerometer[1],
2 * (0.5 - q[1]**2 - q[2]**2) - accelerometer[2],
2 * b[1] * (0.5 - q[2]**2 - q[3]**2) + 2 * b[3] *
(q[1] * q[3] - q[0] * q[2]) - magnetometer[0],
2 * b[1] * (q[1] * q[2] - q[0] * q[3]) + 2 * b[3] *
(q[0] * q[1] + q[2] * q[3]) - magnetometer[1],
2 * b[1] * (q[0] * q[2] + q[1] * q[3]) + 2 * b[3] *
(0.5 - q[1]**2 - q[2]**2) - magnetometer[2]
])
j = np.array([[-2 * q[2], 2 * q[3], -2 * q[0], 2 * q[1]],
[2 * q[1], 2 * q[0], 2 * q[3], 2 * q[2]],
[0, -4 * q[1], -4 * q[2], 0],
[
-2 * b[3] * q[2], 2 * b[3] * q[3],
-4 * b[1] * q[2] - 2 * b[3] * q[0],
-4 * b[1] * q[3] + 2 * b[3] * q[1]
],
[
-2 * b[1] * q[3] + 2 * b[3] * q[1],
2 * b[1] * q[2] + 2 * b[3] * q[0],
2 * b[1] * q[1] + 2 * b[3] * q[3],
-2 * b[1] * q[0] + 2 * b[3] * q[2]
],
[
2 * b[1] * q[2], 2 * b[1] * q[3] - 4 * b[3] * q[1],
2 * b[1] * q[0] - 4 * b[3] * q[2], 2 * b[1] * q[1]
]])
step = j.T.dot(f)
step /= norm(step) # normalise step magnitude
# Compute rate of change of quaternion
qdot = (q * Quaternion(0, gyroscope[0], gyroscope[1],
gyroscope[2])) * 0.5 - self.beta * step.T
# Integrate to yield quaternion
q += qdot * self.samplePeriod
self.quaternion = Quaternion(q / norm(q)) # normalise quaternion
def update_imu(self, gyroscope, accelerometer):
"""
Perform one update step with data from a IMU sensor array
:param gyroscope: A three-element array containing the gyroscope data in radians per second.
:param accelerometer: A three-element array containing the accelerometer data. Can be any unit since a normalized value is used.
"""
q = self.quaternion
gyroscope = np.array(gyroscope, dtype=float).flatten()
accelerometer = np.array(accelerometer, dtype=float).flatten()
# Normalise accelerometer measurement
if norm(accelerometer) is 0:
warnings.warn("accelerometer is zero")
return
accelerometer /= norm(accelerometer)
# Gradient descent algorithm corrective step
f = np.array([
2 * (q[1] * q[3] - q[0] * q[2]) - accelerometer[0],
2 * (q[0] * q[1] + q[2] * q[3]) - accelerometer[1],
2 * (0.5 - q[1]**2 - q[2]**2) - accelerometer[2]
])
j = np.array([[-2 * q[2], 2 * q[3], -2 * q[0], 2 * q[1]],
[2 * q[1], 2 * q[0], 2 * q[3], 2 * q[2]],
[0, -4 * q[1], -4 * q[2], 0]])
step = j.T.dot(f)
step /= norm(step) # normalise step magnitude
# Compute rate of change of quaternion
qdot = (q * Quaternion(0, gyroscope[0], gyroscope[1],
gyroscope[2])) * 0.5 - self.beta * step.T
# Integrate to yield quaternion
q += qdot * self.samplePeriod
self.quaternion = Quaternion(q / norm(q)) # normalise quaternion
def attitude(self):
rot = Quaternion(self.state[6:10]).as_rotation_matrix()
return RotToRPY(rot)
def test_from_v_theta(self):
x, y, z = np.eye(3)
q = Quaternion.from_v_theta(x, np.pi / 2)
expected = [np.cos(np.pi / 4), np.sin(np.pi / 4), 0, 0]
self.assertEqual(q, Quaternion(expected))
JointType.BASE: 0,
JointType.FIXED: 0,
JointType.REVOLUTE: 1,
JointType.SPHERE: 3,
}
NAME2TYPE = {
"none": JointType.BASE,
"fixed": JointType.FIXED,
"revolute": JointType.REVOLUTE,
"spherical": JointType.SPHERE,
}
POSE2QUAT = {
JointType.BASE:
lambda x: Quaternion(x[0], x[1], x[2], x[3]),
JointType.FIXED:
lambda x: Quaternion(1, 0, 0, 0),
JointType.REVOLUTE:
lambda x: Quaternion(math.cos(x / 2), 0.0, 0.0, math.sin(x / 2)),
JointType.SPHERE:
lambda x: Quaternion(x[0], x[1], x[2], x[3]),
}
VEL2OMG = {
JointType.BASE: lambda x: np.array([x[0], x[1], x[2]]),
JointType.FIXED: lambda x: np.array([0, 0, 0]),
JointType.REVOLUTE: lambda x: np.array([0, 0, x[0]]),
JointType.SPHERE: lambda x: np.array([x[0], x[1], x[2]]),
}