From c821f22523031980c8a71c7bf1e23696db84ebdc Mon Sep 17 00:00:00 2001 From: Michel Breyer Date: Sun, 12 Sep 2021 00:21:58 +0200 Subject: [PATCH] Use KDL for checking IKs --- active_grasp/baselines.py | 4 +- active_grasp/controller.py | 11 +---- active_grasp/nbv.py | 2 +- active_grasp/policy.py | 12 ++++- active_grasp/simulation.py | 10 +++-- launch/active_grasp.rviz | 90 +++++++++++++++++++++++--------------- 6 files changed, 76 insertions(+), 53 deletions(-) diff --git a/active_grasp/baselines.py b/active_grasp/baselines.py index dffb2d2..4e8b356 100644 --- a/active_grasp/baselines.py +++ b/active_grasp/baselines.py @@ -13,14 +13,14 @@ class TopView(SingleViewPolicy): def activate(self, bbox, view_sphere): super().activate(bbox, view_sphere) self.x_d = self.view_sphere.get_view(0.0, 0.0) - self.done = False if self.view_sphere.is_feasible(self.x_d) else True + self.done = False if self.is_feasible(self.x_d) else True class TopTrajectory(MultiViewPolicy): def activate(self, bbox, view_sphere): super().activate(bbox, view_sphere) self.x_d = self.view_sphere.get_view(0.0, 0.0) - self.done = False if self.view_sphere.is_feasible(self.x_d) else True + self.done = False if self.is_feasible(self.x_d) else True def update(self, img, x): self.integrate(img, x) diff --git a/active_grasp/controller.py b/active_grasp/controller.py index 44c3c19..1653a89 100644 --- a/active_grasp/controller.py +++ b/active_grasp/controller.py @@ -90,7 +90,7 @@ class GraspController: return from_bbox_msg(res.bbox) def search_grasp(self, bbox): - self.view_sphere = ViewHalfSphere(bbox, self.min_z_dist, self.moveit) + self.view_sphere = ViewHalfSphere(bbox, self.min_z_dist) self.policy.activate(bbox, self.view_sphere) timer = rospy.Timer(rospy.Duration(1.0 / self.control_rate), self.send_vel_cmd) r = rospy.Rate(self.policy_rate) @@ -160,12 +160,9 @@ class GraspController: class ViewHalfSphere: - # TODO move feasibility check to a robot interface module - def __init__(self, bbox, min_z_dist, moveit): + def __init__(self, bbox, min_z_dist): self.center = bbox.center self.r = 0.5 * bbox.size[2] + min_z_dist - self.moveit = moveit - self.T_cam_ee = tf.lookup("camera_depth_optical_frame", "panda_link8") def get_view(self, theta, phi): eye = self.center + spherical_to_cartesian(self.r, theta, phi) @@ -174,7 +171,3 @@ class ViewHalfSphere: def sample_view(self): raise NotImplementedError - - def is_feasible(self, view): - success, _ = self.moveit.plan(view * self.T_cam_ee) - return success diff --git a/active_grasp/nbv.py b/active_grasp/nbv.py index 3b9b38a..293a86d 100644 --- a/active_grasp/nbv.py +++ b/active_grasp/nbv.py @@ -20,7 +20,7 @@ class NextBestView(MultiViewPolicy): self.view_candidates = [] for theta, phi in itertools.product(thetas, phis): view = self.view_sphere.get_view(theta, phi) - if self.view_sphere.is_feasible(view): + if self.is_feasible(view): self.view_candidates.append(view) def update(self, img, x): diff --git a/active_grasp/policy.py b/active_grasp/policy.py index 017ab2d..02bf507 100644 --- a/active_grasp/policy.py +++ b/active_grasp/policy.py @@ -4,6 +4,7 @@ from pathlib import Path import rospy from .visualization import Visualizer +from robot_helpers.model import KDLModel from robot_helpers.ros import tf from robot_helpers.ros.conversions import * from vgn.detection import * @@ -11,21 +12,30 @@ from vgn.perception import UniformTSDFVolume class Policy: - def __init__(self, rate=5): + def __init__(self): self.load_parameters() + self.init_robot_model() self.init_visualizer() def load_parameters(self): self.base_frame = rospy.get_param("~base_frame_id") + self.cam_frame = rospy.get_param("~camera/frame_id") self.task_frame = "task" info_topic = rospy.get_param("~camera/info_topic") msg = rospy.wait_for_message(info_topic, CameraInfo, rospy.Duration(2.0)) self.intrinsic = from_camera_info_msg(msg) self.qual_threshold = rospy.get_param("vgn/qual_threshold") + def init_robot_model(self): + self.model = KDLModel.from_parameter_server(self.base_frame, self.cam_frame) + def init_visualizer(self): self.vis = Visualizer() + def is_feasible(self, view, q_init=None): + q_init = q_init if q_init else [0.0, -0.79, 0.0, -2.356, 0.0, 1.57, 0.79] + return self.model.ik(q_init, view) is not None + def activate(self, bbox, view_sphere): self.vis.clear() diff --git a/active_grasp/simulation.py b/active_grasp/simulation.py index ac56332..59eef06 100644 --- a/active_grasp/simulation.py +++ b/active_grasp/simulation.py @@ -6,7 +6,7 @@ import rospkg from active_grasp.bbox import AABBox from robot_helpers.bullet import * -from robot_helpers.model import Model +from robot_helpers.model import KDLModel from robot_helpers.spatial import Rotation @@ -36,10 +36,12 @@ class Simulation: def load_robot(self): path = Path(rospack.get_path("active_grasp")) - panda_urdf = path / "assets/urdfs/franka/panda_arm_hand.urdf" - self.arm = BtPandaArm(panda_urdf) + panda_urdf_path = path / "assets/urdfs/franka/panda_arm_hand.urdf" + self.arm = BtPandaArm(panda_urdf_path) self.gripper = BtPandaGripper(self.arm) - self.model = Model(panda_urdf, self.arm.base_frame, self.arm.ee_frame) + self.model = KDLModel.from_urdf_file( + panda_urdf_path, self.arm.base_frame, self.arm.ee_frame + ) self.camera = BtCamera(320, 240, 0.96, 0.01, 1.0, self.arm.uid, 11) def seed(self, seed): diff --git a/launch/active_grasp.rviz b/launch/active_grasp.rviz index b86d753..1514465 100644 --- a/launch/active_grasp.rviz +++ b/launch/active_grasp.rviz @@ -6,7 +6,7 @@ Panels: Expanded: - /TF1/Frames1 Splitter Ratio: 0.5 - Tree Height: 338 + Tree Height: 755 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -25,7 +25,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: DepthImage + SyncSource: SceneCloud Preferences: PromptSaveOnExit: true Toolbars: @@ -51,6 +51,18 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true + - 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 - Class: rviz/Image Enabled: false Image Topic: /camera/depth/image_rect_raw @@ -98,7 +110,12 @@ Visualization Manager: Marker Topic: visualization_marker_array Name: Markers Namespaces: - {} + bbox: true + best_grasp: true + grasps: true + path: true + views: true + workspace: true Queue Size: 100 Value: true - Acceleration_Scaling_Factor: 0.1 @@ -134,6 +151,10 @@ 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 @@ -239,6 +260,10 @@ 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 @@ -397,6 +422,10 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false panda_hand: Alpha: 1 Show Axes: false @@ -498,12 +527,10 @@ Visualization Manager: All Enabled: false camera_depth_optical_frame: Value: true - panda_EE: - Value: true - panda_K: + camera_optical_frame: Value: true panda_hand: - Value: false + Value: true panda_leftfinger: Value: false panda_link0: @@ -526,6 +553,8 @@ Visualization Manager: Value: false panda_rightfinger: Value: false + task: + Value: true world: Value: false Marker Alpha: 1 @@ -545,30 +574,19 @@ Visualization Manager: panda_link6: panda_link7: panda_link8: - camera_depth_optical_frame: - {} - panda_EE: - panda_K: - {} panda_hand: + camera_depth_optical_frame: + {} + camera_optical_frame: + {} panda_leftfinger: {} panda_rightfinger: {} + task: + {} Update Interval: 0 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 Enabled: true Global Options: Background Color: 48; 48; 48 @@ -597,7 +615,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.8815758228302002 + Distance: 1.4805835485458374 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -605,17 +623,17 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 0.4086047410964966 - Y: 0.23318199813365936 - Z: 0.28523021936416626 + X: 0.2569250166416168 + Y: 0.09482234716415405 + Z: 0.4617196023464203 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.3297969102859497 + Pitch: 0.31479594111442566 Target Frame: - Yaw: 5.052877902984619 + Yaw: 5.353035926818848 Saved: ~ Window Geometry: ColorImage: @@ -624,14 +642,14 @@ Window Geometry: collapsed: false Displays: collapsed: false - Height: 960 + Height: 969 Hide Left Dock: false - Hide Right Dock: true + Hide Right Dock: false MotionPlanning: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -640,6 +658,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1920 - X: 0 - Y: 27 + Width: 1436 + X: 911 + Y: 73