Add rotation conventions to IK (#70)

Co-authored-by: AndreaAntonello <[email protected]>

Author: andreaantonello

Pipfile

@@ -8,6 +8,7 @@ requests = "*"
 websockets = "*"
 zeroconf = "==0.27.1"
 dataclasses = "*"
+pytransform3d = "==1.2.1"
 
 [dev-packages]
 flake8 = "*"

evasdk/Eva.py

@@ -14,7 +14,8 @@ class Eva:
     def __init__(self, host_ip, token, request_timeout=5, renew_period=60 * 20):
         parsed_host_ip = strip_ip(host_ip)
 
-        self.__http_client = EvaHTTPClient(parsed_host_ip, token, request_timeout=request_timeout, renew_period=renew_period)
+        self.__http_client = EvaHTTPClient(parsed_host_ip, token, request_timeout=request_timeout,
+                                           renew_period=renew_period)
         self.__logger = logging.getLogger('evasdk.Eva:{}'.format(host_ip))
 
         self.__eva_locker = EvaWithLocker(self)
@@ -41,11 +42,13 @@ def __exit__(self, type, value, traceback):
     # --------------------------------------------- HTTP HANDLERS ---------------------------------------------
     def api_call_with_auth(self, method, path, payload=None, headers={}, timeout=None, version='v1'):
         self.__logger.debug('Eva.api_call_with_auth {} {}'.format(method, path))
-        return self.__http_client.api_call_with_auth(method, path, payload=payload, headers=headers, timeout=timeout, version=version)
+        return self.__http_client.api_call_with_auth(method, path, payload=payload, headers=headers, timeout=timeout,
+                                                     version=version)
 
     def api_call_no_auth(self, method, path, payload=None, headers={}, timeout=None, version='v1'):
         self.__logger.debug('Eva.api_call_no_auth {} {}'.format(method, path))
-        return self.__http_client.api_call_no_auth(method, path, payload=payload, headers=headers, timeout=timeout, version=version)
+        return self.__http_client.api_call_no_auth(method, path, payload=payload, headers=headers, timeout=timeout,
+                                                   version=version)
 
 
     # Global
@@ -197,9 +200,11 @@ def control_go_to(self, joints, wait_for_ready=True, max_speed=None, time_sec=No
         self.__logger.info('Eva.control_go_to called')
         if mode == 'teach':
             with self.__eva_locker.set_renew_period(Eva.__TEACH_RENEW_PERIOD):
-                return self.__http_client.control_go_to(joints, wait_for_ready=wait_for_ready, max_speed=max_speed, time_sec=time_sec, mode=mode)
+                return self.__http_client.control_go_to(joints, wait_for_ready=wait_for_ready, max_speed=max_speed,
+                                                        time_sec=time_sec, mode=mode)
         else:
-            return self.__http_client.control_go_to(joints, wait_for_ready=wait_for_ready, max_speed=max_speed, time_sec=time_sec, mode=mode)
+            return self.__http_client.control_go_to(joints, wait_for_ready=wait_for_ready, max_speed=max_speed,
+                                                    time_sec=time_sec, mode=mode)
 
 
     def control_pause(self, wait_for_paused=True):
@@ -233,9 +238,11 @@ def calc_forward_kinematics(self, joints, fk_type='both', tcp_config=None):
         return self.__http_client.calc_forward_kinematics(joints, fk_type=fk_type, tcp_config=tcp_config)
 
 
-    def calc_inverse_kinematics(self, guess, target_position, target_orientation, tcp_config=None):
+    def calc_inverse_kinematics(self, guess, target_position, target_orientation, tcp_config=None,
+                                orientation_type=None):
         self.__logger.debug('Eva.calc_inverse_kinematics called')
-        return self.__http_client.calc_inverse_kinematics(guess, target_position, target_orientation, tcp_config=tcp_config)
+        return self.__http_client.calc_inverse_kinematics(guess, target_position, target_orientation,
+                                                          tcp_config=tcp_config, orientation_type=orientation_type)
 
 
     def calc_nudge(self, joints, direction, offset, tcp_config=None):

evasdk/eva_http_client.py

@@ -2,6 +2,7 @@
 import time
 import logging
 import requests
+import pytransform3d.rotations as pyrot  # type: ignore
 
 from .robot_state import RobotState
 from .eva_errors import eva_error, EvaError, EvaAutoRenewError
@@ -426,9 +427,41 @@ def calc_forward_kinematics(self, joints, fk_type='both', tcp_config=None):
         else:
             eva_error('calc_forward_kinematics invalid fk_type {}'.format(fk_type), r)
 
+    def calc_inverse_kinematics(self, guess, target_position, target_orientation, tcp_config=None,
+                                orientation_type=None):
+        """
+        End-effector orientation (target_orientation) can be provided in several standard formats,
+        by specifying the orinetation_type (default is None):
+        - 'matrix': rotation matrix -> 3x3 array, in row major order
+        - 'axis_angle': axis angle -> {'angle': float, 'x': float, 'y': float, 'z': float}
+        - 'euler_zyx': {yaw, pitch, roll} Euler (Tait-Bryan) angles -> {'yaw': float, 'pitch': float, 'roll': float}
+        - 'quat': quaternion -> {'w': float, 'x': float, 'y': float, 'z': float}
+        - None: defaults to quaternion
+        Conversion relies on pytransform3d library
+        """
+        N_DIGITS = 8
+        quat_not_normed = None
+
+        if orientation_type == 'matrix':
+            quat_not_normed = pyrot.quaternion_from_matrix(pyrot.check_matrix(target_orientation))
+        elif orientation_type == 'axis_angle':
+            axis_angle = [target_orientation['x'], target_orientation['y'], target_orientation['z'],
+                          target_orientation['angle']]
+            quat_not_normed = pyrot.quaternion_from_axis_angle(pyrot.check_axis_angle(axis_angle))
+        elif orientation_type == 'euler_zyx':
+            euler_zyx = [target_orientation['yaw'], target_orientation['pitch'], target_orientation['roll']]
+            matrix = pyrot.matrix_from_euler_zyx(euler_zyx)
+            quat_not_normed = pyrot.quaternion_from_matrix(pyrot.check_matrix(matrix))
+        elif orientation_type == 'quat' or orientation_type is None:
+            quat_not_normed = [target_orientation['w'], target_orientation['x'], target_orientation['y'],
+                               target_orientation['z']]
+        else:
+            eva_error(f'calc_inverse_kinematics invalid "{orientation_type}" orientation_type')
+
+        quat_normed = [round(num, N_DIGITS) for num in pyrot.check_quaternion(quat_not_normed)]
+        quaternion = {'w': quat_normed[0], 'x': quat_normed[1], 'y': quat_normed[2], 'z': quat_normed[3]}
 
-    def calc_inverse_kinematics(self, guess, target_position, target_orientation, tcp_config=None):
-        body = {'guess': guess, 'position': target_position, 'orientation': target_orientation}
+        body = {'guess': guess, 'position': target_position, 'orientation': quaternion}
         if tcp_config is not None:
             body['tcp_config'] = tcp_config
 

setup.py

@@ -21,6 +21,7 @@
         'websockets',
         'zeroconf',
         'dataclasses',
+        'pytransform3d',
     ],
     classifiers=[
         "Programming Language :: Python :: 3",