From f3be0d1e65a980e1c901a08019b066a4039f1cac Mon Sep 17 00:00:00 2001 From: victorlouisdg Date: Thu, 1 Feb 2024 14:33:50 +0100 Subject: [PATCH 1/7] Initial path types --- airo-typing/airo_typing/__init__.py | 60 ++++++++++++++++++++++++++++- 1 file changed, 58 insertions(+), 2 deletions(-) diff --git a/airo-typing/airo_typing/__init__.py b/airo-typing/airo_typing/__init__.py index 3add0c8..34272ca 100644 --- a/airo-typing/airo_typing/__init__.py +++ b/airo-typing/airo_typing/__init__.py @@ -1,5 +1,5 @@ from dataclasses import dataclass -from typing import Dict, Optional, Tuple, Union +from typing import Callable, Dict, List, Optional, Tuple, Union import numpy as np @@ -69,11 +69,67 @@ shorthand notation is ^C T^B_A, where C is the frame the velocity is measured in, B is the frame the velocity is expressed in. """ -# manipulator types +##################### +# Manipulator types # +##################### JointConfigurationType = np.ndarray """an (N,) numpy array that represents the joint angles for a robot""" +######################### +# Single-arm path types # +######################### +JointPathType = List[JointConfigurationType] +""" a list of joint configurations that describe a path in joint space""" + +JointPathArrayType = np.ndarray +""" a (N, DoFs) numpy array that represents a path in joint space""" + +PosePathType = List[HomogeneousMatrixType] +""" a list of homogeneous matrices that describe a path in cartesian space""" + +PosePathArrayType = np.ndarray +""" a (N, 4, 4) numpy array that represents a path in cartesian space""" + +TimePathType = List[float] +""" a list of times that describe a path in time, must be monotonically increasing""" + +TimePathArrayType = np.ndarray +""" a (N,) numpy array that represents a path in time, must be monotonically increasing""" + +####################### +# Dual-arm path types # +####################### +DualJointConfigurationType = np.ndarray +""" a (2 * DoFs,) numpy array, result of np.hstack((left_joints, right_joints))""" + +DualJointPathType = List[DualJointConfigurationType] +""" a list of dual arm joint configurations that describe a path in joint space""" + +DualJointPathArrayType = np.ndarray +""" a (N, 2 * DoFs) array, result of np.hstack((left_joint_path, right_joint_path))""" + +####################### +# Single-arm types # +####################### +ForwardKinematicsType = Callable[[JointConfigurationType], HomogeneousMatrixType] +""" a function that computes the forward kinematics of a given joint configuration""" + +InverseKinematicsType = Callable[[HomogeneousMatrixType], List[JointConfigurationType]] +""" a function that computes one or more inverse kinematics solutions of a given TCP pose""" + + +######################### +# Motion planning types # +######################### + +JointConfigurationCheckerType = Callable[[JointConfigurationType], bool] +""" a function that checks a certain condition on a joint configuration, e.g. collision checking""" + +DualJointConfigurationCheckerType = Callable[[DualJointConfigurationType], bool] +""" a function that checks a dual joint configuration, where the JointConfigurationType is the stacked version of the left and right joint configurations""" + + ###################### # camera related types ###################### From baafc8a0e3f1c59183c19a724250b64f89aaf5db Mon Sep 17 00:00:00 2001 From: victorlouisdg Date: Sat, 3 Feb 2024 20:50:22 +0100 Subject: [PATCH 2/7] minimal types proposal --- airo-typing/airo_typing/__init__.py | 45 +++++------------------------ 1 file changed, 8 insertions(+), 37 deletions(-) diff --git a/airo-typing/airo_typing/__init__.py b/airo-typing/airo_typing/__init__.py index 34272ca..85c8cef 100644 --- a/airo-typing/airo_typing/__init__.py +++ b/airo-typing/airo_typing/__init__.py @@ -76,49 +76,24 @@ JointConfigurationType = np.ndarray """an (N,) numpy array that represents the joint angles for a robot""" -######################### -# Single-arm path types # -######################### -JointPathType = List[JointConfigurationType] -""" a list of joint configurations that describe a path in joint space""" - -JointPathArrayType = np.ndarray -""" a (N, DoFs) numpy array that represents a path in joint space""" - -PosePathType = List[HomogeneousMatrixType] -""" a list of homogeneous matrices that describe a path in cartesian space""" +JointPathType = np.ndarray +""" a (T, N) array of joint states (can be position/velocity/acceleration) that describe a path in joint space""" -PosePathArrayType = np.ndarray -""" a (N, 4, 4) numpy array that represents a path in cartesian space""" +PosePathType = np.ndarray +""" a (T, 4, 4) list of homogeneous matrices that describe a path in cartesian space""" -TimePathType = List[float] -""" a list of times that describe a path in time, must be monotonically increasing""" - -TimePathArrayType = np.ndarray -""" a (N,) numpy array that represents a path in time, must be monotonically increasing""" - -####################### -# Dual-arm path types # -####################### -DualJointConfigurationType = np.ndarray -""" a (2 * DoFs,) numpy array, result of np.hstack((left_joints, right_joints))""" - -DualJointPathType = List[DualJointConfigurationType] -""" a list of dual arm joint configurations that describe a path in joint space""" - -DualJointPathArrayType = np.ndarray -""" a (N, 2 * DoFs) array, result of np.hstack((left_joint_path, right_joint_path))""" +TimePathType = np.ndarray +""" a (T,) array of monotonically increasing times (float), corresponding to a path""" ####################### # Single-arm types # ####################### -ForwardKinematicsType = Callable[[JointConfigurationType], HomogeneousMatrixType] +ForwardKinematicsFunctionType = Callable[[JointConfigurationType], HomogeneousMatrixType] """ a function that computes the forward kinematics of a given joint configuration""" -InverseKinematicsType = Callable[[HomogeneousMatrixType], List[JointConfigurationType]] +InverseKinematicsFunctionType = Callable[[HomogeneousMatrixType], List[JointConfigurationType]] """ a function that computes one or more inverse kinematics solutions of a given TCP pose""" - ######################### # Motion planning types # ######################### @@ -126,10 +101,6 @@ JointConfigurationCheckerType = Callable[[JointConfigurationType], bool] """ a function that checks a certain condition on a joint configuration, e.g. collision checking""" -DualJointConfigurationCheckerType = Callable[[DualJointConfigurationType], bool] -""" a function that checks a dual joint configuration, where the JointConfigurationType is the stacked version of the left and right joint configurations""" - - ###################### # camera related types ###################### From 179d2ffebfb4d8474693557b2c088cde7e0b4ca5 Mon Sep 17 00:00:00 2001 From: victorlouisdg Date: Mon, 5 Feb 2024 17:22:36 +0100 Subject: [PATCH 3/7] trajectory dataclass --- airo-typing/airo_typing/__init__.py | 41 ++++++++++++++++++++++------- 1 file changed, 32 insertions(+), 9 deletions(-) diff --git a/airo-typing/airo_typing/__init__.py b/airo-typing/airo_typing/__init__.py index 85c8cef..ac01028 100644 --- a/airo-typing/airo_typing/__init__.py +++ b/airo-typing/airo_typing/__init__.py @@ -79,25 +79,48 @@ JointPathType = np.ndarray """ a (T, N) array of joint states (can be position/velocity/acceleration) that describe a path in joint space""" +TimesType = np.ndarray +""" a (T,) array of monotonically increasing times (float), corresponding to a path""" + + +@dataclass +class JointTrajectory: + positions: JointPathType + velocities: Optional[JointPathType] = None + accelerations: Optional[JointPathType] = None + efforts: Optional[JointPathType] = None + times: Optional[TimesType] = None # time from start of trajectory | assumed 0->1 if None + + +@dataclass +class DualArmJointTrajectory: + positions_left: JointPathType + positions_right: JointPathType + velocities_left: Optional[JointPathType] = None + velocities_right: Optional[JointPathType] = None + accelerations_left: Optional[JointPathType] = None + accelerations_right: Optional[JointPathType] = None + efforts_left: Optional[JointPathType] = None + efforts_right: Optional[JointPathType] = None + times: Optional[TimesType] = None # time from start of trajectory | assumed 0->1 if None + + PosePathType = np.ndarray """ a (T, 4, 4) list of homogeneous matrices that describe a path in cartesian space""" -TimePathType = np.ndarray -""" a (T,) array of monotonically increasing times (float), corresponding to a path""" -####################### -# Single-arm types # -####################### +@dataclass +class PoseTrajectory: + poses: PosePathType + times: Optional[TimesType] = None + + ForwardKinematicsFunctionType = Callable[[JointConfigurationType], HomogeneousMatrixType] """ a function that computes the forward kinematics of a given joint configuration""" InverseKinematicsFunctionType = Callable[[HomogeneousMatrixType], List[JointConfigurationType]] """ a function that computes one or more inverse kinematics solutions of a given TCP pose""" -######################### -# Motion planning types # -######################### - JointConfigurationCheckerType = Callable[[JointConfigurationType], bool] """ a function that checks a certain condition on a joint configuration, e.g. collision checking""" From 3ca5049f8d9695f24b790f066e7b016adeaf0d04 Mon Sep 17 00:00:00 2001 From: Mathieu De Coster Date: Tue, 6 Feb 2024 09:57:45 +0100 Subject: [PATCH 4/7] Proposal for Path/Trajectory separation --- airo-typing/airo_typing/__init__.py | 28 +++++++++++++++------------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/airo-typing/airo_typing/__init__.py b/airo-typing/airo_typing/__init__.py index ac01028..1cc03cd 100644 --- a/airo-typing/airo_typing/__init__.py +++ b/airo-typing/airo_typing/__init__.py @@ -84,25 +84,27 @@ @dataclass -class JointTrajectory: - positions: JointPathType +class JointPathContainer: + positions: Optional[JointPathType] = None velocities: Optional[JointPathType] = None accelerations: Optional[JointPathType] = None efforts: Optional[JointPathType] = None - times: Optional[TimesType] = None # time from start of trajectory | assumed 0->1 if None @dataclass -class DualArmJointTrajectory: - positions_left: JointPathType - positions_right: JointPathType - velocities_left: Optional[JointPathType] = None - velocities_right: Optional[JointPathType] = None - accelerations_left: Optional[JointPathType] = None - accelerations_right: Optional[JointPathType] = None - efforts_left: Optional[JointPathType] = None - efforts_right: Optional[JointPathType] = None - times: Optional[TimesType] = None # time from start of trajectory | assumed 0->1 if None +class SingleArmTrajectory: + path: JointPathContainer + gripper_path: Optional[JointPathContainer] + times: TimesType # time (seconds) from start of trajectory + + +@dataclass +class DualArmTrajectory: + path_left: JointPathContainer + path_right: JointPathContainer + gripper_left: Optional[JointPathContainer] + gripper_right: Optional[JointPathContainer] + times: TimesType # time (seconds) from start of trajectory PosePathType = np.ndarray From d662ffccbe45a88c9376cfcf7ede7543a17504d6 Mon Sep 17 00:00:00 2001 From: Mathieu De Coster Date: Fri, 9 Feb 2024 08:35:54 +0100 Subject: [PATCH 5/7] Initial path types: consensus --- airo-typing/airo_typing/__init__.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/airo-typing/airo_typing/__init__.py b/airo-typing/airo_typing/__init__.py index 1cc03cd..380736d 100644 --- a/airo-typing/airo_typing/__init__.py +++ b/airo-typing/airo_typing/__init__.py @@ -94,7 +94,7 @@ class JointPathContainer: @dataclass class SingleArmTrajectory: path: JointPathContainer - gripper_path: Optional[JointPathContainer] + gripper_path: Optional[JointPathContainer] = None times: TimesType # time (seconds) from start of trajectory @@ -102,8 +102,8 @@ class SingleArmTrajectory: class DualArmTrajectory: path_left: JointPathContainer path_right: JointPathContainer - gripper_left: Optional[JointPathContainer] - gripper_right: Optional[JointPathContainer] + gripper_path_left: Optional[JointPathContainer] = None + gripper_path_right: Optional[JointPathContainer] = None times: TimesType # time (seconds) from start of trajectory From 2bf3d558dd0f4617f1dc9c80dce7523f7a7038fa Mon Sep 17 00:00:00 2001 From: victorlouisdg Date: Mon, 12 Feb 2024 12:25:31 +0100 Subject: [PATCH 6/7] fix dataclass TypeError --- airo-typing/airo_typing/__init__.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/airo-typing/airo_typing/__init__.py b/airo-typing/airo_typing/__init__.py index 380736d..49b2075 100644 --- a/airo-typing/airo_typing/__init__.py +++ b/airo-typing/airo_typing/__init__.py @@ -93,18 +93,18 @@ class JointPathContainer: @dataclass class SingleArmTrajectory: + times: TimesType # time (seconds) from start of trajectory path: JointPathContainer gripper_path: Optional[JointPathContainer] = None - times: TimesType # time (seconds) from start of trajectory @dataclass class DualArmTrajectory: + times: TimesType # time (seconds) from start of trajectory path_left: JointPathContainer path_right: JointPathContainer gripper_path_left: Optional[JointPathContainer] = None gripper_path_right: Optional[JointPathContainer] = None - times: TimesType # time (seconds) from start of trajectory PosePathType = np.ndarray From 8f74bfe9c752e944f2577e7338cd89f7d8a0bd28 Mon Sep 17 00:00:00 2001 From: victorlouisdg Date: Mon, 12 Feb 2024 15:32:53 +0100 Subject: [PATCH 7/7] make times required without default value --- airo-typing/airo_typing/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/airo-typing/airo_typing/__init__.py b/airo-typing/airo_typing/__init__.py index 49b2075..3828d8b 100644 --- a/airo-typing/airo_typing/__init__.py +++ b/airo-typing/airo_typing/__init__.py @@ -113,8 +113,8 @@ class DualArmTrajectory: @dataclass class PoseTrajectory: + times: TimesType poses: PosePathType - times: Optional[TimesType] = None ForwardKinematicsFunctionType = Callable[[JointConfigurationType], HomogeneousMatrixType]