def transformation_from_parent_to_child_link(self, joint_position=None):
""" Return transformation from parent to the child either for the null position or for the specified
position of the joint. """
t0 = self.physx_joint.get_local_pose(0)
tj = self.joint_transformation(joint_position)
t1 = inverse_transform(self.physx_joint.get_local_pose(1))
return multiply_transformations(multiply_transformations(t0, tj), t1)
def reset_hand_pose(self, pose, grip=None):
if grip is None:
grip = GripperCylinderEnv.FINGER_OPEN_POS
self.hand_actors[0].set_global_pose(pose)
self.hand_actors[1].set_global_pose(
multiply_transformations(pose, [0, grip, 0.0584]))
self.hand_actors[2].set_global_pose(
multiply_transformations(pose, [0, -grip, 0.0584]))
self.set_grip(grip)
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 update(self, blocking=False):
""" Update scene if lock can be acquired. Otherwise do nothing. Set blocking to True in order to force it. """
if self.render_lock.acquire(blocking=blocking):
for node, actor, offset in self.nodes_and_actors: # type: pyrender.Node, RigidActor
pose = multiply_transformations(offset,
actor.get_global_pose())
self.scene.set_pose(node, pose_to_transformation_matrix(pose))
self.render_lock.release()
def _update_actors(self):
for actors, offset, start_index in self.actors_and_offsets:
for i, actor in enumerate(actors, start=start_index):
pose = actor.get_global_pose()
if offset is not None:
pose = multiply_transformations(offset, pose)
self.vis_actor(i).set_transform(pose_to_transformation_matrix(pose))
if self.show_frames:
self.vis_frame(i).set_transform(pose_to_transformation_matrix(pose))
def benchmark_sample(env):
if not hasattr(benchmark_sample, "bid"):
benchmark_sample.bid = 0
d0 = bench_data.loc[min(benchmark_sample.bid, bench_data.shape[0] - 1)]
q = quat_from_azimuth_elevation(d0['hazi'], d0['hele'])
tip_pos = np.array([d0['hposx'], d0['hposy'], d0['hposz']])
obj_pos = np.array([d0['o0posx'], d0['o0posy'], d0['o0posz']])
if action_class.is_single_object():
return (tip_pos, q), GripperCylinderEnv.FINGER_OPEN_POS, (obj_pos, npq.one), d0['o0hei'], d0['o0rad']
o1pose = multiply_transformations((tip_pos, q), (np.zeros(3), quat_from_euler('y', np.pi / 2)))
return (tip_pos, q), d0['o1rad'], (obj_pos, npq.one), d0['o0hei'], d0['o0rad'], o1pose, d0['o1hei'], d0['o1rad']
def add_physx_scene(
self,
scene,
render_shapes_with_one_of_flags=(ShapeFlag.VISUALIZATION, ),
offset=np.zeros(3)):
""" Call this function to create a renderer scene from physx scene. """
actors = scene.get_dynamic_rigid_actors(
) + scene.get_static_rigid_actors()
for i, actor in enumerate(actors):
n = self.actor_to_node(actor, render_shapes_with_one_of_flags)
if n is not None:
self.nodes_and_actors.append((n, actor, offset))
self.render_lock.acquire()
self.scene.add_node(n)
pose = multiply_transformations(offset,
actor.get_global_pose())
self.scene.set_pose(n, pose_to_transformation_matrix(pose))
self.render_lock.release()
def compute_link_transformations(
self,
joint_values: Optional[Dict[str,
float]] = None) -> Dict[str, tuple]:
""" Compute transformations of all links for given joint values and return them in a dictionary in which link
name serves as a key and link pose is a value. """
if joint_values is None:
joint_values = {}
link_transforms = {}
for link in anytree.LevelOrderIter(self.root_node): # type: Link
if link.is_root:
link_transforms[link.name] = self.root_pose
continue
parent_transform = link_transforms[link.parent.name]
joint_value = joint_values.get(link.joint_from_parent.name, None)
relative_pose = link.joint_from_parent.transformation_from_parent_to_child_link(
joint_value)
link_transforms[link.name] = multiply_transformations(
parent_transform, relative_pose)
return link_transforms
obj_pose = (data[i, 5:8], (0, 0, 0, 1))
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 _get_actor_pose_matrix(actor, offset):
""" Get actor transformation matrix with applied offset if not none. """
pose = actor.get_global_pose()
if offset is not None:
pose = multiply_transformations(offset, pose)
return pose_to_transformation_matrix(pose)