def move_to_state(self, the_arm, the_state, speed, origin_or_order_bin ): end_state = JointState(); if the_arm == LEFT_ARM: end_state.name.append("arm_left_joint_1_s"); end_state.name.append("arm_left_joint_2_l"); end_state.name.append("arm_left_joint_3_e"); end_state.name.append("arm_left_joint_4_u"); end_state.name.append("arm_left_joint_5_r"); end_state.name.append("arm_left_joint_6_b"); end_state.name.append("arm_left_joint_7_t"); end_state.name.append("torso_joint_b1"); end_state.name.append("head_hinge"); else: end_state.name.append("arm_right_joint_1_s"); end_state.name.append("arm_right_joint_2_l"); end_state.name.append("arm_right_joint_3_e"); end_state.name.append("arm_right_joint_4_u"); end_state.name.append("arm_right_joint_5_r"); end_state.name.append("arm_right_joint_6_b"); end_state.name.append("arm_right_joint_7_t"); end_state.name.append("torso_joint_b1"); end_state.position.append(the_state[end_state.name[0]]); end_state.position.append(the_state[end_state.name[1]]); end_state.position.append(the_state[end_state.name[2]]); end_state.position.append(the_state[end_state.name[3]]); end_state.position.append(the_state[end_state.name[4]]); end_state.position.append(the_state[end_state.name[5]]); end_state.position.append(the_state[end_state.name[6]]); end_state.position.append(the_state[end_state.name[7]]); if the_arm == LEFT_ARM: end_state.position.append(the_state[end_state.name[8]]); cclient = actionlib.SimpleActionClient('compute_plan', compute_planAction) cclient.wait_for_server() cgoal = compute_planGoal() cgoal.end_state = end_state if the_arm == LEFT_ARM: cgoal.arm = "left" else: cgoal.arm = "right" cgoal.dest_type = origin_or_order_bin cclient.send_goal(cgoal) cclient.wait_for_result() if cclient.get_state() != GoalStatus.SUCCEEDED: print "Failed to plan to target configuration" return False print "We have a plan to go to a specific configuration!" return self.execute_plan(speed);
def compute_plan(self, arm, target_configuration, speed): end_state = JointState(); for key in target_configuration.keys(): end_state.name.append(key) end_state.position.append(target_configuration[key]) cclient = actionlib.SimpleActionClient('compute_plan', compute_planAction) cclient.wait_for_server() cgoal = compute_planGoal() cgoal.end_state = end_state cgoal.arm = arm cgoal.dest_type = 'order_bin' cclient.send_goal(cgoal) cclient.wait_for_result() if cclient.get_state() != GoalStatus.SUCCEEDED: print "@@@ Failed to plan to target configuration" return False print "@@@ We have a plan to go to a specific configuration (the plan is stored inside the planner)!" return True
def plan_and_move_to_joints_configuration(joints_configuration, speed): # global g_move_group global g_arm_choice end_state = JointState() if g_arm_choice == "left": end_state.name.append("arm_left_joint_1_s") end_state.name.append("arm_left_joint_2_l") end_state.name.append("arm_left_joint_3_e") end_state.name.append("arm_left_joint_4_u") end_state.name.append("arm_left_joint_5_r") end_state.name.append("arm_left_joint_6_b") end_state.name.append("arm_left_joint_7_t") end_state.name.append("torso_joint_b1") end_state.name.append("head_hinge") end_state.position.append(joints_configuration[end_state.name[0]]) end_state.position.append(joints_configuration[end_state.name[1]]) end_state.position.append(joints_configuration[end_state.name[2]]) end_state.position.append(joints_configuration[end_state.name[3]]) end_state.position.append(joints_configuration[end_state.name[4]]) end_state.position.append(joints_configuration[end_state.name[5]]) end_state.position.append(joints_configuration[end_state.name[6]]) end_state.position.append(joints_configuration[end_state.name[7]]) end_state.position.append(joints_configuration[end_state.name[8]]) else: end_state.name.append("arm_right_joint_1_s") end_state.name.append("arm_right_joint_2_l") end_state.name.append("arm_right_joint_3_e") end_state.name.append("arm_right_joint_4_u") end_state.name.append("arm_right_joint_5_r") end_state.name.append("arm_right_joint_6_b") end_state.name.append("arm_right_joint_7_t") end_state.name.append("torso_joint_b1") end_state.position.append(joints_configuration[end_state.name[0]]) end_state.position.append(joints_configuration[end_state.name[1]]) end_state.position.append(joints_configuration[end_state.name[2]]) end_state.position.append(joints_configuration[end_state.name[3]]) end_state.position.append(joints_configuration[end_state.name[4]]) end_state.position.append(joints_configuration[end_state.name[5]]) end_state.position.append(joints_configuration[end_state.name[6]]) end_state.position.append(joints_configuration[end_state.name[7]]) print "plan_and_move_to_joints_configuration()" cclient = actionlib.SimpleActionClient("compute_plan", compute_planAction) cclient.wait_for_server() cgoal = compute_planGoal() cgoal.end_state = end_state cgoal.arm = g_arm_choice cclient.send_goal(cgoal) cclient.wait_for_result() if cclient.get_state() != GoalStatus.SUCCEEDED: print "Failed to go to target joints configuration " return False print "We have a plan to go to a specific joints configuration!" eclient = actionlib.SimpleActionClient("execute_plan", execute_planAction) eclient.wait_for_server() egoal = execute_planGoal() egoal.speed = speed eclient.send_goal(egoal) eclient.wait_for_result() if eclient.get_state() != GoalStatus.SUCCEEDED: return False print "Joints configuration achieved OKKKKK!!!" return True