def execute_cb(self, goal):
#helper variables
r = rospy.Rate(1)
success = True
rospy.loginfo('%s: Executing, returning value of %s' %
(self._action_name, goal.request))
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
if success:
rob = urx.Robot("172.22.22.2")
robotiqgrip = Robotiq_Two_Finger_Gripper(rob, 1.25)
rospy.loginfo('Print request of %s' % (goal.request))
#need to do this so data type matches the request
#std_msgs/String is not the same as a regular string!
compStr1 = String()
compStr1.data = "open"
compStr2 = String()
compStr2.data = "close"
if (goal.request == compStr1):
#open gripper:
check = True
robotiqgrip.open_gripper()
rospy.loginfo('Reached if')
elif (goal.request == compStr2):
#close gripper:
check = False
robotiqgrip.close_gripper()
rospy.loginfo('Reached elseif')
#rob.send_program(robotiqgrip.ret_program_to_run())
rob.close()
sys.exit()
self._result.OpenOrClose = check
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
def main(num, speed=0.1):
ip = f'192.168.1.{num}'
robot = getRobot(ip)
gripper = Gripper(robot)
try:
gripper.open_gripper()
input("Move tool over object then hit enter: ")
object_position = robot.get_pose()
input("Move tool somewhere else and hit enter: ")
position = robot.get_pose()
robot.set_pose(object_position, speed, speed)
gripper.close_gripper()
robot.set_pose(position, speed, speed)
except:
print('something went wrong')
finally:
robot.close()
return robot, gripper
def execute_cb(self, goal):
r = rospy.Rate(1)
success = True
rospy.loginfo('%s: Executing, returning value of %s' %(self._action_name,
goal.action))
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
if success:
#make sure to comment out all robot commands when robot
#is not present in order to avoid timeout errors
rob = urx.Robot("172.22.22.2")
robotiqgrip = Robotiq_Two_Finger_Gripper(rob, 1.25)
#commands to move the robot to position
a = 0.05
v = 0.1
pose = rob.getl()
rospy.loginfo("Current pose: %s"% (rob.getl()))
rob.movep(pose, acc=a, vel=v, wait=True)
rospy.loginfo('Print request of %s'%(goal.action))
#need to do this so data type matches the request
#std_msgs/String is not the same as a regular string!
compStr1 = String()
compStr1.data = "grab"
compStr2 = String()
compStr2.data = "release"
#conditional statements
if(goal.action == compStr1):
check = True #returns true if grabbed the object
#commands: move to a certain point and then close gripper, grabbing object
rob.movel((pose[0], pose[1], pose[2] + 0.1, pose[3], pose[4], pose[5]), a, v, relative=True)
#this move command can later be customized based on CAMERA/VISION PROCESSING data
robotiqgrip.close_gripper()
rospy.loginfo("grab")
if(goal.action == compStr2):
check = False #returns false if released the object
#commands: move to a certain point and then open gripper, releasing object
rob.movel((pose[0], pose[1], pose[2] - 0.1, pose[3], pose[4], pose[5]), a, v, relative=True)
#move command can later be customized based on CAMERA/VISION PROCESSING data
robotiqgrip.open_gripper()
rospy.loginfo("release")
rob.close()
sys.exit()
self._result.outcome = check
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
if __name__ == '__main__':
rospy.init_node('dope_rospy_youngjae_test')
dope_controller = DopeController()
rospy.sleep(1)
gripper = Robotiq_Two_Finger_Gripper(robot)
# init position joint state
init = [
0.39821290969848633, -4.948345323602194, 1.713571850453512,
4.439863844508789, -2.273259941731588, 2.5900216102600098
]
a = raw_input("object : ")
robot.movej(init, 0.2, 0.2, relative=False)
gripper.open_gripper()
if int(a) == 1:
pan_pose, pan_orient = dope_controller.get_pan_pose()
print('pan_pose x, y, z', pan_pose.x, pan_pose.y, pan_pose.z)
print('pan_orient x, y, z, w', pan_orient.x, pan_orient.y,
pan_orient.z, pan_orient.w)
dope_list = [pan_pose.x, pan_pose.y, pan_pose.z, 0, 0, 0]
h, v = convert(dope_list)
robot_move(h, v, 0.1, 0.1)
else:
oil_bowl_pose, oil_bowl_orient = dope_controller.get_oil_bowl_pose()
print('oil_bowl_pose x, y, z', oil_bowl_pose.x, oil_bowl_pose.y,
oil_bowl_pose.z)
print('oil_bowl_orient x, y, z, w', oil_bowl_orient.x,
oil_bowl_orient.y, oil_bowl_orient.z, oil_bowl_orient.w)
rob = urx.Robot("192.168.0.3")
robotiqgrip = Robotiq_Two_Finger_Gripper(rob)
PORT = 63352
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect(("192.168.0.3", PORT))
time.sleep(0.1)
robotiqgrip.gripper_action(128)
time.sleep(0.1)
robotiqgrip.close_gripper()
time.sleep(0.1)
robotiqgrip.open_gripper()
rob.close()
'''
import socket
import time
import struct
HOST = "192.168.0.3" # The remote host
PORT_63352 = 63352
SOCKET_NAME = "gripper_socket"
HEADER="def gripper_action():\n socket_close(\"G\")\n sync()\n socket_open(\"192.168.0.3\",63352,\"G\")\n sync()\n"
FEEDBACK=""
ENDING=" socket_close(\"G\")\nend\n"
print "Starting Program"
robot.movel([0.192, -0.616, 0.3, 3, 0, 0], acc=acc,
vel=vel) # Bouge le robot aux coordonnées cartésiennes
robot.movej(angular_pos, acc=acc,
vel=vel) # Bouge le robot en articulaire (radian)
print(robot.get_pose()) # Renvoie le (x, y, z) du TCP
robot.up()
robot.down()
robot.translate([0.1, 0.1, 0.1])
# Commandes pince
'''
Les différentes commandes sont disponibles dans le fichier python-urx-0.11.0/urx/robotiq_two_fingers_gripper.py
dans la classe Robotiq_Two_Finger_Gripper
'''
gripper.open_gripper() # Ferme entièrement le gripper
gripper.close_gripper() # Ouvre entièrement le gripper
gripper.gripper_action(
150) # Ouvre le gripper à une certaine taille (0:ouvert, 255: fermé)
print(gripper.send_opening(
TCP_Mon_Ordi, TCP_PORT_GRIPPER)) # Retourne l'ouverture de la pince
robot.close() # Fermeture de la connection
# Commandes capteur de force ouvrir une connection avec le capteur de force
robot = urx.Robot(TCP_Robot, useForce=True)
robot.set_tcp(
(0, 0, 0.3, 0, 0, -0.43)
) # Position du Tool Center Point par rapport au bout du robot (x,y,z,Rx,Ry,Rz)
# (en mm et radians)
robot.set_payload(1, (0, 0, 0.1))
print(robot.force_sensor.getZForce())
# Main Program Execution
connected = False
while not connected:
s = input("Enter Command (h for help): ")
if s == 'c': # connect
if simulate == 0:
while True:
print("Connecting...")
try:
rob = urx.Robot("192.168.1.6")
# Initialize Robot Control
r_grip = Robotiq_Two_Finger_Gripper(rob)
# Initialize Gripper Control
r_grip.open_gripper()
# Open Gripper if previously closed
rob.movej((-1.96, -1.53, 1.58, -2.12, -1.56, 1.19), a, v)
# Move Robot arm to a safe position
rob.movel(snap_pose, a, v)
# Move Robot arm to the Snap Pose
print(
"Robot initialized at 192.168.1.6, Gripper initialized and Snap position reached"
)
connected = True
break
except:
s = input("Try again (y/n)?")
if s == 'n':
break
else:
class Interface(HWInterface):
def __init__(self):
rospy.init_node('ur_interface')
# robot setup
print('Connecting robot...')
self.rob = urx.Robot("192.168.1.5", True)
self.rob.set_tcp((0, 0, 0.17, 0, 0, 0))
self.rob.set_payload(1, (0, 0, 0.1))
self.rob.set_gravity((0, 0, 9.82))
self.rob.set_csys(m3d.Transform([0, 0, 0, -2.9, 1.2, 0]))
time.sleep(0.2)
self.gripper = Robotiq_Two_Finger_Gripper(self.rob)
# robot init
print('Homing robot...')
self.gripper.open_gripper()
self.move_home()
self.state_gripper, self.target_gripper = 0, 0
self.process_gripper = Thread(target=self._set_gripper)
self.target_pose = self.get_tcp_pose()
self.target_tool = self.get_tool_rot()
print('Robot ready!')
# force monitoring
self.process_force = Thread(target=self._watch_force)
self.input_force = False
self.process_push = Thread(target=self._watch_push)
self.push_stopped = 0
def disconnect(self):
"""
terminate connection with robot
:return: None
"""
self.rob.close()
def move_home(self):
"""
move back to home (Blocking)
:return: None
"""
self.rob.movej(HOMEJ, ACCE * 2, VELO * 3, wait=True)
def move_tcp_relative(self, pose):
"""
move eff to relative pose
:param pose: relative differences in [x y z R P Y] (meter, radian)
:return: None
"""
self.rob.add_pose_tool(m3d.Transform(pose), ACCE, VELO, wait=False)
def move_tcp_absolute(self, pose):
"""
move eff to absolute pose in robot base frame
:param pose: list [x y z R P Y] (meter, radian)
:return: None
"""
if (self.is_program_running() and
self.dist_linear(pose, self.target_pose) > TOL_TARGET_POSE) or \
(not self.is_program_running() and
self.dist_linear(pose, self.get_tcp_pose()) > TOL_TARGET_POSE):
self.target_pose = pose
self.rob.set_pose(m3d.Transform(pose), ACCE, VELO, False)
def get_tcp_pose(self):
"""
get eff pose
:return: list [x y z R P Y] (meter, radian)
"""
return self.rob.getl()
def set_gripper(self, val):
"""
gripper position control
:param val: boolean (False:released, True:gripped)
:return: None
"""
if self.target_gripper != val:
self.target_gripper = val
if not self.process_gripper.is_alive():
self.process_gripper = Thread(target=self._set_gripper)
self.process_gripper.start()
def get_gripper(self):
"""
get gripper position
:return: boolean (False:released, True:gripped)
"""
return self.state_gripper
def rot_tool(self, val):
"""
rotate wrist_3 joint
:param val: float (0 to 1)
:return: None
"""
if (self.is_program_running() and
((val-self.target_tool)**2)**0.5 > TOL_TARGET_POSE) or \
(not self.is_program_running() and
((val-self.get_tool_rot())**2)**0.5 > TOL_TARGET_POSE):
self.target_tool = val
joints = self.rob.getj()
joints[5] = (TOOL_RANGE[1] - TOOL_RANGE[0]) * val + TOOL_RANGE[0]
self.rob.movej(joints, ACCE * 4, VELO * 6, wait=False)
def get_tool_rot(self):
"""
get wrist_3 joint value
:return: float (0 to 1)
"""
val = self.rob.getj()[5]
return (min(max(val, TOOL_RANGE[0]), TOOL_RANGE[1]) -
TOOL_RANGE[0]) / (TOOL_RANGE[1] - TOOL_RANGE[0])
def wait_push_input(self, force):
"""
wait for push input
require calling get_push_input to reset before reuse
:param force: minimum amount of force required (newton)
:return: None
"""
if not self.process_force.isAlive() and not self.input_force:
self.rob.stopj(5)
time.sleep(1)
th_force = self.rob.get_force(wait=True) + force
self.process_force = Thread(target=self._watch_force,
args=(th_force, ))
self.process_force.start()
def get_push_input(self):
"""
get if a push input has been registered
:return: boolean (True:Yes, False:No)
"""
if self.input_force:
self.input_force = False
return True
else:
return False
def push(self, force, max_dist):
"""
initiate applying force and/or torque in 4 dimensions
require calling get_push to reset before reuse
:param force: list (x y z R) (newton, newton meter)
:param max_dist: list (x y z R) maximum travel in each dimensions (meter, radian)
:return: None
"""
if not self.process_push.isAlive() and not self.push_stopped:
self.rob.stopj(5)
time.sleep(1)
self.process_push = Thread(target=self._watch_push,
args=(30, self._get_tcp_state(),
max_dist))
self._force_mode_start(force, (0.01, 0.03), (0.02, 0.4),
0,
duration=30)
self.process_push.start()
def screw(self, cw):
"""
applying downward force and torque for a duration
:param cw: boolean (False:clockwise, True:counter-clockwise)
:return: None
"""
val = self.rob.getj()[5]
return self.push(
[0, 0, 20, 2 if cw else -2],
[0, 0, 0, (TOOL_RANGE[1] - val) if cw else (TOOL_RANGE[0] - val)])
def get_push(self):
"""
get if push has completed
:return: int (0:No, 1:max distance reached, 2:force acquired)
"""
if self.push_stopped:
val = self.push_stopped
self.push_stopped = 0
return val
else:
return 0
def is_program_running(self):
"""
check if a program is already running
:return: boolean (False:stopped, True:Moving)
"""
return self.rob.secmon.is_program_running()
def _get_tcp_state(self):
return self.get_tcp_pose()[:3] + [self.rob.getj()[5]]
def _set_gripper(self):
while self.state_gripper != self.target_gripper:
if self.target_gripper:
self.gripper.close_gripper()
self.state_gripper = 1
else:
self.gripper.open_gripper()
self.state_gripper = 0
def _watch_force(self, th_force):
time0 = time.time()
while time.time() - time0 < 5: # 5 seconds input window
force = self.rob.get_force(wait=True)
if force > th_force:
self.input_force = True
break
def _watch_push(self, duration, pose0, max_dist):
time0 = time.time()
cnt_stopped = 0
push_stopped = False
pose1_ = self._get_tcp_state()
pose1_ = [pose1_ for _ in range(10)]
while time.time() - time0 < duration:
pose1 = self._get_tcp_state()
# max displacement check
for i in range(4):
if (max_dist[i] > 0 and pose1[i] - pose0[i] > max_dist[i]) or \
(max_dist[i] < 0 and pose1[i] - pose0[i] < max_dist[i]):
push_stopped = True
break
if push_stopped:
# print('max_dist')
self.rob.send_program('end_force_mode()')
self.rob.stopj(2)
self.push_stopped = 1
break
# stationary check
if self.dist(pose1, pose1_.pop(0)) < 0.0001 and \
self.dist(pose1, pose0) > 0.001:
cnt_stopped += 1
else:
cnt_stopped = 0
if cnt_stopped > 128:
# print('stationary')
self.rob.send_program('end_force_mode()')
self.rob.stopj(2)
self.push_stopped = 2
break
pose1_.append(pose1)
def _force_mode_start(self, force, deviation, speed, damp, duration):
force = force[:3] + [0, 0] + [force[-1]]
axis = [0 if i == 0 else 1 for i in force]
limit = [speed[0] if i else deviation[0] for i in axis[:3]] + \
[speed[1] if i else deviation[1] for i in axis[3:]]
prog = 'def force_run():\n' + \
' stopl(5)\n' + \
' sleep(1)\n' + \
' force_mode_set_damping({})\n'.format(str(damp)) + \
' force_mode(tool_pose(), {}, {}, 2, {})\n'.format(str(axis), str(force), str(limit)) + \
' sleep({})\n'.format(str(duration)) + \
' end_force_mode()\n' + \
' stopl(5)\n' + \
' sleep(1)\n' + \
'end'
self.rob.send_program(prog)
class Interface(HWInterface):
def __init__(self):
rospy.init_node('ur_interface')
# robot setup
print('Connecting robot...')
self.rob = urx.Robot("192.168.1.5", True)
self.rob.set_tcp((0, 0, 0.17, 0, 0, 0))
self.rob.set_payload(1, (0, 0, 0.1))
self.rob.set_gravity((0, 0, 9.82))
self.rob.set_csys(m3d.Transform([0, 0, 0, -2.9, 1.2, 0]))
time.sleep(0.2)
self.gripper = Robotiq_Two_Finger_Gripper(self.rob)
# robot init
print('Homing robot...')
self.gripper.open_gripper()
self.move_home()
self.state_gripper, self.target_gripper = 0, 0
self.process_gripper = Thread(target=self._set_gripper)
self.target_pose = self.get_tcp_pose()
self.target_tool = self.get_tool_rot()
print('Robot ready!')
# force monitoring
self.process_force = Thread(target=self._watch_force)
self.input_force = False
self.timeout_push = None
self.offset_force = 0
def disconnect(self):
"""
terminate connection with robot
:return:
"""
self.rob.close()
def move_home(self):
"""
move back to home (Blocking)
:return:
"""
self.rob.movej(HOMEJ, ACCE * 2, VELO * 3, wait=True)
def move_tcp_relative(self, pose):
"""
move eff to relative pose
:param pose: relative differences in [x y z R P Y] (meter, radian)
:return: None
"""
self.rob.add_pose_tool(m3d.Transform(pose), ACCE, VELO, wait=False)
def move_tcp_absolute(self, pose):
"""
move eff to absolute pose in robot base frame
:param pose: list [x y z R P Y] (meter, radian)
:return: None
"""
if (self.is_program_running() and
self.dist_linear(pose, self.target_pose) > TOL_TARGET_POSE) or \
(not self.is_program_running() and
self.dist_linear(pose, self.get_tcp_pose()) > TOL_TARGET_POSE):
self.target_pose = pose
self.rob.set_pose(m3d.Transform(pose), ACCE, VELO, False)
def get_tcp_pose(self):
"""
get eff pose
:return: list [x y z R P Y] (meter, radian)
"""
return self.rob.getl()
def set_gripper(self, val):
"""
gripper position control
:param val: Boolean (False:released, True:gripped)
:return: None
"""
if self.target_gripper != val:
self.target_gripper = val
if not self.process_gripper.is_alive():
self.process_gripper = Thread(target=self._set_gripper)
self.process_gripper.start()
def get_gripper(self):
"""
get gripper position
:return: Boolean (False:released, True:gripped)
"""
return self.state_gripper
def rot_tool(self, val):
"""
rotate wrist_3 joint
:param val: float (0 to 1)
:return: None
"""
if (self.is_program_running() and
((val-self.target_tool)**2)**0.5 > TOL_TARGET_POSE) or \
(not self.is_program_running() and
((val-self.get_tool_rot())**2)**0.5 > TOL_TARGET_POSE):
self.target_tool = val
joints = self.rob.getj()
joints[5] = (TOOL_RANGE[1] - TOOL_RANGE[0]) * val + TOOL_RANGE[0]
self.rob.movej(joints, ACCE * 4, VELO * 6, wait=False)
def get_tool_rot(self):
"""
get wrist_3 joint value
:return: float (0 to 1)
"""
val = self.rob.getj()[5]
return (min(max(val, TOOL_RANGE[0]), TOOL_RANGE[1]) - TOOL_RANGE[0]
) / (TOOL_RANGE[1] - TOOL_RANGE[0]) #缩放，-2pi ~ 2pi 等比缩放
def waitfor_push(self, force):
"""
wait for push input
require calling get_push_input to reset before reuse
:param force: minimum amount of force required (newton)
:return: None
"""
if not self.process_force.isAlive() and not self.input_force:
self.rob.stopj(5)
time.sleep(1)
th_force = self.rob.get_force(wait=True) + force
self.process_force = Thread(target=self._watch_force,
args=(th_force, ))
self.process_force.start()
def get_push_input(self):
"""
get if a push input has been registered
:return: Boolean (True:Yes, False:No)
"""
if self.input_force:
self.input_force = False
return True
else:
return False
def push(self, force, duration):
"""
apply force and/or torque in 6 dimensions for a duration
require calling get_push to reset before reuse
:param force: list (x y z R P Y) (newton, newton meter)
:param duration: float (second)
:return: boolean (False:moving, True:done)
"""
if self.timeout_push is None:
self.timeout_push = time.time(
) + duration + 2 # 2 seconds are the sleeps in force program
self.offset_force = self.rob.get_force(wait=True)
self._force_mode_start(force, duration, (0.01, 0.03), (0.02, 0.4),
0)
def get_push_timeout(self):
if self.timeout_push is None:
return False
elif time.time() > self.timeout_push:
self.timeout_push = None
return True
else:
return False
def get_force(self):
"""
get tcp force after offset
:return: int #!=force [x y z Tx Ty Tz] or
"""
return self.rob.get_force(wait=True) - self.offset_force
def screw(self, cw, duration):
"""
applying downward force and torque for a duration
:param cw: boolean (False:clockwise, True:counter-clockwise)
:param duration: float (second)
:return: boolean (False:moving, True:done)
"""
return self.push([0.0, 0.0, 20.0, 0.0, 0.0, 2.0 if cw else -2.0],
duration)
def is_program_running(self):
"""
check if a program is already running
:return: Boolean (False:stopped, True:Moving)
"""
return self.rob.secmon.is_program_running()
def _set_gripper(self):
while self.state_gripper != self.target_gripper:
if self.target_gripper:
self.gripper.close_gripper()
self.state_gripper = 1
else:
self.gripper.open_gripper()
self.state_gripper = 0
def _watch_force(self, th_force):
time0 = time.time()
while time.time() - time0 < 5: # 5 seconds input window
force = self.rob.get_force(wait=True)
if force > th_force:
self.input_force = True
break
def _force_mode_start(self, force, duration, deviation, speed, damp):
# force, duration, (0.01, 0.03), (0.02, 0.4), 0
#[0.0, 0.0, 20.0, 0.0, 0.0, 0.0], 4, (0.01, 0.03), (0.02, 0.4), 0
axis = [0 if i == 0 else 1 for i in force] #[0,0,1,0,0,0]
limit = [speed[0] if i else deviation[0] for i in axis[:3]] + \
[speed[1] if i else deviation[1] for i in axis[3:]]
prog = 'def force_run():\n' + \
' stopl(5)\n' + \
' sleep(1)\n' + \
' force_mode_set_damping({})\n'.format(str(damp)) + \
' force_mode(tool_pose(), {}, {}, 2, {})\n'.format(str(axis), str(force), str(limit)) + \
' sleep({})\n'.format(str(duration)) + \
' end_force_mode()\n' + \
' stopl(5)\n' + \
' sleep(1)\n' + \
'end'
self.rob.send_program(prog)
class URRobotController(object):
"""Universal Robot (URx) controller RTC.
This class is using urx package.
"""
# Exceptions
URxException = urx.urrobot.RobotException
# Private member
__instance = None
__robot = None
__gripper = None
__sync_mode = False
_accel = 0.6
_velocity = 0.6
# singleton
def __new__(cls, ip="192.168.1.101", realtime=True):
if not cls.__instance:
cls.__instance = object.__new__(cls, ip, realtime)
return cls.__instance
def __init__(self, ip="192.168.1.101", realtime=True):
if self.__robot:
logging.info("instance is already exist")
return
logging.basicConfig(format='%(asctime)s:%(levelname)s: %(message)s',
level=logging.INFO)
# not use realtime port in stub mode
if ip == "localhost":
self.__realtime = False
else:
self.__realtime = realtime
logging.info("Create URRobotController IP: " + ip, self.__realtime)
try:
self.__robot = urx.Robot(ip, use_rt=self.__realtime)
self.__robot.set_tcp((0, 0, 0, 0, 0, 0))
self.__robot.set_payload(0, (0, 0, 0))
except self.URxException:
logging.error("URx exception was ooccured in init robot")
self.__robot = None
return
except Exception as e:
logging.error("exception: " + format(str(e)) + " in init robot")
self.__robot = None
return
try:
self.__gripper = Robotiq_Two_Finger_Gripper(self.__robot)
except self.URxException:
logging.error("URx exception was ooccured in init gripper")
self.__gripper = None
except Exception as e:
logging.error("exception: " + format(str(e)) + " in init gripper")
self.__gripper = None
self._update_send_time()
# use pause() for service port
self._joints_goal = []
self._pause = False
def __del__(self):
self.finalize()
def __enter__(self):
return self
def __exit__(self, exc_type, exc_value, traceback):
self.finalize()
def _update_send_time(self):
self.__latest_send_time = datetime.now()
def _expire_send_time(self):
diff = datetime.now() - self.__latest_send_time
# prevent control until 100ms after
if diff.microseconds > 100000:
return True
else:
return False
def set_middle(self, middle):
self._middle = middle
def unset_middle(self):
self._middle = None
@property
def robot_available(self):
"""Get robot instance is available or not.
Note:
None.
Args:
None.
Returns:
True: Available.
False: NOT available.
"""
if self.__robot:
return True
else:
return False
@property
def gripper_available(self):
"""Get gripper instance is available or not.
Note:
None.
Args:
None.
Returns:
True: Available.
False: NOT available.
"""
if self.__gripper:
return True
else:
return False
@property
def vel(self):
"""Get velocity for move.
Note:
None.
Args:
None.
Returns:
v: Target velocity.
"""
return self._velocity
@vel.setter
def vel(self, v=None):
"""Set velocity for move.
Note:
None.
Args:
None.
Returns:
v: Target velocity.
"""
if v:
self._velocity = v
@property
def acc(self):
"""Set accel and velocity for move.
Note:
None.
Args:
None.
Returns:
a: Target acceleration.
"""
return self._accel
@acc.setter
def acc(self, a=None):
"""Get accel and velocity for move.
Note:
None.
Args:
a: Target acceleration.
Returns:
True: Success.
"""
if a:
self._accel = a
def finalize(self):
"""Finalize URRobotController instance.
Note:
None.
Args:
None.
Returns:
True: Success.
False: Failed.
"""
if self.__robot:
self.__robot.close()
self.__robot = None
return True
else:
logging.error("robot is not initialized in " +
sys._getframe().f_code.co_name)
return False
def set_payload(self, weight, vector=(0, 0, 0)):
"""Set payload in kg and cog.
Note:
None.
Args:
weight: Weight in kg.
vector: Center of gravity in 3d vector.
Returns:
True: Success.
False: Failed.
"""
if self.__robot:
# set payload in kg & cog
self.__robot.set_payload(weight, vector)
self._update_send_time()
return True
else:
logging.error("robot is not initialized in " +
sys._getframe().f_code.co_name)
return False
@property
def is_moving(self):
"""Get status of runnning program.
Note:
None.
Args:
None.
Returns:
True: Program is running.
False: Program is NOT running.
"""
if self.__robot:
r = self.__robot
return r.is_program_running() or not self._expire_send_time()
else:
logging.error("robot is not initialized in " +
sys._getframe().f_code.co_name)
return False
@property
def sync_mode(self):
"""Get synchronous mode.
Note:
None.
Args:
None.
Returns:
True: Synchronous mode.
False: Asynchronous mode.
"""
return self.__sync_mode
@sync_mode.setter
def sync_mode(self, val):
"""Set synchronous mode (wait until program ends).
Note:
None.
Args:
None.
Returns:
True: Success.
"""
self.__sync_mode = val
def movel(self, pos, a=None, v=None):
"""Move the robot in a linear path.
Note:
None.
Args:
a: Target acceleration.
v: Target velocity.
Returns:
True: Success.
False: Failed.
"""
if self.__robot:
self.acc = a
self.vel = v
self.__robot.movel(pos,
acc=self.acc,
vel=self.vel,
wait=self.sync_mode)
self._update_send_time()
return True
else:
logging.error("robot is not initialized in " +
sys._getframe().f_code.co_name)
return False
def movej(self, joints, a=None, v=None):
"""Move the robot by joint movement.
Note:
None.
Args:
a: Target acceleration.
v: Target velocity.
Returns:
True: Success.
False: Failed.
"""
if self._pause:
return False
if self.__robot:
self._joints_goal = joints
self.acc = a
self.vel = v
self.__robot.movej(joints,
acc=self.acc,
vel=self.vel,
wait=self.sync_mode)
self._update_send_time()
return True
else:
logging.error("robot is not initialized in " +
sys._getframe().f_code.co_name)
return False
def movels(self, poslist, a=None, v=None):
"""Sequential move the robot in a linear path.
Note:
None.
Args:
poslist: Target position list
a: Target acceleration.
v: Target velocity.
Returns:
True: Success.
False: Failed.
"""
if self.__robot:
self.acc = a
self.vel = v
self.__robot.movels(poslist,
acc=self.acc,
vel=self.vel,
wait=self.sync_mode)
self._update_send_time()
return True
else:
logging.error("robot is not initialized in " +
sys._getframe().f_code.co_name)
return False
def translate_tool(self, vec, a=None, v=None):
"""Move tool in base coordinate, keeping orientation.
Note:
None.
Args:
poslist: Target position list
a: Target acceleration.
v: Target velocity.
Returns:
True: Success.
False: Failed.
"""
if self.__robot:
self.acc = a
self.vel = v
self.__robot.translate_tool(vec,
acc=self.acc,
vel=self.vel,
wait=self.sync_mode)
self._update_send_time()
return True
else:
logging.error("robot is not initialized in " +
sys._getframe().f_code.co_name)
return False
def getl(self):
"""Get TCP position.
Note:
None.
Args:
None.
Returns:
position: list of TCP position (X, Y, Z, Rx, Ry, Rz)
if failed to get, return (0, 0, 0, 0, 0, 0)
"""
if self.__robot:
return self.__robot.getl()
else:
logging.error("robot is not initialized in " +
sys._getframe().f_code.co_name)
return (0, 0, 0, 0, 0, 0)
def getj(self):
"""Get joint position.
Note:
None.
Args:
None.
Returns:
position: list of joint position (J0, J1, J2, J3, J4, J5)
if failed to get, return (0, 0, 0, 0, 0, 0)
"""
if self.__robot:
return self.__robot.getj()
else:
logging.error("robot is not initialized in " +
sys._getframe().f_code.co_name)
return (0, 0, 0, 0, 0, 0)
def get_force(self):
"""Get TCP force.
Note:
None.
Args:
None.
Returns:
force: value of TCP force
if failed to get, return 0
"""
if not self.__realtime:
logging.info("cannot use realtime port in " +
sys._getframe().f_code.co_name)
return 0
elif self.__robot:
return self.__robot.get_force()
else:
logging.error("robot is not initialized in " +
sys._getframe().f_code.co_name)
return 0
def start_freedrive(self, time=60):
"""Start freedrive mode.
Note:
None.
Args:
time: Time to keep freedrive mode in seconds (default=60s).
Returns:
True: Success.
False: Failed.
"""
if self.__robot:
self.__robot.set_freedrive(True, timeout=time)
self._update_send_time()
return True
else:
logging.error("robot is not initialized in " +
sys._getframe().f_code.co_name)
return False
def end_freedrive(self):
"""End freedrive mode.
Note:
None.
Args:
None.
Returns:
True: Success.
False: Failed.
"""
if self.__robot:
self.__robot.set_freedrive(None)
self._update_send_time()
return True
else:
logging.error("robot is not initialized in " +
sys._getframe().f_code.co_name)
return False
def open_gripper(self):
"""Open gripper.
Note:
None.
Args:
None.
Returns:
True: Success.
False: Failed.
"""
if self.__robot and self.__gripper:
try:
self.__gripper.open_gripper()
self._update_send_time()
except self.URxException:
logging.error("URx exception was ooccured in " +
sys._getframe().f_code.co_name)
return False
except Exception as e:
logging.error("except: " + format(str(e)) + " in " +
sys._getframe().f_code.co_name)
return False
else:
logging.error("robot or gripper is not initialized in " +
sys._getframe().f_code.co_name)
return False
return True
def gripper_action(self, value):
"""gripper action.
Note:
None.
Args:
value: value from 0 to 255.
0 is open, 255 is close.
Returns:
True: Success.
False: Failed.
"""
if value < 0 or value > 255:
return False
if self.__robot and self.__gripper:
try:
self.__gripper.gripper_action(value)
self._update_send_time()
except self.URxException:
logging.error("URx exception was ooccured in " +
sys._getframe().f_code.co_name)
return False
except Exception as e:
logging.error("except: " + format(str(e)) + " in " +
sys._getframe().f_code.co_name)
return False
else:
logging.error("robot or gripper is not initialized in " +
sys._getframe().f_code.co_name)
return False
return True
def close_gripper(self):
"""Close gripper.
Note:
None.
Args:
None.
Returns:
True: Success.
False: Failed.
"""
if self.__robot and self.__gripper:
try:
self.__gripper.close_gripper()
self._update_send_time()
except self.URxException:
logging.error("URx exception was ooccured in " +
sys._getframe().f_code.co_name)
return False
except Exception as e:
logging.error("except: " + format(str(e)) + " in " +
sys._getframe().f_code.co_name)
return False
else:
logging.error("robot or gripper is not initialized in " +
sys._getframe().f_code.co_name)
return False
return True
def pause(self):
"""Pause.
Note:
None.
Args:
None.
Returns:
True: Success.
False: Failed.
"""
if self.__robot:
self._pause = True
self.__robot.stopj(self.acc)
self._update_send_time()
return True
else:
logging.error("robot is not initialized in " +
sys._getframe().f_code.co_name)
return False
def resume(self):
"""Resume.
Note:
None.
Args:
None.
Returns:
True: Success.
False: Failed.
"""
if self.__robot:
self._pause = False
return True
else:
logging.error("robot is not initialized in " +
sys._getframe().f_code.co_name)
return False
def stopj(self, a=None):
"""Decelerate joint speeds to zero.
Note:
None.
Args:
a: joint acceleration [rad/s^2]
Returns:
True: Success.
False: Failed.
"""
if self.__robot:
self.acc = a
self.__robot.stopj(self.acc)
self._update_send_time()
return True
else:
logging.error("robot is not initialized in " +
sys._getframe().f_code.co_name)
return False
def get_pos(self):
"""get current transform from base to to tcp
Note:
None.
Args:
None.
Returns:
True: Success.
False: Failed.
"""
pose = []
if self.__robot:
pose = self.__robot.get_pos()
self._update_send_time()
return True, pose
else:
logging.error("robot is not initialized in " +
sys._getframe().f_code.co_name)
return False, pose
def get_joints_goal(self):
return self._joints_goal
class Interface:
def __init__(self):
rospy.init_node('interface')
# robot settings
rospy.loginfo('Connecting robot...')
self.rob = urx.Robot("192.168.1.5")
self.rob.set_tcp((0, 0, 0.17, 0, 0, 0))
self.rob.set_payload(1, (0, 0, 0.1))
rospy.sleep(rospy.Duration(2))
self.target_pose = self.get_tcp_pose()
# robot init
rospy.loginfo('Homing robot...')
self.gripper = Robotiq_Two_Finger_Gripper(self.rob)
self.gripper.open_gripper()
self.target_gripper = 0
# robot homing
self.move_joint(HOMEJ, True)
rospy.loginfo('Robot ready!')
def close_connection(self):
'''
close robot connection
:return:
'''
self.rob.close()
def move_home(self, wait):
'''
move back to home
:param wait: blocking wait (Boolean)
:return:
'''
self.move_joint(HOMEJ, wait)
def move_joint(self, joints, wait):
'''
move joints
:param joints: 6 joints values in radian
:param wait: blocking wait (Boolean)
:return:
'''
self.rob.movej(joints, ACCE, VELO, wait=wait)
def move_tcp_relative(self, pose, wait):
'''
move eff to relative pose
:param pose: relative differences in [x y z R P Y] in meter and radian
:param wait: blocking wait (Boolean)
:return: None
'''
self.rob.add_pose_tool(m3d.Transform(pose), ACCE, VELO, wait=wait)
def move_tcp_absolute(self, pose, wait):
'''
move eff to absolute pose in robot base frame
:param pose: list [x y z R P Y]
:param wait: blocking wait (Boolean)
:return: None
pose = [x y z R P Y] (x, y, z in m) (R P Y in rad)
'''
if (self.is_program_running() and
self.dist_linear(pose, self.target_pose) > TOL_TARGET_POSE) or \
(not self.is_program_running() and
self.dist_linear(pose, self.get_tcp_pose()) > TOL_TARGET_POSE):
self.target_pose = pose
self.rob.set_pose(m3d.Transform(pose), ACCE, VELO, wait)
def set_gripper(self, val):
'''
gripper control (Blocking wait)
:param val: False:RELEASE, True:GRIP
:return: None
val = true or false
'''
if self.target_gripper != val:
self.target_gripper = val
if val:
self.gripper.close_gripper()
else:
self.gripper.open_gripper()
def get_tcp_pose(self):
'''
get eff pose
:return: list [x y z R P Y]
'''
return copy.deepcopy(self.rob.getl())
def get_gripper(self):
"""
get gripper status
:return: Boolean (0 opened / 1 closed)
"""
return self.target_gripper
def get_state(self):
'''
get eff and gripper state
:return: list [x y z R P Y g]
'''
return self.get_tcp_pose() + [self.get_gripper()]
def is_program_running(self):
'''
check if a program is already running
:return: Boolean
'''
return self.rob.secmon.is_program_running()
@staticmethod
def dist_joint(val1, val2):
dist = 0
for i in range(6):
dist += (val1[i] - val2[i]) ** 2
return dist ** 0.5
@staticmethod
def dist_linear(val1, val2):
dist = 0
for i in range(3):
dist += (val1[i] - val2[i]) ** 2
for i in range(3, 6):
dist += ((val1[i] - val2[i]) / 5) ** 2
return dist ** 0.5