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