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