Merge branch 'devel' of github.com:mbreyer/active_grasp into devel

cfg/active_grasp.rviz

@@ -1,11 +1,12 @@
 Panels:
   - Class: rviz/Displays
-    Help Height: 0
+    Help Height: 70
     Name: Displays
     Property Tree Widget:
-      Expanded: ~
-      Splitter Ratio: 0.501006007194519
-    Tree Height: 419
+      Expanded:
+        - /TF1/Frames1
+      Splitter Ratio: 0.6881287693977356
+    Tree Height: 217
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -18,14 +19,13 @@ Panels:
   - Class: rviz/Views
     Expanded:
       - /Current View1
-      - /Current View1/Focal Point1
     Name: Views
     Splitter Ratio: 0.5
   - Class: rviz/Time
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: DepthImage
+    SyncSource: PointCloud
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -56,6 +56,42 @@ Visualization Manager:
       Frame Timeout: 15
       Frames:
         All Enabled: false
+        camera_depth_optical_frame:
+          Value: true
+        panda_EE:
+          Value: false
+        panda_K:
+          Value: false
+        panda_NE:
+          Value: false
+        panda_hand:
+          Value: false
+        panda_leftfinger:
+          Value: false
+        panda_link0:
+          Value: false
+        panda_link1:
+          Value: false
+        panda_link2:
+          Value: false
+        panda_link3:
+          Value: false
+        panda_link4:
+          Value: false
+        panda_link5:
+          Value: false
+        panda_link6:
+          Value: false
+        panda_link7:
+          Value: false
+        panda_link8:
+          Value: false
+        panda_rightfinger:
+          Value: false
+        task:
+          Value: false
+        world:
+          Value: false
       Marker Alpha: 1
       Marker Scale: 0.30000001192092896
       Name: TF
@@ -63,7 +99,27 @@ Visualization Manager:
       Show Axes: true
       Show Names: false
       Tree:
-        {}
+        world:
+          panda_link0:
+            panda_link1:
+              panda_link2:
+                panda_link3:
+                  panda_link4:
+                    panda_link5:
+                      panda_link6:
+                        panda_link7:
+                          camera_depth_optical_frame:
+                            {}
+                          panda_link8:
+                            panda_NE:
+                              panda_EE:
+                                panda_K:
+                                  {}
+                            panda_hand:
+                              panda_leftfinger:
+                                {}
+                              panda_rightfinger:
+                                {}
       Update Interval: 0
       Value: true
     - Alpha: 1
@@ -76,6 +132,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: ""
@@ -84,7 +204,7 @@ Visualization Manager:
       Visual Enabled: true
     - Acceleration_Scaling_Factor: 0.1
       Class: moveit_rviz_plugin/MotionPlanning
-      Enabled: true
+      Enabled: false
       Move Group Namespace: ""
       MoveIt_Allow_Approximate_IK: false
       MoveIt_Allow_External_Program: false
@@ -137,7 +257,7 @@ Visualization Manager:
         Goal State Color: 250; 128; 0
         Interactive Marker Size: 0
         Joint Violation Color: 255; 0; 255
-        Planning Group: hand
+        Planning Group: ""
         Query Goal State: true
         Query Start State: false
         Show Workspace: false
@@ -163,7 +283,7 @@ Visualization Manager:
         Robot Alpha: 1
         Show Robot Collision: false
         Show Robot Visual: true
-      Value: true
+      Value: false
       Velocity_Scaling_Factor: 0.1
     - Class: moveit_rviz_plugin/PlanningScene
       Enabled: false
@@ -190,18 +310,6 @@ Visualization Manager:
         Show Robot Collision: false
         Show Robot Visual: false
       Value: false
-    - Class: rviz/Image
-      Enabled: false
-      Image Topic: /camera/color/image_raw
-      Max Value: 1
-      Median window: 5
-      Min Value: 0
-      Name: ColorImage
-      Normalize Range: true
-      Queue Size: 2
-      Transport Hint: raw
-      Unreliable: false
-      Value: false
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -214,7 +322,7 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: RGB8
       Decay Time: 0
-      Enabled: false
+      Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Min Color: 0; 0; 0
@@ -229,9 +337,21 @@ Visualization Manager:
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
-      Value: false
+      Value: true
     - Class: rviz/Image
       Enabled: true
+      Image Topic: /camera/color/image_raw
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: ColorImage
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+    - Class: rviz/Image
+      Enabled: false
       Image Topic: /camera/depth/image_rect_raw
       Max Value: 1
       Median window: 5
@@ -241,13 +361,25 @@ Visualization Manager:
       Queue Size: 2
       Transport Hint: raw
       Unreliable: false
-      Value: true
+      Value: false
+    - Class: rviz/Image
+      Enabled: false
+      Image Topic: /camera/infra1/image_rect_raw
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: InfraImage
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: false
     - Class: rviz/MarkerArray
       Enabled: true
       Marker Topic: visualization_marker_array
       Name: Markers
       Namespaces:
-        {}
+        bbox: true
       Queue Size: 100
       Value: true
     - Alpha: 1
@@ -366,26 +498,46 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.5308048725128174
+      Distance: 1.2000000476837158
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
-      Field of View: 0.7853981852531433
+      Field of View: 0.7799999713897705
       Focal Point:
-        X: 0.34312182664871216
-        Y: -0.10157600790262222
-        Z: 0.30506056547164917
+        X: 0.44999998807907104
+        Y: 0
+        Z: 0.4000000059604645
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.4247966408729553
+      Pitch: 0.25999999046325684
       Target Frame: <Fixed Frame>
-      Yaw: 1.9687141180038452
-    Saved: ~
+      Yaw: 1.0399999618530273
+    Saved:
+      - Class: rviz/Orbit
+        Distance: 1.2000000476837158
+        Enable Stereo Rendering:
+          Stereo Eye Separation: 0.05999999865889549
+          Stereo Focal Distance: 1
+          Swap Stereo Eyes: false
+          Value: false
+        Field of View: 0.7799999713897705
+        Focal Point:
+          X: 0.44999998807907104
+          Y: 0
+          Z: 0.4000000059604645
+        Focal Shape Fixed Size: false
+        Focal Shape Size: 0.05000000074505806
+        Invert Z Axis: false
+        Name: Panda
+        Near Clip Distance: 0.009999999776482582
+        Pitch: 0.25999999046325684
+        Target Frame: <Fixed Frame>
+        Yaw: 1.0399999618530273
 Window Geometry:
   ColorImage:
     collapsed: false
@@ -393,14 +545,16 @@ Window Geometry:
     collapsed: false
   Displays:
     collapsed: false
-  Height: 837
+  Height: 960
   Hide Left Dock: false
   Hide Right Dock: true
+  InfraImage:
+    collapsed: false
   MotionPlanning:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 000000ff00000000fd00000004000000000000029400000366fc0200000015fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d0000015c000000c900fffffffa000000000100000002fb000000100044006900730070006c0061007900730100000000000001f30000015600fffffffb000000140043006f006c006f00720049006d0061006700650000000000ffffffff0000000000000000fb000000140043006f006c006f00720049006d00610067006501000001e4000001bf0000000000000000fb00000014004400650070007400680049006d006100670065000000029a000001090000001600fffffffb000000140043006f006c006f00720049006d00610067006501000001d2000001920000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a004400650070007400680000000166000000b50000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067000000015b000001af0000017d00fffffffb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c00460069006c0074006500720065006400200043006c006f00750064000000029a000001050000000000000000fb0000000a0049006d00610067006501000002d7000000c80000000000000000fb0000000a0049006d00610067006500000001bc0000008a0000000000000000fb0000000a0049006d006100670065000000021c000000ee0000000000000000fb0000000c00430061006d00650072006100000002db000000c80000000000000000fb000000140043006f006c006f00720049006d006100670065010000019f000002040000001600ffffff000000010000017b00000366fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000366000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000780000000a9fc0100000002fb000000140049006e0066007200610049006d0061006700650200000371000001280000033b00000237fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000056300000039fc0100000002fb0000000800540069006d0065000000000000000563000004f300fffffffb0000000800540069006d00650100000000000004500000000000000000000004e60000036600000004000000040000000800000008fc00000002000000020000000000000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -408,7 +562,7 @@ Window Geometry:
   Tool Properties:
     collapsed: false
   Views:
-    collapsed: false
-  Width: 1098
-  X: 821
+    collapsed: true
+  Width: 1920
+  X: 0
   Y: 27

cfg/active_grasp.yaml

@@ -4,6 +4,10 @@ bt_sim:
   gripper_force: 10
   scene: random
 
+hw:
+  roi_calib_file: $(find active_grasp)/cfg/hw/T_base_tag.txt
+  scene_file: $(find active_grasp)/cfg/hw/scene01.yaml
+
 grasp_controller:
   base_frame_id: panda_link0
   ee_grasp_offset: [0.0, 0.0, -0.383, 0.924, 0.0, 0.0, 0.065]  # offset to panda_link8

cfg/hw/T_base_tag.txt

@@ -0,0 +1,4 @@
+9.998979853190785860e-01 1.338047579286792167e-02 4.998181909874715480e-03 3.216251487106734364e-01
+-1.341414276126873424e-02 9.998871470166096342e-01 6.764170678302552919e-03 -1.756941528330971392e-01
+-4.907110028114804005e-03 -6.830526959274958038e-03 9.999646314608484632e-01 -3.868988796629513338e-02
+0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00

cfg/hw/apriltag.yaml

@@ -0,0 +1,12 @@
+ # AprilTag 3 code parameters
+tag_family:        'tag36h11' # options: tagStandard52h13, tagStandard41h12, tag36h11, tag25h9, tag16h5, tagCustom48h12, tagCircle21h7, tagCircle49h12
+tag_threads:       2          # default: 2
+tag_decimate:      1.0        # default: 1.0
+tag_blur:          0.0        # default: 0.0
+tag_refine_edges:  1          # default: 1
+tag_debug:         0          # default: 0
+publish_tf:        true       # default: false
+
+# Definitions of tags to detect
+standalone_tags: [{id: 0, size: 0.091}]
+tag_bundles: []

cfg/hw/scene01.yaml

@@ -0,0 +1,4 @@
+target:
+  min: [0.1, 0.1, 0.0]
+  max: [0.2, 0.2, 0.1]
+q0: [0.0, -0.79, 0.0, -2.356, 0.0, 1.57, 0.79]

launch/apriltag.launch

@@ -0,0 +1,13 @@
+<launch>
+  <!-- Robot and camera drivers -->
+  <include file="$(find active_grasp)/launch/hw.launch" />
+
+  <!-- AprilTag -->
+  <node pkg="apriltag_ros" type="apriltag_ros_continuous_node" name="apriltag_ros" clear_params="true" output="screen">
+    <rosparam command="load" file="$(find active_grasp)/cfg/hw/apriltag.yaml" />
+    <remap from="image_rect" to="/camera/infra1/image_rect_raw" />
+    <remap from="camera_info" to="/camera/infra1/camera_info" />
+    <param name="camera_frame" type="str" value="camera_depth_optical_frame" />
+    <param name="publish_tag_detections_image" type="bool" value="true" />
+  </node>
+</launch>

launch/env.launch

@@ -1,6 +1,6 @@
 <?xml version="1.0" ?>
 <launch>
-  <arg name="simulation" default="true" />
+  <arg name="sim" />
   <arg name="launch_rviz" default="false" />
 
   <!-- Load parameters -->
@@ -11,14 +11,14 @@
   <include file="$(find panda_moveit_config)/launch/move_group.launch" />
 
   <!-- Simulated environment -->
-  <group if="$(arg simulation)">
+  <group if="$(arg sim)">
     <param name="robot_description" command="$(find xacro)/xacro $(find active_grasp)/assets/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>
 
   <!-- Real environment -->
-  <group unless="$(arg simulation)">
+  <group unless="$(arg sim)">
     <node pkg="active_grasp" type="hw_node.py" name="hw" output="screen" />
   </group>
 

launch/hw.launch

@@ -32,8 +32,10 @@
 
   <!-- Camera -->
   <include file="$(find realsense2_camera)/launch/rs_camera.launch">
+    <arg name="enable_infra1" value="true" />
     <arg name="enable_pointcloud" value="true" />
     <arg name="publish_tf" value="false" />
   </include>
 
+
 </launch>

scripts/bt_sim_node.py

@@ -317,7 +317,7 @@ class CameraPlugin(Plugin):
         if self.cam_noise:
             depth = apply_noise(depth)
 
-        msg = self.cv_bridge.cv2_to_imgmsg(depth)
+        msg = self.cv_bridge.cv2_to_imgmsg((1000 * depth).astype(np.uint16))
         msg.header.stamp = stamp
         self.depth_pub.publish(msg)
 

scripts/calibrate_roi.py

@@ -0,0 +1,17 @@
+#!/usr/bin/env python3
+
+import numpy as np
+import rospy
+
+from robot_helpers.ros import tf
+
+
+def main():
+    rospy.init_node("calibrate_roi")
+    tf.init()
+    T_base_roi = tf.lookup("panda_link0", "tag_0")
+    np.savetxt("cfg/hw/T_base_tag.txt", T_base_roi.as_matrix())
+
+
+if __name__ == "__main__":
+    main()

scripts/hw_node.py

@@ -1,24 +1,40 @@
 #!/usr/bin/env python3
 
 from controller_manager_msgs.srv import *
+import numpy as np
 import rospy
 
 from active_grasp.bbox import AABBox, to_bbox_msg
+from active_grasp.rviz import Visualizer
 from active_grasp.srv import *
-
-
+from robot_helpers.io import load_yaml
 from robot_helpers.ros.moveit import MoveItClient
+from robot_helpers.ros.panda import PandaGripperClient
+from robot_helpers.spatial import Transform
 
 
 class HwNode:
     def __init__(self):
+        self.load_parameters()
+        self.init_robot_connection()
+        self.init_visualizer()
         self.advertise_services()
-        self.switch_controller = rospy.ServiceProxy(
+        rospy.spin()
+
+    def load_parameters(self):
+        self.cfg = rospy.get_param("hw")
+        self.T_base_roi = Transform.from_matrix(np.loadtxt(self.cfg["roi_calib_file"]))
+
+    def init_robot_connection(self):
+        self.gripper = PandaGripperClient()
+        self.switch_to_joint_trajectory_controller = rospy.ServiceProxy(
             "controller_manager/switch_controller", SwitchController
         )
-
         self.moveit = MoveItClient("panda_arm")
-        rospy.spin()
+
+    def init_visualizer(self):
+        self.vis = Visualizer()
+        rospy.Timer(rospy.Duration(1), self.draw_bbox)
 
     def advertise_services(self):
         rospy.Service("seed", Seed, self.seed)
@@ -29,17 +45,33 @@ class HwNode:
         return SeedResponse()
 
     def reset(self, req):
+        q0, bbox = self.load_config()
+
+        # Move to the initial configuration
+        self.switch_to_joint_trajectory_controller()
+        self.moveit.goto(q0)
+        self.gripper.move(0.04)
+
+        return ResetResponse(to_bbox_msg(bbox))
+
+    def load_config(self):
+        scene_config = load_yaml(self.cfg["scene_file"])
+        q0 = scene_config["q0"]
+        bbox_min = self.T_base_roi.apply(scene_config["target"]["min"])
+        bbox_max = self.T_base_roi.apply(scene_config["target"]["max"])
+        bbox = AABBox(bbox_min, bbox_max)
+        return q0, bbox
+
+    def switch_to_joint_trajectory_controller(self):
         req = SwitchControllerRequest()
         req.start_controllers = ["position_joint_trajectory_controller"]
         req.stop_controllers = ["cartesian_velocity_controller"]
+        req.strictness = 1
         self.switch_controller(req)
 
-        # Move to the initial configuration
-        self.moveit.goto([0.0, -0.79, 0.0, -2.356, 0.0, 1.57, 0.79])
-
-        # Detect target
-        bbox = AABBox([0.4, -0.1, 0.0], [0.5, 0.1, 0.1])
-        return ResetResponse(to_bbox_msg(bbox))
+    def draw_bbox(self, event):
+        _, bbox = self.load_config()
+        self.vis.bbox("panda_link0", bbox)
 
 
 def main():

scripts/run.py

@@ -1,3 +1,5 @@
+#!/usr/bin/env python3
+
 import argparse
 from datetime import datetime
 import pandas as pd
@@ -33,7 +35,7 @@ def main():
 def create_parser():
     parser = argparse.ArgumentParser()
     parser.add_argument("policy", type=str, choices=registry.keys())
-    parser.add_argument("--runs", type=int, default=100)
+    parser.add_argument("--runs", type=int, default=1)
     parser.add_argument("--logdir", type=Path, default="logs")
     parser.add_argument("--seed", type=int, default=1)
     return parser

src/active_grasp/controller.py

@@ -59,12 +59,14 @@ class GraspController:
         req = SwitchControllerRequest()
         req.start_controllers = ["cartesian_velocity_controller"]
         req.stop_controllers = ["position_joint_trajectory_controller"]
+        req.strictness = 1
         self.switch_controller(req)
 
     def switch_to_joint_trajectory_control(self):
         req = SwitchControllerRequest()
         req.start_controllers = ["position_joint_trajectory_controller"]
         req.stop_controllers = ["cartesian_velocity_controller"]
+        req.strictness = 1
         self.switch_controller(req)
 
     def init_camera_stream(self):
@@ -85,6 +87,7 @@ class GraspController:
                 res = self.execute_grasp(grasp)
         else:
             res = "aborted"
+        self.gripper.move(0.04)
         return self.collect_info(res)
 
     def reset(self):
@@ -110,7 +113,7 @@ class GraspController:
     def get_state(self):
         q, _ = self.arm.get_state()
         msg = copy.deepcopy(self.latest_depth_msg)
-        img = self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32)
+        img = self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32) * 0.001
         pose = tf.lookup(self.base_frame, self.cam_frame, msg.header.stamp)
         return img, pose, q
 

src/active_grasp/simulation.py

@@ -158,7 +158,7 @@ class Scene:
 class YamlScene(Scene):
     def __init__(self, config_name):
         super().__init__()
-        self.config_path = pkg_root / "cfg" / config_name
+        self.config_path = pkg_root / "cfg/sim" / config_name
 
     def load_config(self):
         self.scene = load_cfg(self.config_path)