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