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