Constrained move function

def move_ee_pose(arm_group):
    """
    Move end-effector to specified pose
    """
    current_pose = arm_group.get_current_pose(end_effector_link="tool0")
    print "current_pose:", current_pose
    # pose home
  # position:
    # x: 0.422562465178
    # y: 0.191221247515
    # z: 0.418782506626
  # orientation:
    # x: -0.000369305197861
    # y: 0.707106486605
    # z: 0.707106290394
    # w: 0.000987066022963

  # pose home + joint_goal[4] = pi/2
  # position:
    # x: 0.504844264658
    # y: 0.108320076232
    # z: 0.418621428128
  # orientation:
    # x: 0.500459099487
    # y: 0.498982558429
    # z: 0.49980940138
    # w: 0.500747100257
    joint_values = arm_group.get_current_joint_values()
    pose_goal.position.x = 0.504
    pose_goal.position.y = 0.108
    pose_goal.position.z = 0.418
    arm_group.set_position_target([pose_goal.position.x, pose_goal.position.y, pose_goal.position.z])
    # arm_group.set_pose_target(pose_goal)
    raw_input("Set constraints?")
    print "Const-0:", arm_group.get_known_constraints()
    joint_names = arm_group.get_active_joints()
    print "joint_names:", joint_names
    for i in range(len(joint_names)):
        if i<3:
            joint_constraint.joint_name = joint_names[i]
            joint_constraint.position = joint_values[i]
            joint_constraint.weight = 10.0 # Closer to zero means less important
            goal_constraint.joint_constraints.append(joint_constraint)
        elif i>=3:
            joint_constraint.weight = 0.0 # Closer to zero means less important
        else:
            print "Sth is wrong"
        print "i:", i

    print "Const-1:", arm_group.get_known_constraints()
    moveit_goal.request.goal_constraints.append(goal_constraint)
    print "Const-2:", arm_group.get_known_constraints()
    moveit_goal.request.goal_constraints.append(goal_constraint)
    moveit_goal.request.num_planning_attempts = 1
    moveit_goal.request.allowed_planning_time = 5.0
    moveit_goal.planning_options.plan_only = False
    moveit_goal.planning_options.planning_scene_diff.is_diff = True
    moveit_goal.request.group_name = arm_group

    pose_goal.motion_plan_request.goal_constraints.joint_constraints.append(goal_constraint)
    print "Const-3:", arm_group.get_known_constraints()
    arm_group.go(pose_goal, wait=True)
    print "Final joint values:", arm_group.get_current_joint_values()
    raw_input("Done")