Add backoff pose to object placement

ethz-asl · Jan 23, 2023 · 7ab6ee9 · 7ab6ee9
1 parent 40d0e04
commit 7ab6ee9
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 0 deletions.
diff --git 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
@@ -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"},
         )