########################################################################
#  Author(s):	Peter Kazanzides
#
#  $Id: collect.py,v 1.4 2006/07/11 15:48:57 pkaz Exp $
#
#              Developed by the Engineering Research Center for
#          Computer-Integrated Surgical Systems & Technology (CISST)
#
#               Copyright(c) 2005, The Johns Hopkins University
#                          All Rights Reserved.
#
########################################################################

# Import wrapped cisst and mskccRobot libraries
from cisstCommonPython import *
from cisstVectorPython import *
from mskccRobotPython import *

# Create global robot object (rob)
rob = mskccRobot()

from time import sleep
from random import random
deltaz = vctDouble3(0,0,5)
xaxis = vctDouble3(1,0,0)
yaxis = vctDouble3(0,1,0)
zaxis = vctDouble3(0,0,1)

# Setup: Initialize and home robot.  Change software travel limits
#        so that robot can reach all markers. Create output file.
def Setup():
    f = open('collect.txt', 'w')
    f.write("Point data\n")
    rob.Init()
    rob.SetJointLowerLimit(0,-150)
    rob.SetJointLowerLimit(2,-150)
    rob.Home()
    rob.MoveXYZ1(vctDouble3(0,0,50))

# WaitMotion: Move to specified goal and wait for motion to finish.
def WaitMove(goal):
    rob.MoveXYZ1(goal)
    rob.WaitMotion()

# Collect: Get a single marker position by having user guide the
#          robot to the marker.
def Collect(a):
    done = False
    while not done:
       rob.StartForceCompliance()
       s = raw_input("Press Enter to collect point " + str(a) + ": ")
       rob.StopForceCompliance()
       v = rob.GetPositionXYZ1()
       spd = rob.GetSpeed()
       rob.SetSpeed(1.0)
       WaitMove(v + 0.5*deltaz)
       rob.SetSpeed(spd)
       s = raw_input("Try again? ")
       if s == "n":
          done = True
    f = open('collect.txt', 'a')
    f.write(str(a) + ": " + str(v) + "\n")
    f.close()
    WaitMove(v + deltaz)
    return v + deltaz

# The CNC coordinates of the 20 hemispherical holes on the phantom.
pos_cnc = (
            vctDouble3( 10,     9.125,  0),
            vctDouble3( 10,    22.5,  -30),
            vctDouble3( 10,    40.875,  0),
            vctDouble3( 20,    27.5,  -20),
            vctDouble3( 30,    32.5,  -10),
            vctDouble3( 35,    17.5,  -15),
            vctDouble3( 40,    22.5,  -30),
            vctDouble3( 40,    45,      0),
            vctDouble3( 50,    27.5,  -20),
            vctDouble3( 50.64, 40.875,  0),
            vctDouble3( 55.72,  9.125,  0),
            vctDouble3( 65,    32.5,  -10),
            vctDouble3( 70,    22.5,  -30),
            vctDouble3( 75,    45,      0),
            vctDouble3( 80,    27.5,  -20),
            vctDouble3( 85,    17.5,  -15),
            vctDouble3(100,    22.5,  -30),
            vctDouble3(100,    32.5,  -10),
            vctDouble3(110,     5,    -10),
            vctDouble3(110,    45,      0)
          )

# The CNC coordinates of the 4 large registration holes.
pos_reg = (
            vctDouble3(20,     9.125, 0),
            vctDouble3(20,    40.875, 0),
            vctDouble3(60.64, 40.875, 0),
            vctDouble4(65.72,  9.125, 0)
          )

# CollectAll:  Collect all 20 markers (small hemispherical holes)
#              and all 4 large registration holes on the phantom.
def CollectAll():
    WaitMove(vctDouble3(0,0,-50))
    pos0 = Collect(1)
    for i in range(2,21):
        posdelta = vctDouble3(pos_cnc[0].X() - pos_cnc[i-1].X(),
                              pos_cnc[0].Y() - pos_cnc[i-1].Y(),
                              0)
        WaitMove(pos0 + posdelta)
        WaitMove(pos0 + posdelta + (pos_cnc[i-1].Z()-pos_cnc[0].Z())*zaxis)
        pos = Collect(i)
        WaitMove(vctDouble3(pos.X(), pos.Y(), pos0.Z()))
    for i in range(1,5):
        posdelta = vctDouble3(pos_cnc[0].X() - pos_reg[i-1].X(),
                              pos_cnc[0].Y() - pos_reg[i-1].Y(),
                              5)
        WaitMove(pos0 + posdelta)
        pos = Collect("R" + str(i))
        WaitMove(vctDouble3(pos.X(), pos.Y(), pos0.Z()+5))


# CollectRepeat:  Perform repeatability test on single marker.
def CollectRepeat():
    WaitMove(vctDouble3(0,0,0))
    for i in range(1,11):
        pos = Collect("R" + str(i))
        WaitMove(pos + 100*vctDouble3(random()-0.5, random()-0.5, 0))

def Deltax(dx):
    pos = rob.GetPositionXYZ1()
    WaitMove(pos + dx*xaxis)
    print "Actual motion = ", (rob.GetPositionXYZ1() - pos).X()

def Deltay(dy):
    pos = rob.GetPositionXYZ1()
    WaitMove(pos + dy*yaxis)
    print "Actual motion = ", (rob.GetPositionXYZ1() - pos).Y()

def Deltaz1(dz):
    pos = rob.GetPositionXYZ1()
    WaitMove(pos + dz*zaxis)
    print "Actual motion = ", (rob.GetPositionXYZ1() - pos).Z()

def Deltaz2(dz):
    pos = rob.GetPositionZ2()
    rob.MoveZ2(dz)
    rob.WaitMotion()
    print "Actual motion = ", rob.GetPositionZ2() - pos


def GetPos():
    pos = rob.GetJointPosition()
    print pos
    return pos

        
def PrintHelp():
    print "Setup()      -- Initializes and homes robot"
    print "CollectAll() -- Collects all 20 points on phantom"
    print "CollectRepeat() -- Collects one point 10 times"
    print "Deltax(dx)   -- Move robot X axis by dx mm"
    print "Deltay(dy)   -- Move robot Y axis by dy mm"
    print "Deltaz1(dz)  -- Move robot Z1 axis by dz mm"
    print "Deltaz2(dz)  -- Move robot Z2 axis by dz mm"
    print "GetPos()     -- Print robot position (X, Y, Z1, Z2)"


print "Data collection routines loaded."
PrintHelp()