Add prismatic joint #416

fantaosha · 2023-01-02T04:02:06Z

Motivation and Context

How Has This Been Tested

Types of changes

Docs change / refactoring / dependency upgrade
Bug fix (non-breaking change which fixes an issue)
New feature (non-breaking change which adds functionality)
Breaking change (fix or feature that would cause existing functionality to change)

Checklist

My code follows the code style of this project.
My change requires a change to the documentation.
I have updated the documentation accordingly.
I have read the CONTRIBUTING document.
I have completed my CLA (see CONTRIBUTING)
I have added tests to cover my changes.
All new and existing tests passed.

luisenp

Left some comments to address.

luisenp · 2023-01-12T16:58:35Z

theseus/embodied/robot/joint.py

+        if prismatic_axis.ndim == 1:
+            prismatic_axis = prismatic_axis.view(-1, 1)
+
+        if prismatic_axis.ndim != 2 or prismatic_axis.shape != (3, 1):


Here similar comment as I had for RevoluteJoint on using numel().

Refactored. And now the error message is "The prismatic axis must have 3 elements."

luisenp · 2023-01-23T17:26:48Z

theseus/embodied/robot/joint.py

+        super().__init__(name, parent, child, origin, dtype)
+        self._axis[3] = 1
+
+    def _translation_impl(self, angle: torch.Tensor) -> torch.Tensor:


The code in _translation_impl and relative_pose of these 3 classes is exactly the same except for one number, so you can just have a base class _PrismaticJointAxis that defines a member variable self._axis_idx: Optional[int] = None, which gets overridden by all base classes. Then we have

def _translation_impl(self, angle: torch.Tensor) -> torch.Tensor: self._check_angle_shape(angle) trans = angle.new_zeros(angle.shape[0], 3) trans[:, self._axis_idx] = angle return trans def relative_pose(self, angle: torch.Tensor) -> torch.Tensor: self._check_angle_shape(angle) ret = angle.new_empty(angle.shape[0], 3, 4) ret[:, :, :3] = self.origin[:, :, :3] ret[:, :, 3] = angle * self.origin[:, :, self._axis_idx] + self.origin[:, :, 3] return ret

Refactored. Thanks!

luisenp · 2023-01-23T17:27:45Z

theseus/embodied/robot/joint.py

+        return 1
+
+    @abc.abstractmethod
+    def _translation_impl(self, angle: torch.Tensor) -> torch.Tensor:


Same comment as in RevoluteJoint regarding a translation() method, with similar structure.

Refactored.

luisenp · 2023-01-23T17:28:09Z

theseus/embodied/robot/joint.py

+        return 1
+
+    @abc.abstractmethod
+    def _translation_impl(self, angle: torch.Tensor) -> torch.Tensor:


Maybe translation() and rotation() (in the other class) should be called translate() and rotate()?

I think noun should be better than verb since the outputs are 3x3 or 3x1 tensors, which are transformations.

mhmukadam · 2023-01-23T19:01:43Z

theseus/embodied/robot/joint.py

        name: str,
+        revolute_axis: torch.Tensor,


Maybe just axis, I am guessing revolute is implied from the class name.

mhmukadam

Looks good. Besides comments here, my other comments are similar to the revolute joint PR so we can follow here what we decide there.

exhaustin · 2023-01-31T00:49:05Z

theseus/embodied/robot/joint.py

+        if prismatic_axis.ndim == 1:
+            prismatic_axis = prismatic_axis.view(-1, 1)
+
+        if prismatic_axis.ndim != 2 or prismatic_axis.shape != (3, 1):
+            raise ValueError("The prismatic axis must be a 3-D vector.")


I imagine this logic of "checking if the input is a valid vector of correct length, and unsqueezing it into a vertical vector if it's 1-D" would be very commonly used, not just within the robot model module. Should this be a piece of logic that can be standardized and deduplicated within an utils of some sort?

If we anticipate this to be used outside robot model, it would be useful to make an utility function. Otherwise, we should at least delegate this repeated logic to the super class.

* add link * add id to joint * add id to link * add robot model * add name to robot * backup * refactor joint * refactor link * add robot model * robot model works * a minior refactor of joint * add ancestor to links * update dof computation * add forward kinematics function * Add differentiable forward kinematics (#420) * fixed a bug in FixedJoint * add angle_ids * add jacobians to FK * forward kinematics works * rename FK * add tests for differentiable kinematics * rename tests * simplify the computation of origin * add jacobian tests for forward kinematics * fixed a bug for vectorize=True in SE3 * improve the efficiency of forward kinematics * add tests for backward propagation * update lie group load paths * Add device to robot model (#422) * add device to robot model * Refactor Joint.dof as a property (#423) * refactor joint dof * add virtual end-effector for tests * seems to work after name changes * rename parent->parent_link and child->child_link * rename angle_ids -> ancestor_active_joint_ids * refactor tests with torch.testing.assert_close * small refactoring of the code * add Robot.get_links() * remove some setters for link and joint * made some functions private for encapsulation * consolidate if block and classmethod->staticmethod * refactor set_robot_from_urdf_file * rename ancestor_active_joint_ids -> ancestor_non_fixed_joint_ids * id=-1 -> id=None * refactor the code for simplicity * move link into joint

* add prismatic joint * refactor prismatic joint * Add robot model (#417) * add link * add id to joint * add id to link * add robot model * add name to robot * backup * refactor joint * refactor link * add robot model * robot model works * a minior refactor of joint * add ancestor to links * update dof computation * add forward kinematics function * Add differentiable forward kinematics (#420) * fixed a bug in FixedJoint * add angle_ids * add jacobians to FK * forward kinematics works * rename FK * add tests for differentiable kinematics * rename tests * simplify the computation of origin * add jacobian tests for forward kinematics * fixed a bug for vectorize=True in SE3 * improve the efficiency of forward kinematics * add tests for backward propagation * update lie group load paths * Add device to robot model (#422) * add device to robot model * Refactor Joint.dof as a property (#423) * refactor joint dof * add virtual end-effector for tests * seems to work after name changes * rename parent->parent_link and child->child_link * rename angle_ids -> ancestor_active_joint_ids * refactor tests with torch.testing.assert_close * small refactoring of the code * add Robot.get_links() * remove some setters for link and joint * made some functions private for encapsulation * consolidate if block and classmethod->staticmethod * refactor set_robot_from_urdf_file * rename ancestor_active_joint_ids -> ancestor_non_fixed_joint_ids * id=-1 -> id=None * refactor the code for simplicity * move link into joint

* add revolute joint * refactor revolute joint * add parent and child to joint * update the path to load so3 and se3 * Add prismatic joint (#416) * add prismatic joint * refactor prismatic joint * Add robot model (#417) * add link * add id to joint * add id to link * add robot model * add name to robot * backup * refactor joint * refactor link * add robot model * robot model works * a minior refactor of joint * add ancestor to links * update dof computation * add forward kinematics function * Add differentiable forward kinematics (#420) * fixed a bug in FixedJoint * add angle_ids * add jacobians to FK * forward kinematics works * rename FK * add tests for differentiable kinematics * rename tests * simplify the computation of origin * add jacobian tests for forward kinematics * fixed a bug for vectorize=True in SE3 * improve the efficiency of forward kinematics * add tests for backward propagation * update lie group load paths * Add device to robot model (#422) * Refactor Joint.dof as a property (#423) * refactor joint dof * add virtual end-effector for tests * seems to work after name changes * rename parent->parent_link and child->child_link * rename angle_ids -> ancestor_active_joint_ids * refactor tests with torch.testing.assert_close * small refactoring of the code * add Robot.get_links() * remove some setters for link and joint * made some functions private for encapsulation * consolidate if block and classmethod->staticmethod * refactor set_robot_from_urdf_file * rename ancestor_active_joint_ids -> ancestor_non_fixed_joint_ids * id=-1 -> id=None * refactor the code for simplicity * move link into joint * simplify the code * minor revision to be consistent with Lie group refatcoring * fixed a CI error * move files to labs

fantaosha added the enhancement New feature or request label Jan 2, 2023

fantaosha requested review from luisenp and mhmukadam January 2, 2023 04:02

fantaosha self-assigned this Jan 2, 2023

facebook-github-bot added the CLA Signed This label is managed by the Facebook bot. Authors need to sign the CLA before a PR can be reviewed. label Jan 2, 2023

fantaosha force-pushed the taoshaf.robot.prismatic_joint branch from 26d2a70 to e9fd82c Compare January 2, 2023 04:55

luisenp requested changes Jan 23, 2023

View reviewed changes

fantaosha force-pushed the taoshaf.robot.revolute_joint branch from 1a2ce88 to 51702ff Compare January 23, 2023 18:37

fantaosha added 2 commits January 23, 2023 10:43

add prismatic joint

8591927

refactor prismatic joint

99ccd46

fantaosha force-pushed the taoshaf.robot.prismatic_joint branch from 635c542 to 99ccd46 Compare January 23, 2023 18:43

mhmukadam reviewed Jan 23, 2023

View reviewed changes

exhaustin reviewed Jan 31, 2023

View reviewed changes

fantaosha and others added 2 commits March 1, 2023 16:25

simplify the code of joints

7683400

fantaosha merged commit a2a104f into taoshaf.robot.revolute_joint Mar 1, 2023

mhmukadam deleted the taoshaf.robot.prismatic_joint branch June 23, 2023 15:57

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add prismatic joint #416

Add prismatic joint #416

fantaosha commented Jan 2, 2023

luisenp left a comment

luisenp Jan 12, 2023

fantaosha Mar 1, 2023

luisenp Jan 23, 2023

fantaosha Mar 1, 2023

luisenp Jan 23, 2023

fantaosha Mar 1, 2023

luisenp Jan 23, 2023

fantaosha Mar 1, 2023 •

edited

Loading

mhmukadam Jan 23, 2023

mhmukadam left a comment

exhaustin Jan 31, 2023

luisenp Feb 1, 2023

Add prismatic joint #416

Add prismatic joint #416

Conversation

fantaosha commented Jan 2, 2023

Motivation and Context

How Has This Been Tested

Types of changes

Checklist

luisenp left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

fantaosha Mar 1, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mhmukadam left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

fantaosha Mar 1, 2023 •

edited

Loading