def reset(self, T_world_tcp):
T_world_body = T_world_tcp * self.T_tcp_body
self.body = self.world.load_urdf(self.urdf_path, T_world_body)
self.body.set_pose(
T_world_body) # sets the position of the COM, not URDF link
self.constraint = self.world.add_constraint(
self.body,
None,
None,
None,
pybullet.JOINT_FIXED,
[0.0, 0.0, 0.0],
Transform.identity(),
T_world_body,
)
self.update_tcp_constraint(T_world_tcp)
# constraint to keep fingers centered
self.world.add_constraint(
self.body,
self.body.links["panda_leftfinger"],
self.body,
self.body.links["panda_rightfinger"],
pybullet.JOINT_GEAR,
[1.0, 0.0, 0.0],
Transform.identity(),
Transform.identity(),
).change(gearRatio=-1, erp=0.1, maxForce=50)
self.joint1 = self.body.joints["panda_finger_joint1"]
self.joint1.set_position(0.5 * self.max_opening_width, kinematics=True)
self.joint2 = self.body.joints["panda_finger_joint2"]
self.joint2.set_position(0.5 * self.max_opening_width, kinematics=True)
def evaluate_grasp_point(sim, pos, normal, num_rotations=6):
# define initial grasp frame on object surface
z_axis = -normal
x_axis = np.r_[1.0, 0.0, 0.0]
if np.isclose(np.abs(np.dot(x_axis, z_axis)), 1.0, 1e-4):
x_axis = np.r_[0.0, 1.0, 0.0]
y_axis = np.cross(z_axis, x_axis)
x_axis = np.cross(y_axis, z_axis)
R = Rotation.from_matrix(np.vstack((x_axis, y_axis, z_axis)).T)
# try to grasp with different yaw angles
yaws = np.linspace(0.0, np.pi, num_rotations)
outcomes, widths = [], []
for yaw in yaws:
ori = R * Rotation.from_euler("z", yaw)
sim.restore_state()
candidate = Grasp(Transform(ori, pos), width=sim.gripper.max_opening_width)
outcome, width = sim.execute_grasp(candidate, remove=False)
outcomes.append(outcome)
widths.append(width)
# detect mid-point of widest peak of successful yaw angles
# TODO currently this does not properly handle periodicity
successes = (np.asarray(outcomes) == Label.SUCCESS).astype(float)
if np.sum(successes):
peaks, properties = signal.find_peaks(
x=np.r_[0, successes, 0], height=1, width=1
)
idx_of_widest_peak = peaks[np.argmax(properties["widths"])] - 1
ori = R * Rotation.from_euler("z", yaws[idx_of_widest_peak])
width = widths[idx_of_widest_peak]
return Grasp(Transform(ori, pos), width), int(np.max(outcomes))
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 generate_packed_scene(self, object_count, table_height):
attempts = 0
max_attempts = 12
while self.num_objects < object_count and attempts < max_attempts:
self.save_state()
urdf = self.rng.choice(self.object_urdfs)
x = self.rng.uniform(0.08, 0.22)
y = self.rng.uniform(0.08, 0.22)
z = 1.0
angle = self.rng.uniform(0.0, 2.0 * np.pi)
rotation = Rotation.from_rotvec(angle * np.r_[0.0, 0.0, 1.0])
pose = Transform(rotation, np.r_[x, y, z])
scale = self.rng.uniform(0.7, 0.9)
body = self.world.load_urdf(urdf,
pose,
scale=self.global_scaling * scale)
lower, upper = self.world.p.getAABB(body.uid)
z = table_height + 0.5 * (upper[2] - lower[2]) + 0.002
body.set_pose(pose=Transform(rotation, np.r_[x, y, z]))
self.world.step()
if self.world.get_contacts(body):
self.world.remove_body(body)
self.restore_state()
else:
self.remove_and_wait()
attempts += 1
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 __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 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 select_index(qual_vol, rot_vol, width_vol, index):
i, j, k = index
score = qual_vol[i, j, k]
ori = Rotation.from_quat(rot_vol[:, i, j, k])
pos = np.array([i, j, k], dtype=np.float64)
width = width_vol[i, j, k]
return Grasp(Transform(ori, pos), width), score
def read_grasp(df, i):
scene_id = df.loc[i, "scene_id"]
orientation = Rotation.from_quat(df.loc[i, "qx":"qw"].to_numpy(np.double))
position = df.loc[i, "x":"z"].to_numpy(np.double)
width = df.loc[i, "width"]
label = df.loc[i, "label"]
grasp = Grasp(Transform(orientation, position), width)
return scene_id, grasp, label
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 draw_workspace(size):
scale = size * 0.005
pose = Transform.identity()
scale = [scale, 0.0, 0.0]
color = [0.5, 0.5, 0.5]
msg = _create_marker_msg(Marker.LINE_LIST, "task", pose, scale, color)
msg.points = [
ros_utils.to_point_msg(point) for point in workspace_lines(size)
]
pubs["workspace"].publish(msg)
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 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()
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 main(args):
rospy.init_node("vgn_vis", anonymous=True)
dataset = Dataset(args.dataset, augment=args.augment)
i = np.random.randint(len(dataset))
voxel_grid, (label, rotations, width), index = dataset[i]
grasp = Grasp(Transform(Rotation.from_quat(rotations[0]), index), width)
vis.clear()
vis.draw_workspace(40)
vis.draw_tsdf(voxel_grid, 1.0)
vis.draw_grasp(grasp, float(label), 40.0 / 6.0)
rospy.sleep(1.0)
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 __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")
self.setup_panda_control()
self.tf_tree = ros_utils.TransformTree()
self.define_workspace()
self.create_planning_scene()
self.tsdf_server = TSDFServer()
self.plan_grasps = self.select_grasp_planner(args.model)
self.logger = Logger(args.logdir, args.description)
rospy.loginfo("Ready to take action")
def to_grasp_list(self, grasp_configs):
grasps, scores = [], []
for grasp_config in grasp_configs.grasps:
# orientation
x_axis = ros_utils.from_vector3_msg(grasp_config.axis)
y_axis = -ros_utils.from_vector3_msg(grasp_config.binormal)
z_axis = ros_utils.from_vector3_msg(grasp_config.approach)
orientation = Rotation.from_matrix(np.vstack([x_axis, y_axis, z_axis]).T)
# position
position = ros_utils.from_point_msg(grasp_config.position)
# width
width = grasp_config.width.data
# score
score = grasp_config.score.data
if score < 0.0:
continue # negative score is larger than positive score (https://github.com/atenpas/gpd/issues/32#issuecomment-387846534)
grasps.append(Grasp(Transform(orientation, position), width))
scores.append(score)
return grasps, scores
def __init__(self, model_path):
# define frames
self.task_frame_id = "task"
self.cam_frame_id = "camera_depth_optical_frame"
self.T_cam_task = Transform(
Rotation.from_quat([-0.679, 0.726, -0.074, -0.081]),
[0.166, 0.101, 0.515])
# broadcast the tf tree (for visualization)
self.tf_tree = ros_utils.TransformTree()
self.tf_tree.broadcast_static(self.T_cam_task, self.cam_frame_id,
self.task_frame_id)
# define camera parameters
self.cam_topic_name = "/camera/depth/image_rect_raw"
self.intrinsic = CameraIntrinsic(640, 480, 383.265, 383.26, 319.39,
242.43)
# setup a CV bridge
self.cv_bridge = cv_bridge.CvBridge()
# construct the grasp planner object
self.device = torch.device(
"cuda" if torch.cuda.is_available() else "cpu")
self.net = load_network(model_path, self.device)
# initialize the visualization
vis.clear()
vis.draw_workspace(0.3)
# subscribe to the camera
self.img = None
rospy.Subscriber(self.cam_topic_name, sensor_msgs.msg.Image,
self.sensor_cb)
# setup cb to detect grasps
rospy.Timer(rospy.Duration(0.1), self.detect_grasps)
def get_pose(self):
pos, ori = self.p.getBasePositionAndOrientation(self.uid)
return Transform(Rotation.from_quat(ori), np.asarray(pos))
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")
def from_transform_msg(msg):
"""Convert a Transform message to a Transform object."""
translation = from_vector3_msg(msg.translation)
rotation = from_quat_msg(msg.rotation)
return Transform(rotation, translation)
def camera_on_sphere(origin, radius, theta, phi):
eye = np.r_[radius * sin(theta) * cos(phi), radius * sin(theta) * sin(phi),
radius * cos(theta), ]
target = np.array([0.0, 0.0, 0.0])
up = np.array([0.0, 0.0, 1.0]) # this breaks when looking straight down
return Transform.look_at(eye, target, up) * origin.inverse()
def create_tsdf(size, resolution, depth_imgs, intrinsic, extrinsics):
tsdf = TSDFVolume(size, resolution)
for i in range(depth_imgs.shape[0]):
extrinsic = Transform.from_list(extrinsics[i])
tsdf.integrate(depth_imgs[i], intrinsic, extrinsic)
return tsdf
class Gripper(object):
"""Simulated Panda hand."""
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 reset(self, T_world_tcp):
T_world_body = T_world_tcp * self.T_tcp_body
self.body = self.world.load_urdf(self.urdf_path, T_world_body)
self.body.set_pose(
T_world_body) # sets the position of the COM, not URDF link
self.constraint = self.world.add_constraint(
self.body,
None,
None,
None,
pybullet.JOINT_FIXED,
[0.0, 0.0, 0.0],
Transform.identity(),
T_world_body,
)
self.update_tcp_constraint(T_world_tcp)
# constraint to keep fingers centered
self.world.add_constraint(
self.body,
self.body.links["panda_leftfinger"],
self.body,
self.body.links["panda_rightfinger"],
pybullet.JOINT_GEAR,
[1.0, 0.0, 0.0],
Transform.identity(),
Transform.identity(),
).change(gearRatio=-1, erp=0.1, maxForce=50)
self.joint1 = self.body.joints["panda_finger_joint1"]
self.joint1.set_position(0.5 * self.max_opening_width, kinematics=True)
self.joint2 = self.body.joints["panda_finger_joint2"]
self.joint2.set_position(0.5 * self.max_opening_width, kinematics=True)
def update_tcp_constraint(self, T_world_tcp):
T_world_body = T_world_tcp * self.T_tcp_body
self.constraint.change(
jointChildPivot=T_world_body.translation,
jointChildFrameOrientation=T_world_body.rotation.as_quat(),
maxForce=300,
)
def set_tcp(self, T_world_tcp):
T_word_body = T_world_tcp * self.T_tcp_body
self.body.set_pose(T_word_body)
self.update_tcp_constraint(T_world_tcp)
def move_tcp_xyz(self,
target,
eef_step=0.002,
vel=0.10,
abort_on_contact=True):
T_world_body = self.body.get_pose()
T_world_tcp = T_world_body * self.T_body_tcp
diff = target.translation - T_world_tcp.translation
n_steps = int(np.linalg.norm(diff) / eef_step)
dist_step = diff / n_steps
dur_step = np.linalg.norm(dist_step) / vel
for _ in range(n_steps):
T_world_tcp.translation += dist_step
self.update_tcp_constraint(T_world_tcp)
for _ in range(int(dur_step / self.world.dt)):
self.world.step()
if abort_on_contact and self.detect_contact():
return
def detect_contact(self, threshold=5):
if self.world.get_contacts(self.body):
return True
else:
return False
def move(self, width):
self.joint1.set_position(0.5 * width)
self.joint2.set_position(0.5 * width)
for _ in range(int(0.5 / self.world.dt)):
self.world.step()
def read(self):
width = self.joint1.get_position() + self.joint2.get_position()
return width
def get_pose(self):
link_state = self.p.getLinkState(self.body_uid, self.link_index)
pos, ori = link_state[0], link_state[1]
return Transform(Rotation.from_quat(ori), pos)
