def execute_grasp(self, grasp):
T_task_grasp = grasp.pose
T_base_grasp = self.T_base_task * T_task_grasp
T_grasp_pregrasp = Transform(Rotation.identity(), [0.0, 0.0, -0.05])
T_grasp_retreat = Transform(Rotation.identity(), [0.0, 0.0, -0.05])
T_base_pregrasp = T_base_grasp * T_grasp_pregrasp
T_base_retreat = T_base_grasp * T_grasp_retreat
self.pc.goto_pose(T_base_pregrasp * self.T_tcp_tool0, velocity_scaling=0.2)
self.approach_grasp(T_base_grasp)
if self.robot_error:
return False
self.pc.grasp(width=0.0, force=20.0)
if self.robot_error:
return False
self.pc.goto_pose(T_base_retreat * self.T_tcp_tool0)
# lift hand
T_retreat_lift_base = Transform(Rotation.identity(), [0.0, 0.0, 0.1])
T_base_lift = T_retreat_lift_base * T_base_retreat
self.pc.goto_pose(T_base_lift * self.T_tcp_tool0)
if self.gripper_width > 0.004:
return True
else:
return False
def apply_transform(voxel_grid, orientation, position):
angle = np.pi / 2.0 * np.random.choice(4)
R_augment = Rotation.from_rotvec(np.r_[0.0, 0.0, angle])
z_offset = np.random.uniform(6, 34) - position[2]
t_augment = np.r_[0.0, 0.0, z_offset]
T_augment = Transform(R_augment, t_augment)
T_center = Transform(Rotation.identity(), np.r_[20.0, 20.0, 20.0])
T = T_center * T_augment * T_center.inverse()
# transform voxel grid
T_inv = T.inverse()
matrix, offset = T_inv.rotation.as_matrix(), T_inv.translation
voxel_grid[0] = ndimage.affine_transform(voxel_grid[0],
matrix,
offset,
order=0)
# transform grasp pose
position = T.transform_point(position)
orientation = T.rotation * orientation
return voxel_grid, orientation, position
def acquire_tsdf(self, n, N=None):
"""Render synthetic depth images from n viewpoints and integrate into a TSDF.
If N is None, the n viewpoints are equally distributed on circular trajectory.
If N is given, the first n viewpoints on a circular trajectory consisting of N points are rendered.
"""
tsdf = TSDFVolume(self.size, 40)
high_res_tsdf = TSDFVolume(self.size, 120)
origin = Transform(Rotation.identity(), np.r_[self.size / 2,
self.size / 2, 0])
r = 2.0 * self.size
theta = np.pi / 6.0
N = N if N else n
phi_list = 2.0 * np.pi * np.arange(n) / N
extrinsics = [
camera_on_sphere(origin, r, theta, phi) for phi in phi_list
]
timing = 0.0
for extrinsic in extrinsics:
depth_img = self.camera.render(extrinsic)[1]
tic = time.time()
tsdf.integrate(depth_img, self.camera.intrinsic, extrinsic)
timing += time.time() - tic
high_res_tsdf.integrate(depth_img, self.camera.intrinsic,
extrinsic)
return tsdf, high_res_tsdf.get_cloud(), timing
def define_workspace(self):
z_offset = -0.06
t_tag_task = np.r_[[-0.5 * self.size, -0.5 * self.size, z_offset]]
T_tag_task = Transform(Rotation.identity(), t_tag_task)
self.T_base_task = T_base_tag * T_tag_task
self.tf_tree.broadcast_static(self.T_base_task, self.base_frame_id, "task")
rospy.sleep(1.0) # wait for the TF to be broadcasted
def __init__(self, world):
self.world = world
self.urdf_path = Path("data/urdfs/panda/hand.urdf")
self.max_opening_width = 0.08
self.finger_depth = 0.05
self.T_body_tcp = Transform(Rotation.identity(), [0.0, 0.0, 0.022])
self.T_tcp_body = self.T_body_tcp.inverse()
def execute_grasp(self, grasp, remove=True, allow_contact=False):
T_world_grasp = grasp.pose
T_grasp_pregrasp = Transform(Rotation.identity(), [0.0, 0.0, -0.05])
T_world_pregrasp = T_world_grasp * T_grasp_pregrasp
approach = T_world_grasp.rotation.as_matrix()[:, 2]
angle = np.arccos(np.dot(approach, np.r_[0.0, 0.0, -1.0]))
if angle > np.pi / 3.0:
# side grasp, lift the object after establishing a grasp
T_grasp_pregrasp_world = Transform(Rotation.identity(),
[0.0, 0.0, 0.1])
T_world_retreat = T_grasp_pregrasp_world * T_world_grasp
else:
T_grasp_retreat = Transform(Rotation.identity(), [0.0, 0.0, -0.1])
T_world_retreat = T_world_grasp * T_grasp_retreat
self.gripper.reset(T_world_pregrasp)
if self.gripper.detect_contact():
result = Label.FAILURE, self.gripper.max_opening_width
else:
self.gripper.move_tcp_xyz(T_world_grasp, abort_on_contact=True)
if self.gripper.detect_contact() and not allow_contact:
result = Label.FAILURE, self.gripper.max_opening_width
else:
self.gripper.move(0.0)
self.gripper.move_tcp_xyz(T_world_retreat,
abort_on_contact=False)
if self.check_success(self.gripper):
result = Label.SUCCESS, self.gripper.read()
if remove:
contacts = self.world.get_contacts(self.gripper.body)
self.world.remove_body(contacts[0].bodyB)
else:
result = Label.FAILURE, self.gripper.max_opening_width
self.world.remove_body(self.gripper.body)
if remove:
self.remove_and_wait()
return result
def place_table(self, height):
urdf = self.urdf_root / "setup" / "plane.urdf"
pose = Transform(Rotation.identity(), [0.15, 0.15, height])
self.world.load_urdf(urdf, pose, scale=0.6)
# define valid volume for sampling grasps
lx, ux = 0.02, self.size - 0.02
ly, uy = 0.02, self.size - 0.02
lz, uz = height + 0.005, self.size
self.lower = np.r_[lx, ly, lz]
self.upper = np.r_[ux, uy, uz]
def draw_grasp(grasp, score, finger_depth):
radius = 0.1 * finger_depth
w, d = grasp.width, finger_depth
color = cmap(float(score))
markers = []
# left finger
pose = grasp.pose * Transform(Rotation.identity(), [0.0, -w / 2, d / 2])
scale = [radius, radius, d]
msg = _create_marker_msg(Marker.CYLINDER, "task", pose, scale, color)
msg.id = 0
markers.append(msg)
# right finger
pose = grasp.pose * Transform(Rotation.identity(), [0.0, w / 2, d / 2])
scale = [radius, radius, d]
msg = _create_marker_msg(Marker.CYLINDER, "task", pose, scale, color)
msg.id = 1
markers.append(msg)
# wrist
pose = grasp.pose * Transform(Rotation.identity(), [0.0, 0.0, -d / 4])
scale = [radius, radius, d / 2]
msg = _create_marker_msg(Marker.CYLINDER, "task", pose, scale, color)
msg.id = 2
markers.append(msg)
# palm
pose = grasp.pose * Transform(
Rotation.from_rotvec(np.pi / 2 * np.r_[1.0, 0.0, 0.0]),
[0.0, 0.0, 0.0])
scale = [radius, radius, w]
msg = _create_marker_msg(Marker.CYLINDER, "task", pose, scale, color)
msg.id = 3
markers.append(msg)
pubs["grasp"].publish(MarkerArray(markers=markers))
def render_images(sim, n):
height, width = sim.camera.intrinsic.height, sim.camera.intrinsic.width
origin = Transform(Rotation.identity(), np.r_[sim.size / 2, sim.size / 2, 0.0])
extrinsics = np.empty((n, 7), np.float32)
depth_imgs = np.empty((n, height, width), np.float32)
for i in range(n):
r = np.random.uniform(1.6, 2.4) * sim.size
theta = np.random.uniform(0.0, np.pi / 4.0)
phi = np.random.uniform(0.0, 2.0 * np.pi)
extrinsic = camera_on_sphere(origin, r, theta, phi)
depth_img = sim.camera.render(extrinsic)[1]
extrinsics[i] = extrinsic.to_list()
depth_imgs[i] = depth_img
return depth_imgs, extrinsics
def generate_pile_scene(self, object_count, table_height):
# place box
urdf = self.urdf_root / "setup" / "box.urdf"
pose = Transform(Rotation.identity(), np.r_[0.02, 0.02, table_height])
box = self.world.load_urdf(urdf, pose, scale=1.3)
# drop objects
urdfs = self.rng.choice(self.object_urdfs, size=object_count)
for urdf in urdfs:
rotation = Rotation.random(random_state=self.rng)
xy = self.rng.uniform(1.0 / 3.0 * self.size, 2.0 / 3.0 * self.size,
2)
pose = Transform(rotation, np.r_[xy, table_height + 0.2])
scale = self.rng.uniform(0.8, 1.0)
self.world.load_urdf(urdf, pose, scale=self.global_scaling * scale)
self.wait_for_objects_to_rest(timeout=1.0)
# remove box
self.world.remove_body(box)
self.remove_and_wait()
import rospy
import sensor_msgs.msg
from vgn import vis
from vgn.baselines import GPD
from vgn.experiments.clutter_removal import Logger, State
from vgn.detection import VGN
from vgn.perception import *
from vgn.utils import ros_utils
from vgn.utils.transform import Rotation, Transform
from vgn.utils.panda_control import PandaCommander
vis.set_size(0.3)
# tag lies on the table in the center of the workspace
T_base_tag = Transform(Rotation.identity(), [0.42, 0.02, 0.21])
round_id = 0
class PandaGraspController(object):
def __init__(self, args):
self.robot_error = False
self.base_frame_id = rospy.get_param("~base_frame_id")
self.tool0_frame_id = rospy.get_param("~tool0_frame_id")
self.T_tool0_tcp = Transform.from_dict(rospy.get_param("~T_tool0_tcp")) # TODO
self.T_tcp_tool0 = self.T_tool0_tcp.inverse()
self.finger_depth = rospy.get_param("~finger_depth")
self.size = 6.0 * self.finger_depth
self.scan_joints = rospy.get_param("~scan_joints")
