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
