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