Switch to velocity control

active_grasp/baselines.py

@@ -7,48 +7,50 @@ from vgn.utils import look_at
 
 
 class InitialView(SingleViewPolicy):
-    def update(self, img, extrinsic):
+    def update(self, img, pose):
-        self.target = extrinsic
+        self.x_d = pose
-        super().update(img, extrinsic)
+        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):
+    def update(self, img, x):
-        self.integrate(img, extrinsic)
+        self.integrate(img, x)
-        if np.linalg.norm(extrinsic.translation - self.target.translation) < 0.01:
+        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.h = self.min_z_dist
-        self.duration = 12.0
+        self.duration = 2.0 * np.pi * self.r / self.linear_vel + 2.0
-        self.m = scipy.interpolate.interp1d([0, self.duration], [np.pi, 3.0 * np.pi])
+        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):
+    def update(self, img, x):
-        self.integrate(img, extrinsic)
+        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)

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()
+            img, pose = self.get_state()
-            next_extrinsic = self.policy.update(img, extrinsic)
+            cmd = self.policy.update(img, pose)
             if self.policy.done:
                 break
-            self.send_cmd((self.T_ee_cam * next_extrinsic).inv())
+            else:
-            r.sleep()
+                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)
+        pose = tf.lookup(self.base_frame, self.cam_frame, msg.header.stamp)
-        return img, extrinsic
+        return img, pose
 
     def execute_grasp(self, grasp):
+        return
         if not grasp:
             return "aborted"
 

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):
+    def update(self, img, x):
-        error = extrinsic.translation - self.target.translation
+        linear, angular = self.compute_error(self.x_d, x)
 
-        if np.linalg.norm(error) < 0.01:
+        if np.linalg.norm(linear) < 0.01:
-            self.views.append(extrinsic.inv())
+            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):
+    def integrate(self, img, x):
-        self.views.append(extrinsic.inv())
+        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)
 
         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:

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]

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

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):
+class CartesianVelocityControllerPlugin(Plugin):
-    def __init__(self, arm, controller, rate=30):
+    def __init__(self, arm, model, rate=30):
         super().__init__(rate)
         self.arm = arm
-        self.controller = controller
+        self.model = model
-        rospy.Subscriber("command", PoseStamped, self.target_cb)
+        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.dx_d = from_twist_msg(msg)
-        self.controller.x_d = from_pose_msg(msg.pose)
 
     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):

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()