def transform_end_effector(pose, extra_pose, rot_type='quaternion'): """ Transform end effector pose pose: current pose [x, y, z, ax, ay, az, w] extra_pose: additional transformation [x, y, z, ax, ay, az, w] matrix: if true: return (translation, rotation matrix) else: return translation + quaternion list """ extra_translation = np.array(extra_pose[:3]).reshape(3, 1) extra_rot = tr.vector_to_pyquaternion(extra_pose[3:]).rotation_matrix c_trans = np.array(pose[:3]).reshape(3, 1) c_rot = tr.vector_to_pyquaternion(pose[3:]).rotation_matrix # BE CAREFUL!! Pose from KDL is ax ay az aw # Pose from IKfast is aw ax ay az n_trans = np.matmul(c_rot, extra_translation) + c_trans n_rot = np.matmul(c_rot, extra_rot) if rot_type == 'matrix': return n_trans.flatten(), n_rot quat_rot = np.roll( pyquaternion.Quaternion(matrix=n_rot).normalised.elements, -1) if rot_type == 'euler': euler = np.array(tr.euler_from_quaternion(quat_rot, axes='rxyz')) return np.concatenate((n_trans.flatten(), euler)) elif rot_type == 'quaternion': return np.concatenate((n_trans.flatten(), quat_rot))
def circular_trajectory(): initial_q = [5.1818, 0.3954, -1.5714, -0.3902, 1.5448, -0.5056] arm.set_joint_positions(initial_q, wait=True, t=2) target_position = [0.20, -0.30, 0.80] target_orienation = [0.01504, 0.01646, -0.01734, 0.9996] target_orienation = [0.18603, 0.01902, -0.01464, 0.98225] target_pose = target_position + target_orienation deltax = np.array([0., 0.05, 0.1, 0., 0., 0.]) initial_pose = transformations.pose_euler_to_quaternion(target_pose, deltax, ee_rotation=True) initial_pose = initial_pose[:3] final_pose = target_position[:3] target_q = transformations.vector_to_pyquaternion(target_orienation) p1 = target_q.rotate(initial_pose - final_pose) p2 = np.zeros(3) steps = 20 duration = 10 traj = traj_utils.circunference(p1, p2, steps) traj = np.apply_along_axis(target_q.rotate, 1, traj) trajectory = traj + final_pose for position in trajectory: cmd = np.concatenate([position, target_orienation]) arm.set_target_pose(cmd, wait=True, t=(duration/steps))
def convert_wrench(wrench_force, pose): ee_transform = tr.vector_to_pyquaternion(pose[3:]).transformation_matrix # # # Wrench force transformation wFtS = force_frame_transform(ee_transform) wrench = np.dot(wFtS, wrench_force) return wrench
def quaternions_orientation_error(Qd, Qc): """ Calculates the orientation error between to quaternions Qd is the desired orientation Qc is the current orientation both with respect to the same fixed frame return vector part """ if isinstance(Qd, Quaternion) and isinstance(Qd, Quaternion): ne = Qc.scalar * Qd.scalar + np.dot(np.array(Qc.vector).T, Qd.vector) ee = Qc.scalar * np.array(Qd.vector) - Qd.scalar * np.array( Qc.vector) + np.dot(skew(Qc.vector), Qd.vector) ee *= np.sign(ne) # disambiguate the sign of the quaternion return ee else: assert isinstance(Qd, (list, np.ndarray)) assert isinstance(Qc, (list, np.ndarray)) q1 = tr.vector_to_pyquaternion(Qd) q2 = tr.vector_to_pyquaternion(Qc) return quaternions_orientation_error(q1, q2)
def circular_trajectory2(): initial_q = [2.159, -1.8183, -1.9143, 0.4768, 2.5238, 4.6963] # print(arm.end_effector().tolist()) arm.set_joint_positions(initial_q, wait=True, t=1) target_position = [0.35951, -0.54521, 0.34393] target_orienation = spalg.face_towards(target_position, arm.end_effector()[:3])[3:].tolist() target_pose = target_position + target_orienation deltax = np.array([0., 0.0, 0.2, 0., 0., 0.]) initial_pose_ = transformations.pose_euler_to_quaternion(target_pose, deltax, ee_rotation=True) initial_pose = initial_pose_[:3] final_pose = target_position[:3] circle_orientation = [0,0,0,1.0] target_q = transformations.vector_to_pyquaternion(circle_orientation) p1 = target_q.rotate(initial_pose - final_pose) p2 = np.zeros(3) steps = 20 duration = 10 traj = traj_utils.circunference(p1, p2, steps, axes=[0,2,1]) traj = np.apply_along_axis(target_q.rotate, 1, traj) trajectory = traj + final_pose rot = spalg.face_towards(target_position, trajectory[0])[3:].tolist() cmd = np.concatenate([trajectory[0], rot]) arm.set_target_pose(cmd, wait=True, t=(duration/steps)) for position in trajectory: rot = spalg.face_towards(target_position, position)[3:].tolist() cmd = np.concatenate([position, rot]) cmd_q = arm._solve_ik(cmd) if check_ik(arm.joint_angles(), cmd_q): arm.set_target_pose(cmd, wait=True, t=(duration/steps)) from ur_gazebo.gazebo_spawner import GazeboModels spawner = GazeboModels('ur3_gazebo') m1 = create_gazebo_marker(initial_pose_, "base_link", marker_id="obj") spawner.load_models([m1])
def gazebo_callback(msg): global robot robot = [] world = np.zeros(7) world[6] = 1 for i, obj_name in enumerate(msg.name): if obj_name.endswith("robot"): pose = msg.pose[i] robot = np.concatenate([conversions.from_point(pose.position), conversions.from_quaternion(pose.orientation)]) # subtract robot_position to camera_position world[:3] -= robot[:3] world[:3] = Quaternion(np.roll(robot[3:],1)).inverse.rotate(world[:3]) world[3:] = np.roll((Quaternion(axis=[1.0, 0.0, 0.0], degrees=-90).inverse * Quaternion(np.roll(world[3:],1))).elements,-1) world[3:] = transformations.vector_from_pyquaternion(transformations.vector_to_pyquaternion([0.5,-0.5,-0.5,0.5]).inverse) # world[3:] = np.roll((Quaternion(axis=[0.0, 0.0, 0.0], degrees=0).inverse * Quaternion(np.roll(world[3:],1))).elements,-1) # print(world)orientation br.sendTransform(world[:3], world[3:], rospy.Time.now(), "sim_world", "base_link")