def diff_drive(self, robot, index, target_pose): """ this diff drive is very hacky it tries to transport the target pose to match an end pose by computing the pose difference between current pose and target pose then it estimates a cartesian velocity for the end effector to follow. It uses differential IK to compute the required joint velocity, and set the joint velocity as current step target velocity. This technique makes the trajectory very unstable but it still works some times. """ pf = robot.get_compute_functions()['passive_force'](True, True, False) max_v = 0.1 max_w = np.pi qpos, qvel, poses = robot.get_observation() current_pose: Pose = poses[index] delta_p = target_pose.p - current_pose.p delta_q = qmult(target_pose.q, qinverse(current_pose.q)) axis, theta = quat2axangle(delta_q) if (theta > np.pi): theta -= np.pi * 2 t1 = np.linalg.norm(delta_p) / max_v t2 = theta / max_w t = max(np.abs(t1), np.abs(t2), 0.001) thres = 0.1 if t < thres: k = (np.exp(thres) - 1) / thres t = np.log(k * t + 1) v = delta_p / t w = theta / t * axis target_qvel = robot.get_compute_functions()['cartesian_diff_ik']( np.concatenate((v, w)), 9) robot.set_action(qpos, target_qvel, pf)
def get_yaw(self): o = self.client.getOrientation() vec, theta = quaternions.quat2axangle( [o.w_val, o.x_val, o.y_val, o.z_val]) new_vec = self.units.vel3d_from_as(vec) roll, pitch, yaw = euler.axangle2euler(new_vec, theta) return yaw
def diff_drive2(self, robot, index, target_pose, js1, joint_target, js2): """ This is a hackier version of the diff_drive It uses specified joints to achieve the target pose of the end effector while asking some other specified joints to match a global pose """ pf = robot.get_compute_functions()['passive_force'](True, True, False) max_v = 0.1 max_w = np.pi qpos, qvel, poses = robot.get_observation() current_pose: Pose = poses[index] delta_p = target_pose.p - current_pose.p delta_q = qmult(target_pose.q, qinverse(current_pose.q)) axis, theta = quat2axangle(delta_q) if (theta > np.pi): theta -= np.pi * 2 t1 = np.linalg.norm(delta_p) / max_v t2 = theta / max_w t = max(np.abs(t1), np.abs(t2), 0.001) thres = 0.1 if t < thres: k = (np.exp(thres) - 1) / thres t = np.log(k * t + 1) v = delta_p / t w = theta / t * axis target_qvel = robot.get_compute_functions()['cartesian_diff_ik']( np.concatenate((v, w)), 9) for j, target in zip(js2, joint_target): qpos[j] = target robot.set_action(qpos, target_qvel, pf)
def test_axis_angle_from_quaternion2(self): q = [0,0,0,1.0000001] axis2, angle2 = quat2axangle([q[-1],q[0],q[1],q[2]]) x,y,z, angle = speed_up_and_execute(spw.axis_angle_from_quaternion, list(q)) axis = [x,y,z] angle = float(angle) compare_axis_angle(angle, axis, angle2, axis2, 2)
def euler2axangle(z=0, y=0, x=0): ''' Return axis, angle corresponding to these Euler angles Uses the z, then y, then x convention above Parameters ---------- z : scalar Rotation angle in radians around z-axis (performed first) y : scalar Rotation angle in radians around y-axis x : scalar Rotation angle in radians around x-axis (performed last) Returns ------- vector : array shape (3,) axis around which rotation occurs theta : scalar angle of rotation Examples -------- >>> vec, theta = euler2axangle(0, 1.5, 0) >>> np.allclose(vec, [0, 1, 0]) True >>> theta 1.5 ''' # delayed import to avoid cyclic dependencies import transforms3d.quaternions as nq return nq.quat2axangle(euler2quat(z, y, x))
def test_quat2axangle(): ax, angle = tq.quat2axangle([1, 0, 0, 0]) assert_array_equal(ax, [1, 0, 0]) assert_array_equal(angle, 0) # Non-normalized quaternion, unit quaternion ax, angle = tq.quat2axangle([5, 0, 0, 0]) assert_array_equal(ax, [1, 0, 0]) assert_array_equal(angle, 0) # Rotation by 90 degrees around x r2d2 = np.sqrt(2) / 2. quat_x_90 = np.array([r2d2, r2d2, 0, 0]) ax, angle = tq.quat2axangle(quat_x_90) assert_almost_equal(ax, [1, 0, 0]) assert_almost_equal(angle, np.pi / 2) # Not-normalized version of same, gives same output ax, angle = tq.quat2axangle(quat_x_90 * 7) assert_almost_equal(ax, [1, 0, 0]) assert_almost_equal(angle, np.pi / 2) # Any non-finite value gives nan angle for pos in range(4): for val in np.nan, np.inf, -np.inf: q = [1, 0, 0, 0] q[pos] = val ax, angle = tq.quat2axangle(q) assert_almost_equal(ax, [1, 0, 0]) assert np.isnan(angle) # Infinite length likewise, because of length overflow f64info = np.finfo(np.float64) ax, angle = tq.quat2axangle([2, f64info.max, 0, 0]) assert_almost_equal(ax, [1, 0, 0]) assert np.isnan(angle) # Very small values give indentity transformation ax, angle = tq.quat2axangle([0, f64info.eps / 2, 0, 0]) assert_almost_equal(ax, [1, 0, 0]) assert angle == 0
def test_quat2axangle(): ax, angle = tq.quat2axangle([1, 0, 0, 0]) assert_array_equal(ax, [1, 0, 0]) assert_array_equal(angle, 0) # Non-normalized quaternion, unit quaternion ax, angle = tq.quat2axangle([5, 0, 0, 0]) assert_array_equal(ax, [1, 0, 0]) assert_array_equal(angle, 0) # Rotation by 90 degrees around x r2d2 = np.sqrt(2) / 2. quat_x_90 = np.array([r2d2, r2d2, 0, 0]) ax, angle = tq.quat2axangle(quat_x_90) assert_almost_equal(ax, [1, 0, 0]) assert_almost_equal(angle, np.pi / 2) # Not-normalized version of same, gives same output ax, angle = tq.quat2axangle(quat_x_90 * 7) assert_almost_equal(ax, [1, 0, 0]) assert_almost_equal(angle, np.pi / 2) # Any non-finite value gives nan angle for pos in range(4): for val in np.nan, np.inf, -np.inf: q = [1, 0, 0, 0] q[pos] = val ax, angle = tq.quat2axangle(q) assert_almost_equal(ax, [1, 0, 0]) assert np.isnan(angle) # Infinite length likewise, because of length overflow f64info = np.finfo(np.float64) ax, angle = tq.quat2axangle([2, f64info.max, 0, 0]) assert_almost_equal(ax, [1, 0, 0]) assert np.isnan(angle) # Very small values give indentity transformation ax, angle = tq.quat2axangle([0, f64info.eps / 2, 0, 0]) assert_almost_equal(ax, [1, 0, 0]) assert angle == 0
def test_axis_angle_from_quaternion(self, q): axis2, angle2 = quat2axangle([q[-1], q[0], q[1], q[2]]) axis = w.compile_and_execute( lambda x, y, z, w_: w.axis_angle_from_quaternion(x, y, z, w_)[0], q) angle = w.compile_and_execute( lambda x, y, z, w_: w.axis_angle_from_quaternion(x, y, z, w_)[1], q) compare_axis_angle(angle, axis, angle2, axis2, 2)
def test_axis_angle(): for M, q in eg_pairs: vec, theta = tq.quat2axangle(q) q2 = tq.axangle2quat(vec, theta) assert tq.nearly_equivalent(q, q2) aa_mat = taa.axangle2mat(vec, theta) assert_array_almost_equal(aa_mat, M) aa_mat2 = sympy_aa2mat(vec, theta) assert_array_almost_equal(aa_mat, aa_mat2) aa_mat22 = sympy_aa2mat2(vec, theta) assert_array_almost_equal(aa_mat, aa_mat22)
def _interpolate_transformation(t_1, t_2, coefficient): assert 0 <= coefficient < 1 t_result = np.identity(4) t_result[0:3, 3] = t_1[0:3, 3] * (1 - coefficient) + t_2[0:3, 3] * coefficient rot_diff = t_2[0:3, 0:3].dot(t_1[0:3, 0:3].transpose()) q_diff = quaternions.mat2quat(rot_diff) axis, angle = quaternions.quat2axangle(q_diff) rot_delta = quaternions.quat2mat(quaternions.axangle2quat(axis, angle * coefficient)) rot_result = rot_delta.dot(t_1[0:3, 0:3]) t_result[0:3, 0:3] = rot_result return t_result
def test_axis_angle(): for M, q in eg_pairs: vec, theta = tq.quat2axangle(q) q2 = tq.axangle2quat(vec, theta) assert tq.nearly_equivalent(q, q2) aa_mat = taa.axangle2mat(vec, theta) assert_array_almost_equal(aa_mat, M) aa_mat2 = sympy_aa2mat(vec, theta) assert_array_almost_equal(aa_mat, aa_mat2) aa_mat22 = sympy_aa2mat2(vec, theta) assert_array_almost_equal(aa_mat, aa_mat22)
def rotation(value, r): q0 = quaternions.axangle2quat( [float(value[0]), float(value[1]), float(value[2])], float(value[3])) q1 = quaternions.axangle2quat([r[0], r[1], r[2]], r[3]) qr = quaternions.qmult(q1, q0) v, theta = quaternions.quat2axangle(qr) return [ WebotsParser.str(v[0]), WebotsParser.str(v[1]), WebotsParser.str(v[2]), WebotsParser.str(theta) ]
def rot_diff(self, other): '''Compute rotation btw frames Arguments: other {Frame} -- target frame Returns: np.array, float -- axis and angle ''' dq = quaternions.qmult(other.q, quaternions.qinverse(self.q)) axis, angle = quaternions.quat2axangle(dq) return axis, angle
def test_angle_axis_imps(): for x, y, z in euler_tuples: M = ttb.euler2mat(z, y, x) q = tq.mat2quat(M) vec, theta = tq.quat2axangle(q) M1 = axangle2mat(vec, theta) M2 = axangle2aff(vec, theta)[:3,:3] assert_array_almost_equal(M1, M2) v1, t1 = mat2axangle(M1) M3 = axangle2mat(v1, t1) assert_array_almost_equal(M, M3) A = np.eye(4) A[:3,:3] = M v2, t2, point = aff2axangle(A) M4 = axangle2mat(v2, t2) assert_array_almost_equal(M, M4)
def rotation_mat2vec(rotation): """ Rotation vector from rotation matrix. Parameters ---------- rotation: array (3, 3) rotation matrix. Returns ------- vec: array (3, ) rotation vector, where norm of vec is the angle theta, and the axis of rotation is given by vec / theta. """ ax, angle = quat2axangle(mat2quat(rotation)) return ax * angle
def diff_drive(self, robot: PandaArm, index, target_pose, max_v=0.1, max_w=np.pi, active_joints_ids=[], override=None): obs = robot.observation qpos, qvel, poses = obs['qpos'], obs['qvel'], obs['poses'] current_pose: Pose = poses[index] delta_p = target_pose.p - current_pose.p delta_q = quat.qmult(target_pose.q, quat.qinverse(current_pose.q)) axis, theta = quat.quat2axangle(delta_q) if theta > np.pi: theta -= np.pi * 2 t1 = np.linalg.norm(delta_p) / max_v t2 = theta / max_w t = max(np.abs(t1), np.abs(t2), 0.01) thres = 0.1 if t < thres: k = (np.exp(thres) - 1) / thres t = np.log(k * t + 1) v = delta_p / t w = theta / t * axis index = index if index >= 0 else len(robot._robot.get_joints()) + index cal_qvel = robot.get_compute_functions()['cartesian_diff_ik']( np.concatenate((v, w)), index, active_joints_ids) target_qvel = np.zeros_like(qvel) indx = np.arange( len(qvel)) if active_joints_ids == [] else active_joints_ids target_qvel[indx] = cal_qvel pf = robot.get_compute_functions()['passive_force']() if override is not None: override_indx, override_qpos, override_qvel = override qpos[override_indx] = override_qpos target_qvel[override_indx] = override_qvel robot.set_action(qpos, target_qvel, pf)
def quar_axis_error(q_sp, q_state): """Compute the error in quaternions from the setpoints and robot state Args: q_sp (np.array): Reference signal Set point quaternion q_state (np.array): Sensor Feedback quaternion Returns: exponential angle (np.array) """ # Quaternion multiplication q_set * (q_state)' target - state q_state_conj = quaternions.qconjugate(q_state) q_error = quaternions.qmult(q_sp, q_state_conj) # Nearest rotation if (q_error[0] < 0): q_error = -1. * q_error axis_error = quaternions.quat2axangle(q_error) return axis_error[0] * axis_error[1]
def angular_distance(quat): vec, theta = quat2axangle(quat) return theta / np.pi * 180
def quatToAxisAngle(q): [vec, theta] = quat.quat2axangle(q) return theta * vec
def generate_one_im_anno(cnt, cam_name, peg_top, peg_bottom, hole_top, hole_bottom, rob_arm, im_data_path, delta_rotation_quat, delta_translation): # Get keypoint location peg_keypoint_top_pose = rob_arm.get_object_matrix(peg_top) peg_keypoint_bottom_pose = rob_arm.get_object_matrix(peg_bottom) hole_keypoint_top_pose = rob_arm.get_object_matrix(hole_top) hole_keypoint_bottom_pose = rob_arm.get_object_matrix(hole_bottom) peg_keypoint_top_in_world = peg_keypoint_top_pose[:3,3] peg_keypoint_bottom_in_world = peg_keypoint_bottom_pose[:3, 3] hole_keypoint_top_in_world = hole_keypoint_top_pose[:3,3] hole_keypoint_bottom_in_world = hole_keypoint_bottom_pose[:3, 3] # Get RGB images from camera rgb = rob_arm.get_rgb(cam_name=cam_name) # rotate rgb 180 (h, w) = rgb.shape[:2] center = (w / 2, h / 2) M = cv2.getRotationMatrix2D(center, 180, 1.0) rgb = cv2.warpAffine(rgb, M, (w, h)) # Get depth from camera depth = rob_arm.get_depth(cam_name=cam_name, near_plane=0.01, far_plane=1.5) # rotate depth 180 (h, w) = depth.shape[:2] center = (w / 2, h / 2) M = cv2.getRotationMatrix2D(center, 180, 1.0) depth = cv2.warpAffine(depth, M, (w, h)) depth_mm = (depth*1000).astype(np.uint16) # type: np.uint16 ; uint16 is needed by keypoint detection network #rob_arm.visualize_image(rgb=rgb, depth=depth) # Save image rgb_file_name = str(cnt).zfill(6)+'_rgb.png' rgb_file_path = os.path.join(im_data_path, rgb_file_name) depth_file_name = str(cnt).zfill(6)+'_depth.png' depth_file_path = os.path.join(im_data_path, depth_file_name) cv2.imwrite(rgb_file_path, rgb) cv2.imwrite(depth_file_path, depth_mm) # Get camera info cam_pos, cam_quat, cam_matrix= rob_arm.get_camera_pos(cam_name=cam_name) camera_to_world = {'quaternion':{'x':cam_quat[0], 'y':cam_quat[1], 'z':cam_quat[2], 'w':cam_quat[3]}, 'translation':{'x':cam_pos[0], 'y':cam_pos[1], 'z':cam_pos[2]}} camera2world_map = camera_to_world world2camera = world2camera_from_map(camera2world_map) peg_keypoint_top_in_world = np.append(peg_keypoint_top_in_world,[1],axis = 0).reshape(4,1) peg_keypoint_bottom_in_world = np.append(peg_keypoint_bottom_in_world, [1], axis=0).reshape(4, 1) hole_keypoint_top_in_world = np.append(hole_keypoint_top_in_world,[1],axis = 0).reshape(4,1) hole_keypoint_bottom_in_world = np.append(hole_keypoint_bottom_in_world,[1],axis = 0).reshape(4,1) peg_keypoint_top_in_camera = world2camera.dot(peg_keypoint_top_in_world) peg_keypoint_bottom_in_camera = world2camera.dot(peg_keypoint_bottom_in_world) hole_keypoint_top_in_camera = world2camera.dot(hole_keypoint_top_in_world) hole_keypoint_bottom_in_camera = world2camera.dot(hole_keypoint_bottom_in_world) peg_keypoint_top_in_camera = peg_keypoint_top_in_camera[:3].reshape(3,) peg_keypoint_bottom_in_camera = peg_keypoint_bottom_in_camera[:3].reshape(3, ) hole_keypoint_top_in_camera = hole_keypoint_top_in_camera[:3].reshape(3,) hole_keypoint_bottom_in_camera = hole_keypoint_bottom_in_camera[:3].reshape(3, ) keypoint_in_camera = np.array([peg_keypoint_top_in_camera, peg_keypoint_bottom_in_camera, hole_keypoint_top_in_camera, hole_keypoint_bottom_in_camera]) #keypoint_in_camera = np.array([peg_keypoint_bottom_in_camera, hole_keypoint_top_in_camera]) xy_depth = point2pixel(keypoint_in_camera) xy_depth_list = [] (h, w) = rgb.shape[:2] for i in range(len(xy_depth)): x = int(xy_depth[i][0]) y = int(xy_depth[i][1]) z = int(xy_depth[i][2]) xy_depth_list.append([x,y,z]) rot_vec, rot_theta = quat2axangle(delta_rotation_quat) delta_rotation_matrix = quat2mat(delta_rotation_quat) info = {'3d_keypoint_camera_frame': [[float(v) for v in peg_keypoint_top_in_camera], [float(v) for v in peg_keypoint_bottom_in_camera], [float(v) for v in hole_keypoint_top_in_camera], [float(v) for v in hole_keypoint_bottom_in_camera]], #'3d_keypoint_camera_frame':[ [float(v) for v in peg_keypoint_bottom_in_camera], # [float(v) for v in hole_keypoint_top_in_camera]], 'bbox_bottom_right_xy': [rgb.shape[1], rgb.shape[0]], 'bbox_top_left_xy': [0,0], 'camera_to_world': camera_to_world, 'depth_image_filename': depth_file_name, 'rgb_image_filename': rgb_file_name, 'keypoint_pixel_xy_depth': xy_depth_list, 'delta_rotation_quat': delta_rotation_quat.tolist(), 'delta_rotation_matrix': delta_rotation_matrix.tolist(), 'delta_translation': delta_translation.tolist(), 'delta_rot_vec': rot_vec.tolist(), 'delta_rot_theta': rot_theta } return info