Seed the simulation

.gitignore

@@ -128,8 +128,6 @@ dmypy.json
 # Pyre type checker
 .pyre/
 
-# VSCode
 .vscode/
-
 assets/
 logs/

CMakeLists.txt

@@ -16,6 +16,7 @@ add_message_files(
 add_service_files(
   FILES
   Reset.srv
+  Seed.srv
 )
 
 generate_messages(

active_grasp/baselines.py

@@ -1,10 +1,9 @@
 import numpy as np
 import scipy.interpolate
-from robot_utils.spatial import Transform
 import rospy
 
 from active_grasp.policy import BasePolicy
-from robot_utils.ros import tf
+from robot_helpers.ros import tf
 from vgn.utils import look_at
 
 
@@ -14,9 +13,8 @@ class SingleView(BasePolicy):
     """
 
     def update(self):
-        self.integrate_latest_image()
+        self._integrate_latest_image()
-        self.draw_scene_cloud()
+        self.best_grasp = self._predict_best_grasp()
-        self.best_grasp = self.predict_best_grasp()
         self.done = True
 
 
@@ -37,12 +35,10 @@ class TopView(BasePolicy):
         error = current.translation - self.target.translation
 
         if np.linalg.norm(error) < 0.01:
-            self.best_grasp = self.predict_best_grasp()
+            self.best_grasp = self._predict_best_grasp()
             self.done = True
         else:
-            self.integrate_latest_image()
+            self._integrate_latest_image()
-            self.draw_scene_cloud()
-            self.draw_camera_path()
             return self.target
 
 
@@ -71,12 +67,10 @@ class RandomView(BasePolicy):
         error = current.translation - self.target.translation
 
         if np.linalg.norm(error) < 0.01:
-            self.best_grasp = self.predict_best_grasp()
+            self.best_grasp = self._predict_best_grasp()
             self.done = True
         else:
-            self.integrate_latest_image()
+            self._integrate_latest_image()
-            self.draw_scene_cloud()
-            self.draw_camera_path()
             return self.target
 
 
@@ -101,17 +95,15 @@ class FixedTrajectory(BasePolicy):
     def update(self):
         elapsed_time = (rospy.Time.now() - self.tic).to_sec()
         if elapsed_time > self.duration:
-            self.best_grasp = self.predict_best_grasp()
+            self.best_grasp = self._predict_best_grasp()
             self.done = True
         else:
-            self.integrate_latest_image()
+            self._integrate_latest_image()
             t = self.m(elapsed_time)
             eye = self.circle_center + np.r_[self.r * np.cos(t), self.r * np.sin(t), 0]
             center = (self.bbox.min + self.bbox.max) / 2.0
             up = np.r_[1.0, 0.0, 0.0]
             target = self.T_B_task * (self.T_EE_cam * look_at(eye, center, up)).inv()
-            self.draw_scene_cloud()
-            self.draw_camera_path()
             return target
 
 
@@ -122,9 +114,8 @@ class AlignmentView(BasePolicy):
 
     def activate(self, bbox):
         super().activate(bbox)
-        self.integrate_latest_image()
+        self._integrate_latest_image()
-        self.draw_scene_cloud()
+        self.best_grasp = self._predict_best_grasp()
-        self.best_grasp = self.predict_best_grasp()
         if self.best_grasp:
             R, t = self.best_grasp.rotation, self.best_grasp.translation
             center = t
@@ -139,10 +130,8 @@ class AlignmentView(BasePolicy):
         error = current.translation - self.target.translation
 
         if np.linalg.norm(error) < 0.01:
-            self.best_grasp = self.predict_best_grasp()
+            self.best_grasp = self._predict_best_grasp()
             self.done = True
         else:
-            self.integrate_latest_image()
+            self._integrate_latest_image()
-            self.draw_scene_cloud()
-            self.draw_camera_path()
             return self.target

active_grasp/bbox.py

@@ -0,0 +1,26 @@
+import numpy as np
+
+import active_grasp.msg
+from robot_helpers.ros.conversions import to_point_msg, from_point_msg
+
+
+class AABBox:
+    def __init__(self, bbox_min, bbox_max):
+        self.min = bbox_min
+        self.max = bbox_max
+
+    def is_inside(self, p):
+        return np.all(p > self.min) and np.all(p < self.max)
+
+
+def from_bbox_msg(msg):
+    aabb_min = from_point_msg(msg.min)
+    aabb_max = from_point_msg(msg.max)
+    return AABBox(aabb_min, aabb_max)
+
+
+def to_bbox_msg(bbox):
+    msg = active_grasp.msg.AABBox()
+    msg.min = to_point_msg(bbox.min)
+    msg.max = to_point_msg(bbox.max)
+    return msg

active_grasp/controller.py

@@ -1,64 +1,69 @@
+from geometry_msgs.msg import PoseStamped
 import numpy as np
 import rospy
+import time
 
+from active_grasp.bbox import from_bbox_msg
 from active_grasp.srv import Reset, ResetRequest
-from active_grasp.utils import *
+from robot_helpers.ros.conversions import to_pose_stamped_msg
-from robot_utils.ros.panda import PandaGripperClient
+from robot_helpers.ros.panda import PandaGripperClient
-from robot_utils.spatial import Rotation, Transform
+from robot_helpers.spatial import Rotation, Transform
 
 
 class GraspController:
     def __init__(self, policy):
         self.policy = policy
-        self.controller = CartesianPoseControllerClient()
+        self._reset_env = rospy.ServiceProxy("reset", Reset)
-        self.gripper = PandaGripperClient()
+        self._load_parameters()
-        self.reset_env = rospy.ServiceProxy("reset", Reset)
+        self._init_robot_control()
-        self.load_parameters()
 
-    def load_parameters(self):
+    def _load_parameters(self):
         self.T_G_EE = Transform.from_list(rospy.get_param("~ee_grasp_offset")).inv()
 
-    def run(self):
+    def _init_robot_control(self):
-        bbox = self.reset()
+        self.target_pose_pub = rospy.Publisher("command", PoseStamped, queue_size=10)
-        with Timer("exploration_time"):
+        self.gripper = PandaGripperClient()
-            grasp = self.explore(bbox)
-        with Timer("execution_time"):
-            res = self.execute_grasp(grasp)
-        return self.collect_info(res)
 
-    def reset(self):
+    def _send_cmd(self, pose):
-        req = ResetRequest()
+        msg = to_pose_stamped_msg(pose, "panda_link0")
-        res = self.reset_env(req)
+        self.target_pose_pub.publish(msg)
+
+    def run(self):
+        bbox = self._reset()
+        with Timer("search_time"):
+            grasp = self._search_grasp(bbox)
+        res = self._execute_grasp(grasp)
+        return self._collect_info(res)
+
+    def _reset(self):
+        res = self._reset_env(ResetRequest())
         rospy.sleep(1.0)  # wait for states to be updated
         return from_bbox_msg(res.bbox)
 
-    def explore(self, bbox):
+    def _search_grasp(self, bbox):
         self.policy.activate(bbox)
         r = rospy.Rate(self.policy.rate)
-        while not self.policy.done:
+        while True:
             cmd = self.policy.update()
             if self.policy.done:
                 break
-            self.controller.send_target(cmd)
+            self._send_cmd(cmd)
             r.sleep()
         return self.policy.best_grasp
 
-    def execute_grasp(self, grasp):
+    def _execute_grasp(self, grasp):
         if not grasp:
             return "aborted"
 
-        T_B_G = self.postprocess(grasp)
+        T_B_G = self._postprocess(grasp)
-
         self.gripper.move(0.08)
 
         # Move to an initial pose offset.
-        self.controller.send_target(
+        self._send_cmd(T_B_G * Transform.translation([0, 0, -0.05]) * self.T_G_EE)
-            T_B_G * Transform.translation([0, 0, -0.05]) * self.T_G_EE
-        )
         rospy.sleep(3.0)
 
         # Approach grasp pose.
-        self.controller.send_target(T_B_G * self.T_G_EE)
+        self._send_cmd(T_B_G * self.T_G_EE)
         rospy.sleep(2.0)
 
         # Close the fingers.
@@ -66,7 +71,7 @@ class GraspController:
 
         # Lift the object.
         target = Transform.translation([0, 0, 0.2]) * T_B_G * self.T_G_EE
-        self.controller.send_target(target)
+        self._send_cmd(target)
         rospy.sleep(2.0)
 
         # Check whether the object remains in the hand
@@ -74,22 +79,41 @@ class GraspController:
 
         return "succeeded" if success else "failed"
 
-    def postprocess(self, T_B_G):
+    def _postprocess(self, T_B_G):
         # Ensure that the camera is pointing forward.
         rot = T_B_G.rotation
         if rot.as_matrix()[:, 0][0] < 0:
             T_B_G.rotation = rot * Rotation.from_euler("z", np.pi)
         return T_B_G
 
-    def collect_info(self, result):
+    def _collect_info(self, result):
         points = [p.translation for p in self.policy.viewpoints]
         d = np.sum([np.linalg.norm(p2 - p1) for p1, p2 in zip(points, points[1:])])
-
         info = {
             "result": result,
             "viewpoint_count": len(points),
-            "distance_travelled": d,
+            "distance": d,
         }
-        info.update(self.policy.info)
         info.update(Timer.timers)
         return info
+
+
+class Timer:
+    timers = dict()
+
+    def __init__(self, name):
+        self.name = name
+
+    def __enter__(self):
+        self.start()
+        return self
+
+    def __exit__(self, *exc_info):
+        self.stop()
+
+    def start(self):
+        self.tic = time.perf_counter()
+
+    def stop(self):
+        elapsed_time = time.perf_counter() - self.tic
+        self.timers[self.name] = elapsed_time

active_grasp/policy.py

@@ -2,35 +2,37 @@ import cv_bridge
 import numpy as np
 from pathlib import Path
 import rospy
-from rospy import Publisher
+from sensor_msgs.msg import CameraInfo, Image, PointCloud2
-import sensor_msgs.msg
-from visualization_msgs.msg import Marker, MarkerArray
 
-from robot_utils.perception import Image
+from .visualization import Visualizer
-from robot_utils.ros import tf
+from robot_helpers.ros import tf
-from robot_utils.ros.conversions import *
+from robot_helpers.ros.conversions import *
-from robot_utils.ros.rviz import *
-from robot_utils.spatial import Transform
 from vgn.detection import VGN, compute_grasps
 from vgn.perception import UniformTSDFVolume
 from vgn.utils import *
 
 
-class BasePolicy:
+class Policy:
+    def activate(self, bbox):
+        raise NotImplementedError
+
+    def update(self):
+        raise NotImplementedError
+
+
+class BasePolicy(Policy):
     def __init__(self):
         self.cv_bridge = cv_bridge.CvBridge()
         self.vgn = VGN(Path(rospy.get_param("vgn/model")))
         self.finger_depth = 0.05
-
-        self.load_parameters()
-        self.lookup_transforms()
-        self.connect_to_camera()
-        self.connect_to_rviz()
-
         self.rate = 5
-        self.info = {}
+        self._load_parameters()
+        self._lookup_transforms()
+        self._init_camera_stream()
+        self._init_publishers()
+        self._init_visualizer()
 
-    def load_parameters(self):
+    def _load_parameters(self):
         self.task_frame = rospy.get_param("~frame_id")
         self.base_frame = rospy.get_param("~base_frame_id")
         self.ee_frame = rospy.get_param("~ee_frame_id")
@@ -38,114 +40,64 @@ class BasePolicy:
         self.info_topic = rospy.get_param("~camera/info_topic")
         self.depth_topic = rospy.get_param("~camera/depth_topic")
 
-    def lookup_transforms(self):
+    def _lookup_transforms(self):
-        tf._init_listener()
-        rospy.sleep(1.0)  # wait to receive transforms
         self.T_B_task = tf.lookup(self.base_frame, self.task_frame)
         self.T_EE_cam = tf.lookup(self.ee_frame, self.cam_frame)
 
-    def connect_to_camera(self):
+    def _init_camera_stream(self):
-        msg = rospy.wait_for_message(
+        msg = rospy.wait_for_message(self.info_topic, CameraInfo, rospy.Duration(2.0))
-            self.info_topic, sensor_msgs.msg.CameraInfo, rospy.Duration(2.0)
-        )
         self.intrinsic = from_camera_info_msg(msg)
-        rospy.Subscriber(
+        rospy.Subscriber(self.depth_topic, Image, self._sensor_cb, queue_size=1)
-            self.depth_topic, sensor_msgs.msg.Image, self.sensor_cb, queue_size=1
-        )
 
-    def sensor_cb(self, msg):
+    def _sensor_cb(self, msg):
-        self.img = Image(depth=self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32))
+        self.img = self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32)
-        self.extrinsic = tf.lookup(
+        self.extrinsic = tf.lookup(self.cam_frame, self.task_frame, msg.header.stamp)
-            self.cam_frame,
-            self.task_frame,
-            msg.header.stamp,
-            rospy.Duration(0.2),
-        )
 
-    def connect_to_rviz(self):
+    def _init_publishers(self):
-        self.bbox_pub = Publisher("bbox", Marker, queue_size=1, latch=True)
+        self.scene_cloud_pub = rospy.Publisher("scene_cloud", PointCloud2, queue_size=1)
-        self.cloud_pub = Publisher("cloud", PointCloud2, queue_size=1, latch=True)
+
-        self.path_pub = Publisher("path", MarkerArray, queue_size=1, latch=True)
+    def _init_visualizer(self):
-        self.grasps_pub = Publisher("grasps", MarkerArray, queue_size=1, latch=True)
+        self.visualizer = Visualizer(self.task_frame)
 
     def activate(self, bbox):
-        self.clear_grasps()
         self.bbox = bbox
-        self.draw_bbox()
         self.tsdf = UniformTSDFVolume(0.3, 40)
         self.viewpoints = []
         self.done = False
         self.best_grasp = None  # grasp pose defined w.r.t. the robot's base frame
+        self.visualizer.clear()
+        self.visualizer.bbox(bbox)
 
-    def update(self):
+    def _integrate_latest_image(self):
-        raise NotImplementedError
-
-    def integrate_latest_image(self):
         self.viewpoints.append(self.extrinsic.inv())
         self.tsdf.integrate(
             self.img,
             self.intrinsic,
             self.extrinsic,
         )
+        self._publish_scene_cloud()
+        self.visualizer.path(self.viewpoints)
 
-    def predict_best_grasp(self):
+    def _publish_scene_cloud(self):
+        cloud = self.tsdf.get_scene_cloud()
+        msg = to_cloud_msg(self.task_frame, np.asarray(cloud.points))
+        self.scene_cloud_pub.publish(msg)
+
+    def _predict_best_grasp(self):
         tsdf_grid = self.tsdf.get_grid()
         out = self.vgn.predict(tsdf_grid)
         score_fn = lambda g: g.pose.translation[2]
         grasps = compute_grasps(self.tsdf.voxel_size, out, score_fn, max_filter_size=3)
-        grasps = self.filter_grasps_on_target_object(grasps)
+        grasps = self._select_grasps_on_target_object(grasps)
-        self.draw_grasps(grasps)
         return self.T_B_task * grasps[0].pose if len(grasps) > 0 else None
 
-    def filter_grasps_on_target_object(self, grasps):
+    def _select_grasps_on_target_object(self, grasps):
-        return [
+        result = []
-            g
+        for g in grasps:
-            for g in grasps
+            tip = g.pose.rotation.apply([0, 0, 0.05]) + g.pose.translation
-            if self.bbox.is_inside(
+            if self.bbox.is_inside(tip):
-                g.pose.rotation.apply([0, 0, 0.05]) + g.pose.translation
+                result.append(g)
-            )
+        return result
-        ]
-
-    def clear_grasps(self):
-        self.grasps_pub.publish(DELETE_MARKER_ARRAY_MSG)
-
-    def draw_bbox(self):
-        pose = Transform.translation((self.bbox.min + self.bbox.max) / 2.0)
-        scale = self.bbox.max - self.bbox.min
-        color = np.r_[0.8, 0.2, 0.2, 0.6]
-        msg = create_marker(Marker.CUBE, self.task_frame, pose, scale, color)
-        self.bbox_pub.publish(msg)
-
-    def draw_scene_cloud(self):
-        cloud = self.tsdf.get_scene_cloud()
-        msg = to_cloud_msg(self.task_frame, np.asarray(cloud.points))
-        self.cloud_pub.publish(msg)
-
-    def draw_grasps(self, grasps):
-        msg = create_grasp_marker_array(self.task_frame, grasps, self.finger_depth)
-        self.grasps_pub.publish(msg)
-
-    def draw_camera_path(self):
-        identity = Transform.identity()
-        color = np.r_[31, 119, 180] / 255.0
-
-        # Spheres for each viewpoint
-        scale = 0.01 * np.ones(3)
-        spheres = create_marker(
-            Marker.SPHERE_LIST, self.task_frame, identity, scale, color
-        )
-        spheres.id = 0
-        spheres.points = [to_point_msg(p.translation) for p in self.viewpoints]
-
-        # Line strip connecting viewpoints
-        scale = [0.005, 0.0, 0.0]
-        lines = create_marker(
-            Marker.LINE_STRIP, self.task_frame, identity, scale, color
-        )
-        lines.id = 1
-        lines.points = [to_point_msg(p.translation) for p in self.viewpoints]
-
-        self.path_pub.publish(MarkerArray([spheres, lines]))
 
 
 registry = {}

active_grasp/simulation.py

@@ -1,31 +1,40 @@
 from pathlib import Path
 import pybullet as p
+import pybullet_data
 import rospkg
-import time
 
-from active_grasp.utils import *
+from active_grasp.bbox import AABBox
-from robot_utils.bullet import *
+from robot_helpers.bullet import *
-from robot_utils.spatial import Rotation, Transform
+from robot_helpers.model import Model
+from robot_helpers.spatial import Rotation, Transform
 
 
-class Simulation(BtSim):
+class Simulation:
-    def __init__(self, gui, rng):
+    def __init__(self, gui):
-        super().__init__(gui=gui, sleep=False)
+        self.configure_physics_engine(gui, 60, 4)
-        self.rng = rng
-        self.object_uids = []
-
         self.configure_visualizer()
-        self.find_object_urdfs()
+        self.find_urdfs()
         self.load_table()
         self.load_robot()
         self.load_controller()
+        self.object_uids = []
+
+    def configure_physics_engine(self, gui, rate, sub_step_count):
+        self.rate = rate
+        self.dt = 1.0 / self.rate
+        p.connect(p.GUI if gui else p.DIRECT)
+        p.setAdditionalSearchPath(pybullet_data.getDataPath())
+        p.setPhysicsEngineParameter(fixedTimeStep=self.dt, numSubSteps=sub_step_count)
+        p.setGravity(0.0, 0.0, -9.81)
 
     def configure_visualizer(self):
         # p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
         p.resetDebugVisualizerCamera(1.4, 50, -35, [0.0, 0.0, 0.6])
 
-    def find_object_urdfs(self):
+    def find_urdfs(self):
         rospack = rospkg.RosPack()
+        assets_path = Path(rospack.get_path("active_grasp")) / "assets"
+        self.panda_urdf = assets_path / "urdfs/franka/panda_arm_hand.urdf"
         root = Path(rospack.get_path("vgn")) / "assets/urdfs/packed/test"
         self.urdfs = [str(f) for f in root.iterdir() if f.suffix == ".urdf"]
 
@@ -38,9 +47,9 @@ class Simulation(BtSim):
 
     def load_robot(self):
         self.T_W_B = Transform(Rotation.identity(), np.r_[-0.6, 0.0, 0.4])
-        self.arm = BtPandaArm(self.T_W_B)
+        self.arm = BtPandaArm(self.panda_urdf, self.T_W_B)
         self.gripper = BtPandaGripper(self.arm)
-        self.model = Model(self.arm.urdf_path, self.arm.base_frame, self.arm.ee_frame)
+        self.model = Model(self.panda_urdf, self.arm.base_frame, self.arm.ee_frame)
         self.camera = BtCamera(320, 240, 1.047, 0.1, 1.0, self.arm.uid, 11)
 
     def load_controller(self):
@@ -48,6 +57,9 @@ class Simulation(BtSim):
         x0 = self.model.pose(self.arm.ee_frame, q)
         self.controller = CartesianPoseController(self.model, self.arm.ee_frame, x0)
 
+    def seed(self, seed):
+        self.rng = np.random.default_rng(seed)
+
     def reset(self):
         self.remove_all_objects()
         self.set_initial_arm_configuration()
@@ -55,6 +67,9 @@ class Simulation(BtSim):
         uid = self.select_target()
         return self.get_target_bbox(uid)
 
+    def step(self):
+        p.stepSimulation()
+
     def set_initial_arm_configuration(self):
         q = [
             self.rng.uniform(-0.17, 0.17),  # 0.0
@@ -110,8 +125,8 @@ class Simulation(BtSim):
                 self.remove_object(uid)
 
     def select_target(self):
-        img = self.camera.get_image()
+        _, _, mask = self.camera.get_image()
-        uids, counts = np.unique(img.mask, return_counts=True)
+        uids, counts = np.unique(mask, return_counts=True)
         mask = np.isin(uids, self.object_uids)  # remove ids of the floor, table, etc
         uids, counts = uids[mask], counts[mask]
         target_uid = uids[np.argmin(counts)]

active_grasp/utils.py

@@ -1,71 +0,0 @@
-from datetime import datetime
-from geometry_msgs.msg import PoseStamped
-import pandas as pd
-import rospy
-import time
-
-import active_grasp.msg
-from robot_utils.ros.conversions import *
-
-
-class CartesianPoseControllerClient:
-    def __init__(self, topic="/command"):
-        self.target_pub = rospy.Publisher(topic, PoseStamped, queue_size=10)
-
-    def send_target(self, pose):
-        msg = to_pose_stamped_msg(pose, "panda_link0")
-        self.target_pub.publish(msg)
-
-
-class AABBox:
-    def __init__(self, bbox_min, bbox_max):
-        self.min = bbox_min
-        self.max = bbox_max
-
-    def is_inside(self, p):
-        return np.all(p > self.min) and np.all(p < self.max)
-
-
-def from_bbox_msg(msg):
-    aabb_min = from_point_msg(msg.min)
-    aabb_max = from_point_msg(msg.max)
-    return AABBox(aabb_min, aabb_max)
-
-
-def to_bbox_msg(bbox):
-    msg = active_grasp.msg.AABBox()
-    msg.min = to_point_msg(bbox.min)
-    msg.max = to_point_msg(bbox.max)
-    return msg
-
-
-class Timer:
-    timers = dict()
-
-    def __init__(self, name):
-        self.name = name
-
-    def __enter__(self):
-        self.start()
-        return self
-
-    def __exit__(self, *exc_info):
-        self.stop()
-
-    def start(self):
-        self.tic = time.perf_counter()
-
-    def stop(self):
-        elapsed_time = time.perf_counter() - self.tic
-        self.timers[self.name] = elapsed_time
-
-
-class Logger:
-    def __init__(self, logdir, policy, desc):
-        stamp = datetime.now().strftime("%y%m%d-%H%M%S")
-        name = "{}_policy={},{}".format(stamp, policy, desc).strip(",")
-        self.path = logdir / (name + ".csv")
-
-    def log_run(self, info):
-        df = pd.DataFrame.from_records([info])
-        df.to_csv(self.path, mode="a", header=not self.path.exists(), index=False)

active_grasp/visualization.py

@@ -0,0 +1,49 @@
+import numpy as np
+import rospy
+
+
+from robot_helpers.ros.rviz import *
+from robot_helpers.spatial import Transform
+
+
+class Visualizer:
+    def __init__(self, frame, topic="visualization_marker_array"):
+        self.frame = frame
+        self.marker_pub = rospy.Publisher(topic, MarkerArray, queue_size=1)
+
+    def clear(self):
+        marker = Marker(action=Marker.DELETEALL)
+        self.draw([marker])
+
+    def bbox(self, bbox):
+        pose = Transform.translation((bbox.min + bbox.max) / 2.0)
+        scale = bbox.max - bbox.min
+        color = np.r_[0.8, 0.2, 0.2, 0.6]
+        marker = create_cube_marker(self.frame, pose, scale, color, ns="bbox")
+        self.draw([marker])
+
+    def path(self, poses):
+        color = np.r_[31, 119, 180] / 255.0
+        points = [p.translation for p in poses]
+        spheres = create_sphere_list_marker(
+            self.frame,
+            Transform.identity(),
+            np.full(3, 0.01),
+            color,
+            points,
+            "path",
+            0,
+        )
+        lines = create_line_strip_marker(
+            self.frame,
+            Transform.identity(),
+            [0.005, 0.0, 0.0],
+            color,
+            points,
+            "path",
+            1,
+        )
+        self.draw([spheres, lines])
+
+    def draw(self, markers):
+        self.marker_pub.publish(MarkerArray(markers=markers))

config/active_grasp.rviz

@@ -6,7 +6,7 @@ Panels:
       Expanded:
         - /TF1/Tree1
       Splitter Ratio: 0.5
-    Tree Height: 595
+    Tree Height: 574
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -25,7 +25,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: ""
+    SyncSource: SceneCloud
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -61,7 +61,7 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
-        cam_optical_frame:
+        camera_optical_frame:
           Alpha: 1
           Show Axes: false
           Show Trail: false
@@ -136,8 +136,8 @@ Visualization Manager:
       Frame Timeout: 15
       Frames:
         All Enabled: false
-        cam_optical_frame:
+        camera_optical_frame:
-          Value: false
+          Value: true
         panda_hand:
           Value: false
         panda_leftfinger:
@@ -173,57 +173,27 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
+        panda_link0:
+          panda_link1:
+            panda_link2:
+              panda_link3:
+                panda_link4:
+                  panda_link5:
+                    panda_link6:
+                      panda_link7:
+                        panda_link8:
+                          panda_hand:
+                            camera_optical_frame:
+                              {}
+                            panda_leftfinger:
+                              {}
+                            panda_rightfinger:
+                              {}
         world:
-          panda_link0:
-            panda_link1:
-              panda_link2:
-                panda_link3:
-                  panda_link4:
-                    panda_link5:
-                      panda_link6:
-                        panda_link7:
-                          panda_link8:
-                            panda_hand:
-                              cam_optical_frame:
-                                {}
-                              panda_leftfinger:
-                                {}
-                              panda_rightfinger:
-                                {}
           task:
             {}
       Update Interval: 0
       Value: true
-    - Alpha: 0.20000000298023224
-      Autocompute Intensity Bounds: false
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: Intensity
-      Decay Time: 0
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 4096
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: TSDF
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.007499999832361937
-      Style: Boxes
-      Topic: /tsdf
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -247,7 +217,7 @@ Visualization Manager:
       Size (Pixels): 3
       Size (m): 0.009999999776482582
       Style: Spheres
-      Topic: /cloud
+      Topic: /scene_cloud
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
@@ -268,28 +238,12 @@ Visualization Manager:
       Topic: /command
       Unreliable: false
       Value: true
-    - Class: rviz/Marker
-      Enabled: true
-      Marker Topic: /bbox
-      Name: BBox
-      Namespaces:
-        {}
-      Queue Size: 100
-      Value: true
     - Class: rviz/MarkerArray
       Enabled: true
-      Marker Topic: /path
+      Marker Topic: visualization_marker_array
-      Name: CameraPath
+      Name: Markers
       Namespaces:
-        {}
+        bbox: true
-      Queue Size: 100
-      Value: true
-    - Class: rviz/MarkerArray
-      Enabled: true
-      Marker Topic: /grasps
-      Name: Grasps
-      Namespaces:
-        {}
       Queue Size: 100
       Value: true
   Enabled: true
@@ -320,7 +274,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.5774216651916504
+      Distance: 1.3517695665359497
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -328,25 +282,25 @@ Visualization Manager:
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: 0.2026568502187729
+        X: 0.3073185980319977
-        Y: 0.13089965283870697
+        Y: 0.050485748797655106
-        Z: 0.3290979564189911
+        Z: 0.3944588601589203
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.3197976350784302
+      Pitch: 0.4747979938983917
       Target Frame: <Fixed Frame>
-      Yaw: 5.398510456085205
+      Yaw: 5.098489761352539
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 892
+  Height: 871
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002defc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002de000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004fb0000003efc0100000002fb0000000800540069006d00650100000000000004fb000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000039f000002de00000004000000040000000800000008fc00000002000000020000000000000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002c9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002c9000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000050e0000003efc0100000002fb0000000800540069006d006501000000000000050e000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003b2000002c900000004000000040000000800000008fc00000002000000020000000000000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -355,6 +309,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1275
+  Width: 1294
-  X: 61
+  X: 104
-  Y: 163
+  Y: 374

config/active_grasp.yaml

@@ -1,6 +1,5 @@
 bt_sim:
   gui: True
-  seed: 12
 
 active_grasp:
   frame_id: task

launch/active_grasp.launch

@@ -2,11 +2,12 @@
 <launch>
   <arg name="launch_rviz" default="false" />
 
-  <rosparam command="load" file="$(find active_grasp)launch/active_grasp.yaml" subst_value="true" />
+  <rosparam command="load" file="$(find active_grasp)/config/active_grasp.yaml" subst_value="true" />
+
+  <param name="robot_description" command="$(find xacro)/xacro $(find active_grasp)/assets/urdfs/franka/panda_arm_hand.urdf.xacro" />
 
-  <param name="robot_description" command="$(find xacro)/xacro $(find active_grasp)/assets/urdfs/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" />
 
-  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find active_grasp)/launch/active_grasp.rviz" if="$(arg launch_rviz)" />
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find active_grasp)/config/active_grasp.rviz" if="$(arg launch_rviz)" />
 </launch>

notebooks/print_metrics.py

@@ -1,25 +0,0 @@
-#%%
-import pandas as pd
-
-#%%
-logfile = "../logs/210712-132211_policy=top.csv"
-df = pd.read_csv(logfile)
-
-#%%
-metrics = [
-    ("Runs", len(df.index)),
-    ("", ""),
-    ("Succeeded", (df.result == "succeeded").sum()),
-    ("Failed", (df.result == "failed").sum()),
-    ("Aborted", (df.result == "aborted").sum()),
-    ("", ""),
-    ("Success rate", round((df.result == "succeeded").mean(), 2)),
-    ("Mean time", round(df.exploration_time.mean(), 2)),
-    ("Mean distance", round(df.distance_travelled.mean(), 2)),
-    ("Mean viewpoints", round(df.viewpoint_count.mean())),
-]
-
-for k, v in metrics:
-    print("{:<16} {:>8}".format(k, v))
-
-# %%

requirements.txt

@@ -0,0 +1,4 @@
+numpy
+pandas
+pybullet
+tqdm

scripts/bt_sim_node.py

@@ -10,20 +10,16 @@ from sensor_msgs.msg import JointState, Image, CameraInfo
 from threading import Thread
 import tf2_ros
 
-from active_grasp.srv import Reset, ResetResponse
+from active_grasp.bbox import to_bbox_msg
+from active_grasp.srv import *
 from active_grasp.simulation import Simulation
-from active_grasp.utils import *
+from robot_helpers.ros.conversions import *
-from robot_utils.ros.conversions import *
 
 
 class BtSimNode:
     def __init__(self):
         self.gui = rospy.get_param("~gui", True)
-        seed = rospy.get_param("~seed", None)
+        self.sim = Simulation(gui=self.gui)
-
-        rng = np.random.default_rng(seed) if seed else np.random
-        self.sim = Simulation(gui=self.gui, rng=rng)
-
         self._init_plugins()
         self._advertise_services()
         self._broadcast_transforms()
@@ -33,11 +29,13 @@ class BtSimNode:
             PhysicsPlugin(self.sim),
             JointStatePlugin(self.sim.arm, self.sim.gripper),
             ArmControllerPlugin(self.sim.arm, self.sim.controller),
-            GripperControllerPlugin(self.sim.gripper),
+            MoveActionPlugin(self.sim.gripper),
+            GraspActionPlugin(self.sim.gripper),
             CameraPlugin(self.sim.camera),
         ]
 
     def _advertise_services(self):
+        rospy.Service("seed", Seed, self.seed)
         rospy.Service("reset", Reset, self.reset)
 
     def _broadcast_transforms(self):
@@ -50,14 +48,16 @@ class BtSimNode:
         ]
         self.static_broadcaster.sendTransform(msgs)
 
+    def seed(self, req):
+        self.sim.seed(req.seed)
+        return SeedResponse()
+
     def reset(self, req):
         for plugin in self.plugins:
             plugin.is_running = False
         rospy.sleep(1.0)  # TODO replace with a read-write lock
-
         bbox = self.sim.reset()
         res = ResetResponse(to_bbox_msg(bbox))
-
         for plugin in self.plugins:
             plugin.is_running = True
         return res
@@ -137,45 +137,54 @@ class ArmControllerPlugin(Plugin):
         self.arm.set_desired_joint_velocities(cmd)
 
 
-class GripperControllerPlugin(Plugin):
+class MoveActionPlugin(Plugin):
     def __init__(self, gripper, rate=10):
         super().__init__(rate)
         self.gripper = gripper
         self.dt = 1.0 / self.rate
-        self._init_move_action_server()
+        self._init_action_server()
-        self._init_grasp_action_server()
 
-    def _init_move_action_server(self):
+    def _init_action_server(self):
         name = "/franka_gripper/move"
-        self.move_server = SimpleActionServer(name, MoveAction, auto_start=False)
+        self.action_server = SimpleActionServer(name, MoveAction, auto_start=False)
-        self.move_server.register_goal_callback(self._move_action_goal_cb)
+        self.action_server.register_goal_callback(self._action_goal_cb)
-        self.move_server.start()
+        self.action_server.start()
 
-    def _init_grasp_action_server(self):
+    def _action_goal_cb(self):
-        name = "/franka_gripper/grasp"
+        self.elapsed_time = 0.0
-        self.grasp_server = SimpleActionServer(name, GraspAction, auto_start=False)
+        goal = self.action_server.accept_new_goal()
-        self.grasp_server.register_goal_callback(self._grasp_action_goal_cb)
-        self.grasp_server.start()
-
-    def _move_action_goal_cb(self):
-        self.elapsed_time_since_move_action_goal = 0.0
-        goal = self.move_server.accept_new_goal()
-        self.gripper.set_desired_width(goal.width)
-
-    def _grasp_action_goal_cb(self):
-        self.elapsed_time_since_grasp_action_goal = 0.0
-        goal = self.grasp_server.accept_new_goal()
         self.gripper.set_desired_width(goal.width)
 
     def _update(self):
-        if self.move_server.is_active():
+        if self.action_server.is_active():
-            self.elapsed_time_since_move_action_goal += self.dt
+            self.elapsed_time += self.dt
-            if self.elapsed_time_since_move_action_goal > 1.0:
+            if self.elapsed_time > 1.0:
-                self.move_server.set_succeeded()
+                self.action_server.set_succeeded()
-        if self.grasp_server.is_active():
+
-            self.elapsed_time_since_grasp_action_goal += self.dt
+
-            if self.elapsed_time_since_grasp_action_goal > 1.0:
+class GraspActionPlugin(Plugin):
-                self.grasp_server.set_succeeded()
+    def __init__(self, gripper, rate=10):
+        super().__init__(rate)
+        self.gripper = gripper
+        self.dt = 1.0 / self.rate
+        self._init_action_server()
+
+    def _init_action_server(self):
+        name = "/franka_gripper/grasp"
+        self.action_server = SimpleActionServer(name, GraspAction, auto_start=False)
+        self.action_server.register_goal_callback(self._action_goal_cb)
+        self.action_server.start()
+
+    def _action_goal_cb(self):
+        self.elapsed_time = 0.0
+        goal = self.action_server.accept_new_goal()
+        self.gripper.set_desired_width(goal.width)
+
+    def _update(self):
+        if self.action_server.is_active():
+            self.elapsed_time += self.dt
+            if self.elapsed_time > 1.0:
+                self.action_server.set_succeeded()
 
 
 class CameraPlugin(Plugin):
@@ -201,8 +210,8 @@ class CameraPlugin(Plugin):
         msg.header.stamp = stamp
         self.info_pub.publish(msg)
 
-        img = self.camera.get_image()
+        _, depth, _ = self.camera.get_image()
-        msg = self.cv_bridge.cv2_to_imgmsg(img.depth)
+        msg = self.cv_bridge.cv2_to_imgmsg(depth)
         msg.header.stamp = stamp
         self.depth_pub.publish(msg)
 

scripts/run.py

@@ -1,10 +1,24 @@
 import argparse
+from datetime import datetime
+import pandas as pd
 from pathlib import Path
 import rospy
 from tqdm import tqdm
 
 from active_grasp.controller import *
 from active_grasp.policy import make, registry
+from active_grasp.srv import Seed
+
+
+class Logger:
+    def __init__(self, logdir, policy):
+        stamp = datetime.now().strftime("%y%m%d-%H%M%S")
+        name = "{}_policy={}".format(stamp, policy)
+        self.path = logdir / (name + ".csv")
+
+    def log_run(self, info):
+        df = pd.DataFrame.from_records([info])
+        df.to_csv(self.path, mode="a", header=not self.path.exists(), index=False)
 
 
 def create_parser():
@@ -12,17 +26,24 @@ def create_parser():
     parser.add_argument("policy", type=str, choices=registry.keys())
     parser.add_argument("--runs", type=int, default=10)
     parser.add_argument("--logdir", type=Path, default="logs")
-    parser.add_argument("--desc", type=str, default="")
+    parser.add_argument("--seed", type=int, default=12)
     return parser
 
 
+def seed_simulation(seed):
+    rospy.ServiceProxy("seed", Seed)(seed)
+    rospy.sleep(1.0)
+
+
 def main():
     rospy.init_node("active_grasp")
     parser = create_parser()
     args = parser.parse_args()
     policy = make(args.policy)
     controller = GraspController(policy)
-    logger = Logger(args.logdir, args.policy, args.desc)
+    logger = Logger(args.logdir, args.policy)
+
+    seed_simulation(args.seed)
 
     for _ in tqdm(range(args.runs)):
         info = controller.run()

srv/Seed.srv

@@ -0,0 +1,2 @@
+uint32 seed
+---