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)