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
