def move_calib():
from share.tools.classes.f21pa10Class import fpa10Class
from tools.classes.realsenseClass import realsenseClass
camera = realsenseClass()
arm = fpa10Class("_l")
arm.Th2tool = Th2tool_handCamCalib
arm.otc_setToolOffset(
[arm.Th2tool.vec[0], arm.Th2tool.vec[1], arm.Th2tool.vec[2]])
arm.mode_joint()
arm.move_joint(arm.j_ready)
arm.mode_rmrc()
time.sleep(1)
plistData = []
for i in range(len(Pos_B)):
# raw_input("press Enter to move next point >>")
arm.move_rmrc(Pos_B[i])
title = TITLE + str(i) + EXTENSION
# time.sleep(1)
# camera.update_image(camera.IR)
time.sleep(3)
camera.save_irImage(PATH_IMAGE_DATA + title)
plistData.append(arm.t_xyzabc_now)
raw_input("finished. press Enter to back to ready position")
arm.mode_joint()
arm.move_joint(arm.j_ready)
data = {"plistData": plistData}
json_write(data, PATH_IMAGE_DATA + "plistData.json")
#import world path ===================
import WorkspaceDirInfo as shareDir
sys.path.append(shareDir.WorkspaceDir)
import share.tools.geo.geo as geo
from share.tools.classes.f21pa10Class import fpa10Class
from share.tools.classes.parallelGripperClass import parallelGripperClass
#import local path ===================
sys.path.append(os.path.join(".."))
import set_env
baseDir = shareDir.WorkspaceDir + set_env.MyName
sys.path.append(baseDir)
################initial###########################################
arm = fpa10Class("_l")
base_xyzabc = [0.65, 0, 0.4, 0, 0, 0]
# change mode joint and move ready pose
print "mode joint"
arm.mode_joint()
print "move ready"
arm.move_ready()
# wait 1 sencond
time.sleep(1)
arm.mode_rmrc()
j = base_xyzabc[:]
arm.move_rmrc(j)
print "Start Moving!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
import share.tools.geo.geo as geo
from share.tools.classes.f21pa10Class import fpa10Class
from share.data.handCamCalib.Th2c_ir import *
from share.tools.classes.parallelGripperClass import parallelGripperClass
#import local path ===================
sys.path.append(os.path.join(".."))
import set_env
baseDir = shareDir.WorkspaceDir + set_env.MyName
sys.path.append(baseDir)
if NeedRealSense:
from tools.classes.realsenseClass import realsenseClass
################initial###########################################
print "initial pa10"
armR = fpa10Class("_l")
print "mode joint"
armR.mode_joint()
print "move ready"
armR.move_ready()
time.sleep(1)
armR.mode_rmrc()
base = [0.65, 0, 0.25, 0, 0, 0]
base2 = [0.65, 0.3, 0.25, 0, 0, 0]
raw_input("Start Program!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
armR.move_rmrc(base)
#raw_input("Start Program!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
#armR.move_rmrc(base2,v_max=0.07)
#raw_input("Start Program!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
import share.tools.geo.geo as geo
from share.tools.classes.f21pa10Class import fpa10Class
from share.tools.classes.parallelGripperClass import parallelGripperClass
#import local path ===================
sys.path.append(os.path.join(".."))
import set_env
baseDir = shareDir.WorkspaceDir + set_env.MyName
sys.path.append(baseDir)
# ========================================================================================================
#
# instance
#
# ========================================================================================================
arm_r = fpa10Class("_r")
arm_l = fpa10Class("_l")
world_l = [0.65, -0.4, 0.023, 0, 0, 0]
arm_l.coordMode_Table(world_l)
world_r = [0.65, 0.4, 0.02, 0, 0, 0]
arm_r.coordMode_Table(world_r)
import visual
visual.scene.center = (-0.6, 0, 0.3)
# ========================================================================================================
#
# instance
def ConvertFRAME2mat(FRAME):
if isinstance(FRAME, geo.FRAME):
res = np.eye(4)
res[:3, :3] = np.array(FRAME.mat)
res[:3, 3] = np.array(FRAME.vec)
return np.mat(res)
else:
print "EnvSetup.py ConvertFRAME2mat: Argument type is not geo.FRAME!"
# PA10 import
if EnvSetupVar.Need_PA10_L:
from share.tools.classes.f21pa10Class import fpa10Class
ARM_L = fpa10Class("_l")
ARM_L.Load_Th2c()
print "PA10_Left arm was created. Variable name is \"ARM_L\".\r\n"
elif EnvSetupVar.Need_PA10_R:
from share.tools.classes.f21pa10Class import fpa10Class
ARM_R = fpa10Class("_r")
ARM_R.Load_Th2c()
print "PA10_Right arm was created. Variable name is \"ARM_R\".\r\n"
elif EnvSetupVar.Need_PA10_BOTH:
from share.tools.classes.f21pa10Class import fpa10Class
ARM_L = fpa10Class("_l")
ARM_L.Load_Th2c()
print "PA10_Left arm was created. Variable name is \"ARM_L\".\r\n"
ARM_R = fpa10Class("_r")
ARM_R.Load_Th2c()
sys.path.append(shareDir.WorkspaceDir)
import share.tools.geo.geo as geo
from share.tools.classes.f21pa10Class import fpa10Class
from share.data.handCamCalib.Th2c_ir import *
from share.tools.classes.parallelGripperClass import parallelGripperClass
#import local path ===================
sys.path.append(os.path.join(".."))
import set_env
baseDir = shareDir.WorkspaceDir + set_env.MyName
sys.path.append(baseDir)
from tools.classes.realsenseClass import realsenseClass
################initial###########################################
print "initial pa10"
arm = fpa10Class("_l")
armR = fpa10Class("_r")
print "mode joint"
armR.mode_joint()
print "move ready"
armR.move_ready()
time.sleep(1)
armR.mode_rmrc()
print "initial RealSense"
#realsense = realsenseClass()
print "initial Over"
base_xyzabc = [0.65, 0, 0.20, 0, 0, 0]
#base_xyzabc =[0.50, 0.0, 0.55, 0, 0, 0]