def move_circular(self, via_pose, end_pose, elbow=None): """Executes a movement in a circular path from the current TCP pose, through via_pose, to end_pose in the reference coordinate frame. """ check_pose(via_pose) check_pose(end_pose) via_base_pose_q = inv_transform(euler2quat(via_pose, self._axes), self._coord_frame_q) target_base_pose_q = inv_transform(euler2quat(end_pose, self._axes), self._coord_frame_q) self.controller.move_circular(via_base_pose_q, target_base_pose_q, elbow)
def move_linear(self, pose, elbow=None): """Executes a linear/cartesian move from the current TCP pose to the specified pose in the reference coordinate frame. """ check_pose(pose) target_base_pose_q = inv_transform(euler2quat(pose, self._axes), self._coord_frame_q) self.controller.move_linear(target_base_pose_q, elbow)
def main(): axes = 'sxyz' # test inv_transform x1 = (10, 20, 30, 40, 50, 60) x2 = (0, 0, 0, 180, 10, 180) print("x1:", x1) print("x2:", x2) print("\nUsing homogeneous transforms:") x1_m = euler2mat(x1, axes) x2_m = euler2mat(x2, axes) x3_m = np.dot(np.linalg.inv(x2_m), x1_m) x3 = mat2euler(x3_m, axes) print("x1 expressed in x2 frame:", x3) x4_m = np.dot(x2_m, x3_m) x4 = mat2euler(x4_m, axes) print("... and transformed back to base frame:", x4) print("\nUsing quaternions:") x1_q = euler2quat(x1, axes) x2_q = euler2quat(x2, axes) x3_q = transform(x1_q, x2_q) x3 = quat2euler(x3_q, axes) print("x1 expressed in x2 frame:", x3) x4_q = inv_transform(x3_q, x2_q) x4 = quat2euler(x4_q, axes) print("... and transformed back to base frame:", x4) x5 = (0, 0, 79.0, 0, 0, 0) x6 = (41.01, -41.01, 80, -90, 90, -135) x7 = (0, 0, 77.5, 0, 0, -135) x5_m = euler2mat(x5, axes) x6_m = euler2mat(x6, axes) x7_m = euler2mat(x7, axes) x8_m = np.dot(np.linalg.inv(x5_m), x6_m) x9_m = np.dot(x7_m, x8_m) x9 = mat2euler(x9_m, axes) print("\nx9:", x9)
def pose_to_offset_pose(pose, offset): return inv_transform(offset, pose)
def offset_pose_to_pose(offset_pose, offset): inv_offset = inv_transform(euler2quat((0, 0, 0, 0, 0, 0)), offset) return inv_transform(inv_offset, offset_pose)
def offset_tcp(tcp, offset): return inv_transform(offset, tcp)