[action desigs] ParkArms and MoveGripper handle Arms.BOTH correctly

cram2 · Jan 16, 2025 · 362ced9 · 362ced9
1 parent 3b4f7e5
commit 362ced9
Showing 1 changed file with 15 additions and 19 deletions.
diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py
@@ -165,7 +165,12 @@ class SetGripperActionPerformable(ActionAbstract):
 
     @with_tree
     def plan(self) -> None:
-        MoveGripperMotion(gripper=self.gripper, motion=self.motion).perform()
+        arm_chains = RobotDescription.current_robot_description.get_arm_chain(self.gripper)
+        if type(arm_chains) is not list:
+            MoveGripperMotion(gripper=arm_chains.arm_type, motion=self.motion).perform()
+        else:
+            for chain in arm_chains:
+                MoveGripperMotion(gripper=chain.arm_type, motion=self.motion).perform()
 
 
 @dataclass
@@ -215,24 +220,15 @@ class ParkArmsActionPerformable(ActionAbstract):
 
     @with_tree
     def plan(self) -> None:
-        # create the keyword arguments
-        kwargs = dict()
-        left_poses = None
-        right_poses = None
-
-        # add park left arm if wanted
-        if self.arm in [Arms.LEFT, Arms.BOTH]:
-            kwargs["left_arm_config"] = "park"
-            left_poses = RobotDescription.current_robot_description.get_arm_chain(Arms.LEFT).get_static_joint_states(
-                kwargs["left_arm_config"])
-
-        # add park right arm if wanted
-        if self.arm in [Arms.RIGHT, Arms.BOTH]:
-            kwargs["right_arm_config"] = "park"
-            right_poses = RobotDescription.current_robot_description.get_arm_chain(Arms.RIGHT).get_static_joint_states(
-                kwargs["right_arm_config"])
-
-        MoveArmJointsMotion(left_poses, right_poses).perform()
+        joint_poses = {}
+        arm_chains = RobotDescription.current_robot_description.get_arm_chain(self.arm)
+        if type(arm_chains) is not list:
+            joint_poses = arm_chains.get_static_joint_states("park")
+        else:
+            for arm_chain in RobotDescription.current_robot_description.get_arm_chain(self.arm):
+                joint_poses.update(arm_chain.get_static_joint_states("park"))
+
+        MoveJointsMotion(names=list(joint_poses.keys()), positions=list(joint_poses.values())).perform()
 
 
 @dataclass