def get_robot_info(self, robot_body):
# Provide functionality of Obtaining Robot information
tool_link = robot_body.env_body.GetLink("right_gripper")
manip_trans = tool_link.GetTransform()
# This manip_trans is off by 90 degree
pose = OpenRAVEBody.obj_pose_from_transform(manip_trans)
robot_trans = OpenRAVEBody.get_ik_transform(pose[:3], pose[3:])
arm_inds = list(range(10, 17))
return robot_trans, arm_inds
def resample_rrt_planner(pred, netgated, t, plan):
startp, endp = pred.startp, pred.endp
robot = pred.robot
body = pred._param_to_body[robot].env_body
manip_trans = body.GetManipulator("right_arm").GetTransform()
pose = OpenRAVEBody.obj_pose_from_transform(manip_trans)
manip_trans = OpenRAVEBody.get_ik_transform(pose[:3], pose[3:])
gripper_direction = manip_trans[:3, :3].dot(np.array([-1, 1, 0]))
lift_direction = manip_trans[:3, :3].dot(np.array([0, 0, -1]))
active_dof = body.GetManipulator("right_arm").GetArmIndices()
attr_inds = OrderedDict()
res = []
pred_test = [not pred.test(k, negated) for k in range(20)]
resample_ts = np.where(pred_test)[0]
start, end = resample_ts[0] - 1, resample_ts[-1] + 1
rave_body = pred._param_to_body[pred.robot]
dof_value_map = {
"lArmPose": pred.robot.lArmPose[:, start],
"lGripper": 0.02,
"rArmPose": pred.robot.rArmPose[:, start],
"rGripper": 0.02
}
rave_body.set_dof(dof_value_map)
rave_body.set_pose([0, 0, pred.robot.pose[:, start][0]])
body = pred._param_to_body[pred.robot].env_body
active_dof = body.GetManipulator('right_arm').GetArmIndices()
r_arm = pred.robot.rArmPose
traj = get_rrt_traj(plan.env, body, active_dof, r_arm[:, start],
r_arm[:, end])
result = process_traj(traj, end - start)
body.SetActiveDOFs(range(18))
for time in range(start + 1, end):
robot_attr_name_val_tuples = [('rArmPose', result[:,
time - start - 1])]
add_to_attr_inds_and_res(time, attr_inds, res, pred.robot,
robot_attr_name_val_tuples)
return np.array(res), attr_inds
def resample_eereachable(pred, negated, t, plan):
attr_inds, res = OrderedDict(), []
robot, rave_body = pred.robot, pred._param_to_body[pred.robot]
target_pos, target_rot = pred.ee_pose.value.flatten(
), pred.ee_pose.rotation.flatten()
body = rave_body.env_body
rave_body.set_pose([0, 0, robot.pose[0, t]])
# Resample poses at grasping time
grasp_arm_pose = get_ik_from_pose(target_pos, target_rot, body,
'right_arm')
add_to_attr_inds_and_res(t, attr_inds, res, robot,
[('rArmPose', grasp_arm_pose.copy())])
plan.sampling_trace.append({
'type': robot.get_type(),
'data': {
'rArmPose': grasp_arm_pose
},
'timestep': t,
'pred': pred,
'action': "grasp"
})
# Setting poses for environments to extract transform infos
dof_value_map = {
"lArmPose": robot.lArmPose[:, t].reshape((7, )),
"lGripper": 0.02,
"rArmPose": grasp_arm_pose,
"rGripper": 0.02
}
rave_body.set_dof(dof_value_map)
# Prepare grasping direction and lifting direction
manip_trans = body.GetManipulator("right_arm").GetTransform()
pose = OpenRAVEBody.obj_pose_from_transform(manip_trans)
manip_trans = OpenRAVEBody.get_ik_transform(pose[:3], pose[3:])
gripper_direction = manip_trans[:3, :3].dot(np.array([-1, 0, 0]))
lift_direction = manip_trans[:3, :3].dot(np.array([0, 0, -1]))
# Resample grasping and retreating traj
for i in range(const.EEREACHABLE_STEPS):
approach_pos = target_pos - gripper_direction / np.linalg.norm(
gripper_direction) * const.APPROACH_DIST * (3 - i)
# rave_body.set_pose([0,0,robot.pose[0, t-3+i]])
approach_arm_pose = get_ik_from_pose(approach_pos, target_rot, body,
'right_arm')
# rave_body.set_dof({"rArmPose": approach_arm_pose})
add_to_attr_inds_and_res(t - 3 + i, attr_inds, res, robot,
[('rArmPose', approach_arm_pose)])
retreat_pos = target_pos + lift_direction / np.linalg.norm(
lift_direction) * const.RETREAT_DIST * (i + 1)
# rave_body.set_pose([0,0,robot.pose[0, t+1+i]])
retreat_arm_pose = get_ik_from_pose(retreat_pos, target_rot, body,
'right_arm')
add_to_attr_inds_and_res(t + 1 + i, attr_inds, res, robot,
[('rArmPose', retreat_arm_pose)])
robot._free_attrs['rArmPose'][:, t - const.EEREACHABLE_STEPS:t +
const.EEREACHABLE_STEPS + 1] = 0
robot._free_attrs['pose'][:, t - const.EEREACHABLE_STEPS:t +
const.EEREACHABLE_STEPS + 1] = 0
return np.array(res), attr_inds
def resample_eereachable_rrt(pred, negated, t, plan, inv=False):
# Preparing the variables
attr_inds, res = OrderedDict(), []
robot, rave_body = pred.robot, pred.robot.openrave_body
target_pos, target_rot = pred.ee_pose.value.flatten(
), pred.ee_pose.rotation.flatten()
body = rave_body.env_body
manip_name = "right_arm"
active_dof = body.GetManipulator(manip_name).GetArmIndices()
active_dof = np.hstack([[0], active_dof])
# Make sure baxter is well positioned in the env
rave_body.set_pose([0, 0, robot.pose[:, t]])
rave_body.set_dof({
'lArmPose': robot.lArmPose[:, t].flatten(),
'rArmPose': robot.rArmPose[:, t].flatten(),
"lGripper": np.array([0.02]),
"rGripper": np.array([0.02])
})
for param in plan.params.values():
if not param.is_symbol() and param != robot:
param.openrave_body.set_pose(param.pose[:, t].flatten(),
param.rotation[:, t].flatten())
# Resample poses at grasping time
grasp_arm_pose = get_ik_from_pose(target_pos, target_rot, body, manip_name)
# When Ik infeasible
if grasp_arm_pose is None:
return None, None
add_to_attr_inds_and_res(t, attr_inds, res, robot,
[('rArmPose', grasp_arm_pose.copy()),
('pose', robot.pose[:, t])])
# Store sampled pose
plan.sampling_trace.append({
'type': robot.get_type(),
'data': {
'rArmPose': grasp_arm_pose
},
'timestep': t,
'pred': pred,
'action': "grasp"
})
# Prepare grasping direction and lifting direction
manip_trans = body.GetManipulator("right_arm").GetTransform()
pose = OpenRAVEBody.obj_pose_from_transform(manip_trans)
manip_trans = OpenRAVEBody.get_ik_transform(pose[:3], pose[3:])
if inv:
# inverse resample_eereachable used in putdown action
approach_dir = manip_trans[:3, :3].dot(np.array([0, 0, -1]))
retreat_dir = manip_trans[:3, :3].dot(np.array([-1, 0, 0]))
approach_dir = approach_dir / np.linalg.norm(
approach_dir) * const.APPROACH_DIST
retreat_dir = -retreat_dir / np.linalg.norm(
retreat_dir) * const.RETREAT_DIST
else:
# Normal resample eereachable used in grasp action
approach_dir = manip_trans[:3, :3].dot(np.array([-1, 0, 0]))
retreat_dir = manip_trans[:3, :3].dot(np.array([0, 0, -1]))
approach_dir = -approach_dir / np.linalg.norm(
approach_dir) * const.APPROACH_DIST
retreat_dir = retreat_dir / np.linalg.norm(
retreat_dir) * const.RETREAT_DIST
resample_failure = False
# Resample entire approaching and retreating traj
for i in range(const.EEREACHABLE_STEPS):
approach_pos = target_pos + approach_dir * (3 - i)
approach_arm_pose = get_ik_from_pose(approach_pos, target_rot, body,
'right_arm')
retreat_pos = target_pos + retreat_dir * (i + 1)
retreat_arm_pose = get_ik_from_pose(retreat_pos, target_rot, body,
'right_arm')
if approach_arm_pose is None or retreat_arm_pose is None:
resample_failure = True
add_to_attr_inds_and_res(t - 3 + i, attr_inds, res, robot,
[('rArmPose', approach_arm_pose)])
add_to_attr_inds_and_res(t + 1 + i, attr_inds, res, robot,
[('rArmPose', retreat_arm_pose)])
# Ik infeasible
if resample_failure:
plan.sampling_trace[-1]['reward'] = -1
return None, None
# lock the variables
robot._free_attrs['rArmPose'][:, t - const.EEREACHABLE_STEPS:t +
const.EEREACHABLE_STEPS + 1] = 0
robot._free_attrs['pose'][:, t - const.EEREACHABLE_STEPS:t +
const.EEREACHABLE_STEPS + 1] = 0
# finding initial pose
init_timestep, ref_index = 0, 0
for i in range(len(plan.actions)):
act_range = plan.actions[i].active_timesteps
if act_range[0] <= t <= act_range[1]:
init_timestep = act_range[0]
ref_index = i
if pred.ee_resample is True and ref_index > 0:
init_timestep = plan.actions[ref_index - 1].active_timesteps[0]
init_dof = robot.rArmPose[:, init_timestep].flatten()
init_dof = np.hstack([robot.pose[:, init_timestep], init_dof])
end_dof = robot.rArmPose[:, t - const.EEREACHABLE_STEPS].flatten()
end_dof = np.hstack([robot.pose[:, t - const.EEREACHABLE_STEPS], end_dof])
timesteps = t - const.EEREACHABLE_STEPS - init_timestep + 2
raw_traj = get_rrt_traj(plan.env, body, active_dof, init_dof, end_dof)
# Restore dof0
dof = body.GetActiveDOFValues()
dof[0] = 0
body.SetActiveDOFValues(dof)
# trajectory is infeasible
if raw_traj == None:
plan.sampling_trace[-1]['reward'] = -1
return None, None
# initailize feasible trajectory
result_traj = process_traj(raw_traj, timesteps).T[1:-1]
ts = 1
for traj in result_traj:
add_to_attr_inds_and_res(init_timestep + ts, attr_inds, res, robot,
[('rArmPose', traj[1:]), ('pose', traj[:1])])
ts += 1
pred.ee_resample = True
can = plan.params['can0']
can.openrave_body.set_pose(can.pose[:, t], can.rotation[:, t])
rave_body.set_dof({'rArmPose': robot.rArmPose[:, t]})
return np.array(res), attr_inds
