42 | posetree
43 | 44 | 45 | 46 | 47 | 48 | 49 |1from .pose import Transform, Pose, PoseTree, CustomFramePoseTree
50 | ├── tests ├── __init__.py ├── pose_tree_test.py ├── transform_test.py └── pose_test.py ├── posetree ├── __init__.py └── pose.py ├── docs ├── index.html ├── posetree.html └── search.js ├── pyproject.toml ├── LICENSE ├── visualization_helpers.py └── README.md /tests/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /posetree/__init__.py: -------------------------------------------------------------------------------- 1 | from .pose import Transform, Pose, PoseTree, CustomFramePoseTree -------------------------------------------------------------------------------- /docs/index.html: -------------------------------------------------------------------------------- 1 | 2 | 3 |
4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["setuptools", "wheel"] 3 | 4 | [project] 5 | name = "posetree" 6 | version = "1.1.2" 7 | description = "A Python library doing pose math for robotics." 8 | readme = "README.md" 9 | authors = [{name = "Benjie Holson", email = "bmholson@gmail.com"}] 10 | 11 | classifiers = [ 12 | "Development Status :: 4 - Beta", 13 | "Intended Audience :: Developers", 14 | "Topic :: Scientific/Engineering", 15 | "License :: OSI Approved :: MIT License", 16 | "Programming Language :: Python :: 3" 17 | ] 18 | 19 | dependencies = [ 20 | "numpy>=1.21.2", 21 | "scipy>=1.7.1" 22 | ] 23 | 24 | [project.urls] 25 | homepage = "https://github.com/robobenjie/posetree" 26 | documentation = "https://htmlpreview.github.io/?https://raw.githubusercontent.com/robobenjie/posetree/main/docs/posetree/pose.html" 27 | repository = "https://github.com/robobenjie/posetree" 28 | changelog = "https://github.com/robobenjie/posetree/releases" 29 | 30 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 robobenjie 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /visualization_helpers.py: -------------------------------------------------------------------------------- 1 | import plotly.graph_objs as go 2 | import numpy as np 3 | from contextlib import contextmanager 4 | 5 | @contextmanager 6 | def plotly_figure(title): 7 | layout = go.Layout( 8 | title=title, 9 | scene=dict( 10 | aspectmode='data', 11 | xaxis=dict(title='X'), 12 | yaxis=dict(title='Y'), 13 | zaxis=dict(title='Z'))) 14 | 15 | fig = go.Figure(layout=layout) 16 | yield fig 17 | 18 | 19 | x_data = [] 20 | y_data = [] 21 | z_data = [] 22 | 23 | for trace in fig.data: 24 | x_data.extend(trace['x']) 25 | y_data.extend(trace['y']) 26 | z_data.extend(trace['z']) 27 | 28 | x_min, x_max = [min(x_data), max(x_data)] 29 | y_min, y_max = [min(y_data), max(y_data)] 30 | z_min, z_max = [min(z_data), max(z_data)] 31 | 32 | # Set padding 33 | padding = 1.0 # change this value as needed 34 | 35 | fig.update_layout( 36 | scene=dict( 37 | xaxis=dict(range=[x_min - padding, x_max + padding], autorange=False), 38 | yaxis=dict(range=[y_min - padding, y_max + padding], autorange=False), 39 | zaxis=dict(range=[z_min - padding, z_max + padding], autorange=False), 40 | aspectmode='data' 41 | ) 42 | ) 43 | 44 | fig.show() 45 | 46 | def plot_pose(fig, pose, name, length=0.35): 47 | x0, y0, z0 = pose.transform.position 48 | 49 | # The end points of the axis lines in the world coordinate frame 50 | x_end = pose.position + pose.x_axis * length 51 | y_end = pose.position + pose.y_axis * length 52 | z_end = pose.position + pose.z_axis * length 53 | 54 | 55 | # Create the axis lines 56 | x_line = go.Scatter3d(x=[x0, x_end[0]], y=[y0, x_end[1]], z=[z0, x_end[2]], mode='lines', line=dict(color='red')) 57 | y_line = go.Scatter3d(x=[x0, y_end[0]], y=[y0, y_end[1]], z=[z0, y_end[2]], mode='lines', line=dict(color='green')) 58 | z_line = go.Scatter3d(x=[x0, z_end[0]], y=[y0, z_end[1]], z=[z0, z_end[2]], mode='lines', line=dict(color='blue')) 59 | midpoint = pose.translate(.0 * np.array([length, length, length])) 60 | text_trace = go.Scatter3d( 61 | x=[midpoint.x], 62 | y=[midpoint.y], 63 | z=[midpoint.z], 64 | mode='text', 65 | text=[name], 66 | textfont=dict( 67 | color='black', 68 | size=10, 69 | ) 70 | ) 71 | 72 | for trance in [x_line, y_line, z_line, text_trace]: 73 | fig.add_trace(trance) -------------------------------------------------------------------------------- /tests/pose_tree_test.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pytest 3 | from posetree import Transform, CustomFramePoseTree, Pose 4 | from scipy.spatial.transform import Rotation 5 | 6 | class TestPoseTree(CustomFramePoseTree): 7 | def _get_transform(self, parent_frame: str, child_frame: str, timestamp: float) -> Transform: 8 | raise NotImplementedError(f"Non-custom frames not implemented in TestPoseTree: '{parent_frame}' -> '{child_frame}'") 9 | 10 | def test_pose_tree(): 11 | pose_tree = TestPoseTree() 12 | identity_transform = Transform.identity() 13 | 14 | # Add some frames 15 | pose_tree.add_frame(Pose(identity_transform, "parent", pose_tree), "child1") 16 | pose_tree.add_frame(Pose(identity_transform, "child1", pose_tree), "child2") 17 | 18 | # Test getting transforms 19 | assert pose_tree.get_transform("parent", "parent", 0) == identity_transform 20 | assert pose_tree.get_transform("child1", "child1", 0) == identity_transform 21 | assert pose_tree.get_transform("child2", "child2", 0) == identity_transform 22 | assert pose_tree.get_transform("parent", "child1", 0) == identity_transform 23 | assert pose_tree.get_transform("child1", "child2", 0) == identity_transform 24 | assert pose_tree.get_transform("parent", "child2", 0) == identity_transform 25 | 26 | # Test removing a frame 27 | pose_tree.remove_frame("child1") 28 | with pytest.raises(NotImplementedError): 29 | pose_tree.get_transform("parent", "child1", 0) 30 | 31 | def test_single_hop(): 32 | pose_tree = TestPoseTree() 33 | 34 | # Define a known transform 35 | rotation = Rotation.from_euler('xyz', [0.1, 0.2, 0.3], degrees=True) 36 | position = np.array([1, 2, 3]) 37 | known_transform = Transform(position, rotation) 38 | 39 | # Add a child to the root with the known transform 40 | pose_tree.add_frame(Pose(known_transform, "root", pose_tree), "child") 41 | 42 | # The transform from the root to the child should be the known transform 43 | assert Transform.almost_equal(pose_tree.get_transform("root", "child", 0), known_transform) 44 | 45 | # The transform from the child to the root should be the inverse of the known transform 46 | assert Transform.almost_equal(pose_tree.get_transform("child", "root", 0), known_transform.inverse) 47 | 48 | def test_double_hop(): 49 | pose_tree = TestPoseTree() 50 | 51 | # Define two known transforms 52 | rotation1 = Rotation.from_euler('xyz', [0, 0, 90], degrees=True) 53 | position1 = np.array([1, 0, 0]) 54 | known_transform1 = Transform(position1, rotation1) 55 | 56 | rotation2 = Rotation.from_euler('xyz', [0, 0, 0], degrees=True) 57 | position2 = np.array([10, 0, 0]) 58 | known_transform2 = Transform(position2, rotation2) 59 | 60 | # Add a child to the root with the first known transform 61 | pose_tree.add_frame(Pose(known_transform1, "root", pose_tree), "child") 62 | 63 | # Add a grandchild to the child with the second known transform 64 | pose_tree.add_frame(Pose(known_transform2, "child", pose_tree), "grandchild") 65 | 66 | # The transform from the root to the grandchild should be the result of combining the two known transforms 67 | expected_transform = known_transform1 * known_transform2 68 | assert Transform.almost_equal(pose_tree.get_transform("root", "grandchild", 0), expected_transform) 69 | 70 | # The transform from the grandchild to the root should be the inverse of the expected transform 71 | assert Transform.almost_equal(pose_tree.get_transform("grandchild", "root", 0), expected_transform.inverse) 72 | 73 | 74 | def test_siblings(): 75 | pose_tree = TestPoseTree() 76 | 77 | # Define two known transforms 78 | rotation1 = Rotation.from_euler('xyz', [0, 0, 90], degrees=True) 79 | position1 = np.array([1, 0, 0]) 80 | known_transform1 = Transform(position1, rotation1) 81 | 82 | rotation2 = Rotation.from_euler('xyz', [0, 0, 90], degrees=True) 83 | position2 = np.array([10, 0, 0]) 84 | known_transform2 = Transform(position2, rotation2) 85 | 86 | # Add a children to the root with known transforms 87 | pose_tree.add_frame(Pose(known_transform1, "root", pose_tree), "child1") 88 | pose_tree.add_frame(Pose(known_transform2, "root", pose_tree), "child2") 89 | 90 | # The difference should be 9 in the y direction 91 | expected_transform =Transform([0, -9, 0], Rotation.identity()) 92 | assert Transform.almost_equal(pose_tree.get_transform("child1", "child2", 0), expected_transform) 93 | assert Transform.almost_equal(pose_tree.get_transform("child2", "child1", 0), expected_transform.inverse) 94 | 95 | 96 | def test_pose_tree_complex_transforms(): 97 | pose_tree = TestPoseTree() 98 | 99 | # Define some non-identity transforms 100 | parent_t_child1 = Transform.from_position_and_quaternion([1, 2, 3], [0, 0, np.sin(np.pi/4), np.cos(np.pi/4)]) 101 | child1_t_child2 = Transform.from_position_and_quaternion([4, 5, 6], [0, np.sin(np.pi/6), 0, np.cos(np.pi/6)]) 102 | parent_t_child3 = Transform.from_position_and_quaternion([7, 8, 9], [np.sin(np.pi/8), 0, 0, np.cos(np.pi/8)]) 103 | child3_t_child4 = Transform.from_position_and_quaternion([10, 11, 12], [np.sin(np.pi/10), 0, np.sin(np.pi/10), np.cos(np.pi/10)]) 104 | 105 | # Add some frames 106 | pose_tree.add_frame(Pose(parent_t_child1, "parent", pose_tree), "child1") 107 | pose_tree.add_frame(Pose(child1_t_child2, "child1", pose_tree), "child2") 108 | pose_tree.add_frame(Pose(parent_t_child3, "parent", pose_tree), "child3") 109 | pose_tree.add_frame(Pose(child3_t_child4, "child3", pose_tree), "child4") 110 | 111 | # Calculate expected transformations 112 | child2_t_child4 = child1_t_child2.inverse * parent_t_child1.inverse * parent_t_child3 * child3_t_child4 113 | 114 | assert Transform.almost_equal(pose_tree.get_transform("child2", "child4", 0), child2_t_child4) 115 | assert Transform.almost_equal(pose_tree.get_transform("child4", "child2", 0), child2_t_child4.inverse) 116 | 117 | 118 | def test_temporary_frame(): 119 | pose_tree = TestPoseTree() 120 | # Define two known transforms 121 | rotation = Rotation.from_euler('xyz', [0, 0, 0], degrees=True) 122 | position = np.array([0, 0, 0]) 123 | known_transform = Transform(position, rotation) 124 | pose_tree.add_frame(Pose(known_transform, "root", pose_tree), "child") 125 | 126 | p = Pose(known_transform, "root", pose_tree) 127 | 128 | num_frames = len(pose_tree.custom_frames) 129 | 130 | for i in range(10): 131 | p = Pose(known_transform, "child", pose_tree) 132 | with pose_tree.temporary_frame(p.translate([i, 0, 0])) as work_frame: 133 | assert p.in_frame(work_frame).x == -i 134 | 135 | assert len(pose_tree.custom_frames) == num_frames 136 | -------------------------------------------------------------------------------- /tests/transform_test.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pytest 3 | from pytest import approx 4 | from posetree import Transform 5 | from scipy.spatial.transform import Rotation 6 | 7 | 8 | class TestTransform: 9 | def test_transform_init(self): 10 | position = [1, 2, 3] 11 | rotation = Rotation.from_quat([0, 0, 0, 1]) 12 | transform = Transform(position, rotation) 13 | 14 | assert np.all(transform._position == np.array(position)) 15 | assert transform._rotation == rotation 16 | 17 | def test_transform_identity(self): 18 | identity = Transform.identity() 19 | 20 | assert np.all(identity._position == np.zeros(3)) 21 | assert np.allclose(identity._rotation.as_quat(), Rotation.identity().as_quat()) 22 | 23 | def test_transform_almost_equal(self): 24 | identity = Transform.identity() 25 | assert identity.almost_equal(identity) 26 | 27 | def test_transform_almost_equal_atol(self): 28 | identity = Transform.identity() 29 | assert identity.almost_equal(identity, atol=1e-8) 30 | 31 | def test_transform_almost_equal_atol_fail(self): 32 | t1 = Transform.identity() 33 | t2 = Transform(position=[0, 0, 0], rotation=Rotation.from_quat([0, 0, 0.01, 0.99])) 34 | assert not t1.almost_equal(t2) 35 | 36 | def test_transform_inverse(self): 37 | position = [1, 2, 3] 38 | rotation = Rotation.from_quat([0, 0, 0, 1]) 39 | transform = Transform(position, rotation) 40 | inverse = transform.inverse 41 | 42 | assert np.all(inverse._position == approx(-rotation.inv().apply(position))) 43 | assert np.allclose(inverse._rotation.as_quat(), rotation.inv().as_quat()) 44 | 45 | def test_transform_inverse_multiplication(self): 46 | position = [1, 2, 3] 47 | rotation = Rotation.from_quat([0, 0, 0, 1]) 48 | transform = Transform(position, rotation) 49 | 50 | product = transform * transform.inverse 51 | 52 | assert product.almost_equal(Transform.identity()) 53 | 54 | def test_transform_position(self): 55 | position = [1, 2, 3] 56 | rotation = Rotation.from_quat([0, 0, 0, 1]) 57 | transform = Transform(position, rotation) 58 | 59 | assert np.all(transform.position == np.array(position)) 60 | 61 | def test_transform_rotation(self): 62 | position = [1, 2, 3] 63 | rotation = Rotation.from_quat([0, 0, 0, 1]) 64 | transform = Transform(position, rotation) 65 | 66 | assert transform.rotation == rotation 67 | 68 | def test_transform_axis(self): 69 | position = [0, 0, 0] 70 | rotation = Rotation.from_euler('xyz', [90, 0, 0], degrees=True) 71 | transform = Transform(position, rotation) 72 | 73 | assert np.allclose(transform.x_axis, np.array([1, 0, 0])) 74 | assert np.allclose(transform.y_axis, np.array([0, 0, 1])) 75 | assert np.allclose(transform.z_axis, np.array([0, -1, 0])) 76 | 77 | def test_transform_coordinates(self): 78 | position = [1, 2, 3] 79 | rotation = Rotation.from_quat([0, 0, 0, 1]) 80 | transform = Transform(position, rotation) 81 | 82 | assert transform.x == position[0] 83 | assert transform.y == position[1] 84 | assert transform.z == position[2] 85 | 86 | def test_transform_from_position_and_quaternion(self): 87 | position = [1, 2, 3] 88 | quaternion = [0, 0, 0, 1] 89 | transform = Transform.from_position_and_quaternion(position, quaternion) 90 | 91 | expected_transform = Transform(position, Rotation.from_quat(quaternion)) 92 | 93 | assert Transform.almost_equal(transform, expected_transform) 94 | 95 | def test_transform_to_matrix(self): 96 | position = [1, 2, 3] 97 | rotation = Rotation.from_quat([0, 0, 0, 1]) 98 | transform = Transform(position, rotation) 99 | 100 | matrix = np.eye(4) 101 | matrix[:3, :3] = rotation.as_matrix() 102 | matrix[:3, 3] = position 103 | 104 | assert np.all(transform.to_matrix() == matrix) 105 | 106 | def test_transform_from_matrix(self): 107 | position = [1, 2, 3] 108 | rotation = Rotation.from_quat([0, 0, 0, 1]) 109 | transform = Transform(position, rotation) 110 | 111 | matrix = np.eye(4) 112 | matrix[:3, :3] = rotation.as_matrix() 113 | matrix[:3, 3] = position 114 | 115 | new_transform = Transform.from_matrix(matrix) 116 | 117 | assert Transform.almost_equal(new_transform, transform) 118 | 119 | def test_transform_mul_transform(self): 120 | position1 = [1, 2, 3] 121 | quaternion1 = [0, 0, 0, 1] 122 | transform1 = Transform.from_position_and_quaternion(position1, quaternion1) 123 | 124 | position2 = [4, 5, 6] 125 | quaternion2 = [0, 1, 0, 0] 126 | transform2 = Transform.from_position_and_quaternion(position2, quaternion2) 127 | 128 | result = transform1 * transform2 129 | 130 | expected_position = np.add(position1, Rotation.from_quat(quaternion1).apply(position2)) 131 | expected_rotation = Rotation.from_quat(quaternion1) * Rotation.from_quat(quaternion2) 132 | 133 | expected_transform = Transform(expected_position, expected_rotation) 134 | 135 | assert Transform.almost_equal(result, expected_transform) 136 | 137 | def test_transform_mul_sequence(self): 138 | position = [1, 2, 3] 139 | quaternion = [0, 0, 0, 1] 140 | transform = Transform.from_position_and_quaternion(position, quaternion) 141 | 142 | sequence = [4, 5, 6] 143 | 144 | result = transform * sequence 145 | 146 | expected_result = np.add(position, Rotation.from_quat(quaternion).apply(sequence)) 147 | 148 | assert np.allclose(result, expected_result) 149 | 150 | def test_transform_mul_sequence_2(self): 151 | position = [0, 0, 0] 152 | transform = Transform(position=position, rotation=Rotation.from_euler('xyz', [0, 0, 90], degrees=True)) 153 | sequence = [1, 0, 0] 154 | 155 | result = transform * sequence 156 | 157 | expected_result = [0, 1, 0] 158 | 159 | assert np.allclose(result, expected_result) 160 | 161 | def test_interpolate(self): 162 | # Create two transform objects 163 | t1 = Transform(position=np.array([0, 0, 0]), rotation=Rotation.from_quat([0, 0, 0, 1])) 164 | t2 = Transform(position=np.array([1, 1, 1]), rotation=Rotation.from_quat([0, 1, 0, 0])) 165 | 166 | # Interpolate between the transforms at alpha=0.0 (should equal t1) 167 | t_interpolated = t1.interpolate(t2, 0.0) 168 | assert t_interpolated.almost_equal(t1) 169 | 170 | # Interpolate between the transforms at alpha=1.0 (should equal t2) 171 | t_interpolated = t1.interpolate(t2, 1.0) 172 | assert t_interpolated.almost_equal(t2) 173 | 174 | # Interpolate between the transforms at alpha=0.5 (should be midway between t1 and t2) 175 | t_interpolated = t1.interpolate(t2, 0.5) 176 | expected_position = np.array([0.5, 0.5, 0.5]) # Midway between t1.position and t2.position 177 | assert np.allclose(t_interpolated.position, expected_position) 178 | assert t1.angle_to(t_interpolated) == approx(t2.angle_to(t_interpolated)) 179 | 180 | def test_zero_position_transform(self): 181 | zero_position = [0, 0, 0] 182 | rotation = Rotation.from_quat([0, 0, 0, 1]) 183 | transform = Transform(zero_position, rotation) 184 | assert Transform.almost_equal(transform.inverse, Transform.identity()) 185 | 186 | def test_identity_rotation_transform(self): 187 | position = [1, 2, 3] 188 | identity_rotation = Rotation.identity() 189 | transform = Transform(position, identity_rotation) 190 | identity_transform = Transform(-np.array(position), identity_rotation) 191 | assert Transform.almost_equal(transform.inverse, identity_transform) 192 | 193 | def test_transform_mul_invalid_sequence(self): 194 | position = [1, 2, 3] 195 | quaternion = [0, 0, 0, 1] 196 | transform = Transform.from_position_and_quaternion(position, quaternion) 197 | 198 | invalid_sequence = [4, 5, 6, 7] 199 | 200 | with pytest.raises(ValueError): 201 | result = transform * invalid_sequence 202 | 203 | def test_large_position_transform(self): 204 | large_position = [1e6, 1e6, 1e6] 205 | identity_rotation = Rotation.identity() 206 | transform = Transform(large_position, identity_rotation) 207 | large_position_transform = Transform(-np.array(large_position), identity_rotation) 208 | assert Transform.almost_equal(transform.inverse, large_position_transform) 209 | 210 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # posetree 2 | 3 | `posetree` is an object-oriented, thread-safe, programmer-first, Python library for dealing with transforms, poses and frames, designed especially for robotics. It is unique in that Poses know their parent frame and its relationship to other frames which allows for powerful operations with simple code. 4 | 5 | ## Installation 6 | 7 | ``` 8 | pip install posetree 9 | ``` 10 | 11 | ## Features 12 | 13 | * Write pose math designed for humans to read. 14 | * Think about quaternions as little as possible. 15 | * Never wonder if you are supposed to pre-multiply or post-multiply, or if you forgot an inverse somewhere. 16 | * Standalone, and easy to integrate into any robotics stack (works with ROS but does not depend on it). 17 | 18 | ## Basic Usage 19 | 20 | ```python 21 | from scipy.spatial.transform import Rotation 22 | from posetree import Pose 23 | 24 | # Create a pose from perception information 25 | pose_of_tea_bottle = Pose.from_position_and_rotation( 26 | [-0.3, 0.4, 1.2], # Position of the bottle 27 | Rotation.identity(), # Rotation of the bottle 28 | parent_frame="camera", # Parent frame for position and rotation. 29 | pose_tree=pose_tree) # PoseTree object to track frame relationships over time. 30 | 31 | # Calculate things based on other frames 32 | height_of_tea = pose_of_tea_bottle.in_frame("world").z 33 | distance_from_gripper = pose_of_tea_bottle.distance_to(gripper_pose) 34 | 35 | # Calculate some base motion targets relative to the current robot pose 36 | base_target = base_pose.translate([2, 0, 0]).rotate_about_z(np.pi/4) 37 | base_target2 = base_pose.point_x_at(human_pose).translate([1, 0, 0]) 38 | 39 | # Get numbers out of a pose to send to a motion API 40 | x, y, _ = base_target.in_frame("map").position 41 | theta = base_target.angle_about_z_to(map_origin) 42 | navigate_to(x, y, theta) 43 | ``` 44 | 45 | ## Philosophy of Transforms, Poses and Frames 46 | 47 | `posetree` takes an opinionated stance on some words that other robotics stacks use somewhat interchangably. 48 | 49 | - **Transform**: This comes from the verb, to transform. It is how you get from one place to another. It is an operation to change your location from one to another. 50 | - Example: “Take 10 steps forward and then turn 90 degrees to your left” 51 | - Example: "Translate 1m in x, and 2m in y then rotate 30 degrees about the y axis." 52 | 53 | - **Pose**: This is a noun. It is a physical location and orientation in 3D space. 54 | - Example: “Standing at my front door facing the street.” 55 | - Example: "The location of the left camera lens, z pointing out." 56 | 57 | Notice we can take our example pose (standing at my front door facing the street) and transform it into another pose by using our transform instructions (take 10 steps forward and then turn 90 degrees to your left). If we do that we have a new pose that is a bit in front of my house and (in my case) facing our detached garage. 58 | 59 | - **Frame**: This is a pose that is so important we’ve decided to give it a name. 60 | - Example: We could name “Standing at my front door facing the street” “journey_start” and then we could describe that other pose by saying, “from journey_start take 10 steps forward and turn 90 degrees to your left” 61 | - Example: We could name the left camera pose `"camera"`. 62 | 63 | You can sequence Transforms (by multiplying them together, as is traditional). This has a semantic meaning: e.g. "take 10 steps forward and turn left THEN take 1 step backwards and turn around" which is equivalent to "take 10 steps forward and sidestep once right and then turn right". However you cannot sequentially apply positions in 3D space so there is intentionally no multiply operator for Pose. If this seems strange, I promise that as you use this library you will find that nearly every operation is easier and more clearly expressed via `in_frame`, `translate` or `rotate` operations instead of chains of `(world_t_robot * robot_t_camera * camera_t_object).inverse()` that you might be used to. 64 | 65 | ## Anchoring Poses in Frames. 66 | 67 | When constucting poses it is useful to think of what you expect the pose to be fixed relative to. For example, you might 68 | detect an apple on a table and get a pose from your perception system in the `camera` frame, but the apple is actually sitting on the table, and we don't expect the apple to move if the camera moves. So you will want to store your pose object with the parent frame equal to `odometry/map/world`. Then, even if the robot drives around, `apple_pose` will still refer to the best estimation of the apple's true location (with the usual caveats about localization drift and noise). 69 | 70 | Here's how to express that in `posetree`. 71 | ```python 72 | apple_pose = Pose(camera_t_apple, "camera", pose_tree).in_frame("world") 73 | ``` 74 | 75 | Likewise if you have a bin on the back of a mobile robot and you want to define a drop-objects-into-bin pose right above it you can store 76 | that in the `robot` frame. 77 | 78 | ```python 79 | drop_pose = Pose.from_position_and_quaternion([-.3, 0.1, 0.25], [0, 0, 0, 1]), "robot", pose_tree) 80 | ``` 81 | 82 | When designing motion APIs with this library, you should be liberal in what frames you accept, and internally convert them to the frame you want to work in. 83 | 84 | For example, in a function to move the arm to a pose: 85 | ```python 86 | def move_to_pose(self, target_pose: Pose): 87 | # Best Practice: turn Poses into Transforms at the last moment before acting on them. 88 | arm_motion_primitives.move_arm_to_pose_relative_to_base(target_pose.in_frame("robot").transform) 89 | ``` 90 | 91 | This formulation lets you combine the perception outputs and the motion methods for things like this: 92 | 93 | ```python 94 | def grasp_apple(self, apple_pose: Pose): 95 | # pregrasp is a pose 15 cm above the apple, with z pointing at it. 96 | pregrasp = apple_pose.translate([0, 0, 0.15], frame="world").point_z_at(apple_pose) 97 | self.move_to_pose(pregrasp) 98 | 99 | # Move down in the local 'z' of pregrasp until we touch the apple. 100 | self.move_to_pose_until_contact(pregrasp.translate([0, 0, 0.2])) 101 | 102 | # If we feel a contact too early, raise some reasonable error: 103 | # We can check the distance using distance_to, even though the parent frames 104 | # of the two poses are different. 105 | if apple_pose.distance_to(get_tool_pose()) > 0.1: 106 | return "Whoops, we probably didn't get the apple." 107 | 108 | # Close the gripper and move up a bit from where ever we are, to lift the apple. 109 | self.close_gripper() 110 | self.move_to_pose(get_tool_pose().translate([0, 0, -0.1])) 111 | 112 | # Drop it in the bin. 113 | self.move_to_pose(drop_pose) 114 | self.open_gripper() 115 | ``` 116 | 117 | ## Poses are Immutable 118 | 119 | Poses are immutable, and methods like `translate` return a new pose object. Immutability is nice because it makes them safe to pass into methods and also makes them thread safe, but there is a gotcha to watch out for: 120 | 121 | ```python 122 | # BAD!!! Do Not Do! 123 | p1.translate([1,0,0]) 124 | p1.rotate_about_z(np.rad2deg(90)) 125 | p1.with_position_x(5) 126 | # Surprise! p1 has not changed! 127 | 128 | # Better 129 | p1 = p1.translate([1,0,0]) 130 | p1 = p1.rotate_about_z(np.rad2deg(90)) 131 | p1 = p1.with_position_x(5) 132 | # We are replacing p1 with the new pose returned, so this works 133 | 134 | # My favorite 135 | p2 = p1.translate([1,0,0]).rotate_about_z(np.rad2deg(90)).with_position_x(5) 136 | ``` 137 | 138 | ## Immutability and Moving Frames 139 | 140 | While a Pose is immutable, the parent frame can (and does!) change over time relative to other frames, meaning that an individual pose can move relative 141 | to other frames. (For example a pose defined in the `robot` frame will conceptually move as the robot moves relative to a `world` 142 | frame, even though its position and orientation remain immutably constant.) 143 | 144 | To make this very concrete, say the robot starts out at the world origin: 145 | 146 | ```python 147 | pose_in_robot_frame = pose.from_position_and_rotation([1,2,3], Rotation.identity(), "robot", pose_tree) # fixed to robot 148 | pose_in_world_frame = pose_in_robot_frame.in_frame("world") # fixed to world 149 | 150 | pose_in_robot_frame.position # [1,2,3] 151 | pose_in_world_frame.position # [1,2,3] 152 | 153 | # Now the robot moves 1 meter forward in the world frame. 154 | robot.drive_forward_in_x(1) 155 | 156 | # Poses are immutable so they have not changed. One pose is [1,2,3] from the robot 157 | # frame origin, one is [1,2,3] from the world origin. 158 | pose_in_robot_frame.position # [1,2,3] 159 | pose_in_world_frame.position # [1,2,3] 160 | 161 | # But if we express the one in robot frame in the world frame, we see that it 162 | # is now 1 meter farther forward in x (because it was glued to the robot as 163 | # it moved relative to the world.) 164 | pose_in_robot_frame.in_frame("world").position # [2,2,3] 165 | ``` 166 | 167 | ## Connecting it to the rest of your stack. 168 | 169 | ### ROS2 170 | If you use ROS2, you can use this excellent adapter library by Brennand Pierce to connect to other modules and visualize poses. https://github.com/brennand/ros_posetree. 171 | 172 | ### Other Stacks 173 | 174 | To connect a pose_tree instance you need to subclass PoseTree. Lets say your stack uses a (fictional) object called `MyTransformManager` that subscribes to pose messages and implements `get_transform` that returns some flavor of transform struct. You would write something like: 175 | 176 | ```python 177 | class MyPoseTree(CustomFramePoseTree): 178 | """My implementation of PoseTree to integrate with MyTransformManager""" 179 | 180 | def __init__(self, transform_manager: MyTransfrormManager): 181 | self._tfm = transform_manager 182 | 183 | def _get_transform(self, parent_frame: str, child_frame: str, timestamp: Optional[float] = None) -> Transform: 184 | transform_data = self._tfm.get_transform(parent_frame, child_frame, timestamp) 185 | return Transform.from_position_and_quaternion( 186 | [transform_data.tx, transform_data.ty, transform_data.tz], 187 | [transform_data.qx, transform_data.qy, transform_data.qz, transform_data.qw] 188 | ) 189 | ``` 190 | 191 | Then you can use it like this: 192 | 193 | ```python 194 | my_tf_manager = MyTransformManager() 195 | my_tf_manager.subscribe(Channels.ALL_ROBOT_CHANNELS) # idk I'm making this API up. 196 | pose_tree = MyPoseTree(my_tf_manager) 197 | p1 = Pose.from_position_and_rotation([1,2,3], Rotation.identity(), "robot", pose_tree) 198 | p2 = p1.in_frame("camera", timestamp = robot.get_current_timestamp() - 1.0) 199 | ``` 200 | 201 | ## Documentation 202 | 203 | For more detailed information about the API and how to use PoseTree, check out the [documentation](https://htmlpreview.github.io/?https://raw.githubusercontent.com/robobenjie/posetree/main/docs/posetree/pose.html). 204 | 205 | ## Contributing 206 | 207 | We welcome contributions! This is my first open source project so I don't know what I'm doing, but I'm excited to help you figure out how to incorporate it into your projects. 208 | 209 | ## License 210 | 211 | posetree is licensed under the [MIT license](https://github.com/robobenjie/posetree/blob/main/LICENSE). 212 | -------------------------------------------------------------------------------- /tests/pose_test.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pytest 3 | from typing import Optional 4 | from pytest import approx 5 | from posetree import Transform, CustomFramePoseTree, Pose 6 | from scipy.spatial.transform import Rotation 7 | 8 | class TestPoseTree(CustomFramePoseTree): 9 | def _get_transform(self, parent_frame: str, child_frame: str, timestamp: Optional[float] = None) -> Transform: 10 | raise NotImplementedError("Non-custom frames not implemented in TestPoseTree") 11 | 12 | def get_pose_tree() -> TestPoseTree: 13 | pose_tree = TestPoseTree() 14 | 15 | # Robot frame is at [2, 1, 0] in world frame. 16 | robot_position = np.array([2, 1, 0]) 17 | robot_rotation = Rotation.from_euler('xyz', [0, 0, 0], degrees=True) 18 | robot_transform = Transform(robot_position, robot_rotation) 19 | pose_tree.add_frame(Pose(robot_transform, "world", pose_tree), "robot") 20 | 21 | # Camera frame is 1 meter up in z in robot frame and has z facing forward (towards robot x). 22 | camera_position = np.array([0, 0, 1]) 23 | camera_rotation = Rotation.from_euler('yxz', [90, 0, 0], degrees=True) 24 | camera_transform = Transform(camera_position, camera_rotation) 25 | pose_tree.add_frame(Pose(camera_transform, "robot", pose_tree), "camera") 26 | 27 | # Tool frame is at [0.5, 0.5, 0.75] in robot with z facing down. 28 | tool_position = np.array([0.5, 0.5, 0.75]) 29 | tool_rotation = Rotation.from_euler('xyz', [180, 0, 0], degrees=True) 30 | tool_transform = Transform(tool_position, tool_rotation) 31 | pose_tree.add_frame(Pose(tool_transform, "robot", pose_tree), "tool") 32 | 33 | return pose_tree 34 | 35 | def assert_rotations_equal(rotation1: Rotation, rotation2: Rotation): 36 | assert np.allclose(rotation1.as_quat(), rotation2.as_quat()) or np.allclose(rotation1.as_quat(), -rotation2.as_quat()) 37 | 38 | def test_pose(): 39 | pose_tree = get_pose_tree() 40 | transform = Transform(np.array([1, 2, 3]), Rotation.from_quat([0, 0, 0, 1])) 41 | pose = Pose(transform, 'robot', pose_tree) 42 | 43 | assert np.array_equal(pose.position, np.array([1, 2, 3])) 44 | assert_rotations_equal(pose.rotation, Rotation.from_quat([0, 0, 0, 1])) 45 | assert pose.transform == transform 46 | assert pose.frame == 'robot' 47 | assert pose.pose_tree == pose_tree 48 | 49 | 50 | def test_get_pose(): 51 | pose_tree = get_pose_tree() 52 | 53 | # Define a position, quaternion, and parent frame 54 | position = np.array([3, 2, 1]) 55 | quaternion = [0, 0, 0, 1] # Identity quaternion (no rotation) 56 | parent_frame = 'world' 57 | 58 | # Use get_pose to create the pose 59 | pose = pose_tree.get_pose(position, quaternion, parent_frame) 60 | 61 | # Check the pose's properties 62 | assert np.array_equal(pose.position, position) 63 | assert_rotations_equal(pose.rotation, Rotation.from_quat(quaternion)) 64 | assert pose.frame == parent_frame 65 | 66 | # Define a non-identity quaternion 67 | quaternion = [0, 0, np.sin(np.pi/4), np.cos(np.pi/4)] # 90 degree rotation around z axis 68 | 69 | # Create another pose with the non-identity quaternion 70 | pose = pose_tree.get_pose(position, quaternion, parent_frame) 71 | 72 | # Check the new pose's properties 73 | assert np.array_equal(pose.position, position) 74 | assert_rotations_equal(pose.rotation, Rotation.from_quat(quaternion)) 75 | assert pose.frame == parent_frame 76 | 77 | 78 | def test_position_props(): 79 | pose_tree = get_pose_tree() 80 | pose = pose_tree.get_pose([1, 2, 3], [0, 0, 0, 1], 'robot') 81 | assert pose.x == 1 82 | assert pose.y == 2 83 | assert pose.z == 3 84 | 85 | def test_in_frame(): 86 | pose_tree = get_pose_tree() 87 | pose = pose_tree.get_pose([0, 0, 3], [0, 0, 0, 1], 'camera') 88 | pose_in_robot = pose.in_frame('robot') 89 | assert pose_in_robot.frame == 'robot' 90 | assert pose_in_robot.x == approx(3) 91 | assert pose_in_robot.y == approx(0) 92 | assert pose_in_robot.z == approx(1) 93 | 94 | # The 'z' in camera frame is the 'x' in robot frame 95 | assert pose_in_robot.z_axis == approx([1, 0, 0]) 96 | assert pose_in_robot.y_axis == approx([0, 1, 0]) 97 | 98 | def test_in_frame_same_frame(): 99 | pose_tree = get_pose_tree() 100 | pose = pose_tree.get_pose([0, 0, 3], [0, 0, 0, 1], 'camera') 101 | pose_in_camera = pose.in_frame('camera') 102 | assert pose_in_camera.frame == 'camera' 103 | assert pose.transform == pose_in_camera.transform 104 | 105 | def test_with_position(): 106 | pose_tree = get_pose_tree() 107 | pose = pose_tree.get_pose([0, 0, 3], [0, 0, 0, 1], 'camera') 108 | new_pose = pose.with_position([1, 2, 4]) 109 | 110 | # assert new pose correct 111 | assert new_pose.position == pytest.approx(np.array([1, 2, 4])) 112 | assert_rotations_equal(new_pose.rotation, Rotation.from_quat([0, 0, 0, 1])) 113 | 114 | # assert old pose unchanged 115 | assert pose.position == pytest.approx(np.array([0, 0, 3])) 116 | assert_rotations_equal(pose.rotation, Rotation.from_quat([0, 0, 0, 1])) 117 | 118 | def test_with_position_x(): 119 | pose_tree = get_pose_tree() 120 | pose = pose_tree.get_pose([2, 3, 4], [0, 0, 0, 1], 'robot') 121 | new_pose = pose.with_position_x(10) 122 | 123 | # assert new pose's x is updated correctly 124 | assert new_pose.x == 10 125 | # assert other components remain the same 126 | assert new_pose.y == pose.y 127 | assert new_pose.z == pose.z 128 | assert_rotations_equal(new_pose.rotation, pose.rotation) 129 | 130 | def test_with_position_y(): 131 | pose_tree = get_pose_tree() 132 | pose = pose_tree.get_pose([2, 3, 4], [0, 0, 0, 1], 'robot') 133 | new_pose = pose.with_position_y(10) 134 | 135 | # assert new pose's y is updated correctly 136 | assert new_pose.y == 10 137 | # assert other components remain the same 138 | assert new_pose.x == pose.x 139 | assert new_pose.z == pose.z 140 | assert_rotations_equal(new_pose.rotation, pose.rotation) 141 | 142 | def test_with_position_z(): 143 | pose_tree = get_pose_tree() 144 | pose = pose_tree.get_pose([2, 3, 4], [0, 0, 0, 1], 'robot') 145 | new_pose = pose.with_position_z(10) 146 | 147 | # assert new pose's z is updated correctly 148 | assert new_pose.z == 10 149 | # assert other components remain the same 150 | assert new_pose.x == pose.x 151 | assert new_pose.y == pose.y 152 | assert_rotations_equal(new_pose.rotation, pose.rotation) 153 | 154 | def test_with_position_xyz_in_frame(): 155 | pose_tree = get_pose_tree() 156 | pose = pose_tree.get_pose([2, 3, 4], [0, 0, 0, 1], 'robot') 157 | 158 | z1_in_camera = pose.with_position_z(1, frame='camera') 159 | assert z1_in_camera.x == pytest.approx(1) 160 | assert z1_in_camera.y == pytest.approx(3) 161 | assert z1_in_camera.z == pytest.approx(4) 162 | 163 | def test_with_rotation(): 164 | pose_tree = get_pose_tree() 165 | pose = pose_tree.get_pose([0, 0, 3], [0, 0, 0, 1], 'camera') 166 | new_rotation = Rotation.from_euler('xyz', [45, 45, 45], degrees=True) 167 | new_pose = pose.with_rotation(new_rotation) 168 | 169 | # assert new pose correct 170 | assert new_pose.position == pytest.approx(np.array([0, 0, 3])) 171 | assert_rotations_equal(new_pose.rotation, new_rotation) 172 | 173 | # assert old pose unchanged 174 | assert pose.position == pytest.approx(np.array([0, 0, 3])) 175 | assert_rotations_equal(pose.rotation, Rotation.from_quat([0, 0, 0, 1])) 176 | 177 | 178 | def test_with_rotation_matching(): 179 | pose_tree = get_pose_tree() 180 | 181 | pose1 = pose_tree.get_pose([0, 0, 3], [0, 0, 0, 1], 'camera') 182 | pose2 = pose_tree.get_pose([1, 2, 4], [0, 0, 1, 0], 'camera') 183 | 184 | new_pose = pose1.with_rotation_matching(pose2) 185 | 186 | # assert new pose correct 187 | assert new_pose.position == pytest.approx(np.array([0, 0, 3])) 188 | assert_rotations_equal(new_pose.rotation, Rotation.from_quat([0, 0, 1, 0])) 189 | 190 | # assert old poses unchanged 191 | assert pose1.position == pytest.approx(np.array([0, 0, 3])) 192 | assert_rotations_equal(pose1.rotation, Rotation.from_quat([0, 0, 0, 1])) 193 | 194 | assert pose2.position == pytest.approx(np.array([1, 2, 4])) 195 | assert_rotations_equal(pose2.rotation, Rotation.from_quat([0, 0, 1, 0])) 196 | 197 | def test_with_rotation_matching_in_other_frame(): 198 | pose_tree = get_pose_tree() 199 | 200 | pose1 = pose_tree.get_pose([0, 0, 3], [0, 0, 0, 1], 'camera') 201 | pose2 = pose_tree.get_pose([1, 2, 4], [0, 0, 1, 0], 'camera').in_frame('robot') 202 | 203 | new_pose = pose1.with_rotation_matching(pose2) 204 | 205 | # assert new pose correct 206 | assert new_pose.position == pytest.approx(np.array([0, 0, 3])) 207 | assert_rotations_equal(new_pose.rotation, Rotation.from_quat([0, 0, 1, 0])) 208 | 209 | # assert old poses unchanged 210 | assert pose1.position == pytest.approx(np.array([0, 0, 3])) 211 | assert_rotations_equal(pose1.rotation, Rotation.from_quat([0, 0, 0, 1])) 212 | 213 | # Pose 2 is different because it is in a different frame 214 | assert pose2.position != pytest.approx(np.array([1, 2, 4])) 215 | assert pose2.in_frame("camera").position == pytest.approx(np.array([1, 2, 4])) 216 | assert_rotations_equal(pose2.in_frame("camera").rotation, Rotation.from_quat([0, 0, 1, 0])) 217 | 218 | def test_apply_transform(): 219 | pose_tree = get_pose_tree() 220 | 221 | pose = pose_tree.get_pose([0, 0, 3], [0, 0, 0, 1], 'camera') 222 | transform = Transform(np.array([1, 2, -1]), Rotation.from_quat([0, 1, 0, 0])) 223 | 224 | new_pose = pose.apply_transform(transform) 225 | 226 | # assert new pose correct 227 | assert new_pose.position == pytest.approx(np.array([1, 2, 2])) # old position + transformation position 228 | assert_rotations_equal(new_pose.rotation, Rotation.from_quat([0, 1, 0, 0])) # transformation rotation 229 | 230 | # assert old pose unchanged 231 | assert pose.position == pytest.approx(np.array([0, 0, 3])) 232 | assert_rotations_equal(pose.rotation, Rotation.from_quat([0, 0, 0, 1])) 233 | 234 | def test_translate(): 235 | pose_tree = get_pose_tree() 236 | pose = pose_tree.get_pose([1, 2, 3], [0, 0, 0, 1], 'robot') 237 | 238 | # Apply a translation 239 | new_pose = pose.translate([2, -1, 0]) 240 | 241 | # Assert new pose correct 242 | assert new_pose.position == pytest.approx(np.array([3, 1, 3])) 243 | assert_rotations_equal(new_pose.rotation, Rotation.from_quat([0, 0, 0, 1])) 244 | 245 | # Assert old pose unchanged 246 | assert pose.position == pytest.approx(np.array([1, 2, 3])) 247 | assert_rotations_equal(pose.rotation, Rotation.from_quat([0, 0, 0, 1])) 248 | assert pose.frame == 'robot' 249 | 250 | def test_translate_non_identity(): 251 | pose_tree = get_pose_tree() 252 | pose = pose_tree.get_pose([1, 2, 3], [0, 0, 0, 1], 'robot').rotate_about_z(np.pi/2) 253 | 254 | # Apply a translation 255 | new_pose = pose.translate([1, 0, 10]) 256 | 257 | # Assert new pose correct 258 | assert new_pose.position == pytest.approx(np.array([1, 3, 13])) 259 | assert_rotations_equal(new_pose.rotation, pose.rotation) 260 | 261 | def test_translate_in_frame(): 262 | pose_tree = get_pose_tree() 263 | pose = pose_tree.get_pose([0, 0, 2], [0, 0, 0, 1], 'camera') 264 | 265 | # Apply a translation 266 | new_pose = pose.translate([0, 0, 1], frame='robot') 267 | 268 | # Assert new pose correct 269 | assert new_pose.position == pytest.approx(np.array([-1, 0, 2])) 270 | assert_rotations_equal(new_pose.rotation, Rotation.from_quat([0, 0, 0, 1])) 271 | 272 | 273 | def test_rotate_about_axis(): 274 | pose_tree = get_pose_tree() 275 | pose = pose_tree.get_pose([1, 2, 3], [0, 0, 0, 1], 'robot') 276 | 277 | # Rotate 90 degrees about z-axis 278 | new_pose = pose.rotate_about_axis([0, 0, 1], np.pi/2) 279 | 280 | # Check position - should remain the same 281 | assert new_pose.position == pytest.approx(np.array([1, 2, 3])) 282 | 283 | # Check rotation - should be 90 degrees about z-axis 284 | expected_rotation = Rotation.from_euler('z', np.pi/2) 285 | assert_rotations_equal(new_pose.rotation, expected_rotation) 286 | 287 | 288 | def test_rotate_about_axis_chaining(): 289 | pose_tree = get_pose_tree() 290 | pose = pose_tree.get_pose([1, 2, 3], [0, 0, 0, 1], 'robot') 291 | 292 | # Rotate 90 degrees about y-axis then 90 degrees about z-axis 293 | new_pose = pose.rotate_about_axis([0, 1, 0], np.pi/2).rotate_about_axis([0, 0, 1], np.pi/2) 294 | 295 | # Check position - should remain the same 296 | assert new_pose.position == pytest.approx(np.array([1, 2, 3])) 297 | 298 | assert new_pose.x_axis == pytest.approx(np.array([0, 1, 0])) 299 | assert new_pose.y_axis == pytest.approx(np.array([0, 0, 1])) 300 | assert new_pose.z_axis == pytest.approx(np.array([1, 0, 0])) 301 | 302 | def test_rotate_about_in_frame(): 303 | pose_tree = get_pose_tree() 304 | pose = pose_tree.get_pose([1, 2, 3], [0, 0, 0, 1], 'robot') 305 | 306 | # Rotate 90 degrees about y-axis then 90 degrees about z-axis 307 | new_pose = pose.rotate_about_axis([0, 0, 1], np.pi/2, frame="camera") 308 | 309 | assert new_pose.x_axis == pytest.approx(np.array([1, 0, 0])) 310 | assert new_pose.y_axis == pytest.approx(np.array([0, 0, -1])) 311 | 312 | def test_rotate_about_in_frame_non_identity_start(): 313 | pose_tree = get_pose_tree() 314 | pose = pose_tree.get_pose([1, 2, 3], [0, 0, 0, 1], 'robot').rotate_about_axis([0, 0, 1], np.pi/2) 315 | 316 | # Rotate 90 degrees about y-axis then 90 degrees about z-axis 317 | new_pose = pose.rotate_about_axis([0, 0, 1], np.pi/2, frame="camera") 318 | 319 | assert new_pose.x_axis == pytest.approx(np.array([0, 0, -1])) 320 | assert new_pose.y_axis == pytest.approx(np.array([-1, 0, 0])) 321 | 322 | def test_rotate_about_x(): 323 | pose_tree = get_pose_tree() 324 | pose = pose_tree.get_pose([1, 2, 3], [0, 0, 0, 1], 'robot') 325 | 326 | # Rotate 90 degrees about x-axis 327 | new_pose = pose.rotate_about_x(np.pi/2) 328 | 329 | # Check position - should remain the same 330 | assert new_pose.position == pytest.approx(np.array([1, 2, 3])) 331 | 332 | # Check rotation - should be 90 degrees about x-axis 333 | expected_rotation = Rotation.from_euler('x', np.pi/2) 334 | assert_rotations_equal(new_pose.rotation, expected_rotation) 335 | 336 | def test_rotate_about_y(): 337 | pose_tree = get_pose_tree() 338 | pose = pose_tree.get_pose([1, 2, 3], [0, 0, 0, 1], 'robot') 339 | 340 | # Rotate 90 degrees about y-axis 341 | new_pose = pose.rotate_about_y(np.pi/2) 342 | 343 | # Check position - should remain the same 344 | assert new_pose.position == pytest.approx(np.array([1, 2, 3])) 345 | 346 | # Check rotation - should be 90 degrees about y-axis 347 | expected_rotation = Rotation.from_euler('y', np.pi/2) 348 | assert_rotations_equal(new_pose.rotation, expected_rotation) 349 | 350 | def test_rotate_about_z(): 351 | pose_tree = get_pose_tree() 352 | pose = pose_tree.get_pose([1, 2, 3], [0, 0, 0, 1], 'robot') 353 | 354 | # Rotate 90 degrees about z-axis 355 | new_pose = pose.rotate_about_z(np.pi/2) 356 | 357 | # Check position - should remain the same 358 | assert new_pose.position == pytest.approx(np.array([1, 2, 3])) 359 | 360 | # Check rotation - should be 90 degrees about z-axis 361 | expected_rotation = Rotation.from_euler('z', np.pi/2) 362 | assert_rotations_equal(new_pose.rotation, expected_rotation) 363 | 364 | def test_rotate_about_axis_degrees(): 365 | pose_tree = get_pose_tree() 366 | pose = pose_tree.get_pose([1, 2, 3], [0, 0, 0, 1], 'robot') 367 | 368 | # Rotate 90 degrees about z-axis 369 | new_pose_z = pose.rotate_about_z(90, degrees=True) 370 | new_pose_y = pose.rotate_about_y(90, degrees=True) 371 | new_pose_x = pose.rotate_about_x(90, degrees=True) 372 | 373 | # Check position - should remain the same 374 | assert new_pose_z.position == pytest.approx(np.array([1, 2, 3])) 375 | assert new_pose_y.position == pytest.approx(np.array([1, 2, 3])) 376 | assert new_pose_x.position == pytest.approx(np.array([1, 2, 3])) 377 | 378 | # Check rotation - should be 90 degrees about z-axis 379 | for p, axis in zip([new_pose_z, new_pose_y, new_pose_x], ['z', 'y', 'x']): 380 | expected_rotation = Rotation.from_euler(axis, np.pi/2) 381 | assert_rotations_equal(p.rotation, expected_rotation) 382 | 383 | def test_rotate_about_axis_raises_on_non_unit_axis(): 384 | pose_tree = get_pose_tree() 385 | pose = pose_tree.get_pose([1, 2, 3], [0, 0, 0, 1], 'robot') 386 | 387 | with pytest.raises(ValueError): 388 | pose.rotate_about_axis([0, 0, 2], np.pi/2) 389 | 390 | def test_distance_to(): 391 | pose_tree = get_pose_tree() 392 | pose = pose_tree.get_pose([0, 0, 0], [0, 0, 0, 1], 'robot') 393 | pose2 = pose_tree.get_pose([0, 4, 0], [0, 0, 0, 1], 'robot') 394 | 395 | distance = pose.distance_to(pose2) 396 | assert distance == pytest.approx(4) 397 | 398 | def test_distance_to_different_frame(): 399 | pose_tree = get_pose_tree() 400 | pose = pose_tree.get_pose([0, 0, 0], [0, 0, 0, 1], 'robot') 401 | pose2 = pose_tree.get_pose([0, 4, 0], [0, 0, 0, 1], 'robot').in_frame('camera') 402 | 403 | distance = pose.distance_to(pose2) 404 | assert distance == pytest.approx(4) 405 | 406 | def test_angle_to(): 407 | pose_tree = get_pose_tree() 408 | pose = pose_tree.get_pose([0, 0, 0], [0, 0, 0, 1], 'robot') 409 | pose2 = pose_tree.get_pose([0, 4, 0], [0, 0, 0, 1], 'robot').rotate_about_x(np.pi/2) 410 | 411 | angle = pose.angle_to(pose2) 412 | assert angle == pytest.approx(np.pi/2) 413 | 414 | 415 | def test_angle_to2(): 416 | pose_tree = get_pose_tree() 417 | pose = pose_tree.get_pose([0, 0, 0], [0, 0, 0, 1], 'robot').rotate_about_x(-np.pi/2) 418 | pose2 = pose_tree.get_pose([0, 4, 0], [0, 0, 0, 1], 'robot').rotate_about_x(np.pi/2) 419 | 420 | angle = pose.angle_to(pose2) 421 | assert angle == pytest.approx(np.pi) 422 | 423 | def test_angle_to3(): 424 | pose_tree = get_pose_tree() 425 | pose = pose_tree.get_pose([0, 0, 0], [0, 0, 0, 1], 'robot').rotate_about_x(-np.pi/2) 426 | pose2 = pose_tree.get_pose([0, 4, 0], [0, 0, 0, 1], 'robot').rotate_about_y(.1) 427 | 428 | angle = pose.angle_to(pose2) 429 | assert angle > np.pi/2 430 | 431 | def test_angle_about_z_to(): 432 | pose_tree = get_pose_tree() 433 | pose = pose_tree.get_pose([0, 0, 0], [0, 0, 0, 1], 'robot') 434 | pose2 = pose_tree.get_pose([0, 4, 0], [0, 0, 0, 1], 'robot').rotate_about_z(-np.pi/2) 435 | 436 | angle = pose.angle_about_z_to(pose2) 437 | assert angle == pytest.approx(-np.pi/2) 438 | 439 | def test_angle_about_z_to_other_frames(): 440 | pose_tree = get_pose_tree() 441 | pose = pose_tree.get_pose([0, 0, 0], [0, 0, 0, 1], 'robot') 442 | pose2 = pose_tree.get_pose([0, 4, 0], [0, 0, 0, 1], 'robot').rotate_about_z(-np.pi/2).in_frame('camera') 443 | 444 | angle = pose.angle_about_z_to(pose2) 445 | assert angle == pytest.approx(-np.pi/2) 446 | 447 | def test_angle_about_z_to_slight_misalignment(): 448 | pose_tree = get_pose_tree() 449 | pose = pose_tree.get_pose([0, 0, 0], [0, 0, 0, 1], 'robot') 450 | pose2 = pose_tree.get_pose([0, 4, 0], [0, 0, 0, 1], 'robot').rotate_about_x(0.001).rotate_about_z(-np.pi/2) 451 | 452 | angle = pose.angle_about_z_to(pose2) 453 | assert angle == pytest.approx(-np.pi/2) 454 | 455 | def test_angle_about_z_to_no_difference(): 456 | pose_tree = get_pose_tree() 457 | pose = pose_tree.get_pose([0, 0, 0], [0, 0, 0, 1], 'robot').rotate_about_z(-np.pi/2) 458 | pose2 = pose_tree.get_pose([0, 4, 0], [0, 0, 0, 1], 'robot').rotate_about_z(-np.pi/2) 459 | 460 | angle = pose.angle_about_z_to(pose2) 461 | assert angle == pytest.approx(0) 462 | 463 | def test_angle_about_z_180(): 464 | pose_tree = get_pose_tree() 465 | pose = pose_tree.get_pose([0, 0, 0], [0, 0, 0, 1], 'robot').rotate_about_z(np.pi) 466 | pose2 = pose_tree.get_pose([0, 4, 0], [0, 0, 0, 1], 'robot') 467 | 468 | angle = pose.angle_about_z_to(pose2) 469 | assert angle == pytest.approx(np.pi) or angle == pytest.approx(-np.pi) 470 | 471 | def test_angle_about_z_but_zs_90_apart(): 472 | pose_tree = get_pose_tree() 473 | pose = pose_tree.get_pose([0, 0, 0], [0, 0, 0, 1], 'robot').rotate_about_x(np.pi/2) 474 | pose2 = pose_tree.get_pose([0, 4, 0], [0, 0, 0, 1], 'robot') 475 | 476 | # assert Raises Value Error 477 | with pytest.raises(ValueError): 478 | pose.angle_about_z_to(pose2) 479 | 480 | def test_point_at(): 481 | pose_tree = get_pose_tree() 482 | pose = pose_tree.get_pose([0, 0, 0], [0, 0, 0, 1], 'robot') 483 | pose2 = pose_tree.get_pose([0, 4, 0], [0, 0, 0, 1], 'robot') 484 | 485 | pointed_x = pose.point_x_at(pose2) 486 | assert pointed_x.x_axis == pytest.approx(np.array([0, 1, 0])) 487 | 488 | pointed_y = pose.point_y_at(pose2) 489 | assert pointed_y.y_axis == pytest.approx(np.array([0, 1, 0])) 490 | 491 | pointed_z = pose.point_z_at(pose2) 492 | assert pointed_z.z_axis == pytest.approx(np.array([0, 1, 0])) 493 | 494 | def test_point_at_non_identity_start(): 495 | pose_tree = get_pose_tree() 496 | pose = pose_tree.get_pose([0, 0, 0], [0, 0, 0, 1], 'robot').rotate_about_z(3) 497 | pose2 = pose_tree.get_pose([0, 4, 0], [0, 0, 0, 1], 'robot') 498 | 499 | pointed_x = pose.point_x_at(pose2) 500 | assert pointed_x.x_axis == pytest.approx(np.array([0, 1, 0])) 501 | 502 | pointed_y = pose.point_y_at(pose2) 503 | assert pointed_y.y_axis == pytest.approx(np.array([0, 1, 0])) 504 | 505 | pointed_z = pose.point_z_at(pose2) 506 | assert pointed_z.z_axis == pytest.approx(np.array([0, 1, 0])) 507 | 508 | def test_point_at_different_frame(): 509 | pose_tree = get_pose_tree() 510 | pose = pose_tree.get_pose([0, 0, 0], [0, 0, 0, 1], 'robot') 511 | pose2 = pose_tree.get_pose([0, 4, 0], [0, 0, 0, 1], 'robot').in_frame('camera') 512 | 513 | pointed_x = pose.point_x_at(pose2) 514 | assert pointed_x.x_axis == pytest.approx(np.array([0, 1, 0])) 515 | 516 | pointed_y = pose.point_y_at(pose2) 517 | assert pointed_y.y_axis == pytest.approx(np.array([0, 1, 0])) 518 | 519 | pointed_z = pose.point_z_at(pose2) 520 | assert pointed_z.z_axis == pytest.approx(np.array([0, 1, 0])) 521 | 522 | def test_point_at_self(): 523 | pose_tree = get_pose_tree() 524 | pose = pose_tree.get_pose([0, 0, 0], [0, 0, 0, 1], 'robot') 525 | 526 | pointed_x = pose.point_x_at(pose) 527 | assert pointed_x.x_axis == pytest.approx(np.array([1, 0, 0])) 528 | 529 | pointed_y = pose.point_y_at(pose) 530 | assert pointed_y.y_axis == pytest.approx(np.array([0, 1, 0])) 531 | 532 | pointed_z = pose.point_z_at(pose) 533 | assert pointed_z.z_axis == pytest.approx(np.array([0, 0, 1])) 534 | 535 | def test_point_at_180_degrees(): 536 | pose_tree = get_pose_tree() 537 | pose = pose_tree.get_pose([0, 0, 0], [0, 0, 0, 1], 'robot') 538 | pose2 = pose_tree.get_pose([-5, 0, 0], [0, 0, 0, 1], 'robot') 539 | 540 | pointed_x = pose.point_x_at(pose2) 541 | assert pointed_x.x_axis == pytest.approx(np.array([-1, 0, 0])) 542 | 543 | def test_point_x_at_with_fixed_z_axis(): 544 | pose_tree = get_pose_tree() 545 | pose = pose_tree.get_pose([0, 0, 0], [0, 0, 0, 1], 'robot') 546 | pose2 = pose_tree.get_pose([0, 4, 3], [0, 0, 0, 1], 'robot') 547 | 548 | pointed_x = pose.point_x_at(pose2, fixed_axis='z') 549 | assert pointed_x.x_axis == pytest.approx(np.array([0, 1, 0])) 550 | assert pointed_x.z_axis == pytest.approx(np.array([0, 0, 1])) 551 | 552 | def test_point_x_at_with_fixed_z_axis_non_identity_start(): 553 | pose_tree = get_pose_tree() 554 | pose = pose_tree.get_pose([0, 0, 0], [0, 0, 0, 1], 'robot').rotate_about_z(3) 555 | pose2 = pose_tree.get_pose([0, 4, 3], [0, 0, 0, 1], 'robot') 556 | 557 | pointed_x = pose.point_x_at(pose2, fixed_axis='z') 558 | assert pointed_x.x_axis == pytest.approx(np.array([0, 1, 0])) 559 | assert pointed_x.z_axis == pytest.approx(np.array([0, 0, 1])) 560 | 561 | def test_point_x_at_with_fixed_y_axis(): 562 | pose_tree = get_pose_tree() 563 | pose = pose_tree.get_pose([0, 0, 0], [0, 0, 0, 1], 'robot') 564 | pose2 = pose_tree.get_pose([0, 4, 3], [0, 0, 0, 1], 'robot') 565 | 566 | pointed_x = pose.point_x_at(pose2, fixed_axis='y') 567 | assert pointed_x.x_axis == pytest.approx(np.array([0, 0, 1])) 568 | assert pointed_x.y_axis == pytest.approx(np.array([0, 1, 0])) 569 | 570 | def test_point_z_at_with_fixed_x_axis(): 571 | pose_tree = get_pose_tree() 572 | pose = pose_tree.get_pose([0, 0, 0], [0, 0, 0, 1], 'robot') 573 | pose2 = pose_tree.get_pose([2, 4, 0], [0, 0, 0, 1], 'robot') 574 | 575 | pointed_z = pose.point_z_at(pose2, fixed_axis='x') 576 | assert pointed_z.z_axis == pytest.approx(np.array([0, 1, 0])) 577 | assert pointed_z.x_axis == pytest.approx(np.array([1, 0, 0])) 578 | 579 | def test_point_at_with_fixed_axis_non_aligned(): 580 | pose_tree = get_pose_tree() 581 | pose = pose_tree.get_pose([0, 0, 0], [0, 0, 0, 1], 'robot').rotate_about_y(0.2) 582 | pose2 = pose_tree.get_pose([0, 4, 5], [0, 0, 0, 1], 'robot') 583 | 584 | pointed_x = pose.point_x_at(pose2, fixed_axis='z') 585 | assert pointed_x.z_axis == pytest.approx(pose.z_axis) 586 | 587 | # Check that small rotations result in less pointing 588 | 589 | initial_alignment = pointed_x.x_axis.dot([0, 1, 0]) 590 | assert initial_alignment < 0.99 591 | 592 | for delta in [-0.01, 0.01]: 593 | for axis in [[1,0,0], [0,1,0], [0,0,1]]: 594 | test_pose = pose.rotate_about_axis(axis, delta) 595 | new_alignment = test_pose.x_axis.dot([0, 1, 0]) 596 | assert new_alignment < initial_alignment 597 | 598 | def test_point_at_with_fixed_axis_non_aligned_non_identity_start(): 599 | pose_tree = get_pose_tree() 600 | pose = pose_tree.get_pose([0, 0, 0], [0, 0, 0, 1], 'robot').rotate_about_y(0.2).rotate_about_z(0.3).rotate_about_x(0.4) 601 | pose2 = pose_tree.get_pose([0, 4, 5], [0, 0, 0, 1], 'robot') 602 | 603 | pointed_x = pose.point_x_at(pose2, fixed_axis='z') 604 | assert pointed_x.z_axis == pytest.approx(pose.z_axis) 605 | 606 | initial_alignment = pointed_x.x_axis.dot([0, 1, 0]) 607 | assert initial_alignment < 0.99 608 | 609 | for delta in [-0.01, 0.01]: 610 | for axis in [[1,0,0], [0,1,0], [0,0,1]]: 611 | test_pose = pose.rotate_about_axis(axis, delta) 612 | new_alignment = test_pose.x_axis.dot([0, 1, 0]) 613 | assert new_alignment < initial_alignment 614 | 615 | def test_interpolate(): 616 | pose_tree = get_pose_tree() 617 | 618 | # Create two pose objects 619 | t1 = Transform(position=np.array([0, 0, 0]), rotation=Rotation.from_quat([0, 0, 0, 1])) 620 | p1 = Pose(t1, 'world', pose_tree) 621 | 622 | t2 = Transform(position=np.array([1, 1, 1]), rotation=Rotation.from_quat([0, 1, 0, 0])) 623 | p2 = Pose(t2, 'world', pose_tree) 624 | 625 | # Interpolate between the transforms at alpha=0.0 (should equal t1) 626 | p_interpolated = p1.interpolate(p2, 0.0) 627 | assert p_interpolated.transform.almost_equal(p1.transform) 628 | 629 | # Interpolate between the transforms at alpha=1.0 (should equal t2) 630 | p_interpolated = p1.interpolate(p2, 1.0) 631 | assert p_interpolated.transform.almost_equal(p2.transform) 632 | 633 | # Interpolate between the transforms at alpha=0.5 (should be midway between t1 and t2) 634 | p_interpolated = p1.interpolate(p2, 0.5) 635 | expected_position = np.array([0.5, 0.5, 0.5]) # Midway between t1.position and t2.position 636 | assert np.allclose(p_interpolated.position, expected_position) 637 | assert p1.angle_to(p_interpolated) == approx(p2.angle_to(p_interpolated)) # Interpolated rotation should be halfway between t1.rotation and t2.rotation 638 | -------------------------------------------------------------------------------- /docs/posetree.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 |1from .pose import Transform, Pose, PoseTree, CustomFramePoseTree
50 | posetree is a module for representing and manipulating poses in 3D space. There are two main classes: Pose and PoseTree.
A Pose is an immutable location and orientation in 3D space, relative to a frame (i.e. 1 meter forward in X from the robot base).
A PoseTree is a object that keeps track of frames as they change over time, so that poses can know how to transform themselves into other frames, \nand do math operations with poses in different frames at particular points in time. You will need to subclass PoseTree to hook it up to your robot's\nodometry system.
A Pose is an immutable location and orientation in 3D space.\nA pose is defined with a parent_frame (where to start) and a transform (how to get to the pose from the parent frame).
Note: While a Pose is immutable, the parent frame can (and does!) change over time relative to other frames, meaning that an individual pose can move relative\nto other frames. (For example a pose defined in the \"robot\" frame will conceptually move as the robot moves relative to a 'world'\nframe, even though its position and orientation remain immutably constant.)
\n"}, {"fullname": "posetree.pose.Pose.__init__", "modulename": "posetree.pose", "qualname": "Pose.__init__", "kind": "function", "doc": "Create a pose from a transform, frame, and pose tree.
\n\nCreate a pose from a position and quaternion.
\n\n\n\n", "signature": "(\tcls,\tposition: Sequence[float],\tquaternion: Sequence[float],\tparent_frame: str,\tpose_tree: posetree.pose.PoseTree) -> posetree.pose.Pose:", "funcdef": "def"}, {"fullname": "posetree.pose.Pose.from_position_and_rotation", "modulename": "posetree.pose", "qualname": "Pose.from_position_and_rotation", "kind": "function", "doc": "A new Pose.
\n
Create a transform from a position and rotatoin.
\n\n\n\n", "signature": "(\tcls,\tposition: Sequence[float],\trotation: scipy.spatial.transform._rotation.Rotation,\tparent_frame: str,\tpose_tree: posetree.pose.PoseTree) -> posetree.pose.Pose:", "funcdef": "def"}, {"fullname": "posetree.pose.Pose.transform", "modulename": "posetree.pose", "qualname": "Pose.transform", "kind": "variable", "doc": "A new Pose.
\n
The transform from the parent frame to the pose.
\n", "annotation": ": posetree.pose.Transform"}, {"fullname": "posetree.pose.Pose.position", "modulename": "posetree.pose", "qualname": "Pose.position", "kind": "variable", "doc": "The position of the pose relative to the parent frame.
\n", "annotation": ": numpy.ndarray"}, {"fullname": "posetree.pose.Pose.rotation", "modulename": "posetree.pose", "qualname": "Pose.rotation", "kind": "variable", "doc": "The rotation of the pose relative to the parent frame.
\n", "annotation": ": scipy.spatial.transform._rotation.Rotation"}, {"fullname": "posetree.pose.Pose.frame", "modulename": "posetree.pose", "qualname": "Pose.frame", "kind": "variable", "doc": "The parent frame of the pose.
\n", "annotation": ": str"}, {"fullname": "posetree.pose.Pose.pose_tree", "modulename": "posetree.pose", "qualname": "Pose.pose_tree", "kind": "variable", "doc": "The PoseTree object containing the world that the pose exists in.
\n", "annotation": ": posetree.pose.PoseTree"}, {"fullname": "posetree.pose.Pose.x", "modulename": "posetree.pose", "qualname": "Pose.x", "kind": "variable", "doc": "The x coordinate of the position.
\n", "annotation": ": float"}, {"fullname": "posetree.pose.Pose.y", "modulename": "posetree.pose", "qualname": "Pose.y", "kind": "variable", "doc": "The y coordinate of the position.
\n", "annotation": ": float"}, {"fullname": "posetree.pose.Pose.z", "modulename": "posetree.pose", "qualname": "Pose.z", "kind": "variable", "doc": "The z coordinate of the position.
\n", "annotation": ": float"}, {"fullname": "posetree.pose.Pose.x_axis", "modulename": "posetree.pose", "qualname": "Pose.x_axis", "kind": "variable", "doc": "The x axis of the pose.
\n", "annotation": ": numpy.ndarray"}, {"fullname": "posetree.pose.Pose.y_axis", "modulename": "posetree.pose", "qualname": "Pose.y_axis", "kind": "variable", "doc": "The y axis of the pose.
\n", "annotation": ": numpy.ndarray"}, {"fullname": "posetree.pose.Pose.z_axis", "modulename": "posetree.pose", "qualname": "Pose.z_axis", "kind": "variable", "doc": "The z axis of the pose.
\n", "annotation": ": numpy.ndarray"}, {"fullname": "posetree.pose.Pose.in_frame", "modulename": "posetree.pose", "qualname": "Pose.in_frame", "kind": "function", "doc": "Return a new pose representing the same location in space expressed in a different frame.
\n\n\n\n", "signature": "(\tself,\tparent_frame: str,\ttimestamp: Optional[float] = None) -> posetree.pose.Pose:", "funcdef": "def"}, {"fullname": "posetree.pose.Pose.with_position", "modulename": "posetree.pose", "qualname": "Pose.with_position", "kind": "function", "doc": "A new pose with the same transform but in a different frame.
\n
Return a new pose with the same rotation but a different location relative to the parent frame.
\n\n\n\n", "signature": "(self, position: Sequence[float]) -> posetree.pose.Pose:", "funcdef": "def"}, {"fullname": "posetree.pose.Pose.with_rotation", "modulename": "posetree.pose", "qualname": "Pose.with_rotation", "kind": "function", "doc": "A new pose with the same rotation but a different location.
\n
Return a new pose with the same location but a different rotation.
\n\n\n\n", "signature": "(\tself,\trotation: scipy.spatial.transform._rotation.Rotation) -> posetree.pose.Pose:", "funcdef": "def"}, {"fullname": "posetree.pose.Pose.with_rotation_matching", "modulename": "posetree.pose", "qualname": "Pose.with_rotation_matching", "kind": "function", "doc": "A new pose with the same location but a different rotation.
\n
Return a new pose with the same position but a different orientation, matching another pose.
\n\nThis method is frame aware, so even if the other pose has a different frame, the rotation will be matched correctly.
\n\n\n\n", "signature": "(self, other: posetree.pose.Pose) -> posetree.pose.Pose:", "funcdef": "def"}, {"fullname": "posetree.pose.Pose.with_position_x", "modulename": "posetree.pose", "qualname": "Pose.with_position_x", "kind": "function", "doc": "A new pose with the same position but a different orientation.
\n
Return a new pose with the same y and z coordinates but a different x coordinate.
\n\n\n\n", "signature": "(self, x: float, *, frame: Optional[str] = None) -> posetree.pose.Pose:", "funcdef": "def"}, {"fullname": "posetree.pose.Pose.with_position_y", "modulename": "posetree.pose", "qualname": "Pose.with_position_y", "kind": "function", "doc": "A new pose with the same y and z coordinates but a different x coordinate.
\n
Return a new pose with the same x and z coordinates but a different y coordinate.
\n\n\n\n", "signature": "(self, y: float, *, frame: Optional[str] = None) -> posetree.pose.Pose:", "funcdef": "def"}, {"fullname": "posetree.pose.Pose.with_position_z", "modulename": "posetree.pose", "qualname": "Pose.with_position_z", "kind": "function", "doc": "A new pose with the same x and z coordinates but a different y coordinate.
\n
Return a new pose with the same x and y coordinates but a different z coordinate.
\n\n\n\n", "signature": "(self, z: float, *, frame: Optional[str] = None) -> posetree.pose.Pose:", "funcdef": "def"}, {"fullname": "posetree.pose.Pose.apply_transform", "modulename": "posetree.pose", "qualname": "Pose.apply_transform", "kind": "function", "doc": "A new pose with the same x and y coordinates but a different z coordinate.
\n
Return a new pose transformed by a transform.
\n\nThis method is provided to give compatibility with systems that talk about poses in terms of transforms,\nbut it is usually more readable and debuggable to use a rotate/translate method instead.
\n\n\n\n", "signature": "(self, transform: posetree.pose.Transform) -> posetree.pose.Pose:", "funcdef": "def"}, {"fullname": "posetree.pose.Pose.translate", "modulename": "posetree.pose", "qualname": "Pose.translate", "kind": "function", "doc": "A new pose transformed by a transform.
\n
Return a new pose translated by a vector.
\n\nThe translation is relative to the basis of the pose being translated (i.e body-fixed) rather than the parent frame.\nThat means that
\n\np2 = p1.translate((1, 0, 0))\n\nand
\n\np2 = p1.in_frame("some_other_frame").translate((1, 0, 0)).in_frame(p1.frame)\n\nare identical, because the parent frame of a pose does not affect its translation.
\n\n\n\n", "signature": "(\tself,\ttranslation: Sequence[float],\t*,\tframe: str = None) -> posetree.pose.Pose:", "funcdef": "def"}, {"fullname": "posetree.pose.Pose.rotate_about_axis", "modulename": "posetree.pose", "qualname": "Pose.rotate_about_axis", "kind": "function", "doc": "A new pose translated by a vector.
\n
Return a new pose rotated about an axis.
\n\nThis is rotated in the basis of the pose being rotated (i.e body-fixed) rather than the parent frame, and it only\nrotates the orientation. This is not a rotation about the origin, but a change of the orientation of the pose.
\n\nIf you pass a frame, the axis will be interpreted as being expressed in that frame.
\n\n\n\n", "signature": "(\tself,\taxis: Sequence[float],\tangle: float,\t*,\tdegrees: bool = False,\tframe: Optional[str] = None) -> posetree.pose.Pose:", "funcdef": "def"}, {"fullname": "posetree.pose.Pose.rotate_about_x", "modulename": "posetree.pose", "qualname": "Pose.rotate_about_x", "kind": "function", "doc": "A new pose with the same location but rotated about an axis.
\n
Return a new pose rotated about the x axis.
\n\n\n\n", "signature": "(\tself,\tangle: float,\t*,\tdegrees: bool = False,\tframe: Optional[str] = None) -> posetree.pose.Pose:", "funcdef": "def"}, {"fullname": "posetree.pose.Pose.rotate_about_y", "modulename": "posetree.pose", "qualname": "Pose.rotate_about_y", "kind": "function", "doc": "A new pose with the same location but rotated about the x axis.
\n
Return a new pose rotated about the y axis.
\n\n\n\n", "signature": "(\tself,\tangle: float,\t*,\tdegrees: bool = False,\tframe: Optional[str] = None) -> posetree.pose.Pose:", "funcdef": "def"}, {"fullname": "posetree.pose.Pose.rotate_about_z", "modulename": "posetree.pose", "qualname": "Pose.rotate_about_z", "kind": "function", "doc": "A new pose with the same location but rotated about the y axis.
\n
Return a new pose rotated about the z axis.
\n\n\n\n", "signature": "(\tself,\tangle: float,\t*,\tdegrees: bool = False,\tframe: Optional[str] = None) -> posetree.pose.Pose:", "funcdef": "def"}, {"fullname": "posetree.pose.Pose.point_x_at", "modulename": "posetree.pose", "qualname": "Pose.point_x_at", "kind": "function", "doc": "A new pose with the same location but rotated about the z axis.
\n
Return a new pose rotated to point the x axis at a target pose.
\n\n\n\n", "signature": "(\tself,\ttarget: posetree.pose.Pose,\tfixed_axis: str = None) -> posetree.pose.Pose:", "funcdef": "def"}, {"fullname": "posetree.pose.Pose.point_y_at", "modulename": "posetree.pose", "qualname": "Pose.point_y_at", "kind": "function", "doc": "A new pose with the same location but rotated to point the x axis at a target pose.
\n
Return a new pose rotated to point the y axis at a target pose.
\n\n\n\n", "signature": "(\tself,\ttarget: posetree.pose.Pose,\tfixed_axis: str = None) -> posetree.pose.Pose:", "funcdef": "def"}, {"fullname": "posetree.pose.Pose.point_z_at", "modulename": "posetree.pose", "qualname": "Pose.point_z_at", "kind": "function", "doc": "A new pose with the same location but rotated to point the y axis at a target pose.
\n
Return a new pose rotated to point the z axis at a target pose.
\n\n\n\n", "signature": "(\tself,\ttarget: posetree.pose.Pose,\tfixed_axis: str = None) -> posetree.pose.Pose:", "funcdef": "def"}, {"fullname": "posetree.pose.Pose.interpolate", "modulename": "posetree.pose", "qualname": "Pose.interpolate", "kind": "function", "doc": "A new pose with the same location but rotated to point the z axis at a target pose.
\n
Return a new pose interpolated between two poses, in the frame of the first pose.
\n\n\n\n", "signature": "(self, target: posetree.pose.Pose, alpha: float) -> posetree.pose.Pose:", "funcdef": "def"}, {"fullname": "posetree.pose.Pose.distance_to", "modulename": "posetree.pose", "qualname": "Pose.distance_to", "kind": "function", "doc": "A new pose interpolated between two poses.
\n
Return the cartesian distance between two poses.
\n\n\n\n", "signature": "(self, other: posetree.pose.Pose) -> float:", "funcdef": "def"}, {"fullname": "posetree.pose.Pose.angle_to", "modulename": "posetree.pose", "qualname": "Pose.angle_to", "kind": "function", "doc": "The distance between two poses.
\n
Return the angle between two poses' orientations. Ignores position. Will convert to the same frame.
\n\nThis gives the magnitude of the minimal rotation that will align the two poses.
\n\n\n\n", "signature": "(self, target: posetree.pose.Pose) -> float:", "funcdef": "def"}, {"fullname": "posetree.pose.Pose.angle_about_x_to", "modulename": "posetree.pose", "qualname": "Pose.angle_about_x_to", "kind": "function", "doc": "The angle between two poses, in radians.
\n
Return the signed angle between two poses' orientations about the x axis. Ignores position. Will convert to the same frame.
\n\n\n\n\nThe angle between two poses, in radians.
\n
Return the signed angle between two poses' orientations about the y axis. Ignores position. Will convert to the same frame.
\n\n\n\n\nThe angle between two poses, in radians.
\n
Return the signed angle between two poses' orientations about the z axis. Ignores position. Will convert to the same frame.
\n\n\n\n\nThe angle between two poses, in radians.
\n
Abstract base class to manage poses and frames.
\n\nTo hook up this library to your project, you should subclass this class and hook it up to your stream of transforms\nby implementing _get_transform. Usually this will look like adding a callback-subscribe method to your class and storing\nthe transforms between important frames as they come in. If you don't already have a way of interpolating between transforms,\ncheck out ROS's tf2 library. This library is designed to wrap nicely on top of tf2, if you are already using it.
\n", "bases": "abc.ABC"}, {"fullname": "posetree.pose.PoseTree.get_transform", "modulename": "posetree.pose", "qualname": "PoseTree.get_transform", "kind": "function", "doc": "Return the transform from the parent frame to the child frame.
\n", "signature": "(\tself,\tparent_frame: str,\tchild_frame: str,\ttimestamp: float) -> posetree.pose.Transform:", "funcdef": "def"}, {"fullname": "posetree.pose.PoseTree.get_pose", "modulename": "posetree.pose", "qualname": "PoseTree.get_pose", "kind": "function", "doc": "Create a pose object from a position, quaternion, and parent frame. Essentially syntactic sugar on top of Pose's constructor.
\n", "signature": "(\tself,\tposition: Sequence[float],\tquaternion: Sequence[float],\tparent_frame: str) -> posetree.pose.Pose:", "funcdef": "def"}, {"fullname": "posetree.pose.PoseTree.temporary_frame", "modulename": "posetree.pose", "qualname": "PoseTree.temporary_frame", "kind": "function", "doc": "Create a temporary frame from a pose.
\n\nThis is intended to be used for anonymous frames for doing calculations.
\n\nExample 1, find an approach angle that is close to the robot base:
\n\nfor theta in np.linspace(0, 2 * np.pi, 100):\n with tree.temporary_frame(target_pose.rotate_about_z(theta)) as approach_frame:\n if abs(robot_base_pose.in_frame(approach_frame).y) < 0.1:\n return theta\n\nExample 2, find a person that is ready to talk to the robot:
\n\nfor person in people:\n with tree.temporary_frame(person.head_pose) as head_frame:\n x, y, _ = robot_base_pose.in_frame(head_frame).position\n if abs(y) < 0.5 and 0.5 < x < 1.5:\n return person\n\nAdd a static frame to the tree.
\n\nExample:
\n\npose_tree.add_frame(table_corner_pose, 'table_frame')\n\n# Look at center of table:\nrobot.look_at(\n Pose.from_position_and_rotation(\n [table_size_x / 2, tablesize_y_y / 2, 0],\n Rotation.identity(),\n 'table_frame'))\n\nDon't use this for scratch frames for doing calculations. Use the temporary_frame context manager instead. This should \nbe used for permanant, human meaningful things like a workspace frame.
\n\nThis is also not a good solution for frames that are moving, as the frames will not be interpolated to particular timestamps.
\n\nRemove a static frame from the tree.
\n", "signature": "(self, name: str) -> None:", "funcdef": "def"}, {"fullname": "posetree.pose.CustomFramePoseTree", "modulename": "posetree.pose", "qualname": "CustomFramePoseTree", "kind": "class", "doc": "A abstract PoseTree implementation that implements custom frames.
\n\nThis is useful if your underlying pose structure doesn't support custom frames. You subclass this\nclass and implement the _get_transform method and get full custom frame support. In a pinch you can implement\n_get_transform to raise a NotImplemented error and use 'add frame' but it is not particularly efficient and does\nnot implement interpolation to particular timestamps, which will cause problems if you are getting sensor\ninformation off a moving robot.
\n", "bases": "PoseTree, abc.ABC"}, {"fullname": "posetree.pose.CustomFramePoseTree.custom_frames", "modulename": "posetree.pose", "qualname": "CustomFramePoseTree.custom_frames", "kind": "variable", "doc": "\n"}, {"fullname": "posetree.pose.CustomFramePoseTree.add_frame", "modulename": "posetree.pose", "qualname": "CustomFramePoseTree.add_frame", "kind": "function", "doc": "Add a frame to the tree. This is assumed to be a static frame, and will not be interpolated.
\n\nIf that frame is already present, this will update the frame to a new pose.
\n\nRemove a custom frame from the tree.
\n\nThis method is idempotent. If the frame is not present, it will do nothing.
\n", "signature": "(self, name: str) -> None:", "funcdef": "def"}, {"fullname": "posetree.pose.CustomFramePoseTree.get_transform", "modulename": "posetree.pose", "qualname": "CustomFramePoseTree.get_transform", "kind": "function", "doc": "Return the transform from the parent frame to the child frame, at the timestamp.
\n", "signature": "(\tself,\tparent_frame: str,\tchild_frame: str,\ttimestamp: Optional[float] = None) -> posetree.pose.Transform:", "funcdef": "def"}, {"fullname": "posetree.pose.Transform", "modulename": "posetree.pose", "qualname": "Transform", "kind": "class", "doc": "A homogenous transform from a pose or frame to another location and rotation.
\n\nIn posetree, a Transform is a verb. It describes how to transform a pose from one frame to another.\nAlmost all the time, you want to turn your raw data objects (coming from forward kinematics or perception or \nlocalization) into a Pose object as soon as possible, and only go through Transforms at the borders of your sdk.
\n\nUsing Transform objects instead of Pose objects is usually a code smell, unless you're doing something really\nmathy with a bunch of intermediate transforms.
\n"}, {"fullname": "posetree.pose.Transform.__init__", "modulename": "posetree.pose", "qualname": "Transform.__init__", "kind": "function", "doc": "Create a transform from a position and rotation.
\n\nCreate a transform from a position and quaternion.
\n\n\n\n", "signature": "(\tcls,\tposition: Sequence[float],\tquaternion: Sequence[float]) -> posetree.pose.Transform:", "funcdef": "def"}, {"fullname": "posetree.pose.Transform.identity", "modulename": "posetree.pose", "qualname": "Transform.identity", "kind": "function", "doc": "A new transform.
\n
Return the identity transform.
\n", "signature": "(cls) -> posetree.pose.Transform:", "funcdef": "def"}, {"fullname": "posetree.pose.Transform.inverse", "modulename": "posetree.pose", "qualname": "Transform.inverse", "kind": "variable", "doc": "The inverse of the transform.
\n\nHow to get back from applying this transform.
\n\nt1 * t1.inverse == Transform.identity()
\n", "annotation": ": posetree.pose.Transform"}, {"fullname": "posetree.pose.Transform.position", "modulename": "posetree.pose", "qualname": "Transform.position", "kind": "variable", "doc": "The 1x3 cartesian position of the transform.
\n", "annotation": ": numpy.ndarray"}, {"fullname": "posetree.pose.Transform.rotation", "modulename": "posetree.pose", "qualname": "Transform.rotation", "kind": "variable", "doc": "The rotation component of the transform.
\n", "annotation": ": scipy.spatial.transform._rotation.Rotation"}, {"fullname": "posetree.pose.Transform.x_axis", "modulename": "posetree.pose", "qualname": "Transform.x_axis", "kind": "variable", "doc": "The x axis of the transform.
\n\nThis is the unit vector, in the parent frame, pointing in the new x direction, after applying the transform.
\n", "annotation": ": numpy.ndarray"}, {"fullname": "posetree.pose.Transform.y_axis", "modulename": "posetree.pose", "qualname": "Transform.y_axis", "kind": "variable", "doc": "The y axis of the transform.
\n\nThis is the unit vector, in the parent frame, pointing in the new y direction, after applying the transform.
\n", "annotation": ": numpy.ndarray"}, {"fullname": "posetree.pose.Transform.z_axis", "modulename": "posetree.pose", "qualname": "Transform.z_axis", "kind": "variable", "doc": "The z axis of the transform.
\n\nThis is the unit vector, in the parent frame, pointing in the new z direction, after applying the transform.
\n", "annotation": ": numpy.ndarray"}, {"fullname": "posetree.pose.Transform.x", "modulename": "posetree.pose", "qualname": "Transform.x", "kind": "variable", "doc": "The x coordinate of the transform.
\n", "annotation": ": float"}, {"fullname": "posetree.pose.Transform.y", "modulename": "posetree.pose", "qualname": "Transform.y", "kind": "variable", "doc": "The y coordinate of the transform.
\n", "annotation": ": float"}, {"fullname": "posetree.pose.Transform.z", "modulename": "posetree.pose", "qualname": "Transform.z", "kind": "variable", "doc": "The z coordinate of the transform.
\n", "annotation": ": float"}, {"fullname": "posetree.pose.Transform.to_matrix", "modulename": "posetree.pose", "qualname": "Transform.to_matrix", "kind": "function", "doc": "Return the 4x4 matrix representation of the transform.
\n", "signature": "(self) -> numpy.ndarray:", "funcdef": "def"}, {"fullname": "posetree.pose.Transform.from_matrix", "modulename": "posetree.pose", "qualname": "Transform.from_matrix", "kind": "function", "doc": "Create a transform from a matrix.
\n\n\n\n", "signature": "(cls, matrix: Sequence[Sequence[float]]) -> posetree.pose.Transform:", "funcdef": "def"}, {"fullname": "posetree.pose.Transform.apply", "modulename": "posetree.pose", "qualname": "Transform.apply", "kind": "function", "doc": "A new transform.
\n
Multiply this transform by another transform or vector.
\n\nIf other is a transform, this is equivalent to applying this transform, then the other transform.\nIf other is a vector, this is equivalent to applying this transform to the vector.
\n\n\n\n", "signature": "(\tself,\tother: Union[posetree.pose.Transform, Sequence[float], Sequence[Sequence[float]]]) -> posetree.pose.Transform:", "funcdef": "def"}, {"fullname": "posetree.pose.Transform.almost_equal", "modulename": "posetree.pose", "qualname": "Transform.almost_equal", "kind": "function", "doc": "The product of the two transforms.
\n
Check if two transforms are almost equal.
\n\nHandles floating point error in the position and rotation, and the fact that for quaternions negating \nthe vector gives the same rotation.
\n", "signature": "(self, other: posetree.pose.Transform, atol: float = 1e-08) -> bool:", "funcdef": "def"}, {"fullname": "posetree.pose.Transform.interpolate", "modulename": "posetree.pose", "qualname": "Transform.interpolate", "kind": "function", "doc": "Interpolate between two transforms.
\n\n\n\n", "signature": "(\tself,\tother: posetree.pose.Transform,\talpha: float) -> posetree.pose.Transform:", "funcdef": "def"}, {"fullname": "posetree.pose.Transform.angle_to", "modulename": "posetree.pose", "qualname": "Transform.angle_to", "kind": "function", "doc": "The interpolated transform.
\n
Return the angle between two transforms' orientations. Ignores position.
\n\n\n\n", "signature": "(self, target: posetree.pose.Transform) -> float:", "funcdef": "def"}]; 4 | 5 | // mirrored in build-search-index.js (part 1) 6 | // Also split on html tags. this is a cheap heuristic, but good enough. 7 | elasticlunr.tokenizer.setSeperator(/[\s\-.;&_'"=,()]+|<[^>]*>/); 8 | 9 | let searchIndex; 10 | if (docs._isPrebuiltIndex) { 11 | console.info("using precompiled search index"); 12 | searchIndex = elasticlunr.Index.load(docs); 13 | } else { 14 | console.time("building search index"); 15 | // mirrored in build-search-index.js (part 2) 16 | searchIndex = elasticlunr(function () { 17 | this.pipeline.remove(elasticlunr.stemmer); 18 | this.pipeline.remove(elasticlunr.stopWordFilter); 19 | this.addField("qualname"); 20 | this.addField("fullname"); 21 | this.addField("annotation"); 22 | this.addField("default_value"); 23 | this.addField("signature"); 24 | this.addField("bases"); 25 | this.addField("doc"); 26 | this.setRef("fullname"); 27 | }); 28 | for (let doc of docs) { 29 | searchIndex.addDoc(doc); 30 | } 31 | console.timeEnd("building search index"); 32 | } 33 | 34 | return (term) => searchIndex.search(term, { 35 | fields: { 36 | qualname: {boost: 4}, 37 | fullname: {boost: 2}, 38 | annotation: {boost: 2}, 39 | default_value: {boost: 2}, 40 | signature: {boost: 2}, 41 | bases: {boost: 2}, 42 | doc: {boost: 1}, 43 | }, 44 | expand: true 45 | }); 46 | })(); --------------------------------------------------------------------------------The angle between two transform, in radians.
\n