feat: add Vec3 and Quaternion modules with Transform integration #22
Conversation
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Add core math types for 3D vectors and unit quaternions, backed by Nx tensors. All math operations use Nx for consistent performance and potential GPU acceleration. BB.Vec3: - Construction: new/3, from_tensor/1, zero/0, unit_x/y/z - Operations: add, subtract, scale, dot, cross, normalise, magnitude - Interpolation: lerp/3, distance/2 BB.Quaternion: - Construction: new/4, identity/0, from_axis_angle/2, from_rotation_matrix/1, from_euler/4 - Conversion: to_rotation_matrix/1, to_euler/2, to_axis_angle/1 - Operations: multiply/2, conjugate/1, inverse/1, rotate_vector/2 - Interpolation: slerp/3, angular_distance/2 Design decisions: - Single Vec3 type used everywhere (no ad-hoc tuples) - All math via Nx operations (no :math module) - WXYZ quaternion order internally, with XYZW conversion for ROS - Shepperd method for rotation matrix to quaternion (numerical stability) - Nx.select for conditional logic (gimbal lock, Shepperd branches)
Add functions for quaternion/transform conversion: - `from_quaternion/1` - create 4x4 transform from quaternion - `get_quaternion/1` - extract quaternion from 4x4 transform - `from_position_quaternion/2` - create transform from position + quaternion - `get_forward_vector/1`, `get_up_vector/1`, `get_right_vector/1` - extract axis vectors as Vec3 Also fix numerical precision issue in `Quaternion.from_rotation_matrix/1` where tiny negative values from floating point errors caused `Nx.sqrt` to fail.
- Add `orientation_target` type with `:none`, `{:axis, Vec3}`, and
`{:quaternion, Quaternion}` variants
- Change `target` type to use `Vec3.t()` instead of float tuples
- Add `orientation_tolerance` and `strict_orientation` options
- Add `orientation_residual` to solver metadata
- Add `get_translation_vec3/1` to Transform for efficient Vec3 extraction
Computes the quaternion representing the shortest rotation from one vector to another. Handles edge cases: - Parallel vectors return identity quaternion - Anti-parallel vectors return 180° rotation around perpendicular axis This enables computing minimum-rotation quaternions for axis constraints in IK solvers.
4 tasks
Add epsilon to prevent ArithmeticError when computing quaternion cases that won't be selected. The branchless algorithm computes all four cases and selects one, but division by zero in unused cases still raises an error.
- Move BB.Vec3 to BB.Math.Vec3 - Move BB.Quaternion to BB.Math.Quaternion - Delete orphaned BB.Message.Vec3 and BB.Message.Quaternion modules - Split monolithic message_test.exs into per-message-type test files - Update all references across lib and test files
…t pattern
- Move Transform module to BB.Math namespace alongside Vec3 and Quaternion
- Refactor to use struct-wrapped tensor pattern: `%Transform{tensor: ...}`
- Update API to use Vec3 types instead of tuples:
- `translation(Vec3.t())` instead of `translation(x, y, z)`
- `get_translation/1` returns Vec3 instead of tuple
- `apply_to_point/2` takes and returns Vec3
- `from_axis_angle/2` and `translation_along/2` take Vec3 for axis
- Add `from_tensor/1` and `tensor/1` for raw tensor access
- Rename functions: `revolute_transform` -> `from_axis_angle`,
`prismatic_transform` -> `translation_along`
- Update all callers in Kinematics, Builder, and tests
…arget type Raw tensors bypass the type safety of our math structs. Users with raw 4x4 tensors can wrap them with `Transform.from_tensor/1`.
- Point3D: now wraps `BB.Math.Vec3` instead of separate x/y/z floats - Pose: now wraps `BB.Math.Transform` instead of separate Vec3+Quaternion - Remove duplicate `BB.Message.Geometry.Transform` (Pose serves this purpose) - Add `transform_type()` validator to `BB.Message.Option` - Update MoveTo command and documentation accordingly
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Summary
BB.Math.Vec3struct for 3D vectors backed by Nx tensorsBB.Math.Quaternionstruct for unit quaternions with full rotation algebraBB.Robot.TransformtoBB.Math.Transformwith struct-wrapped tensor patternBB.IK.Solvertypes to support orientation targets (axis constraints and full quaternion)Changes
BB.Math.Vec3
new/3,from_tensor/1,from_tuple/1unit_x/0,unit_y/0,unit_z/0,zero/0add/2,subtract/2,scale/2,dot/2,cross/2,normalise/1,magnitude/1BB.Math.Quaternion
new/4,identity/0,from_axis_angle/2,from_rotation_matrix/1,from_tensor/1to_rotation_matrix/1,to_axis_angle/1multiply/2,conjugate/1,inverse/1,normalise/1,rotate_vector/2slerp/3,angular_distance/2from_two_vectors/2for computing shortest rotation between vectorsBB.Math.Transform
%Transform{tensor: ...}translation/1takesVec3instead of three floatsget_translation/1returnsVec3instead of tupleapply_to_point/2takes and returnsVec3from_axis_angle/2andtranslation_along/2takeVec3for axisfrom_tensor/1andtensor/1for raw tensor accessfrom_quaternion/1,get_quaternion/1,from_position_quaternion/2get_forward_vector/1,get_up_vector/1,get_right_vector/1BB.IK.Solver
{position, {:axis, direction}},{position, {:quaternion, q}}orientation_toleranceoptionorientation_residualto result metadataTest plan
mix check --no-retrypasses