diff --git a/.gitignore b/.gitignore index 9783f60..14c2d93 100644 --- a/.gitignore +++ b/.gitignore @@ -128,8 +128,6 @@ dmypy.json # Pyre type checker .pyre/ -# VSCode .vscode/ - assets/ logs/ diff --git a/CMakeLists.txt b/CMakeLists.txt index a8d5ae4..576ceb9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,7 @@ add_message_files( add_service_files( FILES Reset.srv + Seed.srv ) generate_messages( diff --git a/active_grasp/baselines.py b/active_grasp/baselines.py index bea4382..a4b0820 100644 --- a/active_grasp/baselines.py +++ b/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.draw_scene_cloud() - self.best_grasp = self.predict_best_grasp() + self._integrate_latest_image() + 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.draw_scene_cloud() - self.draw_camera_path() + self._integrate_latest_image() 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.draw_scene_cloud() - self.draw_camera_path() + self._integrate_latest_image() 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.draw_scene_cloud() - self.best_grasp = self.predict_best_grasp() + self._integrate_latest_image() + 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.draw_scene_cloud() - self.draw_camera_path() + self._integrate_latest_image() return self.target diff --git a/active_grasp/bbox.py b/active_grasp/bbox.py new file mode 100644 index 0000000..2cccd04 --- /dev/null +++ b/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 diff --git a/active_grasp/controller.py b/active_grasp/controller.py index c531405..ce05e4c 100644 --- a/active_grasp/controller.py +++ b/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_utils.ros.panda import PandaGripperClient -from robot_utils.spatial import Rotation, Transform +from robot_helpers.ros.conversions import to_pose_stamped_msg +from robot_helpers.ros.panda import PandaGripperClient +from robot_helpers.spatial import Rotation, Transform class GraspController: def __init__(self, policy): self.policy = policy - self.controller = CartesianPoseControllerClient() - self.gripper = PandaGripperClient() - self.reset_env = rospy.ServiceProxy("reset", Reset) - self.load_parameters() + self._reset_env = rospy.ServiceProxy("reset", Reset) + self._load_parameters() + self._init_robot_control() - 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): - bbox = self.reset() - with Timer("exploration_time"): - grasp = self.explore(bbox) - with Timer("execution_time"): - res = self.execute_grasp(grasp) - return self.collect_info(res) + def _init_robot_control(self): + self.target_pose_pub = rospy.Publisher("command", PoseStamped, queue_size=10) + self.gripper = PandaGripperClient() - def reset(self): - req = ResetRequest() - res = self.reset_env(req) + def _send_cmd(self, pose): + msg = to_pose_stamped_msg(pose, "panda_link0") + 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( - T_B_G * Transform.translation([0, 0, -0.05]) * self.T_G_EE - ) + self._send_cmd(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 diff --git a/active_grasp/policy.py b/active_grasp/policy.py index 6f18dcd..c27d5c7 100644 --- a/active_grasp/policy.py +++ b/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 -import sensor_msgs.msg -from visualization_msgs.msg import Marker, MarkerArray +from sensor_msgs.msg import CameraInfo, Image, PointCloud2 -from robot_utils.perception import Image -from robot_utils.ros import tf -from robot_utils.ros.conversions import * -from robot_utils.ros.rviz import * -from robot_utils.spatial import Transform +from .visualization import Visualizer +from robot_helpers.ros import tf +from robot_helpers.ros.conversions import * 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): - tf._init_listener() - rospy.sleep(1.0) # wait to receive transforms + def _lookup_transforms(self): 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): - msg = rospy.wait_for_message( - self.info_topic, sensor_msgs.msg.CameraInfo, rospy.Duration(2.0) - ) + def _init_camera_stream(self): + msg = rospy.wait_for_message(self.info_topic, CameraInfo, rospy.Duration(2.0)) self.intrinsic = from_camera_info_msg(msg) - rospy.Subscriber( - self.depth_topic, sensor_msgs.msg.Image, self.sensor_cb, queue_size=1 - ) + rospy.Subscriber(self.depth_topic, Image, self._sensor_cb, queue_size=1) - def sensor_cb(self, msg): - self.img = Image(depth=self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32)) - self.extrinsic = tf.lookup( - self.cam_frame, - self.task_frame, - msg.header.stamp, - rospy.Duration(0.2), - ) + def _sensor_cb(self, msg): + self.img = self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32) + self.extrinsic = tf.lookup(self.cam_frame, self.task_frame, msg.header.stamp) - def connect_to_rviz(self): - self.bbox_pub = Publisher("bbox", Marker, queue_size=1, latch=True) - self.cloud_pub = Publisher("cloud", PointCloud2, queue_size=1, latch=True) - self.path_pub = Publisher("path", MarkerArray, queue_size=1, latch=True) - self.grasps_pub = Publisher("grasps", MarkerArray, queue_size=1, latch=True) + def _init_publishers(self): + self.scene_cloud_pub = rospy.Publisher("scene_cloud", PointCloud2, queue_size=1) + + def _init_visualizer(self): + 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): - raise NotImplementedError - - def integrate_latest_image(self): + 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) - self.draw_grasps(grasps) + grasps = self._select_grasps_on_target_object(grasps) return self.T_B_task * grasps[0].pose if len(grasps) > 0 else None - def filter_grasps_on_target_object(self, grasps): - return [ - g - for g in grasps - if self.bbox.is_inside( - g.pose.rotation.apply([0, 0, 0.05]) + g.pose.translation - ) - ] - - 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])) + def _select_grasps_on_target_object(self, grasps): + result = [] + for g in grasps: + tip = g.pose.rotation.apply([0, 0, 0.05]) + g.pose.translation + if self.bbox.is_inside(tip): + result.append(g) + return result registry = {} diff --git a/active_grasp/simulation.py b/active_grasp/simulation.py index e0813ef..52007b6 100644 --- a/active_grasp/simulation.py +++ b/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 robot_utils.bullet import * -from robot_utils.spatial import Rotation, Transform +from active_grasp.bbox import AABBox +from robot_helpers.bullet import * +from robot_helpers.model import Model +from robot_helpers.spatial import Rotation, Transform -class Simulation(BtSim): - def __init__(self, gui, rng): - super().__init__(gui=gui, sleep=False) - self.rng = rng - self.object_uids = [] - +class Simulation: + def __init__(self, gui): + self.configure_physics_engine(gui, 60, 4) 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() - uids, counts = np.unique(img.mask, return_counts=True) + _, _, mask = self.camera.get_image() + 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)] diff --git a/active_grasp/utils.py b/active_grasp/utils.py deleted file mode 100644 index 0822c86..0000000 --- a/active_grasp/utils.py +++ /dev/null @@ -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) diff --git a/active_grasp/visualization.py b/active_grasp/visualization.py new file mode 100644 index 0000000..7b6acd5 --- /dev/null +++ b/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)) diff --git a/launch/active_grasp.rviz b/config/active_grasp.rviz similarity index 75% rename from launch/active_grasp.rviz rename to config/active_grasp.rviz index 7bd4919..fc77e2a 100644 --- a/launch/active_grasp.rviz +++ b/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: - Value: false + camera_optical_frame: + 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 - Name: CameraPath + Marker Topic: visualization_marker_array + Name: Markers Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /grasps - Name: Grasps - Namespaces: - {} + bbox: true 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 - Y: 0.13089965283870697 - Z: 0.3290979564189911 + X: 0.3073185980319977 + Y: 0.050485748797655106 + 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: - 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 - X: 61 - Y: 163 + Width: 1294 + X: 104 + Y: 374 diff --git a/launch/active_grasp.yaml b/config/active_grasp.yaml similarity index 97% rename from launch/active_grasp.yaml rename to config/active_grasp.yaml index a1cd0e8..e089e71 100644 --- a/launch/active_grasp.yaml +++ b/config/active_grasp.yaml @@ -1,6 +1,5 @@ bt_sim: gui: True - seed: 12 active_grasp: frame_id: task diff --git a/launch/active_grasp.launch b/launch/active_grasp.launch index a034301..6d11885 100644 --- a/launch/active_grasp.launch +++ b/launch/active_grasp.launch @@ -2,11 +2,12 @@ - + + + - - + diff --git a/notebooks/print_metrics.py b/notebooks/print_metrics.py deleted file mode 100644 index d2b9c58..0000000 --- a/notebooks/print_metrics.py +++ /dev/null @@ -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)) - -# %% diff --git a/requirements.txt b/requirements.txt index e69de29..30f69db 100644 --- a/requirements.txt +++ b/requirements.txt @@ -0,0 +1,4 @@ +numpy +pandas +pybullet +tqdm diff --git a/scripts/bt_sim_node.py b/scripts/bt_sim_node.py index 0b068dd..e43f32d 100755 --- a/scripts/bt_sim_node.py +++ b/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_utils.ros.conversions import * +from robot_helpers.ros.conversions import * class BtSimNode: def __init__(self): self.gui = rospy.get_param("~gui", True) - seed = rospy.get_param("~seed", None) - - rng = np.random.default_rng(seed) if seed else np.random - self.sim = Simulation(gui=self.gui, rng=rng) - + self.sim = Simulation(gui=self.gui) 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_grasp_action_server() + self._init_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.move_server.register_goal_callback(self._move_action_goal_cb) - self.move_server.start() + self.action_server = SimpleActionServer(name, MoveAction, auto_start=False) + self.action_server.register_goal_callback(self._action_goal_cb) + self.action_server.start() - def _init_grasp_action_server(self): - name = "/franka_gripper/grasp" - self.grasp_server = SimpleActionServer(name, GraspAction, auto_start=False) - 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() + 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.move_server.is_active(): - self.elapsed_time_since_move_action_goal += self.dt - if self.elapsed_time_since_move_action_goal > 1.0: - self.move_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: - self.grasp_server.set_succeeded() + if self.action_server.is_active(): + self.elapsed_time += self.dt + if self.elapsed_time > 1.0: + self.action_server.set_succeeded() + + +class GraspActionPlugin(Plugin): + 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() - msg = self.cv_bridge.cv2_to_imgmsg(img.depth) + _, depth, _ = self.camera.get_image() + msg = self.cv_bridge.cv2_to_imgmsg(depth) msg.header.stamp = stamp self.depth_pub.publish(msg) diff --git a/scripts/run.py b/scripts/run.py index ade6b07..c7b5213 100644 --- a/scripts/run.py +++ b/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() diff --git a/srv/Seed.srv b/srv/Seed.srv new file mode 100644 index 0000000..275de7f --- /dev/null +++ b/srv/Seed.srv @@ -0,0 +1,2 @@ +uint32 seed +---