def execute_plan(self,speed,traj_type="query"): eclient = actionlib.SimpleActionClient('execute_plan', execute_planAction) eclient.wait_for_server() egoal = execute_planGoal() egoal.speed = speed egoal.traj_type = traj_type eclient.send_goal(egoal) eclient.wait_for_result() if eclient.get_state() != GoalStatus.SUCCEEDED: print "Failed to execute movement to target configuration" return False print "Target state achieved!" 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