def reproject_path(self, path_file, tol, iteration, idx, offset):
tt = []
with open(path_file, "r") as file:
for line in file.readlines():
split_line = line.split(" ")
if (len(split_line) > 21):
split_line.pop(-1)
if (len(split_line) < 21):
continue
for i in range(21):
split_line[i] = float(split_line[i])
tt.append(split_line)
path = np.array(tt)
self.planner.set_solved_path(path)
self.__update_current_state()
print(offset)
success = self.planner.reproject_path(self.joint_states, tol,
iteration, idx, offset)
if success is True:
print("PROJECTION SUCCESS!!")
else:
print("PROJECTION FAILED")
self.planner.time_parameterize(self.max_velocity_scaling_factor,
self.max_acceleration_scaling_factor)
trajectory = to_trajectory_msgs(self.planner)
return trajectory
def plan_target_pose(self, arm_name, target_pos, target_quat, name=None):
if self.is_real: self.check_robot_error()
self.__update_current_state()
path = self.__load_pickles(name)
if path is not None:
return self.__process_pickles(path)
# self.planner.set_solved_path(path)
# self.planner.add_start_to_path(self.joint_states)
# self.planner.time_parameterize(self.max_velocity_scaling_factor, self.max_acceleration_scaling_factor)
# trajectory = to_trajectory_msgs(self.planner)
# self.reset_collision_all_arm()
# return trajectory
self.__update_current_state()
self.__set_planner_for_coarse_motion(name)
r = self.planner.solve_for_ik_pose(arm_name, target_pos, target_quat)
if r == False:
raise RuntimeError('plan failed')
self.planner.time_parameterize(self.max_velocity_scaling_factor,
self.max_acceleration_scaling_factor)
trajectory = to_trajectory_msgs(self.planner)
path = self.planner.get_solved_path()
self.__save_pickles(name, path)
self.reset_collision_all_arm()
return trajectory
def __path_to_trajectory(self, path):
self.planner.set_solved_path(path)
self.planner.time_parameterize(self.max_velocity_scaling_factor,
self.max_acceleration_scaling_factor)
trajectory = to_trajectory_msgs(self.planner)
self.reset_collision_all_arm()
return trajectory
def plan_dual_arm_pose2(self,
arm_names,
object_id,
desired_pos,
desired_quat,
name=None):
if self.is_real: self.check_robot_error()
self.__update_current_state()
path = self.__load_pickles(name)
if path is not None:
self.planner.set_solved_path(path)
self.planner.add_start_to_path(self.joint_states)
self.planner.time_parameterize(
self.max_velocity_scaling_factor,
self.max_acceleration_scaling_factor)
trajectory = to_trajectory_msgs(self.planner)
self.reset_collision_all_arm()
return trajectory
pos, quat = self.last_object_pose[object_id]
self.dual_arm_task.set_arm_names(
self.__convert_to_name_vector(arm_names))
self.dual_arm_task.set_start_and_goal(self.joint_states, pos, quat,
desired_pos, desired_quat)
r = self.dual_arm_task.solve2()
if r == False:
raise RuntimeError('plan failed')
path = self.dual_arm_task.get_path()
self.planner.set_solved_path(path)
self.planner.time_parameterize(self.max_velocity_scaling_factor,
self.max_acceleration_scaling_factor)
# for dual traj
for arm in arm_names:
if arm in self.arm_names:
continue
self.planner.set_arm_fix(arm, True)
trajectory = to_trajectory_msgs(self.planner)
# reset
self.planner.reset_fix()
self.__save_pickles(name, path)
self.reset_collision_all_arm()
return trajectory
def plan_target_pose(self, arm_name, target_pos, target_quat):
self.__update_current_state()
r = self.planner.solve_for_ik_pose(arm_name, target_pos, target_quat)
if r == False:
return None
self.planner.time_parameterize(self.max_velocity_scaling_factor, self.max_acceleration_scaling_factor)
trajectory = to_trajectory_msgs(self.planner)
return trajectory
def plan_object_ik(self, arm_name, object_id, grasp_param = 0.7):
self.__update_current_state()
self.planner.print_start_state()
r = self.planner.solve_for_object_ik_pose(arm_name, object_id, grasp_param)
# print(r)
if r == False:
return None
self.planner.time_parameterize(self.max_velocity_scaling_factor, self.max_acceleration_scaling_factor)
trajectory = to_trajectory_msgs(self.planner)
return trajectory
def plan_joint_pose_all(self, joint_pose):
if len(joint_pose) is not 7*self.arm_num:
print('error: len(joint_pose) is not', str(7*self.arm_num) ,'but', len(joint_pose))
return None
self.__update_current_state()
r = self.planner.solve_for_joint_pose_all(joint_pose)
if r == False:
return None
self.planner.time_parameterize(self.max_velocity_scaling_factor, self.max_acceleration_scaling_factor)
trajectory = to_trajectory_msgs(self.planner)
return trajectory
def plan_given_path(self, path, name=None):
if self.is_real: self.check_robot_error()
self.__update_current_state()
path = self.__load_pickles(name)
if path is not None:
return self.__process_pickles(path)
# self.planner.set_solved_path(path)
# self.planner.add_start_to_path(self.joint_states)
# self.planner.time_parameterize(self.max_velocity_scaling_factor, self.max_acceleration_scaling_factor)
# trajectory = to_trajectory_msgs(self.planner)
# return trajectory
self.planner.set_solved_path(path)
self.planner.add_start_to_path(self.joint_states)
self.planner.time_parameterize(self.max_velocity_scaling_factor,
self.max_acceleration_scaling_factor)
trajectory = to_trajectory_msgs(self.planner)
self.__save_pickles(name, path)
return trajectory
