Merge pull request #125 from Leusmann/bugfix-retractPose

Bugfix retract pose

src/pycram/designators/action_designator.py

@@ -341,7 +341,7 @@ def perform(self) -> None:
             # MoveTCPMotion(gripper_rotate_pose, self.arm).resolve().perform()
 
             oTg = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame)
-            oTg.pose.position.x -= 0.1 # in x since this is how the gripper is oriented
+            oTg.pose.position.x -= 0.07 # in x since this is how the gripper is oriented
             prepose = object.local_transformer.transform_pose(oTg, "map")
 
             # Perform the motion with the prepose and open gripper
@@ -443,7 +443,8 @@ def perform(self) -> None:
             MoveTCPMotion(target_diff, self.arm).resolve().perform()
             MoveGripperMotion("open", self.arm).resolve().perform()
             BulletWorld.robot.detach(self.object_designator.bullet_world_object)
-            retract_pose = target_diff
+            retract_pose = local_tf.transform_pose(target_diff,BulletWorld.robot.get_link_tf_frame(
+                                                        robot_description.get_tool_frame(self.arm)))
             retract_pose.position.x -= 0.07
             MoveTCPMotion(retract_pose, self.arm).resolve().perform()
 

src/pycram/pose_generator_and_validator.py

@@ -151,6 +151,7 @@ def reachability_validator(pose: Pose,
     if robot in allowed_collision.keys():
         allowed_robot_links = allowed_collision[robot]
 
+    joint_state_before_ik=robot._current_joint_states
     try:
         # resp = request_ik(base_link, end_effector, target_diff, robot, left_joints)
         resp = request_ik(target, robot, left_joints, left_gripper)
@@ -174,6 +175,8 @@ def reachability_validator(pose: Pose,
             res = True
     except IKError:
         pass
+    finally:
+        robot.set_joint_states(joint_state_before_ik)
 
     try:
         # resp = request_ik(base_link, end_effector, target_diff, robot, right_joints)
@@ -198,5 +201,7 @@ def reachability_validator(pose: Pose,
             res = True
     except IKError:
         pass
+    finally:
+        robot.set_joint_states(joint_state_before_ik)
 
     return res, arms