From b86f6614e228fd1730ccf81970da7a8dd0fc9399 Mon Sep 17 00:00:00 2001
From: Michel Breyer <10465414+mbreyer@users.noreply.github.com>
Date: Wed, 22 Dec 2021 12:12:09 +0100
Subject: [PATCH] Minor

---
 cfg/active_grasp.rviz          | 82 ++++++++++++++++++++++++++++++----
 launch/env.launch              |  2 +-
 launch/hw.launch               |  4 +-
 src/active_grasp/policy.py     |  2 +-
 src/active_grasp/rviz.py       |  6 +--
 src/active_grasp/simulation.py | 15 ++++---
 6 files changed, 86 insertions(+), 25 deletions(-)

diff --git a/cfg/active_grasp.rviz b/cfg/active_grasp.rviz
index ef0b466..bf77bd9 100644
--- a/cfg/active_grasp.rviz
+++ b/cfg/active_grasp.rviz
@@ -7,7 +7,7 @@ Panels:
         - /TF1/Frames1
         - /Markers1/Namespaces1
       Splitter Ratio: 0.6881287693977356
-    Tree Height: 349
+    Tree Height: 710
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -26,7 +26,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: ""
+    SyncSource: SceneCloud
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -77,6 +77,70 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
+        camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        panda_hand:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_leftfinger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link7:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link8:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_rightfinger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
       Name: RobotModel
       Robot Description: robot_description
       TF Prefix: ""
@@ -95,8 +159,6 @@ Visualization Manager:
       MoveIt_Planning_Time: 5
       MoveIt_Use_Cartesian_Path: false
       MoveIt_Use_Constraint_Aware_IK: false
-      MoveIt_Warehouse_Host: 127.0.0.1
-      MoveIt_Warehouse_Port: 33829
       MoveIt_Workspace:
         Center:
           X: 0
@@ -220,7 +282,7 @@ Visualization Manager:
       Use rainbow: true
       Value: true
     - Class: rviz/Image
-      Enabled: true
+      Enabled: false
       Image Topic: /camera/color/image_raw
       Max Value: 1
       Median window: 5
@@ -230,7 +292,7 @@ Visualization Manager:
       Queue Size: 2
       Transport Hint: raw
       Unreliable: false
-      Value: true
+      Value: false
     - Class: rviz/Image
       Enabled: false
       Image Topic: /camera/depth/image_rect_raw
@@ -260,7 +322,11 @@ Visualization Manager:
       Marker Topic: visualization_marker_array
       Name: Markers
       Namespaces:
-        {}
+        bbox: true
+        grasp: true
+        ig_views: true
+        path: true
+        views: true
       Queue Size: 100
       Value: true
     - Alpha: 1
@@ -435,7 +501,7 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
diff --git a/launch/env.launch b/launch/env.launch
index 1413d43..62bbd9b 100644
--- a/launch/env.launch
+++ b/launch/env.launch
@@ -12,7 +12,7 @@
 
   <!-- Simulated environment -->
   <group if="$(arg sim)">
-    <param name="robot_description" command="$(find xacro)/xacro $(find active_grasp)/assets/franka/panda_arm_hand.urdf.xacro" />
+    <param name="robot_description" command="$(find xacro)/xacro $(find active_grasp)/assets/urdfs/franka/panda_arm_hand.urdf.xacro" />
     <node pkg="active_grasp" type="bt_sim_node.py" name="bt_sim" output="screen" />
     <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
   </group>
diff --git a/launch/hw.launch b/launch/hw.launch
index 213a100..6aaa505 100644
--- a/launch/hw.launch
+++ b/launch/hw.launch
@@ -3,7 +3,7 @@
   <arg name="robot_ip" default="172.16.0.2" />
   <arg name="load_gripper" default="true" />
 
-  <param name="robot_description" command="$(find xacro)/xacro $(find active_grasp)/assets/franka/panda_arm_hand.urdf.xacro" />
+  <param name="robot_description" command="$(find xacro)/xacro $(find active_grasp)/assets/franka/urdfs/panda_arm_hand.urdf.xacro" />
 
   <!-- Panda control nodes -->
   <node name="franka_control" pkg="franka_control" type="franka_control_node" output="screen" required="true">
@@ -36,6 +36,4 @@
     <arg name="enable_pointcloud" value="true" />
     <arg name="publish_tf" value="false" />
   </include>
-
-
 </launch>
diff --git a/src/active_grasp/policy.py b/src/active_grasp/policy.py
index 65d4f1f..746af8e 100644
--- a/src/active_grasp/policy.py
+++ b/src/active_grasp/policy.py
@@ -69,7 +69,7 @@ class Policy:
 
     def calibrate_task_frame(self):
         xyz = np.r_[self.bbox.center[:2] - 0.15, self.bbox.min[2] - 0.05]
-        self.T_base_task = Transform.translation(xyz)
+        self.T_base_task = Transform.t(xyz)
         self.T_task_base = self.T_base_task.inv()
         tf.broadcast(self.T_base_task, self.base_frame, self.task_frame)
         rospy.sleep(1.0)  # Wait for tf tree to be updated
diff --git a/src/active_grasp/rviz.py b/src/active_grasp/rviz.py
index 4243fc3..2330045 100644
--- a/src/active_grasp/rviz.py
+++ b/src/active_grasp/rviz.py
@@ -92,11 +92,7 @@ class Visualizer(vgn.rviz.Visualizer):
 
     def point(self, frame, point):
         marker = create_sphere_marker(
-            frame,
-            Transform.translation(point),
-            np.full(3, 0.01),
-            [0, 0, 1],
-            "point",
+            frame, Transform.t(point), np.full(3, 0.01), [0, 0, 1], "point"
         )
         self.draw([marker])
 
diff --git a/src/active_grasp/simulation.py b/src/active_grasp/simulation.py
index fad78dc..4c008a0 100644
--- a/src/active_grasp/simulation.py
+++ b/src/active_grasp/simulation.py
@@ -5,17 +5,18 @@ import rospkg
 
 from active_grasp.bbox import AABBox
 from robot_helpers.bullet import *
+from robot_helpers.io import load_yaml
 from robot_helpers.model import KDLModel
 from robot_helpers.spatial import Rotation
 from vgn.perception import UniformTSDFVolume
-from vgn.utils import find_urdfs, load_cfg, view_on_sphere
+from vgn.utils import find_urdfs, view_on_sphere
 from vgn.detection import VGN, select_local_maxima
 
 # import vgn.visualizer as vis
 
 rospack = rospkg.RosPack()
 pkg_root = Path(rospack.get_path("active_grasp"))
-assets_dir = pkg_root / "assets"
+urdfs_dir = pkg_root / "assets/urdfs"
 
 
 class Simulation:
@@ -45,7 +46,7 @@ class Simulation:
         self.rng = np.random.default_rng(seed) if seed else np.random
 
     def load_robot(self):
-        panda_urdf_path = assets_dir / "franka/panda_arm_hand.urdf"
+        panda_urdf_path = urdfs_dir / "franka/panda_arm_hand.urdf"
         self.arm = BtPandaArm(panda_urdf_path)
         self.gripper = BtPandaGripper(self.arm)
         self.model = KDLModel.from_urdf_file(
@@ -124,7 +125,7 @@ class Simulation:
 
 class Scene:
     def __init__(self):
-        self.support_urdf = assets_dir / "plane/model.urdf"
+        self.support_urdf = urdfs_dir / "plane/model.urdf"
         self.support_uid = -1
         self.object_uids = []
 
@@ -161,7 +162,7 @@ class YamlScene(Scene):
         self.config_path = pkg_root / "cfg/sim" / config_name
 
     def load_config(self):
-        self.scene = load_cfg(self.config_path)
+        self.scene = load_yaml(self.config_path)
         self.center = np.asarray(self.scene["center"])
         self.length = 0.3
         self.origin = self.center - np.r_[0.5 * self.length, 0.5 * self.length, 0.0]
@@ -170,7 +171,7 @@ class YamlScene(Scene):
         self.load_config()
         self.add_support(self.center)
         for object in self.scene["objects"]:
-            urdf = assets_dir / object["object_id"] / "model.urdf"
+            urdf = urdfs_dir / object["object_id"] / "model.urdf"
             ori = Rotation.from_euler("xyz", object["rpy"], degrees=True)
             pos = self.center + np.asarray(object["xyz"])
             scale = object.get("scale", 1)
@@ -191,7 +192,7 @@ class RandomScene(Scene):
         self.center = np.r_[0.5, 0.0, 0.2]
         self.length = 0.3
         self.origin = self.center - np.r_[0.5 * self.length, 0.5 * self.length, 0.0]
-        self.object_urdfs = find_urdfs(assets_dir / "test")
+        self.object_urdfs = find_urdfs(urdfs_dir / "test")
 
     def generate(self, rng, object_count=4, attempts=10):
         self.add_support(self.center)