def __convert_to_name_vector(self, name_list):
name_vector = NameVector()
for name in name_list:
name_vector.append(name)
return name_vector
class SuhanMotionPlannerManager:
def __init__(self, argv, is_real=False):
self.working_directory = '/home/dyros/suhan_motion_planning'
self.debug_file_prefix = self.working_directory + '/logs/'
self.data_file_prefix = self.working_directory + '/database/'
self.is_real = is_real # True #to skip 'is_initial_joint_updated' when not using real robot
self.real_arm_num = 3
self.arm_num = 3
moveit_commander.roscpp_initialize(argv)
self.is_initial_joint_updated = [
False, False, False, False, False, False, False, False, False,
False, False, False, False, False, False, False, False, False,
False, False, False
]
self.planner = SuhanMotionPlanner()
self.planner.set_debug_file_prefix(self.debug_file_prefix)
self.max_velocity_scaling_factor = 0.2 #0.9
self.max_acceleration_scaling_factor = 1.0 #0.35
self.need_to_update_current_state = True
self.run_fast = True
self.obj_list = []
self.joint_states = np.zeros(7 * self.arm_num)
if self.real_arm_num == 2:
self.joint_states[7 * 2:7 * 3] = np.array(
[0, 0, 0, -pi / 3, 0, pi / 3, pi / 4])
self.is_initial_joint_updated[7 * 2:7 * 3] = [
True, True, True, True, True, True, True
]
self.joint_subscriber = rospy.Subscriber('/panda_dual/joint_states',
JointState,
self.__joint_state_callback)
# TF
self.listener = TransformListener()
self.transformer = TransformerROS()
self.broadcaster = TransformBroadcaster()
# Scene Object lists
# TODO: add objects
# self.object_data_dict = {'pin_1':[mesh_url, grasp_url, pos, quat],}
self.object_lists = ['pin_1', 'pin_2']
self.last_object_pose = {}
self.arm_names = ['panda_left', 'panda_right', 'panda_top']
arm = NameMap()
obj = NameMap()
arm['panda_left'] = 0
arm['panda_right'] = 1
arm['panda_top'] = 2
obj['dumbbell'] = 0
self.joint_map = {}
for i in range(7):
for name in self.arm_names:
self.joint_map[name + '_joint' +
str(i + 1)] = i + 7 * arm[name]
print(self.joint_map)
self.hand_touch_links = {}
self.hand_touch_links['panda_left'] = NameVector()
self.hand_touch_links['panda_right'] = NameVector()
self.hand_touch_links['panda_top'] = NameVector()
self.hand_touch_links['panda_left'].append('panda_left_hand')
self.hand_touch_links['panda_left'].append('panda_left_leftfinger')
self.hand_touch_links['panda_left'].append('panda_left_rightfinger')
self.hand_touch_links['panda_left'].append('panda_left_link7')
self.hand_touch_links['panda_left'].append('panda_left_link8')
self.hand_touch_links['panda_right'].append('panda_right_hand')
self.hand_touch_links['panda_right'].append(
'panda_right_finger_left_link')
self.hand_touch_links['panda_right'].append(
'panda_right_finger_right_link')
self.hand_touch_links['panda_right'].append('panda_right_link7')
self.hand_touch_links['panda_right'].append('panda_right_link8')
self.hand_touch_links['panda_top'].append('panda_top_hand')
self.hand_touch_links['panda_top'].append('panda_top_finger_left_link')
self.hand_touch_links['panda_top'].append(
'panda_top_finger_right_link')
self.hand_touch_links['panda_top'].append('panda_top_link7')
self.hand_touch_links['panda_top'].append('panda_top_link8')
self.collision_allowed_ids = NameVector()
self.collision_allowed_ids.append('ikea_stefan_side_left_0')
eye = np.array([0, 0, 0, 1])
reversed_eye = np.array([1, 0, 0, 0])
q_arm = np.array([
0, 0, 0, -pi / 3, 0, pi / 3, pi / 4, 0, 0, 0, -pi / 3, 0, pi / 3,
pi / 4, 0, 0, 0, -pi / 3, 0, pi / 3, pi / 4
])
ee_pos = np.array([0, 0, 0.103])
# ee_pos_R = np.array([0, 0, 0.083])
ee_pos_R = np.array([0, 0, 0.078]) # for test 11-12 khc
ee_quat = np.array([0, 0, 0, 1])
# planner needs to be initialized
self.planner.set_environment(arm, obj)
self.planner.set_sigma(0.4)
self.planner.set_max_ik_trials(10000)
self.planner.set_max_planning_time(60)
self.planner.set_sample_rviz(True)
self.planner.set_robot_transform('panda_left',
np.array([0, 0.3, 1.006]), eye)
self.planner.set_robot_transform('panda_right',
np.array([0, -0.3, 1.006]), eye)
self.planner.set_robot_transform('panda_top',
np.array([1.35, 0.3, 1.006]),
np.array([0, 0, 1, 0]))
# pos, quat = self.__convert_to_vec_quat(T_DXL_CTR)
# print (pos, quat)
self.planner.set_end_effector_frame('panda_left', ee_pos, ee_quat)
self.planner.set_end_effector_frame('panda_right', ee_pos_R, ee_quat)
self.planner.set_end_effector_frame('panda_top', ee_pos_R, ee_quat)
self.planner.set_end_effector_margin('panda_left',
np.array([0, 0, 0.03]), eye)
self.planner.set_end_effector_margin('panda_right',
np.array([0, 0, 0.03]), eye)
self.planner.set_end_effector_margin('panda_top',
np.array([0, 0, 0.03]), eye)
self.long_planner = 'rrt_connect' #
self.planner.set_chomp_postprocess(False)
# self.long_planner = 'rrt_connect' #
# self.planner.set_planner('ait_star')
# Calib dasta with closed chain calibration with 2.09e-5 eval
calib_dh_left = np.array(
[[
0.000474560429981023, 0.000483166682165302,
-0.00304883355188383, 0.00148667086907321
],
[
-3.69828865924539e-05, -0.000288069909956647,
-0.00812428761844092, 0.00421567136144437
],
[
-0.000154357131719552, -0.0010921364777817,
0.00031894496234845, -0.0030474925191138
],
[
-0.000117600404870226, 0.000712982958085577,
-0.00571261767823764, 0.00176867969486185
],
[
-0.00058993701921134, -0.000326649645213642,
0.00939394386245098, 0.00123723772258799
],
[
-0.000433705606644922, -0.000293762477507038,
-0.0156742348127345, -0.00529320945025931
],
[
-0.000589815315429364, 6.2389274666678e-05,
0.0291501803388187, 0.00113202442328629
]])
calib_dh_right = np.array(
[[
-0.00153055068483914, -0.000772485919009139,
-0.00374187596482555, -0.00183369895482027
],
[
0.000163834922530543, -0.000212890734425727,
-0.0041768339596184, 0.00292223805919776
],
[
-2.60271767179549e-05, -0.00100930869860036,
0.00208915153420725, -0.00362801030623702
],
[
0.0002126348171224, 0.00108679894872676, 9.81965015663654e-05,
0.00292912798333623
],
[
-0.00136529072609946, -5.62799006356164e-05,
0.0139799357258803, -0.00122374898174726
],
[
-0.000502587880406569, 0.000336192838117574,
-0.0139808833528828, -0.00268675457630875
],
[
-0.00104647166032287, 0.000135297170834114,
0.00498364620994882, 0.000349775306996936
]])
calib_dh_top = np.array(
[[
-0.000171539356569936, 0.000591551783574421,
0.00119525002639617, -0.00650097874689066
],
[
-0.000330274824097498, -0.000124214443868683,
7.10962210595555e-05, 0.00166835357174836
],
[
-0.00027921463294522, -0.000683991542242173,
0.00203760570771006, -0.0018819778569208
],
[
-0.000355247972007034, 0.000891508331427601,
-0.00513318787489872, 0.000168196477584221
],
[
0.000190817101859644, -0.000635118518697479,
0.00445732420517835, -0.00167223312645046
],
[
-0.000244350284995939, -0.000209543585115151,
0.000118913154576129, -0.00356076901549127
],
[
-0.000484192538997231, 4.41640702632187e-05,
-0.0155483388661319, 0.000602582057747216
]])
self.planner.set_calibration_data('panda_left', calib_dh_left)
self.planner.set_calibration_data('panda_right', calib_dh_right)
self.planner.set_calibration_data('panda_top', calib_dh_top)
# let's add some collision meshes for the objects
# Note: Use URL (package:// ... )
# also, grasp candidates should be given.
# Note: YOU SHOULD USE PATH NOT URL (use get_path_from_url function)
# TODO: use this
self.planner.add_box(np.array([0.65, 1.0, 0.2]), 'table',
np.array([0.65, 0.0, 1.1]), np.array([0, 0, 0,
1]))
self.obj_list.append('table')
self.planner.add_box(np.array([0.5, 0.08, 0.1]), 'obstacle',
np.array([0.65, 0.1, 1.3]), np.array([0, 0, 0,
1]))
self.obj_list.append('obstacle')
# for the first state
self.planner.set_start_arm_states(q_arm)
# self.planner.print_start_state()
# for the visualization
# q_arm[7:14] = np.array([-0.516055, 0.377285, 0.567762, -2.48491, -2.54167, 1.81389, 2.01595])
self.planner.update_arm_states(q_arm)
# print('q_arm: ', q_arm)
# self.planner.print_current_collision_infos()
# self.planner.enable_tf_broadcasting()
self.planning_scene = self.planner.get_planning_scene_collision_check()
self.ik_task = self.planner.get_ik_task()
self.object_ik_task = self.planner.get_object_ik_task()
self.assembly_ik_task = self.planner.get_assembly_ik_task()
self.dual_arm_task = self.planner.get_dual_arm_task()
self.dual_arm_task.set_delta(0.25)
# self.dual_arm_task.set_range(0.5)
self.dual_arm_task.set_max_trials(10000)
self.dual_arm_task.set_cost_limit(100.0)
self.dual_arm_task.set_enable_sample_viz(True)
self.triple_ik_task = self.planner.get_triple_ik_task()
self.orientation_constrained_task = self.planner.get_orientation_constrained_task(
)
self.orientation_constrained_task.set_enable_sample_viz(False)
self.orientation_constrained_task.set_cost_limit(7.0)
self.ik_task.set_grad_rate(5e-2)
self.ik_task.set_grad_stop_threshold(3e-2)
self.ik_task.set_max_opt_trials(20)
self.ik_task.set_cost_limit(5.0)
self.ik_task.set_enable_sample_viz(True)
self.object_ik_task.set_grad_rate(5e-2)
self.object_ik_task.set_grad_stop_threshold(2e-2)
self.object_ik_task.set_max_trials(10000)
self.object_ik_task.set_max_opt_trials(20)
self.object_ik_task.set_cost_limit(7.0)
self.object_ik_task.set_enable_sample_viz(True)
self.assembly_ik_task.set_grad_rate(5e-2)
self.assembly_ik_task.set_grad_stop_threshold(2e-2)
self.assembly_ik_task.set_max_opt_trials(20)
self.assembly_ik_task.set_cost_limit(5.0)
self.assembly_ik_task.set_enable_sample_viz(True)
self.triple_ik_task.set_grad_rate(5e-2)
self.triple_ik_task.set_grad_stop_threshold(2e-2)
self.triple_ik_task.set_max_opt_trials(20)
self.triple_ik_task.set_cost_limit(8.0)
self.triple_ik_task.set_enable_sample_viz(True)
self.controller_not_working = False
self.start_q_threshold = 0.087266444 # rad
if self.is_real:
is_completed = False
while is_completed is False and rospy.is_shutdown() is False:
is_completed = True
for value in self.is_initial_joint_updated:
if value is False:
is_completed = False
print(
'waiting for joint states. please use SuhanMotionPlannerManager(sys.argv, is_real=False) if you are using this in just sim mode.'
)
rospy.sleep(0.1)
else:
self.joint_states = np.array([
0, 0, 0, -pi / 3, 0, pi / 3, pi / 4, 0, 0, 0, -pi / 3, 0,
pi / 3, pi / 4, 0, 0, 0, -pi / 3, 0, pi / 3, pi / 4
])
# else:
# self.joint_states = np.array([])
def __del__(self):
self.save_ik_solutions()
moveit_commander.roscpp_shutdown()
def __joint_state_callback(self, data):
for i in range(0, len(joint_names)):
for j in range(0, len(data.name)):
if joint_names[i] == data.name[j]:
self.joint_states[i] = data.position[j]
self.is_initial_joint_updated[i] = True
# print(self.joint_states)
def __robot_error_callback(self, subscriber):
# print("test_subscribe")
if subscriber.data is True:
self.controller_not_working = True
def __update_current_state(self):
# TODO: implement this code (insepct TF, /panda_dual/joint_states)
# Noise option (?)
self.planner.update_arm_states(self.joint_states)
# self.planner.publish_planning_scene_msg()
if self.need_to_update_current_state is False:
return
self.planner.set_start_arm_states(self.joint_states)
for col_object in self.object_lists:
# get obj pose
# pos = ?
# quat = ?
# self.planner.update_object_states(col_object, pos, quat)
# self.planner.set_start_object_states(col_object, pos, quat)
pass
pass
self.print_current_collision_info()
def __convert_to_name_vector(self, name_list):
name_vector = NameVector()
for name in name_list:
name_vector.append(name)
return name_vector
def __convert_to_param_vector(self, param_list):
param_vector = ParamVector()
for param in param_list:
param_vector.append(param)
return param_vector
def __convert_to_vec_quat(self, matrix):
pos = np.empty(3)
pos[0:3] = matrix[0:3, 3].T
quat = quaternion_from_matrix(matrix)
return pos, quat
def __load_pickles(self, name):
if name == None:
return None
file_name = self.data_file_prefix + name + '.pkl'
if os.path.isfile(file_name):
with open(file_name, 'rb') as f:
return pickle.load(f)
else:
return None
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 __process_pickles(self, path):
if path is None:
raise RuntimeError(
'Why did you call me even if the path is empty? Please modify the codes'
)
start_q = path[0]
diff = np.linalg.norm(self.joint_states - start_q)
print('process_pickle - diff:', diff)
if (diff < self.start_q_threshold):
self.planner.add_start_to_path(self.joint_states)
new_path = np.concatenate(([self.joint_states], path), axis=0)
return self.__path_to_trajectory(new_path)
print(
'the initial q of the given joint path is too far away from the current state'
)
print('self.joint_states', self.joint_states)
print('start_q', start_q)
print('try to plan')
for i in range(5):
print(' - try', i + 1)
r = self.planner.solve_for_joint_pose_all(start_q)
if r == True:
print('okay we planner the safe motion to the start')
path_pre = self.planner.get_solved_path()
new_path = np.concatenate((path_pre, path), axis=0)
return self.__path_to_trajectory(new_path)
raise RuntimeError('plan failed (cannot move to saved pickle)')
def __save_pickles(self, name, data):
if name == None:
return
with open(self.data_file_prefix + name + '.pkl', 'wb') as f:
pickle.dump(data, f)
def __check_safe_diff(self, q1, q2):
d = LA.norm(q1 - q2)
if d > 0.1:
return False
return True
def __set_planner_for_coarse_motion(self, name):
if name is None or self.run_fast:
self.planner.set_planner('rrt_connect')
else:
self.planner.set_planner(self.long_planner)
def check_robot_error(self):
self.controller_not_working = False
sub_once = None
sub_once = rospy.Subscriber('/panda_dual/panda_left/has_error', Bool,
self.__robot_error_callback, sub_once)
sub_once = None
sub_once = rospy.Subscriber('/panda_dual/panda_right/has_error', Bool,
self.__robot_error_callback, sub_once)
sub_once = None
sub_once = rospy.Subscriber('/panda_dual/panda_top/has_error', Bool,
self.__robot_error_callback, sub_once)
rospy.sleep(0.05)
# sub_once.unregister()
if self.controller_not_working:
print('-------------------------------------------------')
print('controller not working ... attempting UnlockError')
print('-------------------------------------------------')
UnlockErrorSM().execute()
rospy.sleep(0.7)
def update_joint_states(self):
self.planner.update_arm_states(self.joint_states)
self.planner.set_start_arm_states(self.joint_states)
def display_joint_and_env(self):
while rospy.is_shutdown() is False:
self.planner.update_arm_states(self.joint_states)
self.planner.set_start_arm_states(self.joint_states)
self.planner.publish_planning_scene_msg()
rospy.sleep(0.1)
def display_joint_and_env_given(self, joint):
while rospy.is_shutdown() is False:
self.planner.update_arm_states(joint)
self.planner.set_start_arm_states(joint)
self.planner.publish_planning_scene_msg()
rospy.sleep(0.1)
def set_planning_time_default(self):
self.planner.set_sigma(0.3)
self.planner.set_max_ik_trials(20000)
self.planner.set_max_planning_time(10)
def disable_collision_with_arm(self, arm_name, object_id, enable):
for i in range(7):
self.change_collision('{0}_link{1}'.format(arm_name, i + 1),
object_id, enable)
self.change_collision(arm_name + '_hand', object_id, enable)
if arm_name != 'panda_left':
self.change_collision(arm_name + '_finger_left_link', object_id,
enable)
self.change_collision(arm_name + '_finger_right_link', object_id,
enable)
def set_collision_arm(self, arm_name):
not_col_obs = self.planning_scene.get_all_attached_objects()
for obj in self.obj_list:
if obj in not_col_obs:
continue
for i in range(7):
self.change_collision('{0}_link{1}'.format(arm_name, i + 1),
obj, True)
self.change_collision(arm_name + '_hand', obj, True)
if arm_name != 'panda_left':
self.change_collision(arm_name + '_finger_left_link', obj,
True)
self.change_collision(arm_name + '_finger_right_link', obj,
True)
def reset_collision_arm(self, arm_name):
for obj in self.obj_list:
for i in range(7):
self.change_collision('{0}_link{1}'.format(arm_name, i + 1),
obj, False)
self.change_collision(arm_name + '_hand', obj, True)
if arm_name != 'panda_left':
self.change_collision(arm_name + '_finger_left_link', obj,
False)
self.change_collision(arm_name + '_finger_right_link', obj,
False)
def reset_collision_all_arm(self):
arm_list = ['panda_left', 'panda_right', 'panda_top']
for arm in arm_list:
for obj in self.obj_list:
for i in range(7):
self.change_collision('{0}_link{1}'.format(arm, i + 1),
obj, False)
self.change_collision(arm + '_hand', obj, False)
if arm != 'panda_left':
self.change_collision(arm + '_finger_left_link', obj,
False)
self.change_collision(arm + '_finger_right_link', obj,
False)
def set_fixed_arm(self, arm_name):
self.set_collision_arm(arm_name)
self.planner.set_arm_fix(arm_name, True)
def reset_fixed_arm(self, arm_name):
print('reset_fixed_arm1')
self.reset_collision_arm(arm_name)
self.planner.set_arm_fix(arm_name, False)
def reset_fixed_arm_all(self):
self.reset_collision_all_arm()
self.planner.reset_fix()
def get_kinematics(self, arm_name):
self.__update_current_state()
pos = np.empty(3)
quat = np.empty(4)
self.planner.get_arm_ee_pose(arm_name, pos, quat)
return pos, quat
# transform w(base) - 0(robot_base) - 7(flange) - e(end_effector) - o(furniture_base)
def calc_obj_pos(self, arm_name, object_id, grasp_param):
T_w0_ = np.eye(4)
if arm_name == 'panda_left':
T_w0_[0:3, 3] = np.array([0.0, 0.3, 1.0])
elif arm_name == 'panda_right':
T_w0_[0:3, 3] = np.array([0.0, -0.3, 1.0])
elif arm_name == 'panda_top':
T_w0_[0, 0] = -1.0
T_w0_[1, 1] = -1.0
T_w0_[0:3, 3] = np.array([1.35, 0.3, 1.0])
p_07_, q_07_ = self.get_kinematics(arm_name)
T_07_ = quaternion_matrix(q_07_)
T_07_[0:3, 3] = p_07_
T_7e_ = np.eye(4)
T_7e_[0:3, 3] = np.array([0.0, 0.0, 0.0775])
p_oe_, q_oe_ = self.get_grasp_pose(object_id, grasp_param)
T_oe_ = quaternion_matrix(q_oe_)
T_oe_[0:3, 3] = p_oe_
T_wo_ = np.dot(T_w0_, np.dot(np.dot(T_07_, T_7e_),
np.linalg.inv(T_oe_)))
quat = quaternion_from_matrix(T_wo_)
pos = translation_from_matrix(T_wo_)
return pos, quat
# def calc_obj_pos(self,arm_name,object_id, grasp_index, grasp_param):
# T_w0_ = np.eye(4)
# if arm_name == 'panda_left':
# T_w0_[0:3,3] = np.array([0.0, 0.3, 1.0])
# elif arm_name == 'panda_right':
# T_w0_[0:3,3] = np.array([0.0, -0.3, 1.0])
# elif arm_name == 'panda_top':
# T_w0_[0,0] = -1.0
# T_w0_[1,1] = -1.0
# T_w0_[0:3,3] = np.array([1.35, 0.3, 1.0])
# p_07_, q_07_ = self.get_kinematics(arm_name)
# T_07_ = quaternion_matrix(q_07_)
# T_07_[0:3,3] = p_07_
# T_7e_ = np.eye(4)
# T_7e_[0:3,3] = np.array([0.0, 0.0, 0.0775])
# p_oe_, q_oe_ = self.get_grasp_pose(object_id, grasp_index, grasp_param)
# T_oe_ = quaternion_matrix(q_oe_)
# T_oe_[0:3,3] = p_oe_
# T_wo_ = np.dot(T_w0_,np.dot(np.dot(T_07_,T_7e_),np.linalg.inv(T_oe_)))
# quat = quaternion_from_matrix(T_wo_)
# pos = translation_from_matrix(T_wo_)
# return pos, quat
# transform w(base) - 0(robot_base) - 7(flange) - e(end_effector) - o(furniture_base)
def calc_obj_pos_point(self, arm_name, T_oe_):
T_w0_ = np.eye(4)
if arm_name == 'panda_left':
T_w0_[0:3, 3] = np.array([0.0, 0.3, 1.0])
elif arm_name == 'panda_right':
T_w0_[0:3, 3] = np.array([0.0, -0.3, 1.0])
elif arm_name == 'panda_top':
T_w0_[0, 0] = -1.0
T_w0_[1, 1] = -1.0
T_w0_[0:3, 3] = np.array([1.35, 0.3, 1.0])
p_07_, q_07_ = self.get_kinematics(arm_name)
T_07_ = quaternion_matrix(q_07_)
T_07_[0:3, 3] = p_07_
T_7e_ = np.eye(4)
T_7e_[0:3, 3] = np.array([0.0, 0.0, 0.0825])
T_wo_ = np.dot(T_w0_, np.dot(np.dot(T_07_, T_7e_),
np.linalg.inv(T_oe_)))
quat = quaternion_from_matrix(T_wo_)
pos = translation_from_matrix(T_wo_)
return pos, quat
def update_object_states(self, object_list):
for obj in object_list:
self.update_object_states_each(obj[0], obj[1], obj[2])
def update_box_states(self, box_list):
for box in box_list:
self.planner.update_object_states(box[0], box[1], box[2])
def get_last_object_state(self, object_name):
return self.last_object_pose[object_name]
def save_ik_solutions(self):
self.planner.save_ik_solutions()
def attach_object(self, arm_name, object_id, is_arm=True):
self.__update_current_state()
self.planner.attach_object(object_id, arm_name + '_hand',
self.hand_touch_links[arm_name])
self.planner.publish_planning_scene_msg()
def detach_object(self, arm_name, object_id):
self.__update_current_state()
self.planner.detach_object(object_id, arm_name + '_hand')
def remove_collision_object(self, name1):
self.__update_current_state()
self.planner.remove_collision_object(name1)
# change collision state with name1 and name2 / ignore collision when True
def change_collision(self, name1, name2, allowed):
# self.__update_current_state()
self.planner.change_collision(name1, name2, allowed)
# change collision state with name1 and all the other elements that were used in this function / ignore collision when True
def change_collisions(self, name1, allowed):
# self.__update_current_state()
self.planner.change_collisions(name1, self.collision_allowed_ids,
allowed)
if allowed:
no_overlap_ = True
for i in range(len(self.collision_allowed_ids)):
if self.collision_allowed_ids[i] == name1:
no_overlap_ = False
if no_overlap_: self.collision_allowed_ids.append(name1)
else:
for i in range(len(self.collision_allowed_ids)):
if self.collision_allowed_ids[i] == name1:
self.collision_allowed_ids.erase(
self.collision_allowed_ids.begin() + i)
break
# change collision state with name1 and all the other elements / ignore collision when True
def change_collisions_all(self, name1, allowed):
self.__update_current_state()
self.planner.change_collisions_all(name1, allowed)
if allowed:
no_overlap_ = True
for i in range(len(self.collision_allowed_ids)):
if self.collision_allowed_ids[i] == name1:
no_overlap_ = False
if no_overlap_: self.collision_allowed_ids.append(name1)
else:
for i in range(len(self.collision_allowed_ids)):
if self.collision_allowed_ids[i] == name1:
self.collision_allowed_ids.erase(
self.collision_allowed_ids.begin() + i)
break
def print_current_collision_info(self):
self.planner.print_current_collision_infos()
def make_follow_trajectory_goal(self, trajectory):
if self.is_real == False:
display_from_trajectory_msg(self.planner, trajectory,
self.joint_states, self.joint_map)
q = np.array(trajectory.points[-1].positions)
for i in range(len(trajectory.joint_names)):
idx = self.joint_map[trajectory.joint_names[i]]
self.joint_states[idx] = q[i]
return_goal = FollowJointTrajectoryGoal()
return_goal.trajectory = trajectory
return return_goal
def plan_joint_pose(self, arm_name, joint_pose, 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_joint_pose(arm_name, joint_pose)
if r == False:
raise RuntimeError('plan failed')
self.planner.time_parameterize(self.max_velocity_scaling_factor,
self.max_acceleration_scaling_factor)
path = self.planner.get_solved_path()
self.planner.update_arm_states(path[-1])
self.planner.set_start_arm_states(path[-1])
trajectory = to_trajectory_msgs(self.planner)
self.__save_pickles(name, path)
self.reset_collision_all_arm()
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
def plan_pickle(self, name):
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
raise RuntimeError('plan failed')
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
# CAUTION: this resets all fixed arms
def plan_dual_arm_pose(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:
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()
pos, quat = self.last_object_pose[object_id]
self.dual_arm_task.set_arm_names(
self.__convert_to_name_vector(arm_names))
print('joints:', self.joint_states)
self.dual_arm_task.set_start_and_goal(self.joint_states, pos, quat,
desired_pos, desired_quat)
self.dual_arm_task.set_timeout_limit(360.0)
r = self.dual_arm_task.solve()
if r == False:
raise RuntimeError('plan failed')
path = self.dual_arm_task.get_path()
self.planner.set_solved_path(path)
self.planner.time_parameterize(0.05, 0.2)
# for dual traj
print(arm_names)
for arm in self.arm_names:
if arm in arm_names:
continue
self.planner.set_arm_fix(arm, True)
print('fix', arm)
trajectory = to_trajectory_msgs(self.planner)
# reset
self.planner.reset_fix()
self.__save_pickles(name, path)
self.reset_collision_all_arm()
return trajectory
# CAUTION: this resets all fixed arms
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 display_path(self, dt=0.01):
display(self.planner, dt)
def get_moved_joint_pose(self, arm_name, pos, quat, q0):
q_out = np.zeros(7)
suc = self.planner.get_moved_joints(arm_name, pos, quat, q0, q_out)
print('q0', q0)
print('q_', q_out)
if suc is False:
print("Fail")
return None
else:
return q_out
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