def poseFrontPreGrasp():
        x = 0.25
        y = 0
        z = 0.10
        roll = 0
        pitch = math.pi
        yaw = 0
        return createPose(x, y, z, roll, pitch, yaw)
 
 
if __name__ == "__main__":
    rospy.init_node('youbot_ik_solver_test')
    time.sleep(0.5)
    
    an = ArmConfiguration()
    
    #rospy.sleep(5.0)
    
    print "moveto config"
    #an.moveToConfiguration("zeroposition")
    
    
    an.moveGripperOpen()
    
    rospy.sleep(1.0)
    
    an.moveGripperClose()
    
    rospy.sleep(1.0)
    
    
def poseFrontPreGrasp():
        x = 0.25
        y = 0
        z = 0.10
        roll = 0
        pitch = math.pi
        yaw = 0
        return createPose(x, y, z, roll, pitch, yaw)
 
 
if __name__ == "__main__":
    rospy.init_node('youbot_ik_solver_test')
    time.sleep(0.5)
    
    an = ArmConfiguration()

    #front center
    targetPose_front_01 = an._createPose(0.27, 0.0, 0.05, 0, math.pi, math.pi / 2)
    
    #front left
    targetPose_front_02 = an._createPose(0.27, 0.08, 0.05, 0, math.pi, math.pi / 2)
    
    #front right
    targetPose_front_03 = an._createPose(0.27, -0.08, 0.05, 0, math.pi, math.pi / 2)
    
    
    #end begin
    targetPose_back_01 = an._createPose(0.033 + 0.024 - 0.235, 0.0, 0.105, 0, -math.pi + 0.2, 0)
    
    #end middle