Minor

2021-12-22 12:12:09 +01:00 · 2021-12-22 12:12:09 +01:00 · b86f6614e2
commit b86f6614e2
parent 3062fc68d7
6 changed files with 86 additions and 25 deletions
--- 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:
--- 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>
--- 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>
--- 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
--- 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])

--- 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)