def apply_axial_constraint(q, ref_q, axis, k1, k2):
""" lee 2000 p. 48"""
q0 = ref_q
w = axis
#rel_q = quaternion_multiply(quaternion_inverse(q0), q)
#rel_q = normalize(rel_q)
w_prime = rotate_vector_by_quaternion(w, q)
v = np.cross(w, w_prime)
phi = acos(np.dot(w, w_prime))
#remove rotation around v
# psi_q = exp(-phi*v)
inv_phi_q = quaternion_about_axis(-phi, v)
psi_q = quaternion_multiply(inv_phi_q, ref_q)
# get angle around w from psi_q
w_prime2 = rotate_vector_by_quaternion(w, psi_q)
w_prime2 = normalize(w_prime2)
psi = acos(np.dot(w, w_prime2))
if k1 < psi > k2: # apply
delta_psi1 = k1 - psi
delta_psi2 = k2 - psi
delta_psi = min(delta_psi1, delta_psi2)
delta_q = quaternion_about_axis(delta_psi, w)
q = quaternion_multiply(delta_q, q)
print("apply", psi)
return q
def graspExecute(limb, W, H, Ang):
endEffPos = pixelToWorld(W, H)
quat1 = transformations.quaternion_about_axis(Ang, (0, 0, 1))
quat2 = transformations.quaternion_about_axis(np.pi, (1, 0, 0))
quat = transformations.quaternion_multiply(quat1, quat2)
# print(quat)
top_grasp_joint = ik_service_client(limb='right',
use_advanced_options=True,
p_x=endEffPos[0],
p_y=endEffPos[1],
p_z=0.5,
q_x=quat[0],
q_y=quat[1],
q_z=quat[2],
q_w=quat[3])
down_grasp_joint = ik_service_client(limb='right',
use_advanced_options=True,
p_x=endEffPos[0],
p_y=endEffPos[1],
p_z=0.1,
q_x=quat[0],
q_y=quat[1],
q_z=quat[2],
q_w=quat[3])
move(limb, positions=top_grasp_joint, move_speed=0.2)
rospy.sleep(2)
move(limb, positions=down_grasp_joint, move_speed=0.2)
rospy.sleep(2)
move(limb, positions=top_grasp_joint, move_speed=0.2)
def rotate(self, yaw, pitch, roll):
self.yaw += yaw
self.pitch += pitch
self.roll += roll
q1 = transformations.quaternion_about_axis(self.pitch, (1, 0, 0))
q2 = transformations.quaternion_about_axis(self.yaw, (0, 0, 1))
q3 = transformations.quaternion_about_axis(self.roll, (0, 1, 0))
self.orientation = transformations.quaternion_multiply(transformations.quaternion_multiply(q1, q2), q3)
def on_timer(self, event):
# auto rotating
qy = tr.quaternion_about_axis(0.005, [0,0,1])
qx = tr.quaternion_about_axis(0.005, [0,1,0])
q = tr.quaternion_multiply(qx,qy)
self.rot = tr.quaternion_multiply(self.rot, q)
self.model = tr.quaternion_matrix(self.rot)
self.program_data['u_model'] = self.model
self.program_axis['u_model'] = self.model
self.program_plane['u_model'] = self.model
self.update()
def on_timer(self, event):
# auto rotating
qy = tr.quaternion_about_axis(0.005, [0, 0, 1])
qx = tr.quaternion_about_axis(0.005, [0, 1, 0])
q = tr.quaternion_multiply(qx, qy)
self.rot = tr.quaternion_multiply(self.rot, q)
self.model = tr.quaternion_matrix(self.rot)
self.program_data['u_model'] = self.model
self.program_axis['u_model'] = self.model
self.program_plane['u_model'] = self.model
self.update()
def get_transform(self):
""" Copied from ThinMatrix shadow tutorial
src: https://www.youtube.com/watch?v=o6zDfDkOFIc
https://www.dropbox.com/sh/g9vnfiubdglojuh/AACpq1KDpdmB8ZInYxhsKj2Ma/shadows
"""
yaw = math.radians(self.yaw)
pitch = math.radians(self.pitch)
rot_y = trans.quaternion_about_axis(-yaw, np.array([0, 1, 0]))
rot_y = trans.quaternion_matrix(rot_y)
rot_x = trans.quaternion_about_axis(-pitch, np.array([1, 0, 0]))
rot_x = trans.quaternion_matrix(rot_x)
transform = np.dot(rot_y, rot_x)
transform[3, :3] = -self.get_world_position()
return transform
def lamePath(dist, ang, side=50.8, r=3, oset=10, compAng=numpy.radians(0)):
q = numpy.array([[0., 0., -0.71, 0.71], [0, 0, 1, 0], [0., 0., 0.71,
0.71]])
if numpy.round(ang % 3.141, 2) > .01: d = side / numpy.tan(ang)
else: d = 0
do = oset * numpy.cos(ang)
zo = oset * numpy.sin(ang)
s = numpy.array([[-do, side / 2, -zo], [d + do, side / 2, side + zo]])
s1 = numpy.array([[d + do, -side / 2, zo + side], [-do, -side / 2, -zo]])
top = [[d, ((side / 2) + oset), side], [d, -((side / 2) + oset), side]]
topAng = (numpy.pi / 2) - ang
botAng = -topAng
topAng = thresh(topAng, numpy.radians(50))
botAng = thresh(botAng, numpy.radians(50))
qBot = transformations.quaternion_multiply(
q[1], transformations.quaternion_about_axis(botAng, [0, 1, 0]))
print '\n\n\QBOT', qBot
angVec = [numpy.cos(ang), 0, numpy.sin(ang)]
q[0] = transformations.quaternion_multiply(
q[0], transformations.quaternion_about_axis(-compAng, angVec))
q[1] = transformations.quaternion_multiply(
q[1], transformations.quaternion_about_axis(topAng, [0, 1, 0]))
q[2] = transformations.quaternion_multiply(
q[2], transformations.quaternion_about_axis(compAng, angVec))
q = unitVector(q)
s = numpy.hstack((s, [q[0], q[0]]))
s2 = numpy.hstack((s1, [q[2], q[2]]))
top = numpy.hstack((top, [q[1], q[1]]))
top = numpy.array([s, top, s2])
numpy.set_printoptions(precision=3, suppress=True)
top[:, :, 0] -= numpy.min(top[:, :, 0]) + dist
bf = 1.375
bottom = [[-dist + (bf * do), -((side / 2) + oset), side],
[-dist + (bf * do), (side / 2) + oset, side]]
bottom = numpy.hstack((bottom, [qBot, qBot]))
return (top, bottom)
def reset(self):
t0 = 0.0
self.trace = []
self.Q[3:] = tr.quaternion_about_axis(self.a, (0, 0, 1))
self.Q[:3] = (0, self.w, 0)
self.R = ode(self.ode).set_integrator('dopri5')
self.R.set_initial_value(self.Q, t0)
def build_bone_shape(cls, offset_vector, size, material=None):
REF_VECTOR = [0, 1, 0]
array_type = GL_QUADS
uv_pos = -1
normal_pos = 12
weight_pos = -1
color_pos = -1
bone_id_pos = -1
offset = 24
rotation = np.eye(4)
length = np.linalg.norm(offset_vector)
offset_vector /= length
q = quaternion_from_vector_to_vector(REF_VECTOR, offset_vector)
q = quaternion_multiply(q, quaternion_about_axis(np.radians(180), [0, 1, 0]))
rotation[:3, :3] = quaternion_matrix(q)[:3, :3]
vertex_list = construct_quad_box_based_on_height(size, length, size)
index_list = None
m = Mesh(vertex_list, array_type, normal_pos=normal_pos, color_pos=color_pos,
uv_pos=uv_pos, weight_pos=weight_pos,
bone_id_pos=bone_id_pos, stride=offset,
index_list=index_list, material=material)
m.transform = rotation
return m
def axes_to_q(g_twist, g_swing, flip=False):
q = [1, 0, 0, 0]
q_y = quaternion_from_vector_to_vector(Y, g_twist)
q_y = normalize(q_y)
q = quaternion_multiply(q_y, q)
X_prime = rotate_vector(q_y, X)
X_prime = normalize(X_prime)
q_x = quaternion_from_vector_to_vector(X_prime, g_swing)
q_x = normalize(q_x)
q = quaternion_multiply(q_x, q)
q = normalize(q)
Y_prime = rotate_vector(q, Y)
dot = np.dot(Y_prime, g_twist)
#dot = min(dot,1)
dot = max(dot, -1)
if dot == -1:
q180 = quaternion_about_axis(np.deg2rad(180), g_swing)
q180 = normalize(q180)
q = quaternion_multiply(q180, q)
q = normalize(q)
elif abs(dot) != 1.0:
q_y = quaternion_from_vector_to_vector(Y_prime, g_twist)
q = quaternion_multiply(q_y, q)
q = normalize(q)
return q
def reset( self ) :
t0 = 0.0
self.trace = []
self.Q[3:] = tr.quaternion_about_axis(self.a,(0,0,1))
self.Q[:3] = (0,self.w,0)
self.R = ode(self.ode).set_integrator('dopri5')
self.R.set_initial_value(self.Q,t0)
def align_joint(new_skeleton,
free_joint_name,
local_target_axes,
global_src_up_vec,
global_src_x_vec,
joint_cos_map,
apply_spine_fix=False):
# first align the twist axis
q, axes = align_axis(local_target_axes, "y", global_src_up_vec)
q = normalize(q)
# then align the swing axis
qx, axes = align_axis(axes, "x", global_src_x_vec)
q = quaternion_multiply(qx, q)
q = normalize(q)
# handle cases when twist axis alignment was lost
dot = np.dot(axes["y"], global_src_up_vec)
if dot <= -1:
q180 = quaternion_about_axis(np.deg2rad(180), global_src_x_vec)
q180 = normalize(q180)
q = quaternion_multiply(q180, q)
q = normalize(q)
elif abs(dot) != 1.0:
qy, axes = align_axis(axes, "y", global_src_up_vec)
q = quaternion_multiply(qy, q)
q = normalize(q)
return q
def draw_heading_bug(self):
color = medium_orchid
size = 2
a = math.atan2(self.ve, self.vn)
q0 = transformations.quaternion_about_axis(self.ap_hdg * d2r,
[0.0, 0.0, -1.0])
center = self.ladder_helper(q0, 0, 0)
pts = []
pts.append(self.ladder_helper(q0, 0, 2.0))
pts.append(self.ladder_helper(q0, 0.0, -2.0))
pts.append(self.ladder_helper(q0, 1.5, -2.0))
pts.append(self.ladder_helper(q0, 1.5, -1.0))
pts.append(center)
pts.append(self.ladder_helper(q0, 1.5, 1.0))
pts.append(self.ladder_helper(q0, 1.5, 2.0))
for i, p in enumerate(pts):
if p == None or center == None:
return
cv2.line(self.frame, pts[0], pts[1], color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, pts[1], pts[2], color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, pts[2], pts[3], color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, pts[3], pts[4], color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, pts[4], pts[5], color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, pts[5], pts[6], color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, pts[6], pts[0], color, self.line_width,
cv2.LINE_AA)
def orient_node_to_target_look_at_projected(skeleton,
frame,
node_name,
end_effector,
position,
local_dir=LOOK_AT_DIR,
twist_axis=None,
max_angle=None):
o = skeleton.nodes[node_name].quaternion_frame_index * 4 + 3
q = look_at_target_projected(skeleton, node_name, end_effector, frame,
position, local_dir)
q = to_local_coordinate_system(skeleton, frame, node_name, q)
if max_angle is not None and twist_axis is not None:
t_min_angle = np.radians(-max_angle)
t_max_angle = np.radians(max_angle)
swing_q, twist_q = swing_twist_decomposition(q, twist_axis)
v, a = quaternion_to_axis_angle(twist_q)
sign = 1
if np.sum(v) < 0:
sign = -1
a *= sign
a = max(a, t_min_angle)
a = min(a, t_max_angle)
new_twist_q = quaternion_about_axis(a, twist_axis)
q = quaternion_multiply(swing_q, new_twist_q)
q = normalize(q)
frame[o:o + 4] = q
return frame
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 draw_flight_path_marker(self):
if self.alpha_rad == None or self.beta_rad == None:
return
q0 = transformations.quaternion_about_axis(self.psi_rad + self.beta_rad,
[0.0, 0.0, -1.0])
a0 = (self.the_rad - self.alpha_rad) * r2d
uv = self.ladder_helper(q0, a0, 0)
if uv != None:
r1 = int(round(self.render_h / 60))
r2 = int(round(self.render_h / 30))
uv1 = (uv[0]+r1, uv[1])
uv2 = (uv[0]+r2, uv[1])
uv3 = (uv[0]-r1, uv[1])
uv4 = (uv[0]-r2, uv[1])
uv5 = (uv[0], uv[1]-r1)
uv6 = (uv[0], uv[1]-r2)
cv2.circle(self.frame, uv, r1, self.color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, uv1, uv2, self.color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, uv3, uv4, self.color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, uv5, uv6, self.color, self.line_width,
cv2.LINE_AA)
def draw_bird(self):
color = yellow
size = 2
a1 = 10.0
a2 = 3.0
a2 = 3.0
q0 = transformations.quaternion_about_axis(self.psi_rad, [0.0, 0.0, -1.0])
a0 = self.the_rad*r2d
print 'pitch:', a0, 'ap:', self.ap_pitch
# center point
center = self.ladder_helper(q0, a0, 0.0)
# right vbar
tmp1 = self.ladder_helper(q0, a0-a2, a1)
tmp2 = self.ladder_helper(q0, a0-a2, a1-a2)
if tmp1 != None and tmp2 != None:
uv1 = self.rotate_pt(tmp1, center, self.phi_rad)
uv2 = self.rotate_pt(tmp2, center, self.phi_rad)
if uv1 != None and uv2 != None:
cv2.line(self.frame, center, uv1, color, self.line_width, cv2.LINE_AA)
cv2.line(self.frame, center, uv2, color, self.line_width, cv2.LINE_AA)
cv2.line(self.frame, uv1, uv2, color, self.line_width, cv2.LINE_AA)
# left vbar
tmp1 = self.ladder_helper(q0, a0-a2, -a1)
tmp2 = self.ladder_helper(q0, a0-a2, -a1+a2)
if tmp1 != None and tmp2 != None:
uv1 = self.rotate_pt(tmp1, center, self.phi_rad)
uv2 = self.rotate_pt(tmp2, center, self.phi_rad)
if uv1 != None and uv2 != None:
cv2.line(self.frame, center, uv1, color, self.line_width, cv2.LINE_AA)
cv2.line(self.frame, center, uv2, color, self.line_width, cv2.LINE_AA)
cv2.line(self.frame, uv1, uv2, color, self.line_width, cv2.LINE_AA)
def draw_alpha_beta_marker(self):
if self.alpha_rad == None or self.beta_rad == None:
return
q0 = transformations.quaternion_about_axis(self.psi_rad,
[0.0, 0.0, -1.0])
a0 = self.the_rad * r2d
center = self.ladder_helper(q0, a0, 0.0)
alpha = self.alpha_rad * r2d
beta = self.beta_rad * r2d
tmp = self.ladder_helper(q0, a0 - alpha, beta)
if tmp != None:
uv = self.rotate_pt(tmp, center, self.phi_rad)
if uv != None:
r1 = int(round(self.render_h / 60))
r2 = int(round(self.render_h / 30))
uv1 = (uv[0] + r1, uv[1])
uv2 = (uv[0] + r2, uv[1])
uv3 = (uv[0] - r1, uv[1])
uv4 = (uv[0] - r2, uv[1])
uv5 = (uv[0], uv[1] - r1)
uv6 = (uv[0], uv[1] - r2)
cv2.circle(self.frame, uv, r1, self.color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, uv1, uv2, self.color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, uv3, uv4, self.color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, uv5, uv6, self.color, self.line_width,
cv2.LINE_AA)
def _WorldFromCameraFromViewDict(view_json):
"""Fills the world from camera transform from the view_json.
Args:
view_json: A dictionary of view parameters.
Returns:
A 4x4 transform matrix representing the world from camera transform.
"""
# The camera model transforms the 3d point X into a ray u in the local
# coordinate system:
#
# u = R * (X[0:2] - X[3] * c)
#
# Meaning the world from camera transform is [inv(R), c]
transform = np.identity(4)
position = view_json['position']
transform[0:3, 3] = (position[0], position[1], position[2])
orientation = view_json['orientation']
angle_axis = np.array([orientation[0], orientation[1], orientation[2]])
angle = np.linalg.norm(angle_axis)
epsilon = 1e-7
if abs(angle) < epsilon:
# No rotation
return np.matrix(transform)
axis = angle_axis / angle
rot_mat = transformations.quaternion_matrix(
transformations.quaternion_about_axis(-angle, axis))
transform[0:3, 0:3] = rot_mat[0:3, 0:3]
return np.matrix(transform)
def calculate_limb_joint_rotation(self, frame, target_position):
""" find angle so the distance from root to end effector is equal to the distance from the root to the target"""
root_pos = self.skeleton.nodes[self.limb_root].get_global_position(
frame)
joint_pos = self.skeleton.nodes[self.limb_joint].get_global_position(
frame)
end_effector_pos = self.skeleton.nodes[
self.end_effector].get_global_position(frame)
upper_limb = np.linalg.norm(root_pos - joint_pos)
lower_limb = np.linalg.norm(joint_pos - end_effector_pos)
target_length = np.linalg.norm(root_pos - target_position)
current_length = np.linalg.norm(root_pos - end_effector_pos)
current_angle = calculate_angle2(upper_limb, lower_limb,
current_length)
target_angle = calculate_angle2(upper_limb, lower_limb, target_length)
if self.damp_angle is not None:
target_angle = damp_angle(current_angle, target_angle,
self.damp_angle, self.damp_factor)
#if abs(target_angle - np.pi) < self.min_angle:
# target_angle -= self.min_angle
joint_delta_angle = np.pi - target_angle
joint_delta_q = quaternion_about_axis(joint_delta_angle,
self.local_joint_axis)
joint_delta_q = normalize(joint_delta_q)
frame[self.joint_indices] = joint_delta_q
return joint_delta_q
def virtual(worktarget=[945, 0, 0], cartesians=[945, 0, 1375]):
# returns a virtual target that the robot can actually reach
# virtual target will be pointed towards worktarget and located at cartesians
# Quaternion [0,0,1,0] has z vector [0,0,-1]
z0 = [0, 0, -1]
zq0 = [0, 0, 1, 0]
a = [[0], [0], [1], [1]]
if (len(cartesians) == 3) & (len(worktarget) == 3):
#unit direction vector from defined robot position to worktarget position
dvec = unitv((worktarget - numpy.array(cartesians)))
if (~parallel(dvec, z0)):
rotvec = numpy.cross(dvec, z0)
rotangle = numpy.arcsin(norm(rotvec) / (norm(z0) * norm(dvec)))
rotquat = transformations.quaternion_about_axis(-rotangle, rotvec)
virtualresult = [
cartesians,
transformations.quaternion_multiply(rotquat, zq0)
]
else:
virtualresult = [cartesians, zq0]
return virtualresult
else:
return False
def draw_vbars(self):
color = medium_orchid
size = self.line_width
a1 = 10.0
a2 = 1.5
a3 = 3.0
q0 = transformations.quaternion_about_axis(self.psi_rad,
[0.0, 0.0, -1.0])
a0 = self.ap_pitch
# rotation point (about nose)
rot = self.ladder_helper(q0, self.the_rad * r2d, 0.0)
if rot == None:
return
# center point
tmp1 = self.ladder_helper(q0, a0, 0.0)
if tmp1 == None:
return
center = self.rotate_pt(tmp1, rot, self.ap_roll * d2r)
# right vbar
tmp1 = self.ladder_helper(q0, a0 - a3, a1)
tmp2 = self.ladder_helper(q0, a0 - a3, a1 + a3)
tmp3 = self.ladder_helper(q0, a0 - a2, a1 + a3)
if tmp1 != None and tmp2 != None and tmp3 != None:
uv1 = self.rotate_pt(tmp1, rot, self.ap_roll * d2r)
uv2 = self.rotate_pt(tmp2, rot, self.ap_roll * d2r)
uv3 = self.rotate_pt(tmp3, rot, self.ap_roll * d2r)
if uv1 != None and uv2 != None and uv3 != None:
cv2.line(self.frame, center, uv1, color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, center, uv3, color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, uv1, uv2, color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, uv1, uv3, color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, uv2, uv3, color, self.line_width,
cv2.LINE_AA)
# left vbar
tmp1 = self.ladder_helper(q0, a0 - a3, -a1)
tmp2 = self.ladder_helper(q0, a0 - a3, -a1 - a3)
tmp3 = self.ladder_helper(q0, a0 - a2, -a1 - a3)
if tmp1 != None and tmp2 != None and tmp3 != None:
uv1 = self.rotate_pt(tmp1, rot, self.ap_roll * d2r)
uv2 = self.rotate_pt(tmp2, rot, self.ap_roll * d2r)
uv3 = self.rotate_pt(tmp3, rot, self.ap_roll * d2r)
if uv1 != None and uv2 != None and uv3 != None:
cv2.line(self.frame, center, uv1, color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, center, uv3, color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, uv1, uv2, color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, uv1, uv3, color, self.line_width,
cv2.LINE_AA)
cv2.line(self.frame, uv2, uv3, color, self.line_width,
cv2.LINE_AA)
def transform_quaternion_frames_legacy(quat_frames,
angles,
offset,
rotation_order=None):
""" Applies a transformation on the root joint of a list quaternion frames.
Parameters
----------
*quat_frames: np.ndarray
\tList of frames where the rotation is represented as euler angles in degrees.
*angles: list of floats
\tRotation angles in degrees
*offset: np.ndarray
\tTranslation
"""
if rotation_order is None:
rotation_order = DEFAULT_ROTATION_ORDER
offset = np.array(offset)
if round(angles[0], 3) == 0 and round(angles[2], 3) == 0:
rotation_q = quaternion_about_axis(np.deg2rad(angles[1]), [0, 1, 0])
else:
rotation_q = euler_to_quaternion(angles, rotation_order)
rotation_matrix = euler_angles_to_rotation_matrix(angles,
rotation_order)[:3, :3]
for frame in quat_frames:
ot = frame[:3]
oq = frame[3:7]
frame[:3] = np.dot(rotation_matrix, ot) + offset
frame[3:7] = quaternion_multiply(rotation_q, oq)
return quat_frames
def main():
imu_data = np.loadtxt('./data/imu_noise.txt')
gt_data = np.loadtxt('./data/traj_gt.txt')
imu_parameters = load_imu_parameters()
init_nominal_state = np.zeros((19, ))
init_nominal_state[:10] = gt_data[0, 1:] # init p, q, v
init_nominal_state[10:13] = 0 # init ba
init_nominal_state[13:16] = 0 # init bg
init_nominal_state[16:19] = np.array([0, 0, -9.81]) # init g
estimator = ESEKF(init_nominal_state, imu_parameters)
test_duration_s = [0., 61.]
start_time = imu_data[0, 0]
mask_imu = np.logical_and(
imu_data[:, 0] <= start_time + test_duration_s[1],
imu_data[:, 0] >= start_time + test_duration_s[0])
mask_gt = np.logical_and(gt_data[:, 0] <= start_time + test_duration_s[1],
gt_data[:, 0] >= start_time + test_duration_s[0])
imu_data = imu_data[mask_imu, :]
gt_data = gt_data[mask_gt, :]
traj_est = [gt_data[0, :8]]
update_ratio = 10 # control the frequency of ekf updating.
sigma_measurement_p = 0.02 # in meters
sigma_measurement_q = 0.015 # in rad
sigma_measurement = np.eye(6)
sigma_measurement[0:3, 0:3] *= sigma_measurement_p**2
sigma_measurement[3:6, 3:6] *= sigma_measurement_q**2
for i in range(1, imu_data.shape[0]):
timestamp = imu_data[i, 0]
estimator.predict(imu_data[i, :])
if i % update_ratio == 0:
# we assume the timestamps are aligned.
assert math.isclose(gt_data[i, 0], timestamp)
gt_pose = gt_data[i, 1:8].copy() # gt_pose = [p, q]
# add position noise
gt_pose[:3] += np.random.randn(3, ) * sigma_measurement_p
# add rotation noise, u = [1, 0.5 * noise_angle_axis]
# u = 0.5 * np.random.randn(4,) * sigma_measurement_q
# u[0] = 1
u = np.random.randn(3, ) * sigma_measurement_q
qn = tr.quaternion_about_axis(la.norm(u), u / la.norm(u))
gt_pose[3:] = tr.quaternion_multiply(gt_pose[3:], qn)
# update filter by measurement.
estimator.update(gt_pose, sigma_measurement)
print('[%f]:' % timestamp, estimator.nominal_state)
frame_pose = np.zeros(8, )
frame_pose[0] = timestamp
frame_pose[1:] = estimator.nominal_state[:7]
traj_est.append(frame_pose)
# save trajectory to TUM format
traj_est = np.array(traj_est)
np.savetxt('./data/traj_gt_out.txt', gt_data[:, :8])
np.savetxt('./data/traj_esekf_out.txt', traj_est)
def get_orient_matrix(struct_vec, ref_vec): '''used by orient_to_principal_axes function below to calculate the matrix to align two vectors''' rotvec = numpy.cross(struct_vec,ref_vec) # the vector around which we will rotate to align sine = numpy.linalg.norm(rotvec) cosine = numpy.dot(struct_vec,ref_vec) angle = atan2(sine, cosine) # the angle around the axis we will spin q = transformations.quaternion_about_axis(angle, rotvec) matrix = numpy.matrix(transformations.quaternion_matrix(q)) # convert the quaternion to a rotation matrix return matrix
def rotate_around(self, angle, axis):
'''
Rotate the render object around the axis.
Raises an error if self.orientation is None.
'''
ori = self.orientation
if ori is None:
raise NotImplemented("This RenderObject has no orientation")
self.orientation = quaternion_multiply(ori, quaternion_about_axis(angle, axis))
def vec2quat(vec, rotation=numpy.radians(0)):
z0 = [0,0,1]; zq0 = [1,0,0,0]; a = [[0],[0],[1],[1]];
if (len(vec) == 3):
vec = vec / numpy.linalg.norm(vec)
if (~parallel(vec,z0)):
rotvec = numpy.cross(vec, z0);
rotangle = numpy.arcsin(numpy.linalg.norm(rotvec)/(numpy.linalg.norm(z0)*numpy.linalg.norm(vec)))
rotquat = transformations.quaternion_about_axis(-rotangle, rotvec)
quats = transformations.quaternion_multiply(rotquat, zq0)
else: quats = zq0
#check the rotation to see if we need to fix the sign by rotatating 180 degrees
if numpy.sum(vec + numpy.dot(transformations.quaternion_matrix(quats), [0,0,1,1])[0:3]) < 1e-6:
quats = transformations.quaternion_multiply(quats, [0,0,1,0])
quats = transformations.quaternion_multiply(quats, transformations.quaternion_about_axis(rotation, [0,0,1]))
return quats
else: return False
def apply(self, q):
sq, tq = swing_twist_decomposition(q, self.swing_axis)
tv, ta = quaternion_to_axis_angle(tq)
if self.verbose:
print("before", np.degrees(ta), tv)
if self.angle_range is not None:
ta = max(ta, self.angle_range[0])
ta = min(ta, self.angle_range[1])
if self.verbose:
print("after", np.degrees(ta), tv)
return quaternion_about_axis(ta, self.swing_axis)
def get_transform(self, theta: float = 0.0) -> transform.Transform:
if self.joint.joint_type == 'revolute':
t = transform.Transform(
tf.quaternion_about_axis(theta, self.joint.axis))
elif self.joint.joint_type == 'prismatic':
t = transform.Transform(pos=theta * self.joint.axis)
elif self.joint.joint_type == 'fixed':
t = transform.Transform()
else:
raise ValueError("Unsupported joint type %s." %
self.joint.joint_type)
return self.joint.offset * t
def find_rotation_analytically(new_skeleton,
free_joint_name,
target,
frame,
joint_cos_map,
is_root=False,
max_iter_count=10,
twist_angle=None):
global_src_up_vec = target[0]
if twist_angle is None:
global_src_x_vec = target[1]
else:
global_src_x_vec = None
local_target_axes = dict(joint_cos_map[free_joint_name])
if is_root:
q = align_root_joint(new_skeleton, free_joint_name, local_target_axes,
global_src_up_vec, global_src_x_vec,
joint_cos_map, max_iter_count)
else:
# first align the bone vectors
q = [1, 0, 0, 0]
qy, axes = align_axis_in_place(local_target_axes, "y",
global_src_up_vec)
q = quaternion_multiply(qy, q)
q = normalize(q)
# then align the twisting angles
if global_src_x_vec is not None:
qx, axes = align_axis_in_place(axes, "x", global_src_x_vec)
q = quaternion_multiply(qx, q)
q = normalize(q)
#if "FK" in free_joint_name:
# q = to_local_cos(new_skeleton, free_joint_name, frame, q)
if new_skeleton.nodes[free_joint_name].parent is not None:
#if "upLeg" in free_joint_name: # it does not work for the legs for some reason
# q = to_local_cos(new_skeleton, new_skeleton.nodes[free_joint_name].parent.node_name, frame, q)
#else:
q = to_local_cos(new_skeleton,
new_skeleton.nodes[free_joint_name].parent.node_name,
frame, q)
if twist_angle is not None:
# separate rotation
local_twist_axis = np.array(joint_cos_map[free_joint_name]["y"])
swing_q, twist_q = swing_twist_decomposition(q, local_twist_axis)
# replace
twist_q = quaternion_about_axis(-twist_angle, local_twist_axis)
q = quaternion_multiply(swing_q, twist_q)
q = normalize(q)
return q
def on_mouse_move(self, event):
# drag to rotate
if event.is_dragging:# and not self.timer.running:
# drag delta
dx = event.pos[0]-self.last_drag_pos[0]
dy = event.pos[1]-self.last_drag_pos[1]
dampen = 0.05
# quaternion rotation
qy = tr.quaternion_about_axis(dx*dampen, [0,-1,0])
qx = tr.quaternion_about_axis(dy*dampen, [-1,0,0])
q = tr.quaternion_multiply(qx,qy)
self.rot = tr.quaternion_multiply(self.rot, q)
# rotate model
self.model = tr.quaternion_matrix(self.rot)
# update model
self.program_data['u_model'] = self.model
self.program_axis['u_model'] = self.model
self.program_plane['u_model'] = self.model
self.update()
self.last_drag_pos = event.pos
pass
def on_mouse_move(self, event):
# drag to rotate
if event.is_dragging: # and not self.timer.running:
# drag delta
dx = event.pos[0] - self.last_drag_pos[0]
dy = event.pos[1] - self.last_drag_pos[1]
dampen = 0.05
# quaternion rotation
qy = tr.quaternion_about_axis(dx * dampen, [0, -1, 0])
qx = tr.quaternion_about_axis(dy * dampen, [-1, 0, 0])
q = tr.quaternion_multiply(qx, qy)
self.rot = tr.quaternion_multiply(self.rot, q)
# rotate model
self.model = tr.quaternion_matrix(self.rot)
# update model
self.program_data['u_model'] = self.model
self.program_axis['u_model'] = self.model
self.program_plane['u_model'] = self.model
self.update()
self.last_drag_pos = event.pos
pass
def vec2quat2(vector, partAxis):
partAxis = [-1,0,0]
m = numpy.cross(vector, numpy.cross(partAxis, vector))
m = m/numpy.linalg.norm(m)
m = vecDirection(m, [-1,0,0])
print 'avec', vector, m
T = alignVectors(x1=vector, m1=m, x=[0,0,1], m = [1,0,0])
q = transformations.quaternion_from_matrix(T)
qr = transformations.quaternion_about_axis(numpy.radians(180), [0,0,1])
qf = transformations.quaternion_multiply(q,qr)
return q
def q_to_mat(dt):
w = np.linalg.norm(dt[3:5])
if w < 1:
w = sqrt(1 - pow(w, 1))
quaternion = tf.quaternion_about_axis(w, (dt[3], dt[4], dt[5]))
m = tf.quaternion_matrix(quaternion)
else:
m = np.eye(4, 4)
m[0, 3] = dt[0]
m[1, 3] = dt[1]
m[2, 3] = dt[2]
return np.matrix(m)
def apply_global(self, pm, q):
axis = np.dot(pm, self.axis)
axis = normalize(axis)
sq, tq = swing_twist_decomposition(q, axis)
tv, ta = quaternion_to_axis_angle(tq)
if self.verbose:
print("before", np.degrees(ta), tv)
if self.angle_range is not None:
ta = max(ta, self.angle_range[0])
ta = min(ta, self.angle_range[1])
if self.verbose:
print("after", np.degrees(ta), tv)
return quaternion_about_axis(ta, self.swing_axis)
def calc_joint_local_matrix(self):
self.L = np.eye(4)
# Add in rotation
q = np.r_[0.0, 0.0, 0.0, 1.0]
for i in range(3):
rot_vec = np.r_[0.0, 0.0, 0.0]
rot_vec[i] = 1.0
q = tr.quaternion_about_axis(np.radians(-self.rotation[i]), rot_vec)
# q = tr.quaternion_multiply(q, qi)
Q = np.matrix(tr.quaternion_matrix(q))
self.L = np.dot(self.L, Q)
# Add in translation on the first three columns of bottom row
self.L[3, :3] = self.translation
def quaternion_from_rotvec(r):
"""Returns the quaternion equivalent to the given rotation vector r.
>>> r = np.pi * np.array([0,1,0])
>>> q = quaternion_from_rotvec(r)
>>> M1 = trns.quaternion_matrix(q)
>>> M2 = trns.rotation_matrix(np.pi, [0,1,0])
>>> print(trns.is_same_transform(M1, M2))
True
"""
angle = np.mod(npl.norm(r), 2 * np.pi)
if not np.isclose(angle, 0):
return trns.quaternion_about_axis(angle, r / angle)
else:
return np.array([1, 0, 0, 0]) # unit real number is identity quaternion
import robot import transformations as tr import numpy as np r = robot.Robot() print "joints" print r.getJoints() r.setCartesian([[800,0,1000], [0,0,1,0]]) #quat [0,0,1,0] is tool # flange pointed down quat_down = [0,0,1,0] quat_stuff = tr.quaternion_multiply(quat_down, tr.quaternion_about_axis(np.radians(30), [0,1,0])) #rotate 30 degrees # around the Y axis r.setCartesian([[800,0,1000], quat_stuff]) r.close()
import math
import numpy as np
import random
import sys
sys.path.append('../lib')
import transformations
# start with the identity
#sum = transformations.quaternion_from_euler(0.0, 0.0, 0.0, axes='sxyz')
sum = np.zeros(4)
count = 0
for i in range(0,1000):
rot = random.random()*0.25-0.125
print "rotation =", rot
quat = transformations.quaternion_about_axis(rot, [1, 0, 0])
print "quat =", quat
count += 1
sum[0] += quat[0]
sum[1] += quat[1]
sum[2] += quat[2]
sum[3] += quat[3]
w = sum[0] / float(count)
x = sum[1] / float(count)
y = sum[2] / float(count)
z = sum[3] / float(count)
new_quat = np.array( [ w, x, y, z] )
print "new_quat (raw) =", new_quat
