From c6126d5a3fbdd9acff8485e4b5bf9f8e14674a0e Mon Sep 17 00:00:00 2001
From: Michel Breyer <michel.p.breyer@gmail.com>
Date: Wed, 13 Oct 2021 15:44:06 +0200
Subject: [PATCH] Make logdir

---
 scripts/run.py                 |  1 +
 src/active_grasp/controller.py | 28 ++++++++++++++--------------
 2 files changed, 15 insertions(+), 14 deletions(-)

diff --git a/scripts/run.py b/scripts/run.py
index 4e4de98..6a2dbc6 100644
--- a/scripts/run.py
+++ b/scripts/run.py
@@ -41,6 +41,7 @@ def create_parser():
 
 class Logger:
     def __init__(self, args):
+        args.logdir.mkdir(parents=True, exist_ok=True)
         stamp = datetime.now().strftime("%y%m%d-%H%M%S")
         name = "{}_policy={},seed={}.csv".format(
             stamp,
diff --git a/src/active_grasp/controller.py b/src/active_grasp/controller.py
index 90c7ce9..1d6d61b 100644
--- a/src/active_grasp/controller.py
+++ b/src/active_grasp/controller.py
@@ -83,7 +83,6 @@ class GraspController:
 
         if grasp:
             self.switch_to_joint_trajectory_control()
-            self.create_collision_scene(grasp)
             with Timer("grasp_time"):
                 res = self.execute_grasp(grasp)
         else:
@@ -137,6 +136,20 @@ class GraspController:
         angular = 0.5 * angular.as_rotvec()
         return np.r_[linear, angular]
 
+    def execute_grasp(self, grasp):
+        self.create_collision_scene(grasp)
+        T_base_grasp = self.postprocess(grasp.pose)
+        self.gripper.move(0.08)
+        self.moveit.goto(T_base_grasp * Transform.t([0, 0, -0.05]) * self.T_grasp_ee)
+        self.moveit.scene.remove_world_object(self.target_co_name)
+        rospy.sleep(0.5)  # Wait for the planning scene to be updated
+        self.moveit.gotoL(T_base_grasp * self.T_grasp_ee)
+        self.gripper.grasp()
+        self.moveit.gotoL(Transform.t([0, 0, 0.05]) * T_base_grasp * self.T_grasp_ee)
+        rospy.sleep(1.0)  # Wait to see whether the object slides out of the hand
+        success = self.gripper.read() > 0.002
+        return "succeeded" if success else "failed"
+
     def create_collision_scene(self, grasp):
         # Segment support plane, cluster, and create collision objects from convex hulls
         cloud = self.policy.tsdf.get_scene_cloud()
@@ -161,19 +174,6 @@ class GraspController:
         co = create_collision_object_from_mesh(name, frame, pose, mesh)
         self.moveit.scene.add_object(co)
 
-    def execute_grasp(self, grasp):
-        T_base_grasp = self.postprocess(grasp.pose)
-        self.gripper.move(0.08)
-        self.moveit.goto(T_base_grasp * Transform.t([0, 0, -0.05]) * self.T_grasp_ee)
-        self.moveit.scene.remove_world_object(self.target_co_name)
-        rospy.sleep(0.5)  # Wait for the planning scene to be updated
-        self.moveit.gotoL(T_base_grasp * self.T_grasp_ee)
-        self.gripper.grasp()
-        self.moveit.gotoL(Transform.t([0, 0, 0.05]) * T_base_grasp * self.T_grasp_ee)
-        rospy.sleep(1.0)  # Wait to see whether the object slides out of the hand
-        success = self.gripper.read() > 0.002
-        return "succeeded" if success else "failed"
-
     def postprocess(self, T_base_grasp):
         rot = T_base_grasp.rotation
         if rot.as_matrix()[:, 0][0] < 0:  # Ensure that the camera is pointing forward