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 main(): # tcp = (0, 0, 89.1, 0, 0, -45) tcp = (0, 0, 89.1, 0, 0, -135) print(f"tcp: {tcp}") # offset = (0, 0, 0, -180, 0, 0) offset = (0, 0, 0, 180, 0, 180) print(f"offset: {offset}") os_tcp = quat2euler(offset_tcp(euler2quat(tcp), euler2quat(offset))) print(f"offset tcp: {os_tcp}") # pose = (0, 0, 0, -180, 0, 0) pose = (0, 0, 0, 180, 0, 180) print(f"pose: {pose}") os_pose = quat2euler( pose_to_offset_pose(euler2quat(pose), euler2quat(offset))) print(f"offset pose: {os_pose}") rec_pose = quat2euler( offset_pose_to_pose(euler2quat(os_pose), euler2quat(offset))) print(f"recovered pose: {rec_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 coord_frame(self, frame): """Sets the reference coordinate frame for the robot. """ check_pose(frame) self._coord_frame_q = euler2quat(frame, self._axes)
def tcp(self, tcp): """Sets the tool center point (TCP) of the robot. """ check_pose(tcp) self.controller.tcp = euler2quat(tcp, self._axes)