From 7ab6ee925385886efa276ce55a81a1155818de06 Mon Sep 17 00:00:00 2001
From: smb261-panda <grizzi@ethz.ch>
Date: Mon, 23 Jan 2023 15:33:59 +0100
Subject: [PATCH] Add backoff pose to object placement

---
 .../config/state_machine/piloting.yaml         | 18 ++++++++++++++++++
 .../missions/piloting/sequences.py             |  6 ++++++
 2 files changed, 24 insertions(+)

diff --git a/moma_mission/config/state_machine/piloting.yaml b/moma_mission/config/state_machine/piloting.yaml
index 4e7e9364..00d86cdc 100644
--- a/moma_mission/config/state_machine/piloting.yaml
+++ b/moma_mission/config/state_machine/piloting.yaml
@@ -75,6 +75,24 @@ APPROACH_OBJECT:
       "path_passthrough_controller",
     ]
 
+BACKOFF_OBJECT:
+  default_outcome: None
+  input: frame
+  output: pose
+  control_frame: panda_link0
+  target_frame: object_location
+  pose_topic: "/panda/cartesian_impedance_controller/equilibrium_pose"
+  manager_namespace: panda
+  startlist: ["cartesian_impedance_controller"]
+  stoplist:
+    [
+      "mpc_controller",
+      "mpc_wholebody_controller",
+      "joint_space_controller",
+      "path_passthrough_controller",
+    ]
+  offset: [0, 0, -0.2]
+
 APPROACH_TARGET:
   default_outcome: None
   action_name: /joint_space_controller_server
diff --git a/moma_mission/src/moma_mission/missions/piloting/sequences.py b/moma_mission/src/moma_mission/missions/piloting/sequences.py
index e17fd50f..db538f0c 100755
--- a/moma_mission/src/moma_mission/missions/piloting/sequences.py
+++ b/moma_mission/src/moma_mission/missions/piloting/sequences.py
@@ -45,6 +45,12 @@ def object_placement_sequence_factory():
         object_placement_sequence.add(
             "CLOSE_GRIPPER",
             GripperGrasp,
+            transitions={"Completed": "BACKOFF_OBJECT", "Failure": "Failure"},
+        )
+
+        object_placement_sequence.add(
+            "BACKOFF_OBJECT",
+            PathVisitorState,
             transitions={"Completed": "APPROACH_TARGET", "Failure": "Failure"},
         )