c = Client(parsed_args.client_name)
c.connect()
cam = Camera(c, 'CameraFrame')
time.sleep(0.5)
controllers = []
psm_arms = []
if parsed_args.run_psm_one is True:
# Initial Target Offset for PSM1
# init_xyz = [0.1, -0.85, -0.15]
arm_name = 'psm1'
print('LOADING CONTROLLER FOR ', arm_name)
psm = PSM(c, arm_name)
if psm.is_present():
T_psmtip_c = Frame(Rotation.RPY(3.14, 0.0, -1.57079), Vector(-0.2, 0.0, -1.0))
T_psmtip_b = psm.get_T_w_b() * cam.get_T_c_w() * T_psmtip_c
psm.set_home_pose(T_psmtip_b)
psm_arms.append(psm)
if parsed_args.run_psm_two is True:
# Initial Target Offset for PSM1
# init_xyz = [0.1, -0.85, -0.15]
arm_name = 'psm2'
print('LOADING CONTROLLER FOR ', arm_name)
theta_base = -0.7
psm = PSM(c, arm_name)
if psm.is_present():
T_psmtip_c = Frame(Rotation.RPY(3.14, 0.0, -1.57079), Vector(0.2, 0.0, -1.0))
parsed_args.run_psm_three = False
c = Client()
c.connect()
controllers = []
if parsed_args.run_psm_one is True:
# Initial Target Offset for PSM1
# init_xyz = [0.1, -0.85, -0.15]
arm_name = 'psm3'
print('LOADING CONTROLLER FOR ', arm_name)
leader = GeomagicDevice('/Geomagic/')
theta = -0.7
leader.set_base_frame(Frame(Rotation.RPY(theta, 0, 0), Vector(0, 0,
0)))
leader.set_tip_frame(Frame(Rotation.RPY(theta + 0.7, 0, 0), Vector()))
psm = PSM(c, arm_name)
controller = ControllerInterface(leader, psm)
controllers.append(controller)
if len(controllers) == 0:
print('No Valid PSM Arms Specified')
print('Exiting')
else:
while not rospy.is_shutdown():
for cont in controllers:
cont.run()
time.sleep(0.005)