After applying the ik reste the robot to its previous state

cram2 · Jan 11, 2024 · 1d67ec6 · 1d67ec6
1 parent f7009ab
commit 1d67ec6
Showing 1 changed file with 2 additions and 0 deletions.
diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py
@@ -155,7 +155,9 @@ def reachability_validator(pose: Pose,
         # resp = request_ik(base_link, end_effector, target_diff, robot, left_joints)
         resp = request_ik(target, robot, left_joints, left_gripper)
 
+        joint_state_before_ik=robot._current_joint_states
         _apply_ik(robot, resp, left_joints)
+        robot.set_joint_states(joint_state_before_ik)
 
         for obj in BulletWorld.current_bullet_world.objects:
             if obj.name == "floor":