hofee/nbv_sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Switch to velocity control

Browse Source
This commit is contained in:
Michel Breyer 2021-09-03 22:39:17 +02:00
parent 81588a1075
commit b4f68e78cc
7 changed files with 83 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
active_grasp/baselines.py
View File

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

22
active_grasp/controller.py
View File

@ -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"

47
active_grasp/policy.py
View File

@ -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:

36
active_grasp/simulation.py
View File

@ -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]

5
cfg/active_grasp.yaml
View File

@ -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

29
scripts/bt_sim_node.py
View File

@ -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):

2
scripts/run.py
View File

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