def go_to_push_start(self, vec_env, z_offset=0.09):
for env_index, env_ptr in enumerate(vec_env._scene.env_ptrs):
block_ah = vec_env._scene.ah_map[env_index][vec_env._block_name]
block_transform = vec_env._block.get_rb_transforms(
env_ptr, block_ah)[0]
ee_transform = vec_env._frankas[env_index].get_ee_transform(
env_ptr, vec_env._franka_name)
grasp_transform = gymapi.Transform(p=block_transform.p,
r=ee_transform.r)
pre_grasp_transfrom = gymapi.Transform(p=grasp_transform.p,
r=grasp_transform.r)
pre_grasp_transfrom.p.z += z_offset
pre_grasp_transfrom.p.y -= 0.01
rt = transform_to_RigidTransform(pre_grasp_transfrom)
rot_matrix = RigidTransform.y_axis_rotation(np.pi / 2)
rt.rotation = np.dot(rot_matrix, rt.rotation)
pre_grasp_transfrom = RigidTransform_to_transform(rt)
stiffness = 1e3
vec_env._frankas[env_index].set_attractor_props(
env_index, env_ptr, vec_env._franka_name, {
'stiffness': stiffness,
'damping': 6 * np.sqrt(stiffness)
})
vec_env._frankas[env_index].set_ee_transform(
env_ptr, env_index, vec_env._franka_name, pre_grasp_transfrom)
def goto_side(self, dir, stiffness=1000):
"""
Goes to the direction in "0,1,2,3"
0
3 1
2
"""
#base is -1.57 1.57 1.56
des_quat = rpy_to_quat(np.array(self.dir_to_rpy[dir]))
delta_side = 0.02 + self._cfg["block"]["dims"]["width"] / 2
up_offset = self._cfg["block"]["dims"]["height"] / 2 + 0.07
for env_index, env_ptr in enumerate(self._scene.env_ptrs):
block_ah = self._scene.ah_map[env_index][self._block_name]
block_transform = self._block.get_rb_transforms(env_ptr,
block_ah)[0]
block_transform_trans = vec3_to_np(block_transform.p)
des_ee_trans = np.array([
block_transform_trans[0] +
delta_side * np.sin(self.dir_to_rpy[dir][2]),
block_transform_trans[1], block_transform_trans[2] +
delta_side * np.cos(self.dir_to_rpy[dir][2])
])
up_des_ee_trans = des_ee_trans.copy()
up_des_ee_trans[1] += up_offset
#yzx
#IK to compute joint angles
transformed_pt = gymapi.Transform(p=np_to_vec3(des_ee_trans),
r=des_quat)
up_transformed_pt = gymapi.Transform(p=np_to_vec3(up_des_ee_trans),
r=des_quat)
self._frankas[env_index].set_attractor_props(
env_index, env_ptr, self._franka_name, {
'stiffness': stiffness,
'damping': 4 * np.sqrt(stiffness)
})
ee_pose = self._frankas[env_index].get_ee_transform(
env_ptr, self._franka_name)
if ee_pose.p.y < transformed_pt.p.y + up_offset:
np_ee_pose = transform_to_np(ee_pose, format="wxyz")
np_ee_pose[1] = transformed_pt.p.y + up_offset
ee_pose.p.y = transformed_pt.p.y + up_offset
self.goto_pose(np_to_quat(np_ee_pose[3:], format="wxyz"),
np_ee_pose[0:3], env_index, env_ptr, ee_pose)
self.goto_pose(des_quat, up_des_ee_trans, env_index, env_ptr,
up_transformed_pt)
self.goto_pose(des_quat, des_ee_trans, env_index, env_ptr,
transformed_pt)
self._frankas[env_index].set_attractor_props(
env_index, env_ptr, self._franka_name, {
'stiffness': 500,
'damping': 4 * np.sqrt(stiffness)
})
def _fill_scene(self, cfg):
super()._fill_scene(cfg)
self.real_env_idx = 0
self._block = GymBoxAsset(self._scene.gym,
self._scene.sim,
**cfg['block']['dims'],
shape_props=cfg['block']['shape_props'],
rb_props=cfg['block']['rb_props'],
asset_options=cfg['block']['asset_options'])
self._obstacle = None
if "obstacle" in cfg.keys():
self._obstacle = GymBoxAsset(
self._scene.gym,
self._scene.sim,
**cfg['obstacle']['dims'],
shape_props=cfg['obstacle']['shape_props'],
rb_props=cfg['obstacle']['rb_props'],
asset_options=cfg['obstacle']['asset_options'])
self._obstacle_name = "obstacle"
self._block_name = 'block'
self._scene.add_asset(self._block_name, self._block,
gymapi.Transform())
self._scene.add_asset(self._obstacle_name, self._obstacle,
gymapi.Transform())
rough_coef = 0.9 #0.5
self.dt = cfg['scene']['gym']['dt']
self._board_names = [
name for name in cfg.keys() if "boardpiece" in name
]
self._boards = []
for _board_name in self._board_names:
board = GymBoxAsset(
self._scene.gym,
self._scene.sim,
**cfg[_board_name]['dims'],
shape_props=cfg[_board_name]['shape_props'],
rb_props=cfg[_board_name]['rb_props'],
asset_options=cfg[_board_name]['asset_options'])
self._boards.append(board)
if "goal" in _board_name:
self._scene.add_asset(_board_name,
board,
gymapi.Transform(),
collision_filter=3)
else:
self._scene.add_asset(_board_name, board, gymapi.Transform())
default_x = 0.1
self.get_delta_goal(default_x)
def _reset(self, env_idxs=None):
#self._pre_grasp_transforms = []
#self._grasp_transforms = []
#self._init_ee_transforms = []
if env_idxs is None:
env_idxs = self._scene.env_ptrs
super()._reset(env_idxs)
for env_idx in env_idxs:
env_ptr = self._scene.env_ptrs[env_idx]
block_ah = self._scene.ah_map[env_idx][self._block_name]
self._frankas[env_idx].set_gripper_width(
env_ptr, self._scene.ah_map[env_idx][self._franka_name], 0.02)
#board2_ah = self._scene.ah_map[env_idx][self._board2_name]
eps = 0.001
block_pose = gymapi.Transform(p=np_to_vec3(
np.array([
self._cfg["block"]["pose"]["y"],
self._cfg['table']['dims']['height'] +
self._cfg['block']['dims']['height'] / 2 +
self._cfg['boardpiece_blue1']['dims']['height'] / 2 +
0.01, self._cfg["block"]["pose"]["x"]
])))
for board_name in self._board_names:
board_pose = gymapi.Transform(p=np_to_vec3(
np.array([
self._cfg[board_name]["pose"]["y"],
self._cfg['table']['dims']['height'],
self._cfg[board_name]["pose"]["x"],
])))
board_ah = self._scene.ah_map[env_idx][board_name]
self._block.set_rb_transforms(env_ptr, board_ah, [board_pose])
if self._obstacle is not None:
obstacle_pose = gymapi.Transform(p=np_to_vec3(
np.array([
self._cfg[self._obstacle_name]["pose"]["y"],
self._cfg['table']['dims']['height'] +
self._cfg['obstacle']['dims']['height'] / 2 +
self._cfg['boardpiece_blue1']['dims']['height'] / 2 +
0.01,
self._cfg[self._obstacle_name]["pose"]["x"],
])))
obstacle_ah = self._scene.ah_map[env_idx][self._obstacle_name]
self._block.set_rb_transforms(env_ptr, obstacle_ah,
[obstacle_pose])
self._block.set_rb_transforms(env_ptr, block_ah, [block_pose])
self._scene.render()
self._update_state()
def get_random_xz_pos(self):
pose = gymapi.Transform()
randx = random.uniform(0.5, 0.6)
randz = random.uniform(-0.15, 0.15)
pose.p = gymapi.Vec3(randx, 0.35, randz)
pose.r = gymapi.Quat(0.0, 0.0, 0.0, 0.0)
return pose, randx, randz
def push_in_dir(self, dir, amount, T, stiffness=1200):
delta_t = 0.1
for env_index, env_ptr in enumerate(self._scene.env_ptrs):
block_ah = self._scene.ah_map[env_index][self._block_name]
block_transform = self._block.get_rb_transforms(env_ptr,
block_ah)[0]
block_start = transform_to_np(block_transform)
ee_start = self._frankas[env_index].get_ee_transform(
env_ptr, self._franka_name)
np_ee_start = transform_to_np(ee_start)
des_quat = np_to_quat(np_ee_start[3:], format="wxyz")
block_desired = np.array([
block_start[0] - amount * np.sin(self.dir_to_rpy[dir][2]),
block_start[1],
block_start[2] - amount * np.cos(self.dir_to_rpy[dir][2])
])
t = 0
self._frankas[env_index].set_attractor_props(
env_index, env_ptr, self._franka_name, {
'stiffness': stiffness,
'damping': 4 * np.sqrt(stiffness)
})
goal_tol = 0.02
while (t < T):
ee_pose = self._frankas[env_index].get_ee_transform(
env_ptr, self._franka_name)
np_pose = transform_to_np(ee_pose)[0:3]
block_transform_np = transform_to_np(
self._block.get_rb_transforms(env_ptr, block_ah)[0])
distance = np.linalg.norm(block_transform_np[:3] -
block_desired)
#print("Distance", distance)
des_ee_trans = np.array([
np_pose[0] - distance * np.sin(self.dir_to_rpy[dir][2]),
np_pose[1],
np_pose[2] - distance * np.cos(self.dir_to_rpy[dir][2])
])
#np_pose[2] += action[0]
transformed_pt = gymapi.Transform(p=np_to_vec3(des_ee_trans),
r=ee_start.r)
self._frankas[env_index].set_attractor_props(
env_index, env_ptr, self._franka_name, {
'stiffness': stiffness,
'damping': 4 * np.sqrt(stiffness)
})
self._frankas[env_index].set_ee_transform(
env_ptr, env_index, self._franka_name, transformed_pt)
time_taken = self.goto_pose(des_quat,
des_ee_trans,
env_index,
env_ptr,
transformed_pt,
max_t=T // 5)
t += time_taken
if distance < goal_tol:
break
def get_delta_goal(self, delta_x, visualize=False):
"""
goal state of block that's a delta
"""
box_pose_obs = self.get_block_poses()
delta_goal = box_pose_obs.copy()
delta_goal[:, 2] += delta_x
if visualize:
self._visual_block_name = "visualblock0"
self._scene.add_asset(self._visual_block_name, self._visual_block,
gymapi.Transform())
i = 0
for env_index, env_ptr in enumerate(self._scene.env_ptrs):
ah = self._scene.ah_map[env_index][self._visual_block_name]
goal_pose = gymapi.Transform(p=np_to_vec3(delta_goal[i, :]))
self._visual_block.set_rb_transforms(env_ptr, ah, [goal_pose])
i += 1
world_idx = 0
self.desired_goal = delta_goal[world_idx, 1:3]
return self.desired_goal.copy()
def __init__(self, shared_data,
join_init=None,
rev_stiffness=4.0,
rev_damping=3.0,
prism_stiffness=5.0,
prism_damping=0.0,
kv=1.0,
**base_args):
super(FrankaEnv, self).__init__(**base_args)
self.dt = 0.01
self.kv = kv
self.initial_dof_pos = join_init or [0.0, 0.0, 0.0, -1.75, 0.0, 2.0, 0.8, 0., 0.]
self.franka_pose = gymapi.Transform()
self.shared_data = shared_data
# add franka - 12 Indexes for Rigid Body
self.franka_pose.p = gymapi.Vec3(0.0, 0.0, 0.0)
self.franka_pose.r = gymapi.Quat(-0.707107, 0.0, 0.0, 0.707107)
self.create_actor(self.shared_data.franka_asset, self.franka_pose, "franka", self._env_index, 0)
# for retrieving Franka
self.franka_handle = self._gym.find_actor_handle(self._env, "franka")
# for retrieving finger positions
self.f1_handle = self._gym.find_actor_rigid_body_handle(self._env, self.franka_handle, 'panda_leftfinger')
self.f2_handle = self._gym.find_actor_rigid_body_handle(self._env, self.franka_handle, 'panda_rightfinger')
self.franka_joint_names = self._gym.get_actor_dof_names(self._env, self.franka_handle)
self.body_map = {}
self.joint_map = {}
self.initial_transforms_map = {}
self.prev_joint_positions = {}
self.joint_velocities = {}
# print("franka joints:")
# for jn in self.franka_joint_names:
# self.joint_map[jn] = self.get_joint_handle("franka", jn)
# self.prev_joint_positions[jn] = self.get_joint_position(self.joint_map[jn])
# self.joint_velocities[jn] = 0.0
# #print(jn, " = ", self.prev_joint_positions[jn])
# override default stiffness and damping values for PD control
# setup joint stiffness and damping
self.franka_dof_props = self._setup_joint_props(rev_stiffness, rev_damping, prism_stiffness, prism_damping)
self.step_counter = 0
self.reset()
def go_to_block(self, vec_env):
eps = 0.01
for env_index, env_ptr in enumerate(vec_env._scene.env_ptrs):
block_ah = vec_env._scene.ah_map[env_index][vec_env._block_name]
block_transform = vec_env._block.get_rb_transforms(
env_ptr, block_ah)[0]
ee_transform = vec_env._frankas[env_index].get_ee_transform(
env_ptr, vec_env._franka_name)
grasp_transform = gymapi.Transform(p=block_transform.p,
r=ee_transform.r)
pre_grasp_transfrom = gymapi.Transform(p=grasp_transform.p,
r=grasp_transform.r)
pre_grasp_transfrom.p.z += vec_env._cfg['block']['dims'][
'width'] / 2. - eps
pre_grasp_transfrom.p.y -= 0.01
stiffness = 20
vec_env._frankas[env_index].set_attractor_props(
env_index, env_ptr, vec_env._franka_name, {
'stiffness': stiffness,
'damping': 2 * np.sqrt(stiffness)
})
vec_env._frankas[env_index].set_ee_transform(
env_ptr, env_index, vec_env._franka_name, pre_grasp_transfrom)
def _apply_actions(self, action, planning_env=False):
for env_index, env_ptr in enumerate(self._scene.env_ptrs):
ee_pose = self._frankas[env_index].get_ee_transform(
env_ptr, self._franka_name)
np_pose = transform_to_np(ee_pose)[0:3]
np_pose[2] += action[0]
transformed_pt = gymapi.Transform(p=np_to_vec3(np.array(np_pose)),
r=ee_pose.r)
stiffness = action[1]
self._frankas[env_index].set_attractor_props(
env_index, env_ptr, self._franka_name, {
'stiffness': stiffness,
'damping': 4 * np.sqrt(stiffness)
})
self._frankas[env_index].set_ee_transform(env_ptr, env_index,
self._franka_name,
transformed_pt)
def __init__(self, env, **base_args):
super(FrankaPush, self).__init__(**base_args)
self.prev_pos = None
self.env = env
self._gym = env._gym
self.distance_threshold = 0.05
self.franka_handle = self._gym.find_actor_handle(
self.env._env, "franka")
asset_options = gymapi.AssetImportOptions()
asset_options.fix_base_link = False
# asset_options.flip_visual_attachments = True
asset_options.thickness = 0.004
asset_options.armature = 0.002
pose = gymapi.Transform()
# add cabinet - 1 Index rigid body
pose.p = gymapi.Vec3(0.58, 0.17, 0.0)
pose.r = gymapi.Quat(-0.707107, 0.0, 0.0, 0.707107)
self.env.create_actor(env.shared_data.table_asset, pose, "table")
# add object - 1 Index Rigid body
pose, randx, randz = self.get_random_xz_pos()
self.env.create_actor(env.shared_data.object_asset, pose, "object",
self.env._env_index, 1)
# add target
# acceptable range x: -0.35 - 0.35
# acceptable range y: -0.35 - 0.035
pose, randx, randz = self.get_random_xz_pos()
pose.p.y = 0.3
self.env.create_actor(env.shared_data.target_asset, pose, "target",
self.env._env_index, 1)
self.goal = np.array([randx, pose.p.y, randz])
self.table_handle = self._gym.find_actor_handle(self.env._env, "table")
self.object_handle = self._gym.find_actor_handle(
self.env._env, "object")
self.target_handle = self._gym.find_actor_handle(
self.env._env, "target")
self.cube_rigid_shape_prop = self._gym.get_actor_rigid_shape_properties(
self.env._env, self.object_handle)
self.cube_rigid_body_prop = self._gym.get_actor_rigid_body_properties(
self.env._env, self.object_handle)
self.table_rigid_shape_prop = self._gym.get_actor_rigid_shape_properties(
self.env._env, self.table_handle)
self.table_rigid_body_prop = self._gym.get_actor_rigid_body_properties(
self.env._env, self.table_handle)
self.set_physics("object", "all_friction", 0.3)
self.set_physics("table", "all_friction", 0.3)
self.set_physics("object", "mass", 1.0)
#self.cube_rigid_shape_prop[0].friction = 0.1
#self.cube_rigid_shape_prop[0].rolling_friction = 0.05
#self.cube_rigid_shape_prop[0].torsion_friction = 0.05
self.object_dof_props = self._gym.get_actor_dof_properties(
self.env._env, self.object_handle)
self.table_dof_props = self._gym.get_actor_dof_properties(
self.env._env, self.table_handle)
# self.object_dof_props['stiffness'].fill(6000)
# self.object_dof_props['damping'].fill(1000)
c = 0.5 + 0.5 * np.random.random(3)
color = gymapi.Vec3(c[0], c[1], c[2])
self._gym.set_rigid_body_color(self.env._env, self.target_handle, 0,
gymapi.MESH_VISUAL_AND_COLLISION, color)
color = gymapi.Vec3(0.1, 0.1, 0.5)
self._gym.set_rigid_body_color(self.env._env, self.table_handle, 0,
gymapi.MESH_VISUAL_AND_COLLISION, color)
color = gymapi.Vec3(1.0, 0.5, 0.1)
self._gym.set_rigid_body_color(self.env._env, self.object_handle, 0,
gymapi.MESH_VISUAL_AND_COLLISION, color)
self._gym.set_actor_dof_properties(self.env._env, self.object_handle,
self.object_dof_props)
self._gym.set_actor_dof_properties(self.env._env, self.table_handle,
self.table_dof_props)