From 5828db4cf254602fa691786c2fee9c33a7442167 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jan=20Baumg=C3=A4rtner?= Date: Tue, 15 Mar 2022 18:55:38 +0100 Subject: [PATCH] Revert "URDF parser" --- .gitignore | 1 - docs/source/from_urdf_tutorial.rst | 21 -- src/setup.py | 2 +- src/trip_kinematics/Utility.py | 169 +--------- src/trip_kinematics/urdf_parser.py | 302 ------------------ tests/generate_forward_kinematics_json.py | 109 ------- tests/test_urdf_parser.py | 200 ------------ tests/test_utility.py | 144 --------- .../fixed_to_revolute_joint.urdf | 49 --- tests/urdf_examples/ground_to_many.urdf | 106 ------ .../urdf_invalid_missing_joint_name.urdf | 30 -- .../urdf_invalid_missing_joint_type.urdf | 30 -- .../urdf_invalid_unsupported_joint_type.urdf | 30 -- .../xml_invalid_invalid_token.urdf | 30 -- .../xml_invalid_mismatched_tag.urdf | 30 -- tests/urdf_examples/large_tree_test.urdf | 177 ---------- tests/urdf_examples/one_continuous_joint.urdf | 31 -- tests/urdf_examples/one_fixed_joint.urdf | 30 -- tests/urdf_examples/one_prismatic_joint.urdf | 31 -- tests/urdf_examples/one_revolute_joint.urdf | 31 -- .../fixed_to_revolute_joint.json | 1 - .../ground_to_many.json | 1 - .../large_tree_test.json | 1 - .../one_continuous_joint.json | 1 - .../one_fixed_joint.json | 1 - .../one_prismatic_joint.json | 1 - .../one_revolute_joint.json | 1 - .../prismatic_to_continuous_joint.json | 1 - .../prismatic_to_continuous_joint.urdf | 50 --- 29 files changed, 18 insertions(+), 1593 deletions(-) delete mode 100644 docs/source/from_urdf_tutorial.rst delete mode 100644 src/trip_kinematics/urdf_parser.py delete mode 100644 tests/generate_forward_kinematics_json.py delete mode 100644 tests/test_urdf_parser.py delete mode 100644 tests/test_utility.py delete mode 100644 tests/urdf_examples/fixed_to_revolute_joint.urdf delete mode 100644 tests/urdf_examples/ground_to_many.urdf delete mode 100644 tests/urdf_examples/invalid_urdf/urdf_invalid_missing_joint_name.urdf delete mode 100644 tests/urdf_examples/invalid_urdf/urdf_invalid_missing_joint_type.urdf delete mode 100644 tests/urdf_examples/invalid_urdf/urdf_invalid_unsupported_joint_type.urdf delete mode 100644 tests/urdf_examples/invalid_urdf/xml_invalid_invalid_token.urdf delete mode 100644 tests/urdf_examples/invalid_urdf/xml_invalid_mismatched_tag.urdf delete mode 100644 tests/urdf_examples/large_tree_test.urdf delete mode 100644 tests/urdf_examples/one_continuous_joint.urdf delete mode 100644 tests/urdf_examples/one_fixed_joint.urdf delete mode 100644 tests/urdf_examples/one_prismatic_joint.urdf delete mode 100644 tests/urdf_examples/one_revolute_joint.urdf delete mode 100644 tests/urdf_examples/precomputed_kinematics/fixed_to_revolute_joint.json delete mode 100644 tests/urdf_examples/precomputed_kinematics/ground_to_many.json delete mode 100644 tests/urdf_examples/precomputed_kinematics/large_tree_test.json delete mode 100644 tests/urdf_examples/precomputed_kinematics/one_continuous_joint.json delete mode 100644 tests/urdf_examples/precomputed_kinematics/one_fixed_joint.json delete mode 100644 tests/urdf_examples/precomputed_kinematics/one_prismatic_joint.json delete mode 100644 tests/urdf_examples/precomputed_kinematics/one_revolute_joint.json delete mode 100644 tests/urdf_examples/precomputed_kinematics/prismatic_to_continuous_joint.json delete mode 100644 tests/urdf_examples/prismatic_to_continuous_joint.urdf diff --git a/.gitignore b/.gitignore index 1756ea3..1ce352c 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,4 @@ src/Trip.egg-info -src/trip_kinematics.egg-info docs/build/ build/ src/dist diff --git a/docs/source/from_urdf_tutorial.rst b/docs/source/from_urdf_tutorial.rst deleted file mode 100644 index 8d6add7..0000000 --- a/docs/source/from_urdf_tutorial.rst +++ /dev/null @@ -1,21 +0,0 @@ -How to use the urdf parser -************************** -The URDF parser allows the usage of URDF files with TriP by converting the described robot into a list of TriP Transformations. - -It can be used directly after importing the TriP library by calling the function trip_kinematics.from_urdf with the path to the URDF file as the argument. Note that the function returns a list of Transformations, which you probably want to create a Robot from in most cases: - -.. code-block:: python - - transformations_list = trip_kinematics.urdf_parser.from_urdf(urdf_path) - robot = Robot(transformations_list) - - -This means you can also add other Transformations manually on top of those specified in the URDF file, if required. - -Also note that the parser includes defaults for certain values if the corresponding URDF tag is missing, specifically: - -* defaults to . - - * (The same also applies if only one of xyz and rpy is specified, with the omitted value defaulting to "0 0 0") - -* defaults to diff --git a/src/setup.py b/src/setup.py index ff37104..19a6947 100644 --- a/src/setup.py +++ b/src/setup.py @@ -7,6 +7,6 @@ author='Torben Miller, Jan Baumgärtner', license='MIT', description='...', - install_requires=['casadi>=3.5.5', 'numpy>=1.17.4, < 1.20.0', 'defusedxml>=0.5'], + install_requires=['casadi>=3.5.5', 'numpy>=1.17.4, < 1.20.0'], packages=['trip_kinematics', 'trip_robots'] ) diff --git a/src/trip_kinematics/Utility.py b/src/trip_kinematics/Utility.py index 8d4205d..487b0a5 100644 --- a/src/trip_kinematics/Utility.py +++ b/src/trip_kinematics/Utility.py @@ -1,149 +1,14 @@ -from importlib_metadata import re -import numpy as np +from numpy import array from casadi import cos, sin -class Rotation: - """Represents a 3D rotation. - - Can be initialized from quaternions, rotation matrices, or Euler angles, and can be represented - as quaternions. Reimplements a (small) part of the scipy.spatial.transform.Rotation API and is - meant to be used for converting between rotation representations to avoid depending on SciPy. - This class is not meant to be instantiated directly using __init__; use the methods - from_[representation] instead. - - """ - - def __init__(self, quat): - quat /= np.linalg.norm(quat) - - self._xyzw = quat - - @classmethod - def from_quat(cls, xyzw, scalar_first=True): - """Creates a :py:class`Rotation` object from a quaternion. - - Args: - xyzw (np.array): Quaternion. - scalar_first (bool, optional): Whether the quaternion is in scalar-first (w, x, y, z) or - scalar-last (x, y, z, w) format. Defaults to True. - - Returns: - Rotation: Rotation object. - - """ - if scalar_first: - return cls(xyzw[[1, 2, 3, 0]]) - return cls(xyzw) - - @classmethod - def from_euler(cls, seq, rpy, degrees=False): - """Creates a :py:class`Rotation` object from Euler angles. - - Args: - seq (str): Included for compatibility with the SciPy API. Required to be set to 'xyz'. - rpy (np.array): Euler angles. - degrees (bool, optional): True for degrees and False for radians. Defaults to False. - - Returns: - Rotation: Rotation object. - - """ - # pylint: disable=invalid-name - - assert seq == 'xyz' - rpy = np.array(rpy) - - if degrees: - rpy = rpy * (np.pi / 180) - - sin_r = np.sin(0.5 * rpy[0]) - cos_r = np.cos(0.5 * rpy[0]) - sin_p = np.sin(0.5 * rpy[1]) - cos_p = np.cos(0.5 * rpy[1]) - sin_y = np.sin(0.5 * rpy[2]) - cos_y = np.cos(0.5 * rpy[2]) - - x = sin_r * cos_p * cos_y - cos_r * sin_p * sin_y - y = cos_r * sin_p * cos_y + sin_r * cos_p * sin_y - z = cos_r * cos_p * sin_y - sin_r * sin_p * cos_y - w = cos_r * cos_p * cos_y + sin_r * sin_p * sin_y - - return cls(np.array([x, y, z, w])) - - @classmethod - def from_matrix(cls, matrix): - """Creates a :py:class`Rotation` object from a rotation matrix. - - Uses a very similar algorithm to scipy.spatial.transform.Rotation.from_matrix(). - See https://github.com/scipy/scipy/blob/22f66bbd83867459f1491abf01b860b5ef4e026e/ - scipy/spatial/transform/_rotation.pyx - - Args: - matrix (np.array): Rotation matrix. - - Returns: - Rotation: Rotation object. - - """ - diag1 = matrix[0, 0] - diag2 = matrix[1, 1] - diag3 = matrix[2, 2] - thing = diag1 + diag2 + diag3 - diag = np.array([diag1, diag2, diag3, thing]) - argmax = np.argmax(diag) - quat = np.zeros(4) - - if argmax != 3: - i = argmax - j = (i + 1) % 3 - k = (j + 1) % 3 - - quat[i] = 1 - diag[3] + 2 * matrix[i, i] - quat[j] = matrix[j, i] + matrix[i, j] - quat[k] = matrix[k, i] + matrix[i, k] - quat[3] = matrix[k, j] - matrix[j, k] - - else: - quat[0] = matrix[2, 1] - matrix[1, 2] - quat[1] = matrix[0, 2] - matrix[2, 0] - quat[2] = matrix[1, 0] - matrix[0, 1] - quat[3] = 1 + diag[3] - - return cls(quat) - - def as_quat(self, scalar_first=True): - """Represents the object as a quaternion. - - Args: - scalar_first (bool, optional): Represent the quaternion in scalar-first (w, x, y, z) - or scalar-last (x, y, z, w) format. Defaults to True. - - Returns: - np.array: Quaternion. - - """ - if scalar_first: - return self._xyzw[[3, 0, 1, 2]] - return self._xyzw - - def __str__(self): - return f'''Rotation xyzw={self._xyzw}''' - - def __repr__(self): - return f'''Rotation xyzw={self._xyzw}''' - - -print(1) - - def identity_transformation(): """Returns a 4x4 identity matix Returns: - numpy.array: a 4x4 identity matrix + numy.array: a 4x4 identity matrix """ - return np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], dtype=object) + return array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], dtype=object) def hom_translation_matrix(t_x=0, t_y=0, t_z=0): @@ -157,7 +22,7 @@ def hom_translation_matrix(t_x=0, t_y=0, t_z=0): Returns: numpy.array: A 4x4 homogenous translation matrix """ - return np.array( + return array( [[1, 0, 0, t_x], [0, 1, 0, t_y], [0, 0, 1, t_z], [0., 0., 0., 1.]], dtype=object) @@ -175,7 +40,7 @@ def hom_rotation(rotation_matrix): return matrix -def quat_rotation_matrix(q_w, q_x, q_y, q_z) -> np.array: +def quat_rotation_matrix(q_w, q_x, q_y, q_z) -> array: """Generates a 3x3 rotation matrix from q quaternion Args: @@ -187,10 +52,10 @@ def quat_rotation_matrix(q_w, q_x, q_y, q_z) -> np.array: Returns: numpy.array: A 3x3 rotation matrix """ - return np.array([[1-2*(q_y**2+q_z**2), 2*(q_x*q_y-q_z*q_w), 2*(q_x*q_z + q_y*q_w)], - [2*(q_x*q_y + q_z*q_w), 1-2 * + return array([[1-2*(q_y**2+q_z**2), 2*(q_x*q_y-q_z*q_w), 2*(q_x*q_z + q_y*q_w)], + [2*(q_x*q_y + q_z*q_w), 1-2 * (q_x**2+q_z**2), 2*(q_y*q_z - q_x*q_w)], - [2*(q_x*q_z-q_y*q_w), 2*(q_y*q_z+q_x*q_w), 1-2*(q_x**2+q_y**2)]], dtype=object) + [2*(q_x*q_z-q_y*q_w), 2*(q_y*q_z+q_x*q_w), 1-2*(q_x**2+q_y**2)]], dtype=object) def x_axis_rotation_matrix(theta): @@ -202,9 +67,9 @@ def x_axis_rotation_matrix(theta): Returns: numpy.array: A 3x3 rotation matrix """ - return np.array([[1, 0, 0], - [0, cos(theta), -sin(theta)], - [0, sin(theta), cos(theta)]], dtype=object) + return array([[1, 0, 0], + [0, cos(theta), -sin(theta)], + [0, sin(theta), cos(theta)]], dtype=object) def y_axis_rotation_matrix(theta): @@ -216,9 +81,9 @@ def y_axis_rotation_matrix(theta): Returns: numpy.array: A 3x3 rotation matrix """ - return np.array([[cos(theta), 0, sin(theta)], - [0, 1, 0], - [-sin(theta), 0, cos(theta)]], dtype=object) + return array([[cos(theta), 0, sin(theta)], + [0, 1, 0], + [-sin(theta), 0, cos(theta)]], dtype=object) def z_axis_rotation_matrix(theta): @@ -230,9 +95,9 @@ def z_axis_rotation_matrix(theta): Returns: numpy.array: A 3x3 rotation matrix """ - return np.array([[cos(theta), -sin(theta), 0], - [sin(theta), cos(theta), 0], - [0, 0, 1]], dtype=object) + return array([[cos(theta), -sin(theta), 0], + [sin(theta), cos(theta), 0], + [0, 0, 1]], dtype=object) def get_rotation(matrix): diff --git a/src/trip_kinematics/urdf_parser.py b/src/trip_kinematics/urdf_parser.py deleted file mode 100644 index 25c705b..0000000 --- a/src/trip_kinematics/urdf_parser.py +++ /dev/null @@ -1,302 +0,0 @@ -from collections import defaultdict -from typing import Dict, List - -import defusedxml.ElementTree as ET -import numpy as np -from trip_kinematics.Utility import Rotation -from trip_kinematics.Transformation import Transformation - - -def from_urdf(filename: str) -> List[Transformation]: - """Converts a robot specified in a URDF file into a list of :py:class:`Transformation` objects. - - If the tag does not specify xyz and rpy values or the tag is omitted, these default - to (0, 0, 0). defaults to (0, 0, 1). - - Args: - filename (str): Path to URDF file. - - Raises: - ValueError: Could not parse URDF file. - - Returns: - List[Transformation]: List of transformations that describe the robot. - """ - tree = ET.parse(filename) - root = tree.getroot() - joints = root.findall('joint') - joint_name_to_transformations = {} - - joint_info = _build_joint_info(joints) - - root_joints = [name for name, value in joint_info.items() if not value['parent']] - - for i, joint in enumerate(joints): - name = joint.get('name') - if not name: - raise ValueError('Missing name field in URDF file') - joint_info[name]['joint_index'] = i - - for joint in joints: - joint_name_to_transformations[joint.get('name')] = _get_transformations_for_joint(joint) - - transformations = [] - for joint in root_joints: - transformations.extend(_create_transformations_from_tree(joint, - joint_info, - joint_name_to_transformations, - None) - ) - - return transformations - - -def align_vectors(target: np.ndarray, to_align: np.ndarray) -> np.ndarray: - """Calculates a rotation matrix that rotates to_align so that it becomes parallel to target. - Based on work by Íñigo Quílez and Kevin Moran. See: - - "Avoiding Trigonometry", by Íñigo Quílez - https://iquilezles.org/www/articles/noacos/noacos.htm - - "How to Calculate a Rotation Matrix to Align Vector A to Vector B in 3D", by Kevin Moran - https://gist.github.com/kevinmoran/b45980723e53edeb8a5a43c49f134724 - - Args: - target (np.ndarray): 3D Vector. - to_align (np.ndarray): 3D Vector. - - Returns: - np.ndarray: 3x3 rotation matrix. - """ - target = target / np.linalg.norm(target) - to_align = to_align / np.linalg.norm(to_align) - - # If the vectors are parallel but in opposite directions, return a 180 degree rotation - if np.array_equal(target, -to_align): - return -np.identity(3) - - align_axis = np.cross(target, to_align) - cos_angle = np.dot(target, to_align) - k = 1 / (1 + cos_angle) - - result = np.array([[(align_axis[0] * align_axis[0] * k) + cos_angle, - (align_axis[1] * align_axis[0] * k) - align_axis[2], - (align_axis[2] * align_axis[0] * k) + align_axis[1]], - [(align_axis[0] * align_axis[1] * k) + align_axis[2], - (align_axis[1] * align_axis[1] * k) + cos_angle, - (align_axis[2] * align_axis[1] * k) - align_axis[0]], - [(align_axis[0] * align_axis[2] * k) - align_axis[1], - (align_axis[1] * align_axis[2] * k) + align_axis[0], - (align_axis[2] * align_axis[2] * k) + cos_angle]]) - return result - - -def _build_joint_info(joints: List) -> Dict[str, Dict]: - """Creates a dictionary representing parent-child relationships between joints. Used by - :py:func:`from_urdf()` to build a tree of joints. - - Args: - joints (List[ET.Element]): List of tags from the URDF file. - - Returns: - dict[str, Dict]: A dictionary representing parent-child relationships between joints. - """ - # Keep track of parent and children links of joints - # and use that to search for the parent joint of each joint - joint_info = { - joint.get('name'): { - 'child_link': joint.find('child').get('link'), - 'parent_link': joint.find('parent').get('link') - } for joint in joints} - - parent_link_to_joint = defaultdict(list) - child_link_to_joint = defaultdict(list) - - for joint in joints: - parent_link_to_joint[joint.find('parent').get('link')].append(joint.get('name')) - child_link_to_joint[joint.find('child').get('link')].append(joint.get('name')) - - for val in joint_info.values(): - val['parent'] = child_link_to_joint[val['parent_link']] - val['child'] = parent_link_to_joint[val['child_link']] - - return joint_info - - -def _get_transformations_for_joint(joint) -> List[List]: - """Generates the parameters for the transformations for the input joint. One joint is - represented by up to five transformations. These are: - - 1. origin translation and 2. origin rotation (both taken from the tag of the URDF), - 3. a rotation that makes joint movement parallel to the z axis of the local coord. system, - 4. the joint movement (the only dynamic transformation of the five), - 5. the inverse transformation of number 3. - - Args: - joint (ET.Element): A tag in the URDF file. - - Raises: - ValueError: Could not parse URDF file. - - Returns: - List[List]: A list of parameters for up to four py:class:`Transformation` objects that - describe the input joint. - """ - # Read properties from urdf - name = joint.get('name') - type_ = joint.get('type') - origin = joint.find('origin') - joint_transformations = [] - - # File needs to specify a joint type for each joint - try: - assert type_ is not None - except AssertionError as err: - raise ValueError(f'Error: Invalid URDF file ({err})') from err - - if type_ not in ['fixed', 'continuous', 'revolute', 'prismatic', 'floating', 'planar']: - raise ValueError(f"Unsupported joint type {type_}") - - type_to_mov_dict = { - 'fixed': [{}, []], - 'continuous': [{'rz': 0}, ['rz']], - 'revolute': [{'rz': 0}, ['rz']], - 'prismatic': [{'tz': 0}, ['tz']], - 'floating': [{}, []], # treating floating and planar as fixed until implemented - 'planar': [{}, []], - } - - # Default values if origin rotation or translation are not specified - if origin is None: - xyz_vals = '0 0 0' - rpy_vals = '0 0 0' - else: - xyz_vals = origin.get('xyz') - rpy_vals = origin.get('rpy') - if xyz_vals is None: - xyz_vals = '0 0 0' - if rpy_vals is None: - rpy_vals = '0 0 0' - origin_xyz = np.array(list(map(float, xyz_vals.split(' ')))) - origin_rpy = np.array(list(map(float, rpy_vals.split(' ')))) - - if type_ in ['fixed', 'floating', 'planar']: - axis = None # Fixed joints have no axis - # floating and planar are added to have no axis as they are not yet implemented - - elif type_ in ['continuous', 'revolute', 'prismatic']: - axis = joint.find('axis') - # Default to (0, 0, 1) if the axis is unspecified. Note that there does not seem to be - # a standard default value for the axis. Maybe better to remove the default to prevent - # potential confusion? - if axis is None: - axis = '0 0 1' - else: - axis = axis.get('xyz') - axis = np.array(list(map(float, axis.split(' ')))) - axis /= np.linalg.norm(axis) - - # For each joint, define up to five transformations - # Transformation 1 (tra): defined by the translation of the origin - tra = [name + '_tra', {'tx': origin_xyz[0], 'ty': origin_xyz[1], 'tz': origin_xyz[2]}, []] - - # Transformation 2 (rot): defined by the rotation of the origin - rot_quat = Rotation.from_euler('xyz', [*origin_rpy], degrees=False).as_quat() - rot = [name + '_rot', - {'qw': rot_quat[0], - 'qx': rot_quat[1], - 'qy': rot_quat[2], - 'qz': rot_quat[3]}, - [], - ] - - joint_transformations.extend([tra, rot]) - - if axis is not None and not np.array_equal(axis, np.array([0, 0, 1])): - # Transformation 3 (sta): aligns the joint's axis of movement with the z axis of the - # coordinate system. Only necessary for movable joints (therefore axis is not None) if - # the axis is not already parallel to the z axis - align_transformation = align_vectors(np.array([0, 0, 1]), axis) - align_quat = Rotation.from_matrix(align_transformation).as_quat() - sta = [name + '_sta', - {'qw': align_quat[0], - 'qx': align_quat[1], - 'qy': align_quat[2], - 'qz': align_quat[3]}, - [], - ] - - # Transformation 5 (unsta): after aligning with the z axis and adding motion of movable - # joint, reverses the alignment so that we are still in the correct coordinate system. - # (Note that the movement of the joint is applied between sta and unsta) - unalign_transformation = np.linalg.inv(align_transformation) - unalign_quat = Rotation.from_matrix(unalign_transformation).as_quat() - unsta = [name + '_unsta', - {'qw': unalign_quat[0], - 'qx': unalign_quat[1], - 'qy': unalign_quat[2], - 'qz': unalign_quat[3]}, - [], - ] - - joint_transformations.append(sta) - # unsta will be appended after the joint movement transformation - - # Transformation 4 (mov): represents the movement of the joint. This is the only transformation - # with state variables. - mov = [name + '_mov', *type_to_mov_dict[type_]] - joint_transformations.append(mov) - - if axis is not None and not np.array_equal(axis, np.array([0, 0, 1])): - joint_transformations.append(unsta) - - return joint_transformations - - -def _create_transformations_from_tree(joint: str, - joint_info: Dict[str, Dict], - joint_name_to_transformations: Dict[str, List], - parent: Transformation) -> List[Transformation]: - """Recursively builds a tree of py:class:`Transformation` objects, starting from the root and - traversing the tree towards the children. - - Args: - joint (str): Name of the joint. - joint_info (Dict[str, Dict]): Represents the relationships between all joints. - joint_name_to_transformations (Dict[str, List]): Contains the parameters for the - py:class:`Transformation` objects. - parent (Transformation, optional): The parent of current joint. Should be None for the root, - is set recursively for its children. - - Returns: - List[Transformation]: List of py:class:`Transformation` objects for input node and all its - descendants. - """ - transformations_list = [] - - for transformation in joint_name_to_transformations[joint]: - # transformation[1] is the parametric description of the transformations - # If len(transformation) > 2, then the transformation has state variables - is_nonzero = any(i != 0 for i in transformation[1].values()) or len(transformation[2]) > 0 - if is_nonzero: - tmp = Transformation(name=transformation[0], - values=transformation[1], - state_variables=transformation[2], - parent=parent) - transformations_list.append(tmp) - parent = tmp - - tmp = Transformation(name=joint, values={}, parent=parent) - transformations_list.append(tmp) - parent = tmp - - if joint_info[joint]['child']: - for child in joint_info[joint]['child']: - transformations_list.extend( - _create_transformations_from_tree(child, - joint_info, - joint_name_to_transformations, - parent) - ) - - return transformations_list diff --git a/tests/generate_forward_kinematics_json.py b/tests/generate_forward_kinematics_json.py deleted file mode 100644 index 3c54926..0000000 --- a/tests/generate_forward_kinematics_json.py +++ /dev/null @@ -1,109 +0,0 @@ -import os -import pathlib -import random -import json - -import kinpy as kp -import numpy as np -from tests.test_urdf_parser import ( - urdf_path_to_json_path, - PRECOMPUTED_KINEMATICS_DIR_NAME, - URDF_EXAMPLES_DIR -) - - -def initialize_state(robot): - """Creates a dictionary whose entries each correspond to a movable joint of the input - :py:class:`Robot`, with all values (joint positions) set to 0. - - Args: - robot (Robot): A TriP Robot. - - Returns: - (dict): Dictionary representing the robot's state, with all values initialized to zeros. - - """ - return { - joint_name: 0 - for joint_name in robot.get_actuated_state() - } - - -def create_kinpy_chain(path): - """Takes a path to a URDF file and converts it into a kinpy kinematic chain. - - Args: - path (str): Path to a URDF file. - - Returns: - (Chain): kinpy kinematic chain. - - """ - with open(path, encoding='utf8') as file: - urdf_data_str = file.read() - return kp.build_chain_from_urdf(urdf_data_str) - - -def generate_forward_kinematics_json(urdf_path, rng_states_count=10): - """Calculates forward kinematics for the input URDF file using kinpy and saves these to a - JSON file. - - Args: - path (str): Path to the URDF file. - rng_states_count (int, optional): The number of randomized states. Defaults to 10. - - """ - # Setup kinpy chain - try: - chain_kinpy = create_kinpy_chain(urdf_path) - except KeyError as err: - raise ValueError( - f'File {urdf_path} is not valid. Unsupported joint type? Missing tag? (error was {err})' - ) from err - - # First state: initialize all joint values to zero - state_init = { - joint_name: 0 - for joint_name in chain_kinpy.get_joint_parameter_names() - } - test_states = [state_init] - - # RNG states: initialize a number of states with random values - for _ in range(rng_states_count): - new_state = { - joint: random.uniform(-np.pi, np.pi) - for joint in state_init.keys() - } - test_states.append(new_state) - - # Save forward kinematics results and joint positions for all states - forward_kinematics = [ - { - 'state': state, - 'transformations': { - link: {'rot': list(transform.rot), - 'pos': list(transform.pos)} - for link, transform in chain_kinpy.forward_kinematics(state).items() - } - } - for state in test_states - ] - - return json.dumps(forward_kinematics, separators=(',', ':')) - - -def main(): - precomputed_kinematics_dir = pathlib.Path(URDF_EXAMPLES_DIR) / PRECOMPUTED_KINEMATICS_DIR_NAME - precomputed_kinematics_dir.mkdir(exist_ok=True) - # Iterate through files for which we compute forward kinematics. Skip subdirectories of - # urdf_examples_dir, because as of now, the only subdirectory contains (intentionally) broken - # URDFs. If that changes, change this too. - for entry in os.scandir(URDF_EXAMPLES_DIR): - if entry.is_file() and pathlib.Path(entry).suffix == '.urdf': - with open(urdf_path_to_json_path(entry.path), 'w', encoding='utf8') as file: - forward_kinematics = generate_forward_kinematics_json(entry.path) - file.write(forward_kinematics) - - -if __name__ == '__main__': - main() diff --git a/tests/test_urdf_parser.py b/tests/test_urdf_parser.py deleted file mode 100644 index 14e80cc..0000000 --- a/tests/test_urdf_parser.py +++ /dev/null @@ -1,200 +0,0 @@ -import unittest -import os -import pathlib -import json - -import defusedxml.ElementTree as ET -import numpy as np -from trip_kinematics import Utility, Robot, forward_kinematics, urdf_parser - - -PRECOMPUTED_KINEMATICS_DIR_NAME = 'precomputed_kinematics' -URDF_EXAMPLES_DIR = os.path.join('tests', 'urdf_examples') - - -def urdf_path_to_json_path(path): - """Returns a path to a JSON file with the same name as the input URDF file, within a - subdirectory of the same folder. - - Args: - path (pathlib.Path): Path to a URDF file. - - Returns: - pathlib.Path: Path to JSON file. - - """ - path = pathlib.Path(path) - urdf_directory = path.parent - json_file_name = path.with_suffix('.json').name - return urdf_directory / PRECOMPUTED_KINEMATICS_DIR_NAME / json_file_name - - -def state_to_trip(trip_state, kp_state): - """The function state_to_trip was added to have a uniform way of using the state between kinpy - and TriP in the tests - - Args: - state (dict): dictionary with the state name of each joint as it occurs in TriP as the key - and a float as the value - - Returns: - (dict): the input dictionary where the keys are adjusted to the form that kinpy states use - - """ - - # this now sets the trip states to be equal to the previously generated kinpy states - state = { - joint_name: kp_state["_".join(joint_name.split("_")[:-2])] - for joint_name in trip_state.keys() - } - - return state - - -def initialize_state(robot): - """Creates a dictionary whose entries each correspond to a movable joint of the input - :py:class:`Robot`, with all values (joint positions) set to 0. - - Args: - robot (Robot): A TriP Robot. - - Returns: - (dict): Dictionary representing the robot's state, with all values initialized to zeros. - - """ - return { - joint_name: 0 - for joint_name in robot.get_actuated_state() - } - - -def get_joint_info(path): - """Calculates a dictionary representing parent-child relationships between joints from a URDF - file. Used to build a tree of joints. - - Args: - path (str): Path to a URDF file. - - Returns: - Dict[str, Dict]: Dictionary representing parent-child relationships between joints. - - """ - tree = ET.parse(path) - root = tree.getroot() - joints = root.findall('joint') - - # pylint: disable=protected-access - return urdf_parser._build_joint_info(joints) - - -def compare_urdf_trip_vs_json(urdf_path, atol=1e-08): - """Reads a URDF file, converts it into a TriP robot, calculates forward kinematics, and checks - whether the results are close to those saved in a JSON file. The JSON file must have the - same name as the URDF file, except the extension should be changed from .urdf to .json. - - Args: - urdf_path (str): Path to the URDF file to test. - atol (float, optional): Absolute tolerance of the comparison; see numpy documentation for - :py:func:`allclose`. Defaults to 1e-08. - - Raises: - AssertionError: Results were not within tolerance of each other. - - """ - # Setup TriP robot using the URDF parser - try: - robot = Robot(urdf_parser.from_urdf(urdf_path)) - state_init = initialize_state(robot) - except ValueError as err: - raise ValueError( - f'File {urdf_path} is not valid. Unsupported joint type? (error was {err})' - ) from err - - json_path = urdf_path_to_json_path(urdf_path) - with open(json_path, encoding='utf8') as json_file: - for robot_position in json.load(json_file): - # First set the state from the file to TriP - state = robot_position['state'] - transformations = robot_position['transformations'] - robot.set_actuated_state(state_to_trip(state_init, state)) - - for joint, joint_info in get_joint_info(urdf_path).items(): - # Convert position and quaternion rotation saved in JSON to homogenous matrix - pos_and_rot = transformations[joint_info['child_link']] - pos = pos_and_rot["pos"] - rot = pos_and_rot["rot"] - pos_homogenous = Utility.hom_translation_matrix(*pos).astype('float') - rot_matrix = Utility.quat_rotation_matrix(*rot).astype('float') - rot_homogenous = Utility.hom_rotation(rot_matrix).astype('float') - kp_transformation = pos_homogenous @ rot_homogenous - - # TriP forward kinematics - trip_transformation = forward_kinematics(robot, joint).astype('float') - - assert np.allclose(kp_transformation, trip_transformation, atol=atol) - - -class TestStates(unittest.TestCase): - r"""Codiga really wants a docstring for this class. Have a cute ascii cat instead - - |\__/,| (`\ - _.|o o |_ ) ) - -(((---(((-------- - - (https://www.asciiart.eu/animals/cats) - - """ - - def test_all_urdf_files(self): - names = [ - "one_fixed_joint", - "one_continuous_joint", - "one_revolute_joint", - "one_prismatic_joint", - "fixed_to_revolute_joint", - "prismatic_to_continuous_joint", - "large_tree_test", - "ground_to_many", - ] - - names_exceptions = [ - # Valid XML, but not valid URDF robots - ("urdf_invalid_missing_joint_name", ValueError), - ("urdf_invalid_missing_joint_type", ValueError), - ("urdf_invalid_unsupported_joint_type", ValueError), - - # Invalid XML - ("xml_invalid_mismatched_tag", ET.ParseError), - ("xml_invalid_invalid_token", ET.ParseError), - ] - - urdf_examples_dir = os.path.join('tests', 'urdf_examples') - - for name in names: - full_path = os.path.join(urdf_examples_dir, name + '.urdf') - compare_urdf_trip_vs_json(full_path) - - for name, exception in names_exceptions: - full_path = os.path.join(urdf_examples_dir, 'invalid_urdf', name + '.urdf') - with self.assertRaises(exception): - compare_urdf_trip_vs_json(full_path) - - def test_align_vectors(self): - test_cases = [ - ([1, 2, 3], [4, 5, 6]), # Random angle - ([1, 0, 0], [0, 1, 0]), # 90 degrees - ([1, 0, 0], [-1, 0, 0]), # -180 degrees - ([0, 0, 1], [0, 0, -1]), # -180 degrees - ([0, 1, 4], [0, -1, -4]), # -180 degrees - ([-3, 1, 4], [3, -1, -4]), # -180 degrees - ([8, 9, 3], [8, 9, 3]), # zero angle - ] - - for target, to_align in test_cases: - target /= np.linalg.norm(target) - to_align /= np.linalg.norm(to_align) - - rotation_matrix = urdf_parser.align_vectors(target, to_align) - aligned = to_align @ rotation_matrix - - assert np.all(np.isclose(aligned, target)) diff --git a/tests/test_utility.py b/tests/test_utility.py deleted file mode 100644 index d279288..0000000 --- a/tests/test_utility.py +++ /dev/null @@ -1,144 +0,0 @@ -import unittest - -import numpy as np -from trip_kinematics.Utility import Rotation as R - - -class TestStates(unittest.TestCase): - """Correct results were generated using scipy.spatial.transform.Rotation. - """ - - def test_from_euler_to_quat(self): - from_euler_cases = [ - ([1, 2, 3], - [0.4359528440735657, -0.7182870182434115, 0.3106224510657039, 0.44443511344300074]), - ([1, 0, 0], [0.8775825618903728, 0.479425538604203, 0.0, 0.0]), - ([0, 1, 0], [0.8775825618903728, 0.0, 0.479425538604203, 0.0]), - ([0, 0, 1], [0.8775825618903728, 0.0, 0.0, 0.479425538604203]), - ([1.9259795237086745, -1.3166224746837234, -1.6487569080546618], - [0.6754185988029391, 0.1844402562591348, -0.7139352011035228, 0.00938279741930681]), - ([1.7518638481261277, -1.4762648402511551, 2.783050177056892], - [-0.4241491835815902, 0.5252649613496548, 0.482281714511987, 0.5582101201991901]), - ([-1.5637740430088356, 0.9967772877753394, -1.281072618629502], - [0.7010098084209453, -0.2935149227036473, 0.6418274036266723, -0.1024295982699634]), - ([-2.018987322821037, -2.477643741973166, -1.7329756283622468], - [-0.4975785988500945, -0.562138163026927, -0.1155880687282515, -0.6504272611159234]), - ([-1.547364165870453, 3.1155873968096914, 2.5081666410682404], - [-0.6610669854921286, -0.6825367059399726, 0.2141370877143812, 0.22644953831298628]), - ([0.9232800471655072, 2.9636239629991614, 1.3121379453450235], - [0.3336790888032567, -0.5126269396587794, 0.7307896657891122, -0.3030154299822701]), - ([-3.0969110304794603, 1.6540648344396605, -1.1444726410810802], - [0.4111294068459875, -0.5601567984771901, 0.38036785991321215, 0.6103419230982696]), - ] - - for euler_angles, quat in from_euler_cases: - assert np.allclose(R.from_euler('xyz', euler_angles, degrees=False).as_quat(), quat) - euler_angles_deg = np.array(euler_angles) * (180 / np.pi) - assert np.allclose(R.from_euler('xyz', euler_angles_deg, degrees=True).as_quat(), quat) - - def test_from_matrix_to_quat(self): - test_cases = [ - (np.array([[0.41198224566568303, -0.8337376517741568, -0.3676304629248995], - [-0.058726644927620864, -0.4269176212762076, 0.902381585483331], - [-0.9092974268256819, -0.35017548837401474, -0.2248450953661529]]), - [-0.43595284407356566, 0.7182870182434113, -0.31062245106570385, -0.4444351134430007]), - (np.array([[1., 0., 0.], - [0., 0.54030231, -0.84147098], - [0., 0.84147098, 0.54030231]]), - [0.8775825618903726, 0.47942553860420295, 0.0, 0.0]), - (np.array([[0.54030231, 0., 0.84147098], - [0., 1., 0.], - [-0.84147098, 0., 0.54030231]]), - [0.8775825618903726, 0.0, 0.47942553860420295, 0.0]), - (np.array([[0.54030231, -0.84147098, 0.], - [0.84147098, 0.54030231, 0.], - [0., 0., 1.]]), - [0.8775825618903726, 0.0, 0.0, 0.47942553860420295]), - (np.array([[-0.01958302, -0.27603141, -0.9609491], - [-0.25068215, 0.93178751, -0.26254618], - [0.96787136, 0.23575134, -0.08744336]]), - [-0.6754185988029392, -0.18444025625913482, 0.7139352011035228, -0.00938279741930680]), - (np.array([[-0.08838838, 0.98018011, 0.17729764], - [0.03312264, -0.17500364, 0.98401048], - [0.99553523, 0.09284766, -0.01699786]]), - [-0.4241491835815902, 0.5252649613496548, 0.4822817145119869, 0.5582101201991901]), - (np.array([[0.15513152, -0.23316354, 0.95998384], - [-0.52038015, 0.80671434, 0.28002943], - [-0.83972538, -0.54299793, 0.00381315]]), - [0.7010098084209453, -0.2935149227036473, 0.6418274036266723, -0.10242959826996342]), - (np.array([[0.12716755, -0.51732444, 0.84628827], - [0.7772303, -0.47810987, -0.40905258], - [0.61623167, 0.7097791, 0.34128017]]), - [0.49757859885009453, 0.5621381630269271, 0.11558806872825153, 0.6504272611159234]), - (np.array([[0.80573183, 0.00708378, -0.59223816], - [-0.59170947, -0.0342715, -0.80542248], - [-0.02600233, 0.99938745, -0.02342209]]), - [0.6610669854921286, 0.6825367059399726, -0.2141370877143812, -0.22644953831298634]), - (np.array([[-0.25174377, -0.54702511, 0.7983662], - [-0.95146476, 0.29079054, -0.10077531], - [-0.17703071, -0.78498687, -0.59367983]]), - [0.3336790888032567, -0.5126269396587794, 0.7307896657891123, -0.30301542998227005]), - (np.array([[-0.03439394, -0.92799031, -0.37101353], - [0.07572774, -0.3725858, 0.92490277], - [-0.99653518, 0.00371504, 0.0830893]]), - [0.4111294068459875, -0.5601567984771901, 0.38036785991321215, 0.6103419230982696]) - ] - - for matrix, quat in test_cases: - assert np.allclose(R.from_matrix(matrix).as_quat(), quat) - - -def main(): - test_cases = [ - (np.array([[0.41198224566568303, -0.8337376517741568, -0.3676304629248995], - [-0.058726644927620864, -0.4269176212762076, 0.902381585483331], - [-0.9092974268256819, -0.35017548837401474, -0.2248450953661529]]), - [-0.43595284407356566, 0.7182870182434113, -0.31062245106570385, -0.4444351134430007]), - (np.array([[1., 0., 0.], - [0., 0.54030231, -0.84147098], - [0., 0.84147098, 0.54030231]]), - [0.8775825618903726, 0.47942553860420295, 0.0, 0.0]), - (np.array([[0.54030231, 0., 0.84147098], - [0., 1., 0.], - [-0.84147098, 0., 0.54030231]]), - [0.8775825618903726, 0.0, 0.47942553860420295, 0.0]), - (np.array([[0.54030231, -0.84147098, 0.], - [0.84147098, 0.54030231, 0.], - [0., 0., 1.]]), - [0.8775825618903726, 0.0, 0.0, 0.47942553860420295]), - (np.array([[-0.01958302, -0.27603141, -0.9609491], - [-0.25068215, 0.93178751, -0.26254618], - [0.96787136, 0.23575134, -0.08744336]]), - [-0.6754185988029392, -0.18444025625913482, 0.7139352011035228, -0.009382797419306801]), - (np.array([[-0.08838838, 0.98018011, 0.17729764], - [0.03312264, -0.17500364, 0.98401048], - [0.99553523, 0.09284766, -0.01699786]]), - [-0.4241491835815902, 0.5252649613496548, 0.4822817145119869, 0.5582101201991901]), - (np.array([[0.15513152, -0.23316354, 0.95998384], - [-0.52038015, 0.80671434, 0.28002943], - [-0.83972538, -0.54299793, 0.00381315]]), - [0.7010098084209453, -0.2935149227036473, 0.6418274036266723, -0.10242959826996342]), - (np.array([[0.12716755, -0.51732444, 0.84628827], - [0.7772303, -0.47810987, -0.40905258], - [0.61623167, 0.7097791, 0.34128017]]), - [0.49757859885009453, 0.5621381630269271, 0.11558806872825153, 0.6504272611159234]), - (np.array([[0.80573183, 0.00708378, -0.59223816], - [-0.59170947, -0.0342715, -0.80542248], - [-0.02600233, 0.99938745, -0.02342209]]), - [0.6610669854921286, 0.6825367059399726, -0.2141370877143812, -0.22644953831298634]), - (np.array([[-0.25174377, -0.54702511, 0.7983662], - [-0.95146476, 0.29079054, -0.10077531], - [-0.17703071, -0.78498687, -0.59367983]]), - [0.3336790888032567, -0.5126269396587794, 0.7307896657891123, -0.30301542998227005]), - (np.array([[-0.03439394, -0.92799031, -0.37101353], - [0.07572774, -0.3725858, 0.92490277], - [-0.99653518, 0.00371504, 0.0830893]]), - [0.4111294068459875, -0.5601567984771901, 0.38036785991321215, 0.6103419230982696]) - ] - - for matrix, quat in test_cases: - assert np.allclose(R.from_matrix(matrix).as_quat(), quat) - - -if __name__ == '__main__': - main() diff --git a/tests/urdf_examples/fixed_to_revolute_joint.urdf b/tests/urdf_examples/fixed_to_revolute_joint.urdf deleted file mode 100644 index ca54de1..0000000 --- a/tests/urdf_examples/fixed_to_revolute_joint.urdf +++ /dev/null @@ -1,49 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/tests/urdf_examples/ground_to_many.urdf b/tests/urdf_examples/ground_to_many.urdf deleted file mode 100644 index 42b2db6..0000000 --- a/tests/urdf_examples/ground_to_many.urdf +++ /dev/null @@ -1,106 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/tests/urdf_examples/invalid_urdf/urdf_invalid_missing_joint_name.urdf b/tests/urdf_examples/invalid_urdf/urdf_invalid_missing_joint_name.urdf deleted file mode 100644 index 9ad3c84..0000000 --- a/tests/urdf_examples/invalid_urdf/urdf_invalid_missing_joint_name.urdf +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/tests/urdf_examples/invalid_urdf/urdf_invalid_missing_joint_type.urdf b/tests/urdf_examples/invalid_urdf/urdf_invalid_missing_joint_type.urdf deleted file mode 100644 index b279177..0000000 --- a/tests/urdf_examples/invalid_urdf/urdf_invalid_missing_joint_type.urdf +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/tests/urdf_examples/invalid_urdf/urdf_invalid_unsupported_joint_type.urdf b/tests/urdf_examples/invalid_urdf/urdf_invalid_unsupported_joint_type.urdf deleted file mode 100644 index 343caae..0000000 --- a/tests/urdf_examples/invalid_urdf/urdf_invalid_unsupported_joint_type.urdf +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/tests/urdf_examples/invalid_urdf/xml_invalid_invalid_token.urdf b/tests/urdf_examples/invalid_urdf/xml_invalid_invalid_token.urdf deleted file mode 100644 index f375fe6..0000000 --- a/tests/urdf_examples/invalid_urdf/xml_invalid_invalid_token.urdf +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/tests/urdf_examples/invalid_urdf/xml_invalid_mismatched_tag.urdf b/tests/urdf_examples/invalid_urdf/xml_invalid_mismatched_tag.urdf deleted file mode 100644 index 5615e6b..0000000 --- a/tests/urdf_examples/invalid_urdf/xml_invalid_mismatched_tag.urdf +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/tests/urdf_examples/large_tree_test.urdf b/tests/urdf_examples/large_tree_test.urdf deleted file mode 100644 index a1ed08c..0000000 --- a/tests/urdf_examples/large_tree_test.urdf +++ /dev/null @@ -1,177 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/tests/urdf_examples/one_continuous_joint.urdf b/tests/urdf_examples/one_continuous_joint.urdf deleted file mode 100644 index a6e3af6..0000000 --- a/tests/urdf_examples/one_continuous_joint.urdf +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/tests/urdf_examples/one_fixed_joint.urdf b/tests/urdf_examples/one_fixed_joint.urdf deleted file mode 100644 index 721d30b..0000000 --- a/tests/urdf_examples/one_fixed_joint.urdf +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/tests/urdf_examples/one_prismatic_joint.urdf b/tests/urdf_examples/one_prismatic_joint.urdf deleted file mode 100644 index 96009a3..0000000 --- a/tests/urdf_examples/one_prismatic_joint.urdf +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/tests/urdf_examples/one_revolute_joint.urdf b/tests/urdf_examples/one_revolute_joint.urdf deleted file mode 100644 index 1f7da45..0000000 --- a/tests/urdf_examples/one_revolute_joint.urdf +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/tests/urdf_examples/precomputed_kinematics/fixed_to_revolute_joint.json b/tests/urdf_examples/precomputed_kinematics/fixed_to_revolute_joint.json deleted file mode 100644 index 65fc646..0000000 --- a/tests/urdf_examples/precomputed_kinematics/fixed_to_revolute_joint.json +++ /dev/null @@ -1 +0,0 @@ -[{"state":{"revolute":0},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"f1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.7,0.0,0.0]},"r1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.95,0.0,0.0]}}},{"state":{"revolute":-0.18942539714957718},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"f1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.7,0.0,0.0]},"r1":{"rot":[0.9955181042696519,-0.0668719076719004,0.0,-0.0668719076719004],"pos":[0.95,0.0,0.0]}}},{"state":{"revolute":0.758684670386685},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"f1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.7,0.0,0.0]},"r1":{"rot":[0.9289083760633281,0.26184845701224835,0.0,0.26184845701224835],"pos":[0.95,0.0,0.0]}}},{"state":{"revolute":-0.12262523448295326},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"f1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.7,0.0,0.0]},"r1":{"rot":[0.9981209702373482,-0.043327409179724975,0.0,-0.043327409179724975],"pos":[0.95,0.0,0.0]}}},{"state":{"revolute":-1.7162714018208425},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"f1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.7,0.0,0.0]},"r1":{"rot":[0.6538491787365617,-0.5350146032892578,0.0,-0.5350146032892578],"pos":[0.95,0.0,0.0]}}},{"state":{"revolute":2.688563271117993},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"f1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.7,0.0,0.0]},"r1":{"rot":[0.22458261772269122,0.6890437750305939,0.0,0.6890437750305939],"pos":[0.95,0.0,0.0]}}},{"state":{"revolute":2.505542553571953},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"f1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.7,0.0,0.0]},"r1":{"rot":[0.3126912558824994,0.6716487841478705,0.0,0.6716487841478705],"pos":[0.95,0.0,0.0]}}},{"state":{"revolute":-2.814212054551051},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"f1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.7,0.0,0.0]},"r1":{"rot":[0.1629602778934701,-0.6976546236601902,0.0,-0.6976546236601902],"pos":[0.95,0.0,0.0]}}},{"state":{"revolute":2.318199528284884},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"f1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.7,0.0,0.0]},"r1":{"rot":[0.4001647065103528,0.6480232278489264,0.0,0.6480232278489264],"pos":[0.95,0.0,0.0]}}},{"state":{"revolute":1.0147333382140111},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"f1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.7,0.0,0.0]},"r1":{"rot":[0.8740270123936922,0.343567156176312,0.0,0.343567156176312],"pos":[0.95,0.0,0.0]}}},{"state":{"revolute":2.4054342336710475},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"f1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.7,0.0,0.0]},"r1":{"rot":[0.35982396090809526,0.6597449193273145,0.0,0.6597449193273145],"pos":[0.95,0.0,0.0]}}}] \ No newline at end of file diff --git a/tests/urdf_examples/precomputed_kinematics/ground_to_many.json b/tests/urdf_examples/precomputed_kinematics/ground_to_many.json deleted file mode 100644 index 4abb779..0000000 --- a/tests/urdf_examples/precomputed_kinematics/ground_to_many.json +++ /dev/null @@ -1 +0,0 @@ -[{"state":{"left-shoulder":0,"shoulder":0,"elbow":0,"elbow2":0},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"left-upperarm":{"rot":[1.0,0.0,0.0,0.0],"pos":[1.0,0.0,0.0]},"upperarm":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"forearm":{"rot":[1.0,0.0,0.0,0.0],"pos":[1.0,0.0,0.0]},"fingertip":{"rot":[1.0,0.0,0.0,0.0],"pos":[2.0,0.0,0.0]},"forearm2":{"rot":[1.0,0.0,0.0,0.0],"pos":[1.0,0.0,0.0]}}},{"state":{"left-shoulder":-1.3837665139134732,"shoulder":0.14890539062749353,"elbow":-0.43085329449037824,"elbow2":0.6775791906605124},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"left-upperarm":{"rot":[0.7700458849570215,0.0,-0.6379885069973891,0.0],"pos":[1.0,0.0,0.0]},"upperarm":{"rot":[0.9972296781399181,0.05259738128916376,0.05259738128916376,0.0],"pos":[0.0,0.0,0.0]},"forearm":{"rot":[0.9854224334266928,0.051381607610975016,-0.1617904298187554,-0.01124343887738393],"pos":[0.9944670309630446,0.005532969036955346,-0.10490333922799064]},"fingertip":{"rot":[0.9854224334266928,0.051381607610975016,-0.1617904298187554,-0.01124343887738393],"pos":[1.9418619147655904,-0.03325220952063313,0.21280508695711495]},"forearm2":{"rot":[0.923064263697147,0.0496076214366198,0.38103265026342054,0.017480515263535397],"pos":[0.9944670309630446,0.005532969036955346,-0.10490333922799064]}}},{"state":{"left-shoulder":1.4761953605765452,"shoulder":-2.7237522800003537,"elbow":-2.6113309782883136,"elbow2":-0.3340293158829746},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"left-upperarm":{"rot":[0.7397499329495102,0.0,0.6728818891166526,0.0],"pos":[1.0,0.0,0.0]},"upperarm":{"rot":[0.20740368782269006,-0.6917310569424897,-0.6917310569424897,0.0],"pos":[0.0,0.0,0.0]},"forearm":{"rot":[-0.6132136094312631,-0.181258118511022,-0.3814147533453684,0.6675607460093937],"pos":[0.04301628972245186,0.956983710277548,0.2869351443827192]},"fingertip":{"rot":[-0.6132136094312631,-0.181258118511022,-0.3814147533453684,0.6675607460093937],"pos":[-0.1392128376418012,0.27653808225490806,-0.4228439004227312]},"forearm2":{"rot":[0.08952487598558621,-0.6821059246480368,-0.7165845668453873,0.11499288107887741],"pos":[0.04301628972245186,0.956983710277548,0.2869351443827192]}}},{"state":{"left-shoulder":-1.3535331675661353,"shoulder":-1.9942646596206943,"elbow":-2.037744754819257,"elbow2":2.547020157887636},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"left-upperarm":{"rot":[0.7796018000188905,0.0,-0.6262755251543093,0.0],"pos":[1.0,0.0,0.0]},"upperarm":{"rot":[0.5427131422242039,-0.5939117970107729,-0.5939117970107729,0.0],"pos":[0.0,0.0,0.0]},"forearm":{"rot":[-0.22116731377616988,-0.31140367872897473,-0.7735333197015288,0.5057261823384538],"pos":[0.29453755474286897,0.7054624452571312,0.6446474751194803]},"fingertip":{"rot":[-0.22116731377616988,-0.31140367872897473,-0.7735333197015288,0.5057261823384538],"pos":[-0.41368798163932036,0.963524485497969,-0.01248308496952999]},"forearm2":{"rot":[0.726834921871061,-0.17397254868219755,0.34493444843929855,-0.5678598197914569],"pos":[0.29453755474286897,0.7054624452571312,0.6446474751194803]}}},{"state":{"left-shoulder":-0.3341196697444273,"shoulder":1.9858727045664653,"elbow":-0.40320389117353095,"elbow2":-0.1960178913283288},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"left-upperarm":{"rot":[0.9860779302601875,0.0,-0.1662838400259773,0.0],"pos":[1.0,0.0,0.0]},"upperarm":{"rot":[0.5462326318727091,0.5922963413180152,0.5922963413180152,0.0],"pos":[0.0,0.0,0.0]},"forearm":{"rot":[0.6537706963116142,0.5803006008212955,0.47092347648585153,-0.11860087952941256],"pos":[0.2983700881225865,0.7016299118774134,-0.6470631787334317]},"fingertip":{"rot":[0.6537706963116142,0.5803006008212955,0.47092347648585153,-0.11860087952941256],"pos":[0.8266999094612453,1.0931087053823714,-1.4004634402944185]},"forearm2":{"rot":[0.6015686916105578,0.5894538914278178,0.5360038743658996,-0.05795744835057221],"pos":[0.2983700881225865,0.7016299118774134,-0.6470631787334317]}}},{"state":{"left-shoulder":0.6712876601654156,"shoulder":-0.0749671063904973,"elbow":-0.39369610256065535,"elbow2":2.59507236785634},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"left-upperarm":{"rot":[0.9441984424282946,0.0,0.32937714145942537,0.0],"pos":[1.0,0.0,0.0]},"upperarm":{"rot":[0.9992975738690814,-0.026498668451149374,-0.026498668451149374,0.0],"pos":[0.0,0.0,0.0]},"forearm":{"rot":[0.974816454010891,-0.025986923621565138,-0.22142877310762618,0.005182589157060305],"pos":[0.9985956411406322,0.0014043588593678784,0.052960110187989476]},"fingertip":{"rot":[0.974816454010891,-0.025986923621565138,-0.22142877310762618,0.005182589157060305],"pos":[1.9004805195599932,0.023017010457459033,0.4843955739243331]},"forearm2":{"rot":[0.2951979462660993,-0.007151249830258371,0.955068580307505,-0.025515466986695906],"pos":[0.9985956411406322,0.0014043588593678784,0.052960110187989476]}}},{"state":{"left-shoulder":-1.3107174416124598,"shoulder":-0.12402835208801477,"elbow":-1.3427448320870163,"elbow2":-2.903834259045619},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"left-upperarm":{"rot":[0.7928293589865087,0.0,-0.6094436869219679,0.0],"pos":[1.0,0.0,0.0]},"upperarm":{"rot":[0.9980777371497506,-0.04382254331981128,-0.04382254331981128,0.0],"pos":[0.0,0.0,0.0]},"forearm":{"rot":[0.7542033043682667,-0.034311678759004975,-0.6551770450626083,0.027260300874267385],"pos":[0.9961591693939665,0.0038408306060334725,0.08747660974556833]},"fingertip":{"rot":[0.7542033043682667,-0.034311678759004975,-0.6551770450626083,0.027260300874267385],"pos":[1.1361590006325133,0.08992089720184937,1.0738793010375367]},"forearm2":{"rot":[0.0748581585419135,-0.0051973268929793245,-0.9962308170282111,0.04351325196057231],"pos":[0.9961591693939665,0.0038408306060334725,0.08747660974556833]}}},{"state":{"left-shoulder":-2.7511870016188773,"shoulder":-2.5058318389424414,"elbow":2.609984673227789,"elbow2":2.2615758774833328},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"left-upperarm":{"rot":[0.19396551293579456,0.0,-0.9810083484820882,0.0],"pos":[1.0,0.0,0.0]},"upperarm":{"rot":[0.3125538630602797,-0.6716807585029126,-0.6716807585029126,0.0],"pos":[0.0,0.0,0.0]},"forearm":{"rot":[0.7301956614114581,-0.17644053279795394,0.12513694383688398,-0.6480924160403532],"pos":[0.09768991731390408,0.9023100826860959,0.41987283162668826]},"fingertip":{"rot":[0.7301956614114581,-0.17644053279795394,0.12513694383688398,-0.6480924160403532],"pos":[0.22632384843018902,-0.08831691617314985,0.4658234670596706]},"forearm2":{"rot":[0.7408332808396557,-0.28610044041766813,-0.003317929671353803,-0.6077018836040122],"pos":[0.09768991731390408,0.9023100826860959,0.41987283162668826]}}},{"state":{"left-shoulder":-2.0641780592261787,"shoulder":2.749762945131865,"elbow":1.874258228972355,"elbow2":-0.3776694460555321},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"left-upperarm":{"rot":[0.5130267999510562,0.0,-0.8583725895740025,0.0],"pos":[1.0,0.0,0.0]},"upperarm":{"rot":[0.19466396935928232,0.6935798220224145,0.6935798220224145,0.0],"pos":[0.0,0.0,0.0]},"forearm":{"rot":[-0.44366800341987517,0.4106713856823089,0.5675435958803319,0.5589293179807412],"pos":[0.03789406096671161,0.9621059390332884,-0.2700300024447756]},"fingertip":{"rot":[-0.44366800341987517,0.4106713856823089,0.5675435958803319,0.5589293179807412],"pos":[-0.23112137047967635,0.9322956598211849,0.6926444206583887]},"forearm2":{"rot":[0.321398514629209,0.6812504982993682,0.6447092563919754,-0.1301949618207504],"pos":[0.03789406096671161,0.9621059390332884,-0.2700300024447756]}}},{"state":{"left-shoulder":1.5009234999406011,"shoulder":-2.930288855889959,"elbow":0.6394592753643908,"elbow2":-1.3917192481023337},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"left-upperarm":{"rot":[0.7313740442050082,0.0,0.6819765446576669,0.0],"pos":[1.0,0.0,0.0]},"upperarm":{"rot":[0.10545545507080949,-0.7031639734072731,-0.7031639734072731,0.0],"pos":[0.0,0.0,0.0]},"forearm":{"rot":[0.3211224243351752,-0.6675279258298543,-0.6343822310651485,-0.22101140634636204],"pos":[0.011120853004191522,0.9888791469958086,0.14830495361012513]},"fingertip":{"rot":[0.3211224243351752,-0.6675279258298543,-0.6343822310651485,-0.22101140634636204],"pos":[0.1085471393514072,1.6938714195449358,0.8507962449259729]},"forearm2":{"rot":[-0.36982268844725175,-0.5396804080013985,-0.607282146626873,0.4507600589197631],"pos":[0.011120853004191522,0.9888791469958086,0.14830495361012513]}}},{"state":{"left-shoulder":1.2126668224911636,"shoulder":-1.7073885023331945,"elbow":-1.3847187105377472,"elbow2":0.4238810784312812},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"left-upperarm":{"rot":[0.8217429729102577,0.0,0.5698583038551002,0.0],"pos":[1.0,0.0,0.0]},"upperarm":{"rot":[0.6572032301326386,-0.5329558679211751,-0.5329558679211751,0.0],"pos":[0.0,0.0,0.0]},"forearm":{"rot":[0.1656618923356717,-0.4102385436358021,-0.8297675459284675,0.3402150708995549],"pos":[0.4319160856967739,0.5680839143032261,0.7005206358318804]},"fingertip":{"rot":[0.1656618923356717,-0.4102385436358021,-0.8297675459284675,0.3402150708995549],"pos":[-0.17660486378990686,1.361610518391792,0.6963036891287839]},"forearm2":{"rot":[0.7546092292712313,-0.5210307399527228,-0.38278316831734566,-0.1121112178862191],"pos":[0.4319160856967739,0.5680839143032261,0.7005206358318804]}}}] \ No newline at end of file diff --git a/tests/urdf_examples/precomputed_kinematics/large_tree_test.json b/tests/urdf_examples/precomputed_kinematics/large_tree_test.json deleted file mode 100644 index 19d4283..0000000 --- a/tests/urdf_examples/precomputed_kinematics/large_tree_test.json +++ /dev/null @@ -1 +0,0 @@ -[{"state":{"fc":0,"cr":0,"fp":0,"pr":0,"pc":0},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"fix1":{"rot":[0.8775825618903728,0.0,0.0,0.479425538604203],"pos":[1.0,0.0,0.0]},"cont1":{"rot":[0.8775825618903728,0.0,0.0,0.479425538604203],"pos":[0.1585290151921035,0.5403023058681398,0.0]},"fix2":{"rot":[0.8677282556982174,0.1311442991402941,0.07164445714916154,0.4740421065957454],"pos":[0.1585290151921035,0.5403023058681398,0.5]},"revo1":{"rot":[0.8243771119105122,-0.16439396602553005,0.3009211363333468,0.45035926880694593],"pos":[0.6988313210602433,1.3817732906760363,0.0]},"fix4":{"rot":[0.8243771119105122,-0.16439396602553005,0.3009211363333468,0.45035926880694593],"pos":[1.112077318475284,2.02536579923294,-0.644217687237691]},"pris1":{"rot":[0.7701511529340699,-0.22984884706593015,0.42073549240394825,0.42073549240394825],"pos":[1.5403023058681398,0.8414709848078965,0.0]},"revo2":{"rot":[0.6986292594100998,-0.29014181852594245,0.5311010363670602,0.3816629038934609],"pos":[1.676696919891992,1.053893010289968,0.16209069176044194]},"cont2":{"rot":[0.7701511529340699,-0.22984884706593015,0.42073549240394825,0.42073549240394825],"pos":[1.9949510192809807,1.5495444030814678,0.5403023058681398]},"fix3":{"rot":[0.7701511529340699,-0.22984884706593015,0.42073549240394825,0.42073549240394825],"pos":[1.8322288875945687,1.2961196982207372,-0.8414709848078966]}}},{"state":{"fc":1.3181298353974968,"cr":1.9855786198451186,"fp":1.2125449311070513,"pr":-1.5285394443207776,"pc":-1.5598494527704185},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"fix1":{"rot":[0.8775825618903728,0.0,0.0,0.479425538604203],"pos":[1.0,0.0,0.0]},"cont1":{"rot":[0.40019664105506736,0.0,0.0,0.9164292926834245],"pos":[0.1585290151921035,0.5403023058681398,0.0]},"fix2":{"rot":[0.39570286416240236,0.05980463866147659,0.13694948204271243,0.9061387795787544],"pos":[0.1585290151921035,0.5403023058681398,0.5]},"revo1":{"rot":[0.0904590766144541,-0.8927110178760783,0.3898390783000323,0.20714653521335497],"pos":[-0.5211562817843798,1.273806155260895,0.0]},"fix4":{"rot":[0.0904590766144541,-0.8927110178760783,0.3898390783000323,0.20714653521335497],"pos":[0.08907533017418734,0.6152554430652741,-0.44037295470213567]},"pris1":{"rot":[0.7701511529340699,-0.22984884706593015,0.42073549240394825,0.42073549240394825],"pos":[0.5199809285656662,1.4966118070537608,0.0]},"revo2":{"rot":[0.7684465633606993,-0.5769771013313885,0.18261437574006872,-0.20794060106536266],"pos":[0.6563755425895185,1.709033832535832,0.16209069176044194]},"cont2":{"rot":[0.8434237470077608,-0.4592869215040846,0.1374931663801185,-0.24246141119250014],"pos":[0.9746296419785071,2.204685225327332,0.5403023058681398]},"fix3":{"rot":[0.7701511529340699,-0.22984884706593015,0.42073549240394825,0.42073549240394825],"pos":[0.8119075102920952,1.9512605204666018,-0.8414709848078966]}}},{"state":{"fc":-1.1282714181836186,"cr":1.8539078124172876,"fp":2.04582885796762,"pr":0.11793473499507279,"pc":1.4333597410417962},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"fix1":{"rot":[0.8775825618903728,0.0,0.0,0.479425538604203],"pos":[1.0,0.0,0.0]},"cont1":{"rot":[0.9979440103117609,0.0,0.0,-0.06409174894539704],"pos":[0.1585290151921035,0.5403023058681398,0.0]},"fix2":{"rot":[0.9867381747957767,0.14913088921420378,-0.009577751269366906,-0.06337206769154644],"pos":[0.1585290151921035,0.5403023058681398,0.5]},"revo1":{"rot":[0.28903660054162744,0.06134465196934892,0.9551701896858276,-0.018563026629277297],"pos":[1.1503135106263438,0.4123823519272115,0.0]},"fix4":{"rot":[0.28903660054162744,0.06134465196934892,0.9551701896858276,-0.018563026629277297],"pos":[0.32492415618214554,0.5188407294173797,-0.5544357739471277]},"pris1":{"rot":[0.7701511529340699,-0.22984884706593015,0.42073549240394825,0.42073549240394825],"pos":[-0.18120331799428757,1.9468370341793846,0.0]},"revo2":{"rot":[0.6749223754166902,-0.25833804750727435,0.5472769307195723,0.4221720046073474],"pos":[-0.04480870397043532,2.159259059661456,0.16209069176044194]},"cont2":{"rot":[0.30431208950119176,0.10307089127439698,0.4682149489687739,0.8231314020965841],"pos":[0.2734453954185533,2.6549104524529556,0.5403023058681398]},"fix3":{"rot":[0.7701511529340699,-0.22984884706593015,0.42073549240394825,0.42073549240394825],"pos":[0.11072326373214131,2.4014857475922256,-0.8414709848078966]}}},{"state":{"fc":0.3478330252498676,"cr":-0.8343531706365295,"fp":-2.8728376915981646,"pr":2.9913549318582024,"pc":-1.7291639184382694},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"fix1":{"rot":[0.8775825618903728,0.0,0.0,0.479425538604203],"pos":[1.0,0.0,0.0]},"cont1":{"rot":[0.7813835610999368,0.0,0.0,0.6240510639705548],"pos":[0.1585290151921035,0.5403023058681398,0.0]},"fix2":{"rot":[0.7726094659902878,0.11676850011634506,0.0932570255679223,0.6170436432092995],"pos":[0.1585290151921035,0.5403023058681398,0.5]},"revo1":{"rot":[0.7796211517137761,0.04189009675110774,-0.052451209306400445,0.6226435177828195],"pos":[0.3796495543065407,1.5155487912151728,0.0]},"fix4":{"rot":[0.7796211517137761,0.04189009675110774,-0.052451209306400445,0.6226435177828195],"pos":[0.5987773951172045,2.482006531631975,0.13394933881921725]},"pris1":{"rot":[0.7701511529340699,-0.22984884706593015,0.42073549240394825,0.42073549240394825],"pos":[3.9577118674104916,-0.7107298443474954,0.0]},"revo2":{"rot":[-0.32815568642361104,0.5078286499786404,0.32918178556623967,0.7253021851477786],"pos":[4.094106481434344,-0.4983078188654241,0.16209069176044194]},"cont2":{"rot":[0.8199016041207721,-0.4692679771969494,0.09816523133683083,-0.3129097513569284],"pos":[4.412360580823332,-0.002656426073924245,0.5403023058681398]},"fix3":{"rot":[0.7701511529340699,-0.22984884706593015,0.42073549240394825,0.42073549240394825],"pos":[4.249638449136921,-0.25608113093465457,-0.8414709848078966]}}},{"state":{"fc":-2.130429680656637,"cr":0.9294751382109849,"fp":1.7326456815067957,"pr":-0.42134751558067274,"pc":3.135959089214987},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"fix1":{"rot":[0.8775825618903728,0.0,0.0,0.479425538604203],"pos":[1.0,0.0,0.0]},"cont1":{"rot":[0.8444735520229688,0.0,0.0,-0.5355972553455722],"pos":[0.1585290151921035,0.5403023058681398,0.0]},"fix2":{"rot":[0.8349910243222293,0.1261965505376593,-0.08003865359682777,-0.5295830755076271],"pos":[0.1585290151921035,0.5403023058681398,0.5]},"revo1":{"rot":[0.579358974095796,0.3896714160473093,0.6143930006119509,-0.36745150353399686],"pos":[0.5848001753246832,-0.364293127482717,0.0]},"fix4":{"rot":[0.579358974095796,0.3896714160473093,0.6143930006119509,-0.36745150353399686],"pos":[0.55980144202398,-0.3112429986015168,-0.9982788924739661]},"pris1":{"rot":[0.7701511529340699,-0.22984884706593015,0.42073549240394825,0.42073549240394825],"pos":[0.08233123792746722,1.7776234417784929,0.0]},"revo2":{"rot":[0.7629956267230742,-0.3947900687999165,0.4586844092234479,0.22712791090871454],"pos":[0.21872585195131947,1.9900454672605643,0.16209069176044194]},"cont2":{"rot":[-0.4185644781117622,0.4200863900102012,0.23103305389949563,0.771333216319055],"pos":[0.5369799513403081,2.485696860052064,0.5403023058681398]},"fix3":{"rot":[0.7701511529340699,-0.22984884706593015,0.42073549240394825,0.42073549240394825],"pos":[0.3742578196538961,2.2322721551913336,-0.8414709848078966]}}},{"state":{"fc":3.0906168455727423,"cr":-1.6557961731536792,"fp":-2.87294080829182,"pr":0.30189593607928655,"pc":0.8368668949162812},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"fix1":{"rot":[0.8775825618903728,0.0,0.0,0.479425538604203],"pos":[1.0,0.0,0.0]},"cont1":{"rot":[-0.45690450327775645,0.0,0.0,0.8895157530277397],"pos":[0.1585290151921035,0.5403023058681398,0.0]},"fix2":{"rot":[-0.45177395821977917,-0.06827895568860541,0.13292757293831273,0.8795274499623285],"pos":[0.1585290151921035,0.5403023058681398,0.5]},"revo1":{"rot":[-0.40571455960157504,0.4091005437169185,0.2101366727698746,0.7898575947694803],"pos":[-0.4239475345769099,-0.27254520072161803,0.0]},"fix4":{"rot":[-0.40571455960157504,0.4091005437169185,0.2101366727698746,0.7898575947694803],"pos":[-0.7600124170925532,-0.7415245989712962,0.8167733582562035]},"pris1":{"rot":[0.7701511529340699,-0.22984884706593015,0.42073549240394825,0.42073549240394825],"pos":[3.9577986371162517,-0.710785558534851,0.0]},"revo2":{"rot":[0.6332924184412968,-0.2069780828120884,0.5686920598383087,0.4823796503347411],"pos":[4.094193251140104,-0.4983635330527797,0.16209069176044194]},"cont2":{"rot":[0.5327503288514375,-0.03906174111316685,0.47783158900091366,0.6973723826211897],"pos":[4.412447350529092,-0.0027121402612798606,0.5403023058681398]},"fix3":{"rot":[0.7701511529340699,-0.22984884706593015,0.42073549240394825,0.42073549240394825],"pos":[4.249725218842681,-0.2561368451220102,-0.8414709848078966]}}},{"state":{"fc":0.2750317264959068,"cr":2.421119976621947,"fp":2.9751631407264405,"pr":1.4191553380442663,"pc":1.2086888549136319},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"fix1":{"rot":[0.8775825618903728,0.0,0.0,0.479425538604203],"pos":[1.0,0.0,0.0]},"cont1":{"rot":[0.8035767966632751,0.0,0.0,0.595201085234553],"pos":[0.1585290151921035,0.5403023058681398,0.0]},"fix2":{"rot":[0.7945534954411383,0.120085015792477,0.08894573862371115,0.5885176186360711],"pos":[0.1585290151921035,0.5403023058681398,0.5]},"revo1":{"rot":[0.008225540437761983,-0.5951699021406188,0.8035346965875944,0.006092573373852846],"pos":[0.4500003514633245,1.4968818687547139,0.0]},"fix4":{"rot":[0.008225540437761983,-0.5951699021406188,0.8035346965875944,0.006092573373852846],"pos":[0.15859009532245844,0.5405027647027665,-0.020471246877253203]},"pris1":{"rot":[0.7701511529340699,-0.22984884706593015,0.42073549240394825,0.42073549240394825],"pos":[-0.963211152123093,2.4489584900762895,0.0]},"revo2":{"rot":[0.28134756506215486,0.12590711237375404,0.5919442412435559,0.7447099851256798],"pos":[-0.8268165380992408,2.6613805155583607,0.16209069176044194]},"cont2":{"rot":[0.3946664887849822,0.04993465534835917,0.4768179707825312,0.7838300297667878],"pos":[-0.5085624387102522,3.1570319083498606,0.5403023058681398]},"fix3":{"rot":[0.7701511529340699,-0.22984884706593015,0.42073549240394825,0.42073549240394825],"pos":[-0.6712845703966641,2.9036072034891305,-0.8414709848078966]}}},{"state":{"fc":-0.17123622026850072,"cr":-0.6892512049049029,"fp":1.5621073618522425,"pr":-1.5856488220786704,"pc":-0.8748629258217573},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"fix1":{"rot":[0.8775825618903728,0.0,0.0,0.479425538604203],"pos":[1.0,0.0,0.0]},"cont1":{"rot":[0.9153653614460333,0.0,0.0,0.4026242107285315],"pos":[0.1585290151921035,0.5403023058681398,0.0]},"fix2":{"rot":[0.9050867951423093,0.13679049014551636,0.060167410139928615,0.3981031748451983],"pos":[0.1585290151921035,0.5403023058681398,0.5]},"revo1":{"rot":[0.9153521417031091,-0.0021638521538499943,0.004919513670925693,0.40261839601368093],"pos":[0.8343165050625577,1.277398818229032,0.0]},"fix4":{"rot":[0.9153521417031091,-0.0021638521538499943,0.004919513670925693,0.40261839601368093],"pos":[1.5100649562157824,2.014452750188913,-0.010748588116426781]},"pris1":{"rot":[0.7701511529340699,-0.22984884706593015,0.42073549240394825,0.42073549240394825],"pos":[0.2258342857146678,1.68548119443026,0.0]},"revo2":{"rot":[0.7621964281106361,-0.581955680418938,0.16606676884866892,-0.22979560328187665],"pos":[0.36222889973852,1.8979032199123314,0.16209069176044194]},"cont2":{"rot":[0.8758653325336478,-0.3864364227927687,0.2837529527688808,0.05487323755522716],"pos":[0.6804829991275086,2.393554612703831,0.5403023058681398]},"fix3":{"rot":[0.7701511529340699,-0.22984884706593015,0.42073549240394825,0.42073549240394825],"pos":[0.5177608674410967,2.1401299078431006,-0.8414709848078966]}}},{"state":{"fc":-0.6564147534144382,"cr":2.6025441035667978,"fp":3.1300709330838865,"pr":-0.5499964579471541,"pc":-1.0452482854879275},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"fix1":{"rot":[0.8775825618903728,0.0,0.0,0.479425538604203],"pos":[1.0,0.0,0.0]},"cont1":{"rot":[0.98527990326528,0.0,0.0,0.17094885849738944],"pos":[0.1585290151921035,0.5403023058681398,0.0]},"fix2":{"rot":[0.9742162720203303,0.14723838870773193,0.02554627816234345,0.1690292870883997],"pos":[0.1585290151921035,0.5403023058681398,0.5]},"revo1":{"rot":[-0.07920555617516119,-0.1703955955246969,0.9820911198302442,-0.013742388705911806],"pos":[1.1000819907489827,0.8771672553953755,0.0]},"fix4":{"rot":[-0.07920555617516119,-0.1703955955246969,0.9820911198302442,-0.013742388705911806],"pos":[0.1706983489554481,0.5446562000306477,0.16025743173663412]},"pris1":{"rot":[0.7701511529340699,-0.22984884706593015,0.42073549240394825,0.42073549240394825],"pos":[-1.0935615647125303,2.5326555274839606,0.0]},"revo2":{"rot":[0.7760174810030502,-0.4234578909568136,0.43235864773832566,0.17761273459404317],"pos":[-0.9571669506886781,2.745077552966032,0.16209069176044194]},"cont2":{"rot":[0.8773579753479145,-0.4091793768338068,0.24984211942710477,-0.019852859428258374],"pos":[-0.6389128512996894,3.2407289457575317,0.5403023058681398]},"fix3":{"rot":[0.7701511529340699,-0.22984884706593015,0.42073549240394825,0.42073549240394825],"pos":[-0.8016349829861014,2.9873042408968016,-0.8414709848078966]}}},{"state":{"fc":1.814220592658689,"cr":-2.473429849482053,"fp":-1.8312940426115258,"pr":2.2174659190861528,"pc":-1.9920936189920146},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"fix1":{"rot":[0.8775825618903728,0.0,0.0,0.479425538604203],"pos":[1.0,0.0,0.0]},"cont1":{"rot":[0.16295606590418554,0.0,0.0,0.9866333263097444],"pos":[0.1585290151921035,0.5403023058681398,0.0]},"fix2":{"rot":[0.16112624494029829,0.024351850163966245,0.14744064171994342,0.9755544975829089],"pos":[0.1585290151921035,0.5403023058681398,0.5]},"revo1":{"rot":[0.10298193783383142,0.7646407217314546,-0.12629093354219115,0.6235141435886042],"pos":[-0.788361625977958,0.8618580765589329,0.0]},"fix4":{"rot":[0.10298193783383142,0.7646407217314546,-0.12629093354219115,0.6235141435886042],"pos":[-0.5978002002779357,0.7971450849623649,0.9795399796607576]},"pris1":{"rot":[0.7701511529340699,-0.22984884706593015,0.42073549240394825,0.42073549240394825],"pos":[3.0812831073772946,-0.14798140913769853,0.0]},"revo2":{"rot":[-0.03019349513294728,0.3460628184788197,0.49647871188420734,0.7955110100834246],"pos":[3.217677721401147,0.06444061634437281,0.16209069176044194]},"cont2":{"rot":[0.7718083783116234,-0.4780867693597536,0.035803463925770496,-0.41768765854649315],"pos":[3.5359318207901356,0.5600920091358726,0.5403023058681398]},"fix3":{"rot":[0.7701511529340699,-0.22984884706593015,0.42073549240394825,0.42073549240394825],"pos":[3.3732096891037235,0.3066673042751423,-0.8414709848078966]}}},{"state":{"fc":-0.491041207139681,"cr":-1.523580385448599,"fp":2.3161255558866687,"pr":-3.042547082226536,"pc":2.3347034108602776},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"fix1":{"rot":[0.8775825618903728,0.0,0.0,0.479425538604203],"pos":[1.0,0.0,0.0]},"cont1":{"rot":[0.9677944844108965,0.0,0.0,0.2517416055082017],"pos":[0.1585290151921035,0.5403023058681398,0.0]},"fix2":{"rot":[0.9569271955715184,0.1446254003686142,0.0376197953930512,0.24891481863969447],"pos":[0.1585290151921035,0.5403023058681398,0.5]},"revo1":{"rot":[0.8868924459037945,0.10075970634590929,-0.38736023731784625,0.23069745885235313],"pos":[1.0317813433044094,1.0275705804833024,0.0]},"fix4":{"rot":[0.8868924459037945,0.10075970634590929,-0.38736023731784625,0.23069745885235313],"pos":[1.6252428013526667,1.3587176400495533,0.733583753058819]},"pris1":{"rot":[0.7701511529340699,-0.22984884706593015,0.42073549240394825,0.42073549240394825],"pos":[-0.4086501465825527,2.0928789633335914,0.0]},"revo2":{"rot":[0.4157789110205547,-0.5448126637626569,-0.26349525060528955,-0.6788794528212321],"pos":[-0.2722555325587005,2.3053009888156626,0.16209069176044194]},"cont2":{"rot":[-0.08460371417989254,0.29672031139650307,0.3765712467391694,0.8734949138266559],"pos":[0.045998566830288135,2.8009523816071624,0.5403023058681398]},"fix3":{"rot":[0.7701511529340699,-0.22984884706593015,0.42073549240394825,0.42073549240394825],"pos":[-0.11672356485612384,2.5475276767464323,-0.8414709848078966]}}}] \ No newline at end of file diff --git a/tests/urdf_examples/precomputed_kinematics/one_continuous_joint.json b/tests/urdf_examples/precomputed_kinematics/one_continuous_joint.json deleted file mode 100644 index f1edf90..0000000 --- a/tests/urdf_examples/precomputed_kinematics/one_continuous_joint.json +++ /dev/null @@ -1 +0,0 @@ -[{"state":{"continuous":0},"transformations":{"c1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.8775825618903728,0.479425538604203,0.0,0.0],"pos":[1.0,0.0,0.0]}}},{"state":{"continuous":-2.0586290214928944},"transformations":{"c1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.45231166297016673,0.24709898766598748,-0.7520407651685062,-0.41084174247558425],"pos":[1.0,0.0,0.0]}}},{"state":{"continuous":0.6001779928901865},"transformations":{"c1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.8383635596351858,0.4580001000230052,0.2594179924559784,0.14172069518897865],"pos":[1.0,0.0,0.0]}}},{"state":{"continuous":0.30339114107384146},"transformations":{"c1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.8675046440535844,0.4739199469975244,0.13261540438249877,0.07244812560580022],"pos":[1.0,0.0,0.0]}}},{"state":{"continuous":2.8637523023439897},"transformations":{"c1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.12152216945182437,0.06638786374275067,0.8691280200671199,0.4748068013556715],"pos":[1.0,0.0,0.0]}}},{"state":{"continuous":-0.6738826871232817},"transformations":{"c1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.828236337059759,0.4524675731148472,-0.29013052736294964,-0.1584990294780714],"pos":[1.0,0.0,0.0]}}},{"state":{"continuous":-3.0103669316404535},"transformations":{"c1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.05753939690903935,0.0314339157955183,-0.8756942221674264,-0.4783939339118865],"pos":[1.0,0.0,0.0]}}},{"state":{"continuous":-2.128154685225597},"transformations":{"c1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.4259005899205837,0.23267055269955406,-0.7673068750111449,-0.4191816562928466],"pos":[1.0,0.0,0.0]}}},{"state":{"continuous":1.2146085145947403},"transformations":{"c1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.7206614457149935,0.3936991421285266,0.5007975974323813,0.27358697438509816],"pos":[1.0,0.0,0.0]}}},{"state":{"continuous":-0.2371808797839634},"transformations":{"c1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.8714187651668264,0.4760582411072386,-0.10382913198709426,-0.05672211332286914],"pos":[1.0,0.0,0.0]}}},{"state":{"continuous":0.08637341342459282},"transformations":{"c1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.8767643031312033,0.4789785218067322,0.03788812068347163,0.02069837466488257],"pos":[1.0,0.0,0.0]}}}] \ No newline at end of file diff --git a/tests/urdf_examples/precomputed_kinematics/one_fixed_joint.json b/tests/urdf_examples/precomputed_kinematics/one_fixed_joint.json deleted file mode 100644 index af403fc..0000000 --- a/tests/urdf_examples/precomputed_kinematics/one_fixed_joint.json +++ /dev/null @@ -1 +0,0 @@ -[{"state":{},"transformations":{"f1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[1.0,0.0,0.0]}}},{"state":{},"transformations":{"f1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[1.0,0.0,0.0]}}},{"state":{},"transformations":{"f1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[1.0,0.0,0.0]}}},{"state":{},"transformations":{"f1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[1.0,0.0,0.0]}}},{"state":{},"transformations":{"f1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[1.0,0.0,0.0]}}},{"state":{},"transformations":{"f1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[1.0,0.0,0.0]}}},{"state":{},"transformations":{"f1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[1.0,0.0,0.0]}}},{"state":{},"transformations":{"f1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[1.0,0.0,0.0]}}},{"state":{},"transformations":{"f1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[1.0,0.0,0.0]}}},{"state":{},"transformations":{"f1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[1.0,0.0,0.0]}}},{"state":{},"transformations":{"f1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[1.0,0.0,0.0]}}}] \ No newline at end of file diff --git a/tests/urdf_examples/precomputed_kinematics/one_prismatic_joint.json b/tests/urdf_examples/precomputed_kinematics/one_prismatic_joint.json deleted file mode 100644 index 0cd4679..0000000 --- a/tests/urdf_examples/precomputed_kinematics/one_prismatic_joint.json +++ /dev/null @@ -1 +0,0 @@ -[{"state":{"prismatic":0},"transformations":{"p1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.8775825618903728,0.0,0.479425538604203,0.0],"pos":[1.0,0.0,0.0]}}},{"state":{"prismatic":-2.3390293352087594},"transformations":{"p1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.8775825618903728,0.0,0.479425538604203,0.0],"pos":[1.0,-2.3390293352087594,0.0]}}},{"state":{"prismatic":3.10184502748534},"transformations":{"p1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.8775825618903728,0.0,0.479425538604203,0.0],"pos":[1.0,3.1018450274853406,0.0]}}},{"state":{"prismatic":-1.4941021735940212},"transformations":{"p1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.8775825618903728,0.0,0.479425538604203,0.0],"pos":[1.0,-1.4941021735940212,0.0]}}},{"state":{"prismatic":-0.18894313797532813},"transformations":{"p1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.8775825618903728,0.0,0.479425538604203,0.0],"pos":[1.0,-0.18894313797532813,0.0]}}},{"state":{"prismatic":-1.157735478153664},"transformations":{"p1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.8775825618903728,0.0,0.479425538604203,0.0],"pos":[1.0,-1.157735478153664,0.0]}}},{"state":{"prismatic":3.121002426288527},"transformations":{"p1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.8775825618903728,0.0,0.479425538604203,0.0],"pos":[1.0,3.1210024262885274,0.0]}}},{"state":{"prismatic":-0.12221792588061309},"transformations":{"p1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.8775825618903728,0.0,0.479425538604203,0.0],"pos":[1.0,-0.12221792588061309,0.0]}}},{"state":{"prismatic":2.7448809844665947},"transformations":{"p1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.8775825618903728,0.0,0.479425538604203,0.0],"pos":[1.0,2.744880984466595,0.0]}}},{"state":{"prismatic":-0.7150520129539197},"transformations":{"p1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.8775825618903728,0.0,0.479425538604203,0.0],"pos":[1.0,-0.7150520129539197,0.0]}}},{"state":{"prismatic":2.184727984088709},"transformations":{"p1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.8775825618903728,0.0,0.479425538604203,0.0],"pos":[1.0,2.184727984088709,0.0]}}}] \ No newline at end of file diff --git a/tests/urdf_examples/precomputed_kinematics/one_revolute_joint.json b/tests/urdf_examples/precomputed_kinematics/one_revolute_joint.json deleted file mode 100644 index 640d9f0..0000000 --- a/tests/urdf_examples/precomputed_kinematics/one_revolute_joint.json +++ /dev/null @@ -1 +0,0 @@ -[{"state":{"revolute":0},"transformations":{"r1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.8775825618903728,0.0,0.0,0.479425538604203],"pos":[1.0,0.0,0.0]}}},{"state":{"revolute":2.946959557267255},"transformations":{"r1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.08526856731642571,-0.47715712739358335,0.8734302630215386,0.046582430630376225],"pos":[1.0,0.0,0.0]}}},{"state":{"revolute":2.9695623058864804},"transformations":{"r1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.07539236986296528,-0.477653090489062,0.8743381173782347,0.04118703937136189],"pos":[1.0,0.0,0.0]}}},{"state":{"revolute":-0.6591746844244994},"transformations":{"r1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.830347542155213,0.1551673264265724,-0.2840318858347889,0.4536209297150647],"pos":[1.0,0.0,0.0]}}},{"state":{"revolute":1.8459471495782482},"transformations":{"r1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.5295786895975412,-0.38229370946805147,0.6997839412691756,0.2893101566953486],"pos":[1.0,0.0,0.0]}}},{"state":{"revolute":-1.94307068882347},"transformations":{"r1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.49498498252354894,0.3958869186786173,-0.7246661438277849,0.2704115283878999],"pos":[1.0,0.0,0.0]}}},{"state":{"revolute":-1.2893768260726244},"transformations":{"r1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.7014403803114123,0.2881104994839028,-0.5273827318017259,0.3831986262410999],"pos":[1.0,0.0,0.0]}}},{"state":{"revolute":-0.5469639694162827},"transformations":{"r1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.8449683476957903,0.1294859574906593,-0.23702245532083235,0.46160831218540394],"pos":[1.0,0.0,0.0]}}},{"state":{"revolute":-2.792940747885962},"transformations":{"r1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.15221173348750971,0.4721592080283579,-0.8642816329893845,0.08315364898766602],"pos":[1.0,0.0,0.0]}}},{"state":{"revolute":-0.496313296423621},"transformations":{"r1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.8506994241764716,0.11775529910626172,-0.21554962918058934,0.46473921353628533],"pos":[1.0,0.0,0.0]}}},{"state":{"revolute":-1.9917191464722872},"transformations":{"r1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"ground":{"rot":[0.4772133493910659,0.40234671091214197,-0.7364907141960653,0.26070284095903407],"pos":[1.0,0.0,0.0]}}}] \ No newline at end of file diff --git a/tests/urdf_examples/precomputed_kinematics/prismatic_to_continuous_joint.json b/tests/urdf_examples/precomputed_kinematics/prismatic_to_continuous_joint.json deleted file mode 100644 index 4904e44..0000000 --- a/tests/urdf_examples/precomputed_kinematics/prismatic_to_continuous_joint.json +++ /dev/null @@ -1 +0,0 @@ -[{"state":{"prismatic":0,"continuous":0},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"p1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.7,0.0,0.0]},"c1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.95,0.0,0.0]}}},{"state":{"prismatic":-0.5110890297856008,"continuous":-2.4220578208729444},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"p1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.18891097021439918,0.0,0.0]},"c1":{"rot":[0.3520565494574987,-0.6618369081518798,-0.6618369081518798,0.0],"pos":[0.4389109702143992,0.0,0.0]}}},{"state":{"prismatic":-3.1329966174522546,"continuous":0.31655104805727197},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"p1":{"rot":[1.0,0.0,0.0,0.0],"pos":[-2.432996617452255,0.0,0.0]},"c1":{"rot":[0.9875005557421019,0.11145100360481247,0.11145100360481247,0.0],"pos":[-2.182996617452255,0.0,0.0]}}},{"state":{"prismatic":-0.13318351770496184,"continuous":-3.1401984145082045},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"p1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.5668164822950381,0.0,0.0]},"c1":{"rot":[0.0006971194843305305,-0.7071066093682141,-0.7071066093682141,0.0],"pos":[0.8168164822950381,0.0,0.0]}}},{"state":{"prismatic":-0.46221513672142667,"continuous":1.013446669516492},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"p1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.2377848632785733,0.0,0.0]},"c1":{"rot":[0.8743394130782498,0.34316948490358345,0.34316948490358345,0.0],"pos":[0.4877848632785733,0.0,0.0]}}},{"state":{"prismatic":0.06686267360706921,"continuous":1.9391898513869466},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"p1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.7668626736070692,0.0,0.0]},"c1":{"rot":[0.5656336247808699,0.583120314565208,0.583120314565208,0.0],"pos":[1.0168626736070692,0.0,0.0]}}},{"state":{"prismatic":-2.914804124152472,"continuous":2.5783113728556417},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"p1":{"rot":[1.0,0.0,0.0,0.0],"pos":[-2.2148041241524723,0.0,0.0]},"c1":{"rot":[0.27793202227057057,0.6792473006926826,0.6792473006926826,0.0],"pos":[-1.9648041241524723,0.0,0.0]}}},{"state":{"prismatic":-2.013064002776499,"continuous":1.0533392198992981},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"p1":{"rot":[1.0,0.0,0.0,0.0],"pos":[-1.313064002776499,0.0,0.0]},"c1":{"rot":[0.8644859057051375,0.3554322149420112,0.3554322149420112,0.0],"pos":[-1.063064002776499,0.0,0.0]}}},{"state":{"prismatic":-2.324706432561206,"continuous":-0.92725664440123},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"p1":{"rot":[1.0,0.0,0.0,0.0],"pos":[-1.624706432561206,0.0,0.0]},"c1":{"rot":[0.8944358161528196,-0.3162155679145473,-0.3162155679145473,0.0],"pos":[-1.374706432561206,0.0,0.0]}}},{"state":{"prismatic":0.8515602401563651,"continuous":-0.7485077076133395},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"p1":{"rot":[1.0,0.0,0.0,0.0],"pos":[1.551560240156365,0.0,0.0]},"c1":{"rot":[0.9307806557175856,-0.258502776524684,-0.258502776524684,0.0],"pos":[1.801560240156365,0.0,0.0]}}},{"state":{"prismatic":-0.25415687066273174,"continuous":-1.2173054907071958},"transformations":{"ground":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.0,0.0,0.0]},"p1":{"rot":[1.0,0.0,0.0,0.0],"pos":[0.4458431293372682,0.0,0.0]},"c1":{"rot":[0.8204190720899606,-0.40429725830819585,-0.40429725830819585,0.0],"pos":[0.6958431293372682,0.0,0.0]}}}] \ No newline at end of file diff --git a/tests/urdf_examples/prismatic_to_continuous_joint.urdf b/tests/urdf_examples/prismatic_to_continuous_joint.urdf deleted file mode 100644 index 0a4c612..0000000 --- a/tests/urdf_examples/prismatic_to_continuous_joint.urdf +++ /dev/null @@ -1,50 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -