Integrate cuRobo into action primitives & primitives refactoring #1027
Conversation
This file contains 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
…checks in batched mode; generalize primitives to dual-arm setting
ChengshuLi
reviewed
Dec 6, 2024
| @@ -228,7 +228,7 @@ def __init__( | |||
| num_trajopt_seeds=4, | |||
| num_graph_seeds=4, | |||
| interpolation_dt=0.03, | |||
| collision_activation_distance=0.005, | |||
| collision_activation_distance=0.05, | |||
There was a problem hiding this comment.
should we make this configurable from the outside? Also, 0.05 seems a bit too large, not sure.
Comment on lines
516
to
522
| # Add the pose cost metric | ||
| if motion_constraint is None: | ||
| motion_constraint = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] | ||
| pose_cost_metric = lazy.curobo.wrap.reacher.motion_gen.PoseCostMetric( | ||
| hold_partial_pose=False, hold_vec_weight=self._tensor_args.to_device(motion_constraint) | ||
| ) | ||
| plan_cfg.pose_cost_metric = pose_cost_metric |
There was a problem hiding this comment.
can you add some comments to explain what this means?
for example, if someone passes in motion constraint [1, 0, 0, 0, 0 ,0], what does it mean?
Comment on lines
+599
to
+601
| world_objects_pose_offset=lazy.curobo.types.math.Pose.from_list( | ||
| [0, 0, 0.01, 1, 0, 0, 0], self._tensor_args | ||
| ), |
There was a problem hiding this comment.
hmm I still prefer using scale rather than world_objects_pose_offset. IMO "shrinking" the object is still better than "lifting" the object.
| arm_left: | ||
| name: JointController | ||
| subsume_controllers: [trunk] | ||
| # subsume_controllers: [trunk] |
| use_impedances: true | ||
| pos_kp: 200 | ||
| pos_ki: 0.5 | ||
| max_integral_error: 10.0 |
Comment on lines
+1474
to
+1480
| if isinstance(self.robot, Fetch): | ||
| pose = { | ||
| self.arm: ( | ||
| th.tensor([0.48688125, -0.12507881, 0.97888719]), | ||
| th.tensor([0.61324748, 0.61305553, -0.35266518, 0.35173529]), | ||
| ) | ||
| } |
Comment on lines
+1481
to
+1513
| elif isinstance(self.robot, R1): | ||
| pose = { | ||
| self.robot.arm_names[0]: ( | ||
| th.tensor([0.43, 0.2, 1.2]), | ||
| th.tensor([1.0, 0.0, 0.0, 0.0]), | ||
| ), | ||
| self.robot.arm_names[1]: ( | ||
| th.tensor([0.43, -0.2, 1.2]), | ||
| th.tensor([-1.0, 0.0, 0.0, 0.0]), | ||
| ), | ||
| } | ||
| elif isinstance(self.robot, Tiago): | ||
| # TODO: default trunk position vs. raised trunk position | ||
| # pose = { | ||
| # self.robot.arm_names[0]: ( | ||
| # th.tensor([0.4997, 0.2497, 0.6357]), | ||
| # th.tensor([-0.5609, 0.5617, 0.4299, 0.4302]), | ||
| # ), | ||
| # self.robot.arm_names[1]: ( | ||
| # th.tensor([0.4978, -0.2521, 0.6357]), | ||
| # th.tensor([-0.5609, -0.5617, 0.4299, -0.4302]), | ||
| # ), | ||
| # } | ||
| pose = { | ||
| self.robot.arm_names[0]: ( | ||
| th.tensor([0.5021, 0.2458, 0.7648]), | ||
| th.tensor([-0.5599, 0.5649, 0.4303, 0.4269]), | ||
| ), | ||
| self.robot.arm_names[1]: ( | ||
| th.tensor([0.4999, -0.2486, 0.7633]), | ||
| th.tensor([-0.5592, -0.5646, 0.4311, -0.4274]), | ||
| ), | ||
| } |
There was a problem hiding this comment.
is this the same as the reset joint pos?
| @@ -1584,12 +1571,12 @@ def _get_reset_joint_pos(self): | |||
| 4.50000000e-02, | |||
| ] | |||
| ) | |||
| if self.robot_model == "Fetch": | |||
| if isinstance(self.robot, Fetch): | |||
| context=context, | ||
| ) | ||
| pose_3d = self._get_robot_pose_from_2d_pose(pose_2d) | ||
| # TODO: why was this 0.1? |
| @@ -2042,12 +2049,13 @@ def _settle_robot(self): | |||
| Returns: | |||
| th.tensor or None: Action array for one step for the robot to do nothing | |||
| """ | |||
| # TODO: fix empty action | |||
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.
tiago_primitives_demo.mov