diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py index 9a7908a6066af4add6177116ecfa59f66034543e..c6df9934788ecb644865673882c5fb9756ca3f09 100644 --- a/python/ur_simple_control/managers.py +++ b/python/ur_simple_control/managers.py @@ -399,9 +399,7 @@ class RobotManager: self.gripper.connect(args.robot_ip, 63352) self.gripper.activate() if self.args.gripper == "onrobot": - device = OnRobotDevice() - device.getCB() - self.gripper = TWOFG(device) + self.gripper = TWOFG() # also TODO: figure out how to best solve the gripper_velocity problem # NOTE: you need to initialize differently for other types of joints @@ -426,11 +424,11 @@ class RobotManager: # and you need to minus them to have usable readings. # we provide this with calibrateFT self.wrench_offset = self.calibrateFT() - if self.args.gripper == "robotiq": - self.gripper.connect("192.168.1.102", 63352) - # this is a blocking call - # this isn't actually needed and it's annoying - # TODO: test after rebooting the robot +# if self.args.gripper == "robotiq": +# self.gripper.connect("192.168.1.102", 63352) +# # this is a blocking call +# # this isn't actually needed and it's annoying +# # TODO: test after rebooting the robot # self.gripper.activate() elif not args.pinocchio_only: self.rtde_control = RTDEControlInterface("127.0.0.1") @@ -572,6 +570,7 @@ class RobotManager: of the robot, so we add or remove 2 items to the joint list. also, the gripper is controlled separately so we'd need to do this somehow anyway NOTE: this gripper_past_pos thing is not working atm, but i'll keep it here as a TODO + TODO: make work for new gripper """ if not self.pinocchio_only: q = self.rtde_receive.getActualQ() @@ -744,13 +743,13 @@ class RobotManager: def openGripper(self): if (not self.args.simulation) and (not self.args.pinocchio_only): - self.gripper.move(0,255,255) + self.gripper.open() else: print("not implemented yet, so nothing is going to happen!") def closeGripper(self): if (not self.args.simulation) and (not self.args.pinocchio_only): - self.gripper.move(255,255,255) + self.gripper.close() else: print("not implemented yet, so nothing is going to happen!")