def create_nodes_by_motion_vectors(self, skeleton, motion_vectors):
nodes = dict()
# set default root pos and orientation
default_pos = self.default_pos
default_quat = motion_vectors[0].frames[0][3:7]
bvh_id = 0
frame_id = 0
for mv in motion_vectors:
for idx, frame in enumerate(mv.frames[1:]):
node = frame
prev_node = mv.frames[idx - 1] # used for velocity
node_velocity = self.compute_velocity(skeleton, node,
prev_node)
[leftVel, rightVel, leftFoot, rightFoot
] = self.compute_LR_foot_velocity(skeleton, node, prev_node)
root_pos = node[:3] - prev_node[:3]
root_quat = quaternion_multiply(
quaternion_inverse(prev_node[3:7]), node[3:7])
root_transform = quaternion_matrix(root_quat)
root_transform[:3, 3] = root_pos
# align the pos and quat of all frames to default
node[:3] = default_pos
node[3:7] = default_quat
nodes[frame_id] = MGNode(
node, node_velocity, bvh_id,
[leftVel, rightVel, leftFoot, rightFoot])
frame_id += 1
bvh_id += 1
return nodes
def transform_to_object_part_frame(self, part_frame):
assert not self.in_part_frame
trans = part_frame[:3]
rot_quat = part_frame[3:]
rot_quat_mat = quaternion_matrix(rot_quat)
rot_quat_inv = quaternion_inverse(rot_quat)
rot_quat_inv_mat = quaternion_matrix(rot_quat_inv)
# translate and rotate
new_waypoint_array = []
for i in range(len(self.waypoint_array)):
waypoint = self.waypoint_array[i]
way_pos, way_ori = pose_to_arrays(waypoint)
way_pos_trans = np.asarray(way_pos) - np.asarray(trans)
way_pos_rotated = np.dot(rot_quat_inv_mat,
pos_array_to_column_vector(way_pos_trans))
way_ori_rotated = quaternion_multiply(rot_quat_inv, way_ori)
way_pos_rotated = np.transpose(way_pos_rotated)[0][:-1]
assert way_ori_rotated.ndim == 1 and way_pos_rotated.ndim == 1
assert way_ori_rotated.shape[0] == 4 and way_pos_rotated.shape[
0] == 3
new_waypoint = array_to_pose(
np.hstack((way_pos_rotated, way_ori_rotated)))
new_waypoint_array.append(new_waypoint)
self.waypoint_array = new_waypoint_array
self.update_interpolation()
self.in_part_frame = False
def apply_axial_constraint2(q, ref_q, axis, min_angle, max_angle):
""" get delta orientation
do axis decomposition
restrict angle between min and max
"""
q = normalize(q)
inv_ref_q = quaternion_inverse(ref_q)
inv_ref_q = normalize(inv_ref_q)
delta_q = quaternion_multiply(inv_ref_q, q)
delta_q = normalize(delta_q)
swing_q, twist_q = swing_twist_decomposition(delta_q, axis)
swing_q = normalize(swing_q)
twist_q = normalize(twist_q)
v, a = quaternion_to_axis_angle(twist_q)
print("apply axial", q)
v2, a2 = quaternion_to_axis_angle(swing_q)
sign = 1
if np.sum(v) < 0:
sign = -1
a *= sign
print("twist swing", v, np.degrees(a), v2, np.degrees(a2))
print("boundary", min_angle, max_angle)
a = max(a, min_angle)
a = min(a, max_angle)
new_twist_q = quaternion_about_axis(a, axis)
new_twist_q = normalize(new_twist_q)
delta_q = quaternion_multiply(swing_q, new_twist_q)
delta_q = normalize(delta_q)
q = quaternion_multiply(ref_q, delta_q)
q = normalize(q)
return q
def to_local_cos2(self, joint_name, frame, q):
# bring into parent coordinate system
parent_joint = self.skeleton.nodes[joint_name].parent.node_name
pm = self.skeleton.nodes[parent_joint].get_global_matrix(frame)#[:3, :3]
inv_p = quaternion_inverse(quaternion_from_matrix(pm))
normalize(inv_p)
return quaternion_multiply(inv_p, q)
def compute_mean_quaternion(sigma_pts, init_mean):
#returns the quaternion mean of the quaternion state in the sigma_pts using gradient descent
num_pts = sigma_pts.shape[1]
# error_quat = np.empty(sigma_pts.shape)
error_vec = np.empty([3, num_pts], dtype=np.float64)
mean_error_vec = np.ones([
3,
], dtype=np.float64)
iteration = 0
while (np.linalg.norm(mean_error_vec) > 0.001 and iteration < 50):
iteration += 1
prev_mean = tf.quaternion_inverse(init_mean)
for i in range(0, num_pts):
error_quat = tf.quaternion_multiply(sigma_pts[:, i], prev_mean)
error_vec[:, i] = tf.rotvec_from_quaternion(error_quat)
mean_error_vec = np.mean(error_vec, 1)
init_mean = tf.quaternion_multiply(
tf.quaternion_from_rotvec(mean_error_vec), init_mean)
# print iteration
return init_mean, error_vec
def quaternion_angle(a, b):
a_inv = transformations.quaternion_inverse(a)
res = transformations.quaternion_multiply(b, a_inv)
res = res / np.linalg.norm(res)
angle = math.acos(res[0]) * 2
if angle < math.pi: angle += 2 * math.pi
if angle > math.pi: angle -= 2 * math.pi
return angle
def calculate_anticross(cross, anchor):
'''calculates the opposite rotation than going from anchor to cross'''
#set CROSS_QUAT $MAIN_NEIGHBOR
#set anchor_to_cross [quat_mult $CROSS_QUAT [quat_inv $ANCHOR_QUAT]]
#set anchor_to_anticross [quat_inv $anchor_to_cross]
#set ANTICROSS_QUAT [ quat_to_anchor_hemisphere $ANCHOR_QUAT [quat_mult $anchor_to_anticross $ANCHOR_QUAT]]
anchor_inv = transformations.quaternion_inverse(
anchor).tolist() # invert the anchor
anchor_to_cross = transformations.quaternion_multiply(
cross, anchor_inv).tolist() # find quat from anchor to cross
anchor_to_anticross = transformations.quaternion_inverse(
anchor_to_cross).tolist() # invert that quat
anticross = transformations.quaternion_multiply(
anchor_to_anticross, anchor) # get the anticross
if np.dot(anchor, anticross) < 0.0:
anticross *= 1.0 # if its on the wrong hemisphere than the anchor, then flip it back over
return anticross.tolist()
def set_end_effector_rotation(self, frame, target_orientation):
#print "set orientation", target_orientation
q = self.get_global_joint_orientation(self.end_effector, frame)
delta_orientation = quaternion_multiply(target_orientation,
quaternion_inverse(q))
new_local_q = self._to_local_coordinate_system(frame,
self.end_effector,
delta_orientation)
frame[self.end_effector_indices] = new_local_q
def set_orientation(self, orientation):
print("rotate frames")
current_q = self.mv.frames[self.frame_idx, 3:7]
delta_q = quaternion_multiply(orientation,
quaternion_inverse(current_q))
delta_q /= np.linalg.norm(delta_q)
delta_m = quaternion_matrix(delta_q)[:3, :3]
for idx in range(self.mv.n_frames):
old_t = self.mv.frames[idx, :3]
old_q = self.mv.frames[idx, 3:7]
self.mv.frames[idx, :3] = np.dot(delta_m, old_t)[:3]
self.mv.frames[idx, 3:7] = quaternion_multiply(delta_q, old_q)
def swing_twist_decomposition(q, twist_axis):
""" code by janis sprenger based on
Dobrowsolski 2015 Swing-twist decomposition in Clifford algebra. https://arxiv.org/abs/1506.05481
"""
#q = normalize(q)
#twist_axis = np.array((q * offset))[0]
projection = np.dot(twist_axis, np.array([q[1], q[2], q[3]])) * twist_axis
twist_q = np.array([q[0], projection[0], projection[1],projection[2]])
if np.linalg.norm(twist_q) == 0:
twist_q = np.array([1,0,0,0])
twist_q = normalize(twist_q)
swing_q = quaternion_multiply(q, quaternion_inverse(twist_q))#q * quaternion_inverse(twist)
return swing_q, twist_q
def draw_nose(self):
ned2body = transformations.quaternion_from_euler(
self.psi_rad, self.the_rad, self.phi_rad, 'rzyx')
body2ned = transformations.quaternion_inverse(ned2body)
vec = transformations.quaternion_transform(body2ned, [1.0, 0.0, 0.0])
uv = self.project_point(
[self.ned[0] + vec[0], self.ned[1] + vec[1], self.ned[2] + vec[2]])
r1 = int(round(self.render_h / 80))
r2 = int(round(self.render_h / 40))
if uv != None:
cv2.circle(self.frame, uv, r1, self.color, self.line_width,
cv2.LINE_AA)
cv2.circle(self.frame, uv, r2, self.color, self.line_width,
cv2.LINE_AA)
def quaternion_covariance(quaternions, weights=None, avg=None):
"""
Compute the covariance of the given quaternion set around each of the axes x, y, z
"""
if avg is None:
avg = average_quaternion(quaternions, weights)
avg_inv = transformations.quaternion_inverse(avg)
# Compute the Euler angle representation of each quaternion offset
euler = []
for q in quaternions:
q_diff = transformations.quaternion_multiply(q, avg_inv)
euler.append(transformations.euler_from_quaternion(q_diff))
return np.std(euler, axis=0)
def minus(x1, x2):
"""Returns the SO3 quantity representing the minimal rotation from
orientation x1 to orientation x2 (i.e. the inverse composition of x2 on x1,
call it "x2-x1"). The output will be in the same representation as the
inputs, but the inputs must be in the same representation as each other.
>>> A = trns.rotation_matrix(.2, [0, 1, 0]) # rotation of 0.2 rad about +y axis
>>> B = trns.rotation_matrix(-.2, [0, 1, 0]) # rotation of -0.2 rad about +y axis
>>> AtoB = minus(A, B) # this is "B - A", the rotation from A to B
>>> angle, axis = angle_axis(AtoB) # should be a rotation of (-0.2)-(0.2) = -0.4 rad about +y axis
>>> print(angle, axis)
(0.39999999999999997, array([-0., -1., -0.]))
>>> # The above is 0.4 rad about -y axis, which is equivalent to the expected -0.4 rad about the +y axis.
>>> # As usual, the angle is always returned between 0 and pi, with the accordingly correct axis.
>>> # The rotations need not be about the same axis of course:
>>> NtoA = trns.random_quaternion() # some rotation from frame N to frame A
>>> NtoB = trns.random_quaternion() # some rotation from frame N to frame B
>>> AtoB = minus(NtoA, NtoB) # We say "AtoB = NtoB - NtoA"
>>> NtoAtoB = plus(NtoA, AtoB) # NtoAtoB == NtoB and we say "NtoAtoB = NtoA + AtoB"
>>> print(np.allclose(NtoAtoB, NtoB))
True
>>> # Evidently, plus and minus are inverse operations.
>>> # "x1 + (x2 - x1) = x2" reads as "(N to x1) plus (x1 to x2) = (N to x2)"
>>> A[:3, 3] = trns.random_vector(3)
>>> B[:3, 3] = trns.random_vector(3)
>>> C1 = B.dot(npl.inv(A))
>>> C2 = minus(A, B)
>>> print(np.allclose(C1, C2))
True
"""
xrep = rep(x1)
xrep2 = rep(x2)
if xrep != xrep2:
raise ValueError('Values to "subtract" are not in the same SO3 representation.')
if xrep == 'quaternion':
return normalize(trns.quaternion_multiply(x2, trns.quaternion_inverse(x1)))
elif xrep == 'rotmat':
return x2.dot(x1.T)
elif xrep == 'tfmat':
return x2.dot(npl.inv(x1)) # inverse of a tfmat is not its transpose
elif xrep == 'rotvec':
# Subtracting rotvecs is only valid for small perturbations...
# Perform operation on actual SO3 manifold instead:
x1 = matrix_from_rotvec(x1)
x2 = matrix_from_rotvec(x2)
return get_rotvec(x2.dot(x1.T))
else:
raise ValueError('Values to "subtract" are not on SO3.')
def slerp(x1, x2, fraction):
"""Spherical Linear intERPolation. Returns an SO3 quantity representing an orientation
between x1 and x2. The fraction of the path from x1 to x2 is the fraction input, and it
must be between 0 and 1. The output will be in the same representation as the inputs, but
the inputs must be in the same representation as each other.
>>> x1 = make_affine(trns.random_rotation_matrix()[:3, :3], [1, 1, 1])
>>> x2 = make_affine(trns.random_rotation_matrix()[:3, :3], [2, 2, 2])
>>> nogo = slerp(x1, x2, 0)
>>> print(np.allclose(nogo, x1))
True
>>> allgo = slerp(x1, x2, 1)
>>> print(np.allclose(allgo, x2))
True
>>> first_25 = slerp(x1, x2, 0.25) # 0 to 25 percent from x1 to x2
>>> last_75 = minus(first_25, x2) # 25 to 75 percent from x1 to x2
>>> wholeway = plus(first_25, last_75)
>>> print(np.allclose(wholeway, x2))
True
>>> x1 = trns.quaternion_from_matrix(x1)
>>> x2 = trns.quaternion_from_matrix(x2)
>>> mine = slerp(x1, x2, 0.3)
>>> his = trns.quaternion_slerp(x1, x2, 0.3) # only works on quaternions
>>> print(np.allclose(mine, his))
True
"""
xrep = rep(x1)
xrep2 = rep(x2)
if xrep != xrep2:
raise ValueError('Values to slerp between are not in the same SO3 representation.')
if xrep == 'quaternion':
r12 = get_rotvec(trns.quaternion_multiply(x2, trns.quaternion_inverse(x1)))
x12 = normalize(quaternion_from_rotvec(fraction * r12))
elif xrep == 'rotmat':
r12 = get_rotvec(x2.dot(x1.T))
x12 = matrix_from_rotvec(fraction * r12)
elif xrep == 'tfmat':
M12 = x2.dot(npl.inv(x1))
r12 = get_rotvec(M12)
x12 = make_affine(matrix_from_rotvec(fraction * r12), fraction * M12[:3, 3])
elif xrep == 'rotvec':
R1 = matrix_from_rotvec(x1)
R2 = matrix_from_rotvec(x2)
x12 = fraction * get_rotvec(R2.dot(R1.T))
else:
raise ValueError("One or both values to slerp between are not on SO3.")
return plus(x1, x12)
def get_relative_frames(self):
relative_frames = []
for idx in range(1, len(self.frames)):
delta_frame = np.zeros(len(self.frames[0]))
delta_frame[7:] = self.frames[idx][7:]
delta_frame[:3] = self.frames[idx][:3] - self.frames[idx - 1][:3]
currentq = self.frames[idx][3:7] / np.linalg.norm(
self.frames[idx][3:7])
prevq = self.frames[idx - 1][3:7] / np.linalg.norm(
self.frames[idx - 1][3:7])
delta_q = quaternion_multiply(quaternion_inverse(prevq), currentq)
delta_frame[3:7] = delta_q
#print(idx, self.frames[idx][:3], self.frames[idx - 1][:3], delta_frame[:3], delta_frame[3:7])
relative_frames.append(delta_frame)
return relative_frames
def compute_velocity(self, skeleton, pose, prev_pose):
"""compute velocity via finite difference"""
joints = skeleton.animated_joints
v = np.zeros(len(joints) * 4 + 3) # only preserve the key joints
v[:3] = pose[:3] - prev_pose[:3]
index = 3
root_pos_offset = 3
for joint in joints:
offset = skeleton.nodes[joint].index * 4 + root_pos_offset
qa = pose[offset:offset + 4]
qb = prev_pose[offset:offset + 4]
v[index:index + 4] = quaternion_multiply(quaternion_inverse(qa),
qb)
index += 4
return v
def apply_spherical_constraint(q, ref_q, axis, k):
""" lee 2000 p. 48"""
q0 = ref_q
w = axis
rel_q = quaternion_multiply(quaternion_inverse(q0), q)
rel_q / np.linalg.norm(rel_q)
w_prime = rotate_vector_by_quaternion(w, rel_q)
v = np.cross(w, w_prime)
v = normalize(v)
log_rel_q = log(rel_q)
phi = acos(np.dot(w, w_prime))
if 2 * np.linalg.norm(log_rel_q) > k: # apply
delta_phi = k - phi
delta_q = quaternion_about_axis(delta_phi, v)
delta_q = normalize(delta_q)
q = quaternion_multiply(delta_q, q)
return normalize(q)
def QuaternionFromTo(
self, q1, q2): #usable with both geometry_msgs/Quaternion and list
q1_arr = None
q2_arr = None
if (type(q1) == Quaternion):
q1_arr = self.QuatToArray(q1)
else:
q1_arr = q1[:]
if (type(q2) == Quaternion):
q2_arr = self.QuatToArray(q2)
else:
q2_arr = q2[:]
q1_arr_inv = transformations.quaternion_inverse(q1_arr)
q3_arr = transformations.quaternion_multiply(q1_arr_inv, q2_arr)
return self.ArrayToQuat(q3_arr)
def Quaternion(self, Orientations, Joint, ParentJoint = None):
'''
Function that computes the quaternion matrix using the coordinates of
each joint get with the Kinect Sensor.
'''
Quat = Orientations[Joint].Orientation
QArray = np.array([Quat.w, Quat.x, Quat.y, Quat.z])
# Quat matrix with two joints.
if ParentJoint is not None:
QuatParent = Orientations[ParentJoint].Orientation
QuatArrayParent = np.array([QuatParent.w, QuatParent.x, QuatParent.y, QuatParent.z])
QuatRelativ = tf.quaternion_multiply(tf.quaternion_inverse(QuatArrayParent), QArray) # Compute the relative quat.
return QuatRelativ
else:
return QArray
def quaternion_distance_between_frames(self, skeleton, frame1, frame2):
joints = skeleton.animated_joints
v = np.zeros(len(joints)) # only preserve the key joints
root_pos_offset = 3
index = 0
for joint in joints:
offset = skeleton.nodes[joint].index * 4 + root_pos_offset
qa = frame1.pose[offset:offset + 4]
qb = frame2.pose[offset:offset + 4]
quat_difference = quaternion_multiply(qb, quaternion_inverse(qa))
quat_difference = quat_difference / np.linalg.norm(quat_difference)
v[index] = np.arccos(quat_difference[0])
index += 1
distance = np.linalg.norm(v)
return distance * distance
def minus(q1, q2):
"""Returns a quaternion representing the minimal rotation from
orientation q1 to orientation q2 (i.e. the inverse composition
of q2 on q1, call it "q2-q1").
>>> NtoA = trns.random_quaternion() # some rotation from frame N to frame A
>>> NtoB = trns.random_quaternion() # some rotation from frame N to frame B
>>> AtoB = minus(NtoA, NtoB) # We say "AtoB = NtoB - NtoA"
>>> NtoAtoB = plus(NtoA, AtoB) # NtoAtoB == NtoB and we say "NtoAtoB = NtoA + AtoB"
>>> print(np.allclose(NtoAtoB, NtoB))
True
>>> # Evidently, plus and minus are inverse operations.
>>> # "q1 + (q2 - q1) = q2" reads as "(N to q1) plus (q1 to q2) = (N to q2)"
>>> q = plus(NtoA, minus(NtoA, NtoB))
>>> print(np.allclose(q, NtoB))
True
"""
return normalize(trns.quaternion_multiply(q2, trns.quaternion_inverse(q1)))
def align_euler_frames(euler_frames,
frame_idx,
ref_orientation_euler):
new_euler_frames = deepcopy(euler_frames)
ref_quat = euler_to_quaternion(ref_orientation_euler)
root_rot_angles = euler_frames[frame_idx][3:6]
root_rot_quat = euler_to_quaternion(root_rot_angles)
quat_diff = quaternion_multiply(ref_quat, quaternion_inverse(root_rot_quat))
for euler_frame in new_euler_frames:
root_trans = euler_frame[:3]
new_root_trans = point_rotation_by_quaternion(root_trans, quat_diff)
euler_frame[:3] = new_root_trans
root_rot_angles = euler_frame[3:6]
root_rot_quat = euler_to_quaternion(root_rot_angles)
new_root_quat = quaternion_multiply(quat_diff, root_rot_quat)
new_root_euler = quaternion_to_euler(new_root_quat)
euler_frame[3:6] = new_root_euler
return new_euler_frames
def minus(q1, q2):
"""Returns a quaternion representing the minimal rotation from
orientation q1 to orientation q2 (i.e. the inverse composition
of q2 on q1, call it "q2-q1").
>>> NtoA = trns.random_quaternion() # some rotation from frame N to frame A
>>> NtoB = trns.random_quaternion() # some rotation from frame N to frame B
>>> AtoB = minus(NtoA, NtoB) # We say "AtoB = NtoB - NtoA"
>>> NtoAtoB = plus(NtoA, AtoB) # NtoAtoB == NtoB and we say "NtoAtoB = NtoA + AtoB"
>>> print(np.allclose(NtoAtoB, NtoB))
True
>>> # Evidently, plus and minus are inverse operations.
>>> # "q1 + (q2 - q1) = q2" reads as "(N to q1) plus (q1 to q2) = (N to q2)"
>>> q = plus(NtoA, minus(NtoA, NtoB))
>>> print(np.allclose(q, NtoB))
True
"""
return normalize(trns.quaternion_multiply(q2, trns.quaternion_inverse(q1)))
def unitxrotate(df):
"""rotate a vector along x-axis to other directions
"""
# define quaternion to a point at (1,0,0)
pt = np.asarray([0, 1, 0, 0])
print(pt)
for idx, item in sc.dfLocus.iterrows():
print(10 * '-')
rts = tr.quaternion_from_euler(item['yawRad'], item['pitchRad'],
item['rollRad'],
sc.sequence) #(seq='rzyx')
print('rot quat={}'.format(rts))
pts = tr.quaternion_multiply(
rts, tr.quaternion_multiply(pt, tr.quaternion_inverse(rts)))
print('point={}'.format(pts))
print('yaw/pit/rol={}'.format(
tr.euler_from_quaternion(rts, sc.sequence)))
print('\n---------------------\n')
elif (args.auto_switch == 'new' and auto_switch < 0) or (args.auto_switch == 'old' and auto_switch > 0):
flight_mode = 'manual'
else:
flight_mode = 'auto'
body2cam = transformations.quaternion_from_euler( cam_yaw * d2r,
cam_pitch * d2r,
cam_roll * d2r,
'rzyx')
#print 'att:', [yaw_rad, pitch_rad, roll_rad]
ned2body = transformations.quaternion_from_euler(yaw_rad,
pitch_rad,
roll_rad,
'rzyx')
body2ned = transformations.quaternion_inverse(ned2body)
#print 'ned2body(q):', ned2body
ned2cam_q = transformations.quaternion_multiply(ned2body, body2cam)
ned2cam = np.matrix(transformations.quaternion_matrix(np.array(ned2cam_q))[:3,:3]).T
#print 'ned2cam:', ned2cam
R = ned2proj.dot( ned2cam )
rvec, jac = cv2.Rodrigues(R)
ned = navpy.lla2ned( lat_deg, lon_deg, altitude_m,
ref[0], ref[1], ref[2] )
#print 'ned:', ned
tvec = -np.matrix(R) * np.matrix(ned).T
R, jac = cv2.Rodrigues(rvec)
# is this R the same as the earlier R?
PROJ = np.concatenate((R, tvec), axis=1)
#print 'PROJ:', PROJ
print('euler transform (ypr):', yaw_rad*r2d, pitch_rad*r2d, roll_rad*r2d)
mount_node = getNode('/config/camera/mount', True)
camera_yaw = mount_node.getFloat('yaw_deg')
camera_pitch = mount_node.getFloat('pitch_deg')
camera_roll = mount_node.getFloat('roll_deg')
print('camera:', camera_yaw, camera_pitch, camera_roll)
body2cam = transformations.quaternion_from_euler(camera_yaw * d2r,
camera_pitch * d2r,
camera_roll * d2r,
'rzyx')
print('body2cam:', body2cam)
(yaw_rad, pitch_rad, roll_rad) = transformations.euler_from_quaternion(body2cam, 'rzyx')
print('euler body2cam (ypr):', yaw_rad*r2d, pitch_rad*r2d, roll_rad*r2d)
cam2body = transformations.quaternion_inverse(body2cam)
print('cam2body:', cam2body)
(yaw_rad, pitch_rad, roll_rad) = transformations.euler_from_quaternion(cam2body, 'rzyx')
print('euler cam2body (ypr):', yaw_rad*r2d, pitch_rad*r2d, roll_rad*r2d)
tot = transformations.quaternion_multiply(q, body2cam)
(yaw_rad, pitch_rad, roll_rad) = transformations.euler_from_quaternion(tot, 'rzyx')
print(tot)
print('euler (ypr):', yaw_rad*r2d, pitch_rad*r2d, roll_rad*r2d)
tot_inv = transformations.quaternion_inverse(tot)
def wrap_pi(val):
while val < math.pi:
val += 2*math.pi
while val > math.pi:
omega = np.array([0,p[i], q[i], r[i]])
omega_bias = np.array([0,p[i]-biasEst[0], q[i]-biasEst[1], r[i]-biasEst[2]])
q_ins = q_est + 0.5*dt*trans.quaternion_multiply(q_est, omega_bias)
q_insOnly = q_insOnly + 0.5*dt*trans.quaternion_multiply(q_insOnly, omega)
yawINS.append(trans.euler_from_quaternion(q_insOnly)[2])
pitchINS.append(trans.euler_from_quaternion(q_insOnly)[0])
rollINS.append(trans.euler_from_quaternion(q_insOnly)[1])
# generate truth quaternion
#quat_meas = trans.quaternion_from_euler(yaw[i],pitch[i],roll[i])
q_meas = trans.quaternion_from_euler(roll[i],pitch[i],yaw[i])
yaw_meas.append(trans.euler_from_quaternion(q_meas)[0])
q_error = trans.quaternion_multiply(q_meas,trans.quaternion_inverse(q_ins))
q_error = q_error / np.linalg.norm(q_error)
q_error = np.hstack((1,q_error[1:4]))
#errorAngles = q_error[1:5]
q_innov = np.hstack((1,gain*q_error[1:4]))
biasEst = biasEst - bgain*q_error[1:4]
q_est = trans.quaternion_multiply(q_innov,q_ins)
yawEst.append(trans.euler_from_quaternion(q_est)[2])
pitchEst.append(trans.euler_from_quaternion(q_est)[1])
rollEst.append(trans.euler_from_quaternion(q_est)[0])
# calculate quaternion rotation from INS solution to measured solution
quats = quats0
location = location0
r.setCartesian([location, quats])
elif msg == "dtool":
tool = r.getTool()
print 'dumping tool object to file ', tool
f = open('tool.object', 'wb')
cPickle.dump(tool, f)
f.close()
elif msg == "tool":
a = r.getTool()
print 'current tool: ', a
b = transformations.quaternion_multiply([0, 0, 1, 0], quats)
toolquat = transformations.quaternion_multiply(a[1], b)
toolquat = transformations.quaternion_inverse(toolquat)
tool = [a[0], toolquat]
ctool = tool
quats = quats0
print 'setting tool: ', tool
print 'moving to: ', [location, quats]
if (robON):
r.setTool(tool)
#location = numpy.array(location) - toolcart
r.setCartesian([location, quats])
continue
elif msg == "toolcart":
def to_local_cos_fast(skeleton, node_name, frame, q):
# bring into parent coordinate system
pm = np.array(skeleton.nodes[node_name].get_global_matrix(frame, use_cache=True)[:3,:3])
inv_p = quaternion_inverse(quaternion_from_matrix(pm))
return quaternion_multiply(inv_p, q)
def dist(q1, q2):
""" chapter 2 of lee 2000"""
return np.linalg.norm(log(quaternion_multiply(quaternion_inverse(q1), q2)))
print
(n, R, tvec, mask) = cv2.recoverPose(E=E,
points1=uv1,
points2=uv2,
cameraMatrix=K)
print ' inliers:', n, 'of', len(uv1)
print ' R:', R
print ' tvec:', tvec
# convert R to homogeonous
#Rh = np.concatenate((R, np.zeros((3,1))), axis=1)
#Rh = np.concatenate((Rh, np.zeros((1,4))), axis=0)
#Rh[3,3] = 1
# extract the equivalent quaternion, and invert
q = transformations.quaternion_from_matrix(R)
q_inv = transformations.quaternion_inverse(q)
# generate a rough estimate of uv area of covered by common
# features (used as a weighting factor)
minu = maxu = uv1[0][0]
minv = maxv = uv1[0][1]
for uv in uv1:
#print uv
if uv[0] < minu: minu = uv[0]
if uv[0] > maxu: maxu = uv[0]
if uv[1] < minv: minv = uv[1]
if uv[1] > maxv: maxv = uv[1]
area = (maxu - minu) * (maxv - minv)
# print 'u:', minu, maxu
# print 'v:', minv, maxv
# print 'area:', area
def transformation_from_loc_quat(quat, loc):
T = tf.quaternion_matrix(tf.quaternion_inverse(quat))
T[:3, 3] = -1 * np.dot(T[:3, :3], loc)
return T
def rotate_vector_by_quaternion(v, q):
vq = [0, v[0], v[1], v[2]]
v_prime = quaternion_multiply(
q, quaternion_multiply(vq, quaternion_inverse(q)))[1:]
v_prime /= np.linalg.norm(v_prime)
return v_prime
# and it has an equivalent quaternion expression dq. In differential equation form, the equation
# is qdot = f(w,dt) = ori.quaternion_from_rotvec(w*dt)/dt, but it is crucial to understand that
# integrating this equation requires use of ori.plus, so it cannot be easily fed into a standard solver.
# ---
# Simulate over t_arr:
for i, t in enumerate(t_arr):
# Record current state:
q_history[i, :] = np.copy(q)
roll_history[i], pitch_history[i], yaw_history[i] = trns.euler_from_quaternion(q, 'rxyz')
body_world_history[:, :, i] = ori.qapply_points(q, body)
w_history[i, :] = np.copy(w)
torque_history[i, :] = np.copy(torque)
# Current values needed to compute next state:
I_world = ori.qapply_matrix(q, I) # current inertia matrix in world frame
H = I_world.dot(w) # current angular momentum
wb = ori.qapply_points(trns.quaternion_inverse(q), w) # w in body frame
dq = ori.quaternion_from_rotvec(wb * dt) # change in orientation for this timestep instant
# PD controller:
q_err = ori.error(q, q_des) # q_err is a rotvec
w_err = w_des - w
kpW = np.diag(ori.qapply_matrix(q, np.diag(kp))) # world frame kp gains
kdW = np.diag(ori.qapply_matrix(q, np.diag(kd))) # world frame kd gains
torque = (kpW * q_err) + (kdW * w_err)
# Compute next state:
q = ori.plus(dq, q) # new orientation computed using dq and old q
I_world = ori.qapply_matrix(q, I) # new I_world computed using new q
H = H + (torque * dt) # new H from old H and torque
w = npl.inv(I_world).dot(H) # new angular velocity computed using new I_world and new H
################################################# DISPLAY
def get_quaternion_delta(a, b):
return quaternion_multiply(quaternion_inverse(b), a)
def to_local_cos(skeleton, node_name, frame, q):
# bring into parent coordinate system
pm = skeleton.nodes[node_name].get_global_matrix(frame) #[:3,:3]
inv_p = quaternion_inverse(quaternion_from_matrix(pm))
normalize(inv_p)
return quaternion_multiply(inv_p, q)
def get_quat_delta(qa, qb):
""" get quaternion from quat a to quat b """
return quaternion_multiply(qb, quaternion_inverse(qa))
