From b4f68e78cc37771c940710368a02b26f98b3f7f9 Mon Sep 17 00:00:00 2001 From: Michel Breyer Date: Fri, 3 Sep 2021 22:39:17 +0200 Subject: [PATCH] Switch to velocity control --- active_grasp/baselines.py | 38 ++++++++++++++++-------------- active_grasp/controller.py | 22 ++++++++---------- active_grasp/policy.py | 47 ++++++++++++++++++++++---------------- active_grasp/simulation.py | 36 ----------------------------- cfg/active_grasp.yaml | 5 +++- scripts/bt_sim_node.py | 29 +++++++++++++++-------- scripts/run.py | 2 +- 7 files changed, 83 insertions(+), 96 deletions(-) diff --git a/active_grasp/baselines.py b/active_grasp/baselines.py index 010389e..67246f6 100644 --- a/active_grasp/baselines.py +++ b/active_grasp/baselines.py @@ -7,48 +7,50 @@ from vgn.utils import look_at class InitialView(SingleViewPolicy): - def update(self, img, extrinsic): - self.target = extrinsic - super().update(img, extrinsic) + def update(self, img, pose): + self.x_d = pose + super().update(img, pose) class TopView(SingleViewPolicy): def activate(self, bbox): super().activate(bbox) - eye = np.r_[self.center[:2], self.center[2] + 0.3] + eye = np.r_[self.center[:2], self.center[2] + self.min_z_dist] up = np.r_[1.0, 0.0, 0.0] - self.target = look_at(eye, self.center, up) + self.x_d = look_at(eye, self.center, up) class TopTrajectory(MultiViewPolicy): def activate(self, bbox): super().activate(bbox) - eye = np.r_[self.center[:2], self.center[2] + 0.3] + eye = np.r_[self.center[:2], self.center[2] + self.min_z_dist] up = np.r_[1.0, 0.0, 0.0] - self.target = look_at(eye, self.center, up) + self.x_d = look_at(eye, self.center, up) - def update(self, img, extrinsic): - self.integrate(img, extrinsic) - if np.linalg.norm(extrinsic.translation - self.target.translation) < 0.01: + def update(self, img, x): + self.integrate(img, x) + linear, angular = self.compute_error(self.x_d, x) + if np.linalg.norm(linear) < 0.01: self.done = True else: - return self.target + return self.compute_velocity_cmd(linear, angular) class CircularTrajectory(MultiViewPolicy): def __init__(self, rate): super().__init__(rate) self.r = 0.1 - self.h = 0.3 - self.duration = 12.0 - self.m = scipy.interpolate.interp1d([0, self.duration], [np.pi, 3.0 * np.pi]) + self.h = self.min_z_dist + self.duration = 2.0 * np.pi * self.r / self.linear_vel + 2.0 + self.m = scipy.interpolate.interp1d([0.0, self.duration], [np.pi, 3.0 * np.pi]) def activate(self, bbox): super().activate(bbox) self.tic = rospy.Time.now() - def update(self, img, extrinsic): - self.integrate(img, extrinsic) + def update(self, img, x): + self.integrate(img, x) + elapsed_time = (rospy.Time.now() - self.tic).to_sec() if elapsed_time > self.duration: self.done = True @@ -56,4 +58,6 @@ class CircularTrajectory(MultiViewPolicy): t = self.m(elapsed_time) eye = self.center + np.r_[self.r * np.cos(t), self.r * np.sin(t), self.h] up = np.r_[1.0, 0.0, 0.0] - return look_at(eye, self.center, up) + x_d = look_at(eye, self.center, up) + linear, angular = self.compute_error(x_d, x) + return self.compute_velocity_cmd(linear, angular) diff --git a/active_grasp/controller.py b/active_grasp/controller.py index a81032f..a3e602f 100644 --- a/active_grasp/controller.py +++ b/active_grasp/controller.py @@ -1,6 +1,6 @@ import copy import cv_bridge -from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import Twist import numpy as np import rospy from sensor_msgs.msg import Image @@ -35,13 +35,9 @@ class GraspController: self.T_ee_cam = tf.lookup(self.ee_frame, self.cam_frame) def init_robot_connection(self): - self.target_pose_pub = rospy.Publisher("command", PoseStamped, queue_size=10) + self.cartesian_vel_pub = rospy.Publisher("command", Twist, queue_size=10) self.gripper = PandaGripperClient() - def send_cmd(self, pose): - msg = to_pose_stamped_msg(pose, self.base_frame) - self.target_pose_pub.publish(msg) - def init_camera_stream(self): self.cv_bridge = cv_bridge.CvBridge() rospy.Subscriber(self.depth_topic, Image, self.sensor_cb, queue_size=1) @@ -65,21 +61,23 @@ class GraspController: self.policy.activate(bbox) r = rospy.Rate(self.policy.rate) while True: - img, extrinsic = self.get_state() - next_extrinsic = self.policy.update(img, extrinsic) + img, pose = self.get_state() + cmd = self.policy.update(img, pose) if self.policy.done: break - self.send_cmd((self.T_ee_cam * next_extrinsic).inv()) - r.sleep() + else: + self.cartesian_vel_pub.publish(to_twist_msg(cmd)) + r.sleep() return self.policy.best_grasp def get_state(self): msg = copy.deepcopy(self.latest_depth_msg) img = self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32) - extrinsic = tf.lookup(self.cam_frame, self.base_frame, msg.header.stamp) - return img, extrinsic + pose = tf.lookup(self.base_frame, self.cam_frame, msg.header.stamp) + return img, pose def execute_grasp(self, grasp): + return if not grasp: return "aborted" diff --git a/active_grasp/policy.py b/active_grasp/policy.py index 5aa2ec0..748d94d 100644 --- a/active_grasp/policy.py +++ b/active_grasp/policy.py @@ -1,5 +1,4 @@ import numpy as np -from numpy.core.fromnumeric import sort from sensor_msgs.msg import CameraInfo from pathlib import Path import rospy @@ -19,9 +18,13 @@ class Policy: self.init_visualizer() def load_parameters(self): - self.base_frame = rospy.get_param("active_grasp/base_frame_id") + self.base_frame = rospy.get_param("~base_frame_id") + info_topic = rospy.get_param("~camera/info_topic") + self.linear_vel = rospy.get_param("~linear_vel") + self.min_z_dist = rospy.get_param("~camera/min_z_dist") + self.qual_threshold = rospy.get_param("~qual_threshold") self.task_frame = "task" - info_topic = rospy.get_param("active_grasp/camera/info_topic") + msg = rospy.wait_for_message(info_topic, CameraInfo, rospy.Duration(2.0)) self.intrinsic = from_camera_info_msg(msg) @@ -30,15 +33,11 @@ class Policy: def activate(self, bbox): self.bbox = bbox - self.calibrate_task_frame() - self.vis.clear() self.vis.bbox(self.base_frame, bbox) - self.tsdf = UniformTSDFVolume(0.3, 40) self.vgn = VGN(Path(rospy.get_param("vgn/model"))) - self.views = [] self.best_grasp = None self.done = False @@ -50,6 +49,15 @@ class Policy: tf.broadcast(self.T_base_task, self.base_frame, self.task_frame) rospy.sleep(0.1) + def compute_error(self, x_d, x): + linear = x_d.translation - x.translation + angular = (x_d.rotation * x.rotation.inv()).as_rotvec() + return linear, angular + + def compute_velocity_cmd(self, linear, angular): + linear = linear / np.linalg.norm(linear) * self.linear_vel + return np.r_[linear, angular] + def sort_grasps(self, in_grasps): # Transforms grasps into base frame, checks whether they lie on the target, and sorts by their score grasps, scores = [], [] @@ -68,20 +76,19 @@ class Policy: def score_fn(self, grasp): return grasp.quality - # return grasp.pose.translation[2] - def update(sekf, img, extrinsic): + def update(self, img, pose): raise NotImplementedError class SingleViewPolicy(Policy): - def update(self, img, extrinsic): - error = extrinsic.translation - self.target.translation + def update(self, img, x): + linear, angular = self.compute_error(self.x_d, x) - if np.linalg.norm(error) < 0.01: - self.views.append(extrinsic.inv()) + if np.linalg.norm(linear) < 0.01: + self.views.append(x) - self.tsdf.integrate(img, self.intrinsic, extrinsic * self.T_base_task) + self.tsdf.integrate(img, self.intrinsic, x.inv() * self.T_base_task) tsdf_grid, voxel_size = self.tsdf.get_grid(), self.tsdf.voxel_size self.vis.scene_cloud(self.task_frame, self.tsdf.get_scene_cloud()) self.vis.map_cloud(self.task_frame, self.tsdf.get_map_cloud()) @@ -89,7 +96,7 @@ class SingleViewPolicy(Policy): out = self.vgn.predict(tsdf_grid) self.vis.quality(self.task_frame, voxel_size, out.qual) - grasps = select_grid(voxel_size, out, threshold=0.90) + grasps = select_grid(voxel_size, out, threshold=self.qual_threshold) grasps, scores = self.sort_grasps(grasps) self.vis.grasps(self.base_frame, grasps, scores) @@ -97,13 +104,13 @@ class SingleViewPolicy(Policy): self.best_grasp = grasps[0] if len(grasps) > 0 else None self.done = True else: - return self.target + return self.compute_velocity_cmd(linear, angular) class MultiViewPolicy(Policy): - def integrate(self, img, extrinsic): - self.views.append(extrinsic.inv()) - self.tsdf.integrate(img, self.intrinsic, extrinsic * self.T_base_task) + def integrate(self, img, x): + self.views.append(x) + self.tsdf.integrate(img, self.intrinsic, x.inv() * self.T_base_task) self.vis.scene_cloud(self.task_frame, self.tsdf.get_scene_cloud()) self.vis.map_cloud(self.task_frame, self.tsdf.get_map_cloud()) @@ -112,7 +119,7 @@ class MultiViewPolicy(Policy): tsdf_grid, voxel_size = self.tsdf.get_grid(), self.tsdf.voxel_size out = self.vgn.predict(tsdf_grid) - grasps = select_grid(voxel_size, out, threshold=0.90) + grasps = select_grid(voxel_size, out, threshold=self.qual_threshold) grasps, scores = self.sort_grasps(grasps) if len(grasps) > 0: diff --git a/active_grasp/simulation.py b/active_grasp/simulation.py index a63a983..2133a31 100644 --- a/active_grasp/simulation.py +++ b/active_grasp/simulation.py @@ -16,7 +16,6 @@ class Simulation: 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): @@ -52,11 +51,6 @@ class Simulation: 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): - q, _ = self.arm.get_state() - 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) @@ -84,8 +78,6 @@ class Simulation: p.resetJointState(self.arm.uid, i, q_i, 0) p.resetJointState(self.arm.uid, 9, 0.04, 0) p.resetJointState(self.arm.uid, 10, 0.04, 0) - x0 = self.model.pose(self.arm.ee_frame, q) - self.controller.x_d = x0 def load_object(self, urdf, ori, pos, scale=1.0): uid = p.loadURDF(str(urdf), pos, ori.as_quat(), globalScaling=scale) @@ -139,31 +131,3 @@ class Simulation: aabb_min = np.array(aabb_min) - self.T_world_base.translation aabb_max = np.array(aabb_max) - self.T_world_base.translation return AABBox(aabb_min, aabb_max) - - -class CartesianPoseController: - def __init__(self, model, frame, x0): - self.model = model - self.frame = frame - - self.kp = np.ones(6) * 4.0 - self.max_linear_vel = 0.05 - self.max_angular_vel = 1.57 - - self.x_d = x0 - - def update(self, q): - x = self.model.pose(self.frame, q) - error = np.zeros(6) - error[:3] = self.x_d.translation - x.translation - error[3:] = (self.x_d.rotation * x.rotation.inv()).as_rotvec() - dx = self.limit_rate(self.kp * error) - J_pinv = np.linalg.pinv(self.model.jacobian(self.frame, q)) - cmd = np.dot(J_pinv, dx) - return cmd - - def limit_rate(self, dx): - linear, angular = dx[:3], dx[3:] - linear = np.clip(linear, -self.max_linear_vel, self.max_linear_vel) - angular = np.clip(angular, -self.max_angular_vel, self.max_angular_vel) - return np.r_[linear, angular] diff --git a/cfg/active_grasp.yaml b/cfg/active_grasp.yaml index 9fb367e..4973be2 100644 --- a/cfg/active_grasp.yaml +++ b/cfg/active_grasp.yaml @@ -3,14 +3,17 @@ bt_sim: cam_noise: True calib_error: True -active_grasp: +grasp_controller: base_frame_id: panda_link0 ee_frame_id: panda_hand ee_grasp_offset: [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.065] + linear_vel: 0.1 + qual_threshold: 0.9 camera: frame_id: camera_optical_frame info_topic: /camera/depth/camera_info depth_topic: /camera/depth/image_raw + min_z_dist: 0.22 vgn: model: $(find vgn)/assets/models/vgn_conv.pth diff --git a/scripts/bt_sim_node.py b/scripts/bt_sim_node.py index 4351f6b..5dffab2 100755 --- a/scripts/bt_sim_node.py +++ b/scripts/bt_sim_node.py @@ -3,7 +3,7 @@ from actionlib import SimpleActionServer import cv_bridge from franka_gripper.msg import * -from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import Twist import numpy as np import rospy from sensor_msgs.msg import JointState, Image, CameraInfo @@ -27,7 +27,7 @@ class BtSimNode: self.plugins = [ PhysicsPlugin(self.sim), JointStatePlugin(self.sim.arm, self.sim.gripper), - ArmControllerPlugin(self.sim.arm, self.sim.controller), + CartesianVelocityControllerPlugin(self.sim.arm, self.sim.model), MoveActionPlugin(self.sim.gripper), GraspActionPlugin(self.sim.gripper), CameraPlugin(self.sim.camera), @@ -46,6 +46,7 @@ class BtSimNode: plugin.is_running = False rospy.sleep(1.0) # TODO replace with a read-write lock bbox = self.sim.reset() + self.plugins[2].dx_d = np.zeros(6) res = ResetResponse(to_bbox_msg(bbox)) for plugin in self.plugins: plugin.is_running = True @@ -109,22 +110,32 @@ class JointStatePlugin(Plugin): self.pub.publish(msg) -class ArmControllerPlugin(Plugin): - def __init__(self, arm, controller, rate=30): +class CartesianVelocityControllerPlugin(Plugin): + def __init__(self, arm, model, rate=30): super().__init__(rate) self.arm = arm - self.controller = controller - rospy.Subscriber("command", PoseStamped, self.target_cb) + self.model = model + self.max_linear_vel = 0.1 + self.max_angular_vel = 1.57 + self.dx_d = np.zeros(6) + rospy.Subscriber("command", Twist, self.target_cb) def target_cb(self, msg): - assert msg.header.frame_id == self.arm.base_frame - self.controller.x_d = from_pose_msg(msg.pose) + self.dx_d = from_twist_msg(msg) def update(self): q, _ = self.arm.get_state() - cmd = self.controller.update(q) + dx = self.limit_rate(self.dx_d) + J_pinv = np.linalg.pinv(self.model.jacobian(q)) + cmd = np.dot(J_pinv, dx) self.arm.set_desired_joint_velocities(cmd) + def limit_rate(self, dx): + linear, angular = dx[:3], dx[3:] + linear = np.clip(linear, -self.max_linear_vel, self.max_linear_vel) + angular = np.clip(angular, -self.max_angular_vel, self.max_angular_vel) + return np.r_[linear, angular] + class MoveActionPlugin(Plugin): def __init__(self, gripper, rate=10): diff --git a/scripts/run.py b/scripts/run.py index b75e93d..81a0c14 100644 --- a/scripts/run.py +++ b/scripts/run.py @@ -11,7 +11,7 @@ from active_grasp.srv import Seed def main(): - rospy.init_node("active_grasp") + rospy.init_node("grasp_controller") parser = create_parser() args = parser.parse_args()