def quat_from_azimuth_elevation(azimuth, elevation, angle_z=0.):
"""
Get numpy quaternion from azimuth and elevation.
Angle_z is rotation about z axis of the rotated gripper.
"""
return quat_from_euler("yz", [np.math.pi / 2. + elevation, azimuth
]) * quat_from_euler("z", angle_z)
def move_target(self, d):
target = self._target
eye = self._pose[:3, 3].flatten()
_, a, _ = self.cartesian_to_spherical(eye, target)
v = npq.rotate_vectors(quat_from_euler('z', a), d)
self._n_target += v
self._n_pose[:3, 3] += v
def sample(env: GripperCylinderEnv):
hpos_std = shared_sampler_dict['hpos_std']
sa = shared_sampler_dict['angle_bound_scale']
q = quat_from_azimuth_elevation(
np.random.uniform(hazi_min * sa, hazi_max * sa) + np.pi / 2,
np.random.uniform(hele_min * sa, hele_max * sa))
ohei = np.random.uniform(ohei_min, ohei_max)
orad = np.random.uniform(orad_min, orad_max)
tip_pos = np.zeros(3)
tip_pos[:2] = np.random.normal(0., hpos_std, size=2)
tip_pos[2] = ohei / 2. + np.random.normal(0., hpos_std, size=1)
obj_pos = np.array([0., 0., ohei / 2.])
obj_pos[:2] += np.random.normal(0., 1e-3, size=2)
if not env.two_objects:
return (tip_pos, q), GripperCylinderEnv.FINGER_OPEN_POS, (
obj_pos, npq.one), ohei, orad
tind = np.random.randint(0, env.demo_opos2.shape[1])
tip_pos += env.demo_opos2[0, tind, :]
o2pose = multiply_transformations(
(tip_pos, q), (np.zeros(3), quat_from_euler('y', np.pi / 2)))
o2h = np.random.uniform(ohei_min, ohei_max)
o2rad = np.random.uniform(orad_min, orad_max)
return (tip_pos,
q), o2rad, (obj_pos,
npq.one), ohei, orad, o2pose, o2h, o2rad
def create_hand_actors(self, use_mesh_base=True, use_mesh_fingers=True):
panda_dir = os.path.dirname(
os.path.abspath(__file__)) + '/franka_panda/meshes/collision'
hand_mat = Material(static_friction=1.,
dynamic_friction=1.,
restitution=0)
trimesh_kwargs = dict(split_object=False, group_material=False)
convex_kwargs = dict(material=hand_mat,
quantized_count=64,
vertex_limit=64)
if use_mesh_base:
mesh_hand: trimesh.Trimesh = trimesh.load(
'{}/hand.obj'.format(panda_dir), **trimesh_kwargs)
hand_shape = Shape.create_convex_mesh_from_points(
points=mesh_hand.vertices, **convex_kwargs)
else:
minp, maxp = np.array([-0.0316359, -0.10399,
-0.0259248]), np.array(
[0.0316158, 0.100426, 0.0659622])
hand_shape = Shape.create_box(size=maxp - minp, material=hand_mat)
hand_shape.set_local_pose((minp + maxp) / 2)
if use_mesh_fingers:
mesh_finger: trimesh.Trimesh = trimesh.load(
'{}/finger.obj'.format(panda_dir), **trimesh_kwargs)
finger1_shape = Shape.create_convex_mesh_from_points(
points=mesh_finger.vertices, **convex_kwargs)
finger2_shape = Shape.create_convex_mesh_from_points(
points=mesh_finger.vertices, **convex_kwargs)
finger2_shape.set_local_pose(
(np.zeros(3), quat_from_euler('z', np.pi)))
else:
finger1_shape = Shape.create_box(size=[0.02, 0.02, 0.12],
material=hand_mat)
finger2_shape = Shape.create_box(size=[0.02, 0.02, 0.12],
material=hand_mat)
finger1_shape.set_local_pose([0., 0.01, 0.07])
finger2_shape.set_local_pose([0., -0.01, 0.07])
for s in [hand_shape, finger1_shape, finger2_shape]:
s.set_local_pose(
multiply_transformations((0, 0, -0.15), s.get_local_pose()))
actors = [RigidDynamic() for _ in range(3)]
actors[0].attach_shape(hand_shape)
actors[1].attach_shape(finger1_shape)
actors[2].attach_shape(finger2_shape)
actors[0].set_mass(1000 + 0.81)
actors[1].set_mass(0.1 + 5.)
actors[2].set_mass(0.1 + 5.)
actors[0].set_rigid_body_flag(RigidBodyFlag.KINEMATIC, True)
for a in actors:
a.disable_gravity()
a.set_angular_damping(0.5)
a.set_linear_damping(0.5)
self.scene.add_actor(a)
return actors
def set_joint_position(self, value):
""" Set desired position of the joint. """
value = value if value is not None else self.null_value
if self.is_prismatic:
self.physx_joint.set_drive_position((value, 0, 0))
elif self.is_revolute:
self.physx_joint.set_drive_position(
(np.zeros(3), quat_from_euler('x', value)))
self.commanded_joint_position = value
def joint_transformation(self, joint_position=None):
""" Get transformation of joint. For prismatic, this is defined as translation in x-axis.
For revolute it is rotation about x-axis. """
if self.joint_type == 'fixed':
return unit_pose()
if joint_position is None:
joint_position = self.null_value
if self.joint_type == 'prismatic':
return np.array([joint_position, 0, 0]), npq.one
elif self.joint_type == 'revolute':
return np.zeros(3), quat_from_euler('x', [joint_position])
else:
raise NotImplementedError(
'Only fixed, prismatic and revolute joints are supported.')
def reset(self):
self.iter = 0
if self.reset_state_sampler is None:
self.reset_hand_pose(
pose=((0, 0, 0.05),
quat_from_euler('yz', [-np.pi / 2, -np.pi / 2])))
self.obj.set_global_pose([0.0, 0.0, 0.05])
else:
while True:
if self.two_objects:
hand_pose, grip, obj_pose, self.obj_height, self.obj_radius, obj2_pose, self.obj2_height, self.obj2_radius = self.reset_state_sampler(
self)
for s in self.obj2.get_atached_shapes():
self.obj2.detach_shape(s)
self.obj2.attach_shape(
self.create_cylinder(self.obj2_height,
self.obj2_radius))
self.obj2.set_global_pose(obj2_pose)
else:
hand_pose, grip, obj_pose, self.obj_height, self.obj_radius = self.reset_state_sampler(
self)
[
self.obj.detach_shape(s)
for s in self.obj.get_atached_shapes()
]
self.obj.attach_shape(
self.create_cylinder(self.obj_height, self.obj_radius))
self.reset_hand_pose(hand_pose, grip)
self.obj.set_global_pose(obj_pose)
if not self.hand_plane_hit() and not self.hand_obj_hit():
break
# else:
# print('reset collision detected: hand/plane: {}, hand/obj: {}'.format(self.hand_plane_hit(),
# self.hand_obj_hit()))
# print('Sample:', hand_pose, grip, obj_pose, self.obj_height, self.obj_radius)
self.plane_mat.set_static_friction(np.random.uniform(0., 0.05))
self.plane_mat.set_dynamic_friction(np.random.uniform(0., 0.05))
self.scene.simulation_time = 0.
return self.get_obs()
env.obj_height = data[i, 8]
env.obj_radius = data[i, 9]
[env.obj.detach_shape(s) for s in env.obj.get_atached_shapes()]
env.obj.attach_shape(env.create_cylinder(env.obj_height, env.obj_radius))
env.reset_hand_pose((hpos, hq), env.FINGER_OPEN_POS)
env.obj.set_global_pose(obj_pose)
env.obj2_height = data[i, 10]
env.obj2_radius = data[i, 11]
[env.obj2.detach_shape(s) for s in env.obj2.get_atached_shapes()]
env.obj2.attach_shape(env.create_cylinder(env.obj2_height,
env.obj2_radius))
env.obj.set_global_pose(obj_pose)
o1pose = multiply_transformations(
(hpos, hq), (np.zeros(3), quat_from_euler('y', np.pi / 2)))
env.obj2.set_global_pose(o1pose)
valid[i] = not env.hand_plane_hit() and not env.hand_obj_hit(
) and not env.obj2_plane_hit()
env.reset_hand_pose((hpos, hq), env.obj2_radius)
valid[i] &= not env.hand_plane_hit() and not env.hand_obj_hit()
print(sum(valid))
if int(sum(valid)) == 1000:
break
print(sum(valid))
df = pd.DataFrame(data=data[valid],
columns=[
def step(self, action):
self.iter += 1
""" Step in pyphysx. """
dt = self.control_frequency.period() / self.num_sub_steps
dpose = (0.3 * np.tanh(action[:3]) * dt,
quat_from_euler('xyz',
np.pi * np.tanh(action[3:6]) * dt))
if self.open_gripper_on_leave and self.scene.simulation_time > self.action_end_time:
self.set_grip(self.FINGER_OPEN_POS)
elif self.close_gripper_on_leave and self.scene.simulation_time > self.action_end_time:
self.set_grip(0.)
else:
grip = np.tanh(action[
6]) * self.FINGER_OPEN_POS / 2 + self.FINGER_OPEN_POS / 2
# grip = 0.04 if grip < 0.05 else self.FINGER_OPEN_POS
self.set_grip(grip)
pose = self.hand_actors[0].get_global_pose()
for _ in range(self.num_sub_steps):
pose = multiply_transformations(dpose, pose)
self.hand_actors[0].set_kinematic_target(pose)
self.scene.simulate(dt)
if self.render:
if self.iter == 1:
self.renderer.clear_physx_scenes()
self.renderer.add_physx_scene(self.scene)
self.renderer.update()
# self.renderer.render_scene(self.scene, recompute_actors=self.iter == 1)
if self.realtime:
self.control_frequency.sleep()
""" Compute rewards """
hp, hq = self.hand_actors[0].get_global_pose()
op, oq = self.obj.get_global_pose()
grip = self.get_grip()
hrot = azimuth_elevation_from_quat(hq)
hp[2] -= 0.5 * self.obj_height
op[2] -= 0.5 * self.obj_height
traj_rewards = np.zeros(self.demo_hpos.shape[1])
b, s = self.reward_params['demo_hand_pos']['b'], self.reward_params[
'demo_hand_pos']['scale']
traj_rewards += s * np.exp(-0.5 * np.sum(
(self.demo_hpos[self.iter] - hp)**2, axis=-1) * b)
b, s = self.reward_params['demo_obj_pos']['b'], self.reward_params[
'demo_obj_pos']['scale']
traj_rewards += s * np.exp(-0.5 * np.sum(
(self.demo_opos[self.iter] - op)**2, axis=-1) * b)
b, s = self.reward_params['demo_hand_azi_ele'][
'b'], self.reward_params['demo_hand_azi_ele']['scale']
traj_rewards += s * np.exp(-0.5 * np.sum(
(self.demo_hrot[self.iter] - hrot)**2, axis=-1) * b)
b, s = self.reward_params['demo_obj_rot']['b'], self.reward_params[
'demo_obj_rot']['scale']
traj_rewards += s * np.exp(-0.5 * npq.rotation_intrinsic_distance(
self.demo_orot[self.iter], oq) * b)
b, s = self.reward_params['demo_touch']['b'], self.reward_params[
'demo_touch']['scale']
traj_rewards += s * self.demo_grip_weak_closed[self.iter] * np.exp(
-0.5 * b * (grip - (self.obj_radius - 0.005))**2)
if self.two_objects:
op2, oq2 = self.obj2.get_global_pose()
op2[2] -= 0.5 * self.obj2_height
b, s = self.reward_params['demo_obj_pos']['b'], self.reward_params[
'demo_obj_pos']['scale']
traj_rewards += s * np.exp(-0.5 * np.sum(
(self.demo_opos2[self.iter] - op2)**2, axis=-1) * b)
b, s = self.reward_params['demo_obj_rot']['b'], self.reward_params[
'demo_obj_rot']['scale']
traj_rewards += s * np.exp(-0.5 * npq.rotation_intrinsic_distance(
self.demo_orot2[self.iter], oq2) * b)
b, s = self.reward_params['demo_touch']['b'], self.reward_params[
'demo_touch']['scale']
traj_rewards += s * self.demo_grip_weak_closed[self.iter] * np.exp(
-0.5 * b * (grip - (self.obj2_radius - 0.005))**2)
if traj_rewards.size == 0:
reward = 0.
else:
reward = np.max(traj_rewards) if self.use_max_reward else np.mean(
traj_rewards)
""" Resolve singularities and collisions that we don't want. """
if self.hand_plane_hit():
if self.reset_on_plane_hit:
return EnvStep(self.get_obs(), 0., True, EnvInfo())
reward = 0.
if np.abs(hrot[1]) > np.deg2rad(
85): # do not allow motion close to singularity
if self.reset_on_singularity:
return EnvStep(self.get_obs(), 0., True, EnvInfo())
reward = 0.
return EnvStep(self.get_obs(), reward / self.horizon,
self.iter == self.horizon, EnvInfo())
def transform_to_object_frame(self, object_id=0, xyz_scale=1.):
"""
First, compensate azimuth angle s.t. camera is always looking towards negative x-axis. It is achieved by
computing objects and hand position in camera coordinate frame. The azimuth rotation of hand is adjusted by
substraction.
Second, all positions are updated s.t. obj is always located at [0,0] x,y position.
Third, orientation of object is updated s.t. first rotation is always identity.
"""
oid = 'o{}'.format(object_id)
opos = self.df[[oid + 'posx', oid + 'posy', oid + 'posz'
]].to_numpy() * [1, 1, 0]
hpos = self.df[['hposx', 'hposy', 'hposz']].to_numpy() * [1, 1, 0]
yaw = self.df['cazi'].mean()
t_cam = (self.get_camera_position() * [1, 1, 0],
quat_from_euler('z', yaw))
t_cam_inv = inverse_transform(t_cam)
""" Positions in camera frame. """
opos = npq.rotate_vectors(t_cam_inv[1], opos)
hpos = npq.rotate_vectors(t_cam_inv[1], hpos)
""" Offset zero object position """
pos_offset = opos[0].copy() if self.single_object() else opos.copy()
opos = opos - pos_offset
hpos = hpos - pos_offset
""" Compensate rotation of the object """
orot = self.df[[oid + 'rot0', oid + 'rot1', oid + 'rot2']].to_numpy()
orot0: npq.quaternion = npq.from_rotation_vector(orot[0])
for rot in orot:
rot[:] = npq.as_rotation_vector(orot0.inverse() *
npq.from_rotation_vector(rot))
self.camera_transformed_pos = -pos_offset
self.df[oid + 'posx'] = xyz_scale * opos[:, 0]
self.df[oid + 'posy'] = xyz_scale * opos[:, 1]
for i in range(3):
self.df[oid + 'rot{}'.format(i)] = orot[:, i]
self.df['hposx'] = xyz_scale * hpos[:, 0]
self.df['hposy'] = xyz_scale * hpos[:, 1]
self.df['hazi'] = self.df['hazi'].to_numpy() - yaw
self.df['cazi'] = 0
self.df['cdis'] = 0
self.df['cele'] = 0
self.df[oid + 'posz'] = xyz_scale * self.df[oid + 'posz']
self.df[oid + 'szz'] = xyz_scale * self.df[oid + 'szz']
self.df['hposz'] = xyz_scale * self.df['hposz']
if not self.single_object():
oid = 'o1'
orot = self.df[[oid + 'rot0', oid + 'rot1',
oid + 'rot2']].to_numpy()
opos = self.df[[oid + 'posx', oid + 'posy', oid + 'posz'
]].to_numpy() * [1, 1, 0]
opos = npq.rotate_vectors(t_cam_inv[1], opos)
opos = opos - pos_offset
self.df[oid + 'posx'] = xyz_scale * opos[:, 0]
self.df[oid + 'posy'] = xyz_scale * opos[:, 1]
self.df[oid + 'posz'] = xyz_scale * self.df[oid + 'posz']
self.df[oid + 'szz'] = xyz_scale * self.df[oid + 'szz']
orot0: npq.quaternion = npq.from_rotation_vector(orot[0])
for rot in orot:
rot[:] = npq.as_rotation_vector(orot0.inverse() *
npq.from_rotation_vector(rot))
for i in range(3):
self.df[oid + 'rot{}'.format(i)] = orot[:, i]