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 resetRobot(self):
self.go_to_home_up()
self.rob.set_tcp((0, 0.0, 0, 0, 0, 0))
time.sleep(0.2)
self.control_exted_thumb.set_thumb_length_int(180)
self.robotiqgrip = Robotiq_Two_Finger_Gripper(self.rob)
self.robotiqgrip.gripper_activate()
self.gp_control_distance(0.03)
self.go_to_home()
self.baseTee = self.rob.get_pose()
self.baseTee.orient = np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]])
print("reset")
def __init__(self, is_sim, obj_mesh_dir, num_obj, workspace_limits,
tcp_host_ip, tcp_port, rtc_host_ip, rtc_port, is_testing,
test_preset_cases, test_preset_file):
self.is_sim = is_sim
self.workspace_limits = workspace_limits
self.moveto_limits = ([[0.300, 0.600], [-0.250, 0.180], [0.195,
0.571]])
# Tool pose tolerance for blocking calls
self.tool_pose_tolerance = [0.002, 0.002, 0.002, 0.01, 0.01, 0.01]
# Joint tolerance for blocking calls
self.joint_tolerance = 0.01
# If in simulation...
if self.is_sim:
pass
# If in real-settings...
else:
# Connect to robot client
self.tcp_host_ip = tcp_host_ip
self.tcp_port = tcp_port
# self.tcp_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self.r = urx.Robot(tcp_host_ip)
self.r.set_tcp((0, 0, 0, 0, 0, 0))
self.r.set_payload(0.5, (0, 0, 0))
self.gripper = Robotiq_Two_Finger_Gripper(self.r)
# Connect as real-time client to parse state data
# self.rtc_host_ip = rtc_host_ip
# self.rtc_port = rtc_port
# NOTE: this is for D415
# home_in_deg = np.array(
# [-151.4, -93.7, 85.4, -90, -90, 0]) * 1.0
# NOTE: this is for throw practice
home_in_deg = np.array([-197, -105, 130, -110, -90, -30]) * 1.0
self.home_joint_config = np.deg2rad(home_in_deg)
# Default joint speed configuration
# self.joint_acc = 8 # Safe: 1.4
# self.joint_vel = 3 # Safe: 1.05
self.joint_acc = 0.50 # Safe when set 30% spe71ed on pendant
self.joint_vel = 0.35
# Default tool speed configuration
# self.tool_acc = 1.2 # Safe: 0.5
# self.tool_vel = 0.25 # Safe: 0.2
self.tool_acc = 0.1 # Safe when set 30% speed on pendant
self.tool_vel = 0.1
# Move robot to home pose
self.go_home()
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 __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 throw(self):
self.close_gripper()
start_position = ['p', 0.350, 0.000, 0.250]
start_axisangle = [2.12, -2.21, -0.009]
start_pose = np.concatenate((start_position, start_axisangle))
curled_config_deg = ['j', -196, -107, 126, -90, -90, -12]
curled_config = np.deg2rad(curled_config_deg)
# curr_joint_pose = self.parse_tcp_state_data(self.tcp_socket,
# 'joint_data')
# print('joints', curr_joint_pose)
# end_position = [0.600, 0.000, 0.450]
# end_axisangle = [2.55, -2.06, 0.80]
end_position = ['p', 0.597, 0.000, 0.550]
end_axisangle = [2.18, -2.35, 2.21]
end_pose = np.concatenate((end_position, end_axisangle))
r = min(abs(end_position[0] - start_position[0]) / 2 - 0.01, 0.2)
print(r)
middle_position = np.array(end_position) - np.array([0.020, 0, -0.020])
middle_pose = np.concatenate((middle_position, end_axisangle))
blend_radius = 0.100
K = 1. # 28.
gripper = Robotiq_Two_Finger_Gripper(self.r)
# NOTE: important
throw_pose_list = [
start_pose, middle_pose, "open", end_pose, start_pose
]
# pose_list = [start_pose, middle_pose, end_pose, start_pose]
self.r.throw_primitive(throw_pose_list, wait=True)
# self.r.throw_primitive(["open"], wait=False)
""" this stops between points
print('throw acc will be', self.joint_acc * 1) # 4)
print('throw vel will be', self.joint_vel * 1) # 0)
self.move_to(start_position, start_axisangle, acc_scaling=K,
vel_scaling=K, radius=0) # last # is blend radius
# , acc_scaling=K, vel_scaling=K, radius=0) # last # is blend radius
self.move_joints(curled_config)
self.move_to(end_position, end_axisangle, acc_scaling=K,
vel_scaling=K, radius=0.5) # last # is blend radius
# gripper.open_gripper()
self.move_to(np.array(end_position) - np.array((0.020, 0, -0.020)), end_axisangle, acc_scaling=K,
vel_scaling=K, radius=0.1) # last # is blend radius
self.move_to(start_position, start_axisangle, acc_scaling=K,
vel_scaling=K, radius=0) # last # is blend radius
"""
'''
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 __init__(self):
rospy.Subscriber(name='/tag_detections',
data_class=AprilTagDetectionArray,
callback=self.on_sub,
queue_size=1) # queue_size 是在订阅者接受消息不够快的时候保留的消息的数量
#image_sub = rospy.Subscriber("/camera/rgb/image_raw",data_class=Image,callback=self.rgb_callback)
self.target_on_base = None
self.rbt = URRobot('192.168.31.53')
self.gripper = Robotiq_Two_Finger_Gripper(self.rbt)
#self.gripper.gripper_action(0)
self.pos_init = [0.1, -0.3, 0.3, 0, pi, 0]
self.rbt.movel(self.pos_init, 0.3, 0.3)
#self.gripper.gripper_action(255)
time.sleep(2)
self.compute_Q = False
self.noise_eps = 0
self.random_eps = 0
self.use_target_net = False
self.exploit = False
self.has_object = True
self.block_gripper = False #reach:true pick:false
self.detection_target_storage = []
#self.detection_object_storage = []
self.gripper_state = [0, 0]
self.pick = False
self.object_reach = False
#self.pick = True
#seed = 0
#set_global_seeds(seed)
policy_file = "./policy_best_picknplace.pkl"
# Load policy.
with open(policy_file, 'rb') as f:
self.policy = pickle.load(f)
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)
class ur5_reach:
def __init__(self):
rospy.Subscriber(name='/tag_detections',
data_class=AprilTagDetectionArray,
callback=self.on_sub,
queue_size=1) # queue_size 是在订阅者接受消息不够快的时候保留的消息的数量
#image_sub = rospy.Subscriber("/camera/rgb/image_raw",data_class=Image,callback=self.rgb_callback)
self.target_on_base = None
self.rbt = URRobot('192.168.31.53')
self.gripper = Robotiq_Two_Finger_Gripper(self.rbt)
#self.gripper.gripper_action(0)
self.pos_init = [0.1, -0.3, 0.3, 0, pi, 0]
self.rbt.movel(self.pos_init, 0.3, 0.3)
#self.gripper.gripper_action(255)
time.sleep(2)
self.compute_Q = False
self.noise_eps = 0
self.random_eps = 0
self.use_target_net = False
self.exploit = False
self.has_object = True
self.block_gripper = False #reach:true pick:false
self.detection_target_storage = []
#self.detection_object_storage = []
self.gripper_state = [0, 0]
self.pick = False
self.object_reach = False
#self.pick = True
#seed = 0
#set_global_seeds(seed)
policy_file = "./policy_best_picknplace.pkl"
# Load policy.
with open(policy_file, 'rb') as f:
self.policy = pickle.load(f)
def detection_object_by_color(self):
#u,v=detect_ball()
return Real_XY_of_BOX_on_Camera(u, v, z)
# rbt Transfer (rbt_base to frame_base)
# UR5--Base
def on_sub(self, data):
# target Transfer (kinect2_rgb_optical_frame to frame_base),get g
# Camera--Base
detections = data.detections
for i in range(len(detections)):
if detections[i].id == 12:
self.detection_target = detections[i]
self.detection_target_storage.append([
self.detection_target.pose.pose.position.x,
self.detection_target.pose.pose.position.y,
self.detection_target.pose.pose.position.z
])
'''
if detections[i].id == 13:
self.detection_object = detections[i]
self.detection_object_storage.append([self.detection_object.pose.pose.position.x, self.detection_object.pose.pose.position.y, self.detection_object.pose.pose.position.z])
'''
pose_on_camera_target = self.detection_target_storage[0]
#pose_on_camera_object = self.detection_object_storage[0]
pose_on_camera_object = self.detection_object_by_color().ravel()
self.target_on_base = self.camera2base(pose_on_camera_target)
self.object_on_base = self.camera2base(pose_on_camera_object)
pose_rbt = self.rbt.getl()
grip_pos = [
-pose_rbt[1] + 0.75, pose_rbt[0] + 0.74, pose_rbt[2] + 0.65
] # grip_pos 末端位置
target_pos = [
-self.target_on_base[1] + 0.75, self.target_on_base[0] + 0.74,
self.target_on_base[2] + 0.70
] # 目的地位置, 目的地比夹手末端在z轴上高5cm.
if self.pick == True and sum(
abs(
np.asarray(grip_pos) - np.asarray([
-self.object_on_base[1] + 0.75, self.object_on_base[0]
+ 0.74, self.object_on_base[2] + 0.625
]))) < 0.1:
object_pos = grip_pos # 物体位置
object_rel_pos = [0, 0, 0]
gripper_state = [0.025, 0.025]
self.object_reach = True
if self.object_reach == True:
object_pos = grip_pos # 物体位置
object_rel_pos = [0, 0, 0]
gripper_state = [0.025, 0.025]
else:
object_pos = [
-self.object_on_base[1] + 0.75, self.object_on_base[0] + 0.74,
self.object_on_base[2] + 0.625
] # 物体位置
object_rel_pos = [
object_pos[0] - grip_pos[0], object_pos[1] - grip_pos[1],
object_pos[2] - grip_pos[2]
]
gripper_state = self.gripper_state
achieved_goal = np.squeeze(object_pos.copy())
o = [
np.concatenate([
grip_pos,
np.asarray(object_pos).ravel(),
np.asarray(object_rel_pos).ravel(), gripper_state
])
] # grip_pos
ag = [achieved_goal] # grip_pos object_pos
g = [target_pos]
print(o)
# run model get action, pos_delta (frame_base)
# 这个action是仿真下的。1 action = 0.033 仿真观测 = 0.033真实观测。
action = self.policy.get_actions(
o,
ag,
g,
compute_Q=self.compute_Q,
noise_eps=self.noise_eps if not self.exploit else 0.,
random_eps=self.random_eps if not self.exploit else 0.,
use_target_net=self.use_target_net)
if self.compute_Q:
u, Q = action
else:
u = action
if u.ndim == 1:
# The non-batched case should still have a reasonable shape.
u = u.reshape(1, -1)
pos_ctrl, gripper_ctrl = action[:3], action[3]
if gripper_ctrl < 0:
self.pick = True
else:
self.pick = False
pos_ctrl *= 0.033 #simulation action -> real world action.(1 sim action = 0.033 real action)
gripper_ctrl *= 0.7636
rot_ctrl = [
1., 0., 1., 0.
] # fixed rotation of the end effector, expressed as a quaternion
gripper_ctrl = np.array([gripper_ctrl, gripper_ctrl])
assert gripper_ctrl.shape == (2, )
if self.block_gripper:
gripper_ctrl = np.zeros_like(gripper_ctrl)
action = np.concatenate([pos_ctrl, rot_ctrl, gripper_ctrl]) # action
action, gripper_st = np.split(action, (1 * 7, ))
action = action.reshape(1, 7)
pos_delta = action[:, :3]
#pos_delta = pos_ctrl
self.gripper_state += gripper_st
if self.gripper_state[0] > 0.05:
self.gripper_state = [0.05, 0.05]
if self.gripper_state[0] < 0:
self.gripper_state = [0, 0]
act_on_base = pose_rbt[:3]
her_pose = [act_on_base[0], act_on_base[1],
act_on_base[2]] # 末端关于UR3基坐标系的位置
# test on rule and get right position
rule_pose = [0, 0, 0, 0, 0, 0]
rule_pose[0:3] = [
self.target_on_base[0], self.target_on_base[1],
self.target_on_base[2]
]
rule_pose[3] = pose_rbt[3]
rule_pose[4] = pose_rbt[4]
rule_pose[5] = pose_rbt[5] # rx, ry, rz 固定
# judge if the target is reached
'''
rule_pose_np = np.asarray(rule_pose[:3]).ravel()
her_pose_np = np.asarray(her_pose[:3]).ravel()
if np.matmul((rule_pose_np-her_pose_np).T, rule_pose_np-her_pose_np) < 0.02:
'''
if abs(her_pose[0] - rule_pose[0]) + abs(her_pose[1] -
rule_pose[1]) < 0.04:
print("reach the target")
return
act_on_base[0] += pos_delta[0][1] # left right
act_on_base[1] -= pos_delta[0][0] # front behind
act_on_base[2] += pos_delta[0][2] # up down
robot_delta = [
act_on_base[0], act_on_base[1], act_on_base[2], pose_rbt[3],
pose_rbt[4], pose_rbt[5]
]
self.rbt.movel(tpose=robot_delta, acc=0.1, vel=0.25)
self.gripper.gripper_action(int(255 - 5100 * self.gripper_state[0]))
def camera2base(self, pose_on_camera):
Tm = np.array([[-0.9992, 0.0082, 0.0383, 0.0183],
[-0.0036, 0.9543, -0.2987, -0.2376],
[-0.0390, -0.2986, -0.9536, 1.1127], [0, 0, 0, 1.0000]])
pose_on_base = np.dot(
[pose_on_camera[0], pose_on_camera[1], pose_on_camera[2], 1], Tm.T)
pose_on_base[0] = pose_on_base[0] - 0.03
pose_on_base[1] = pose_on_base[1] * 0.98 - 0.01
pose_on_base[2] = pose_on_base[2] * 0.94 + 0.09
return pose_on_base
import urx
import logging
import time
import numpy as np
import math
from urx import RobotPose
from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper
logging.basicConfig(level=logging.WARN)
# Robotiq TCP = [0, 0, 0.16, 0, 0, 0]
# Ready Pose = [-0.5, 0, 0.35, 0, -math.pi, 0]
rob = urx.Robot("192.168.1.102")
robotiqgrip = Robotiq_Two_Finger_Gripper(rob)
try:
#rob = urx.Robot("localhost")
rob.set_tcp((0, 0, 0.16, 0, 0, 0))
rob.set_payload(0, (0, 0, 0))
v = 0.1
a = 0.1
pose = rob.getl()
joint_pose = rob.getj()
rpose = RobotPose()
rpose.set_pose(pose)
rpose.set_joint_pose(joint_pose)
from urx import URRobot
from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper
rbt = URRobot('192.168.31.53')
robotiqgrip = Robotiq_Two_Finger_Gripper(rbt)
pos_init = [0.1, -0.4, 0.35, 2, 0, 0]
robotiqgrip.open_gripper()
rbt.movel(pos_init, 0.2, 0.2)
print("Robot is moving...")
rbt.close()
TCP_PORT_GRIPPER = 40001 # Ne pas changer
TCP_PORT = 50002 # Ne pas changer
# Connection au robot
robot = urx.Robot(TCP_Robot)
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(
2.152,
(0, 0, 0.1
)) # Poids de l'outil et position de son centre de gravité (en kg et mm)
# Connection à la pince
gripper = Robotiq_Two_Finger_Gripper(robot)
# Caractéristique de mouvement
acc = 0.1 # Accélération maximale des joints
vel = 0.05 # Vitesse maximale des joints
deg2rad = np.pi / 180
angular_pos = [
-71 * deg2rad, -79 * deg2rad, -93 * deg2rad, -93 * deg2rad, 89 * deg2rad,
-12 * deg2rad
]
# Commandes robot
'''
Les différentes commandes sont disponibles dans les fichiers python-urx/urx/robot.py
et python-urx/urx/urrobot.py
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
import sys
import urx
from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper
if __name__ == '__main__':
rob = urx.Robot("192.168.0.110")
robotiqgrip = Robotiq_Two_Finger_Gripper()
if (len(sys.argv) != 2):
print("false")
sys.exit()
if (sys.argv[1] == "close"):
robotiqgrip.close_gripper()
if (sys.argv[1] == "open"):
robotiqgrip.open_gripper()
rob.send_program(robotiqgrip.ret_program_to_run())
rob.close()
print("true")
sys.exit()
TCP_PORT_GRIPPER = 40001 # Ne pas changer
TCP_PORT = 30002 # Ne pas changer
# Connection au robot
robot = urx.Robot(TCP_Robot)
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
)) # Poids de l'outil et position de son centre de gravité (en kg et mm)
# Connection à la pince
gripper = Robotiq_Two_Finger_Gripper(robot)
# Caractéristique de mouvement
acc = 0.4 # Accélération maximale des joints
vel = 0.4 # Vitesse maximale des joints
deg2rad = np.pi / 180
angular_pos = [
-250 * deg2rad, -55 * deg2rad, 50 * deg2rad, -90 * deg2rad, 250 * deg2rad,
-50 * deg2rad
]
# Commandes robot
'''
Les différentes commandes sont disponibles dans les fichiers python-urx-0.11.0/urx/robot.py
et python-urx-0.11.0/urx/urrobot.py
'''
import urx
from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper
robot = urx.Robot('192.168.1.109')
gripper = Robotiq_Two_Finger_Gripper(robot)
gripper.gripper_action(96)
gripper.close_gripper()
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 Robot():
def __init__(self, tcp_host_ip):
self.camera_width = 640
self.camera_height = 480
self.workspace_limits = np.array([[-0.01, 0.12], [0.63, 0.76],
[0.02, 0.08]])
self.heightmap_resolution = 0.0013 / 4.0
# robot gripper parameter
self.finger_length = 0.125
self.l0 = 0.0125
self.l1 = 0.1
self.l2l = 0.019
self.l2r = 0.01
logging.basicConfig(level=logging.WARN)
self.rob = urx.Robot(tcp_host_ip) #"192.168.1.102"
self.control_exted_thumb = Arduino_motor()
self.tcp_host_ip = tcp_host_ip
self.resetRobot()
self.resetFT300Sensor()
self.setCamera()
self.collision_check_initialization(bowl_position=[0.05, 0.695, 0.03])
def resetRobot(self):
self.go_to_home_up()
self.rob.set_tcp((0, 0.0, 0, 0, 0, 0))
time.sleep(0.2)
self.control_exted_thumb.set_thumb_length_int(180)
self.robotiqgrip = Robotiq_Two_Finger_Gripper(self.rob)
self.robotiqgrip.gripper_activate()
self.gp_control_distance(0.03)
self.go_to_home()
self.baseTee = self.rob.get_pose()
self.baseTee.orient = np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]])
print("reset")
def resetFT300Sensor(self):
HOST = self.tcp_host_ip
PORT = 63351
self.serialFT300Sensor = socket.socket(socket.AF_INET,
socket.SOCK_STREAM)
self.serialFT300Sensor.connect((HOST, PORT))
def getFT300SensorData(self):
while True:
data = str(self.serialFT300Sensor.recv(1024),
"utf-8").replace("(", "").replace(")", "").split(",")
try:
data = [float(x) for x in data]
if len(data) == 6:
break
except:
pass
return data
def setCamera(self):
self.cam_intrinsics = np.asarray(
[[612.0938720703125, 0, 321.8862609863281],
[0, 611.785888671875, 238.18316650390625], [0, 0, 1]])
self.eeTcam = np.array([[0, -1, 0, 0.142], [1, 0, 0, -0.003],
[0, 0, 1, 0.0934057 + 0.03], [0, 0, 0, 1]])
self.baseTcam = np.matmul(self.baseTee.get_matrix(), self.eeTcam)
def getCameraData(self, data_order=0):
# Set the camera settings.
self.setCamera()
# Setup:
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, self.camera_width,
self.camera_height, rs.format.z16, 30)
config.enable_stream(rs.stream.color, self.camera_width,
self.camera_height, rs.format.bgr8, 30)
profile = pipeline.start(config)
depth_scale = profile.get_device().first_depth_sensor(
).get_depth_scale()
#print("depth_scale", depth_scale)
# Store next frameset for later processing:
frames = pipeline.wait_for_frames()
color_frame = frames.get_color_frame()
depth_frame = frames.get_depth_frame()
# Cleanup:
pipeline.stop()
#print("Frames Captured")
color = np.asanyarray(color_frame.get_data())
#color_img_path = './picture_20210422/color_img/'+str(data_order)+'_color.jpg'
#cv2.imwrite(color_img_path, color)
# Create alignment primitive with color as its target stream:
align = rs.align(rs.stream.color)
frames = align.process(frames)
# Update color and depth frames:
aligned_depth_frame = frames.get_depth_frame()
depth_image_raw = np.asanyarray(aligned_depth_frame.get_data())
depth_image = depth_image_raw * depth_scale
#depth_img_path = './picture_20210422/depth_img/'+str(data_order)+'_depth.jpg'
#cv2.imwrite(depth_img_path, depth_image*1000)
#colorizer = rs.colorizer()
#colorized_depth = np.asanyarray(colorizer.colorize(aligned_depth_frame).get_data())
return color, depth_image
#return color, depth_image, color_img_path, depth_img_path
def gp_control_int(self, aperture_int, delay_time=0.2):
self.robotiqgrip.gripper_action(aperture_int)
time.sleep(delay_time)
def gp_control_distance(self, aperture_distance, delay_time=0.2): #meter
int_list = np.array([
0, 20, 40, 60, 80, 100, 120, 130, 140, 150, 160, 170, 180, 185,
190, 195, 200, 205, 210
]) #
distance_list = np.array([
125, 116.39, 105.33, 93.91, 82.48, 70.44, 58.09, 51.90, 45.47,
39.05, 32.84, 26.76, 19.65, 16.39, 13.32, 10.20, 7.76, 3.89, 0
]) / 1000
func_from_distance_to_int = interpolate.interp1d(distance_list,
int_list,
kind=3)
self.robotiqgrip.gripper_action(
int(func_from_distance_to_int(aperture_distance)))
time.sleep(delay_time)
def go_to_home(self):
home_position = [100.28, -80.88, -126.70, -61.56, 90.35, -81.93]
Hong_joint0 = math.radians(home_position[0])
Hong_joint1 = math.radians(home_position[1])
Hong_joint2 = math.radians(home_position[2])
Hong_joint3 = math.radians(home_position[3])
Hong_joint4 = math.radians(home_position[4])
Hong_joint5 = math.radians(home_position[5])
self.rob.movej((Hong_joint0, Hong_joint1, Hong_joint2, Hong_joint3,
Hong_joint4, Hong_joint5), 0.3, 0.5)
def go_to_home_up(self):
home_position = [100.21, -77.82, -121.66, -69.66, 90.38, -81.95]
Hong_joint0 = math.radians(home_position[0])
Hong_joint1 = math.radians(home_position[1])
Hong_joint2 = math.radians(home_position[2])
Hong_joint3 = math.radians(home_position[3])
Hong_joint4 = math.radians(home_position[4])
Hong_joint5 = math.radians(home_position[5])
self.rob.movej((Hong_joint0, Hong_joint1, Hong_joint2, Hong_joint3,
Hong_joint4, Hong_joint5), 0.3, 0.5)
def Frame(pos, ori):
mat = R.from_quat(ori).as_matrix()
F = np.concatenate([
np.concatenate([mat, [[0, 0, 0]]], axis=0),
np.reshape([*pos, 1.], [-1, 1])
],
axis=1)
return F
def exe_scoop(self,
pos,
rot_z,
ini_aperture,
theta=60 * pi / 180): # rot_z rad aperture distance
if self.collision_check_scooping(pos, rot_z, ini_aperture,
theta) == True:
print("Collision!")
return -1
self.go_to_home()
self.rob.translate((0, 0, 0.06), acc=0.5, vel=0.8)
if ini_aperture == 0.05:
self.rob.set_tcp((0.0295, 0.0, 0.3389, 0, 0, 0)) #need change
time.sleep(0.3)
elif ini_aperture == 0.04:
self.rob.set_tcp((0.0245, 0.0, 0.3396, 0, 0, 0)) #need change
time.sleep(0.3)
elif ini_aperture == 0.035:
self.rob.set_tcp((0.022, 0.0, 0.3396, 0, 0, 0)) #need change
time.sleep(0.3)
elif ini_aperture == 0.03:
self.rob.set_tcp((0.0195, 0.0, 0.3414, 0, 0, 0)) #need change
time.sleep(0.3)
elif ini_aperture == 0.025:
self.rob.set_tcp((0.017, 0.0, 0.3414, 0, 0, 0)) #need change
time.sleep(0.3)
elif ini_aperture == 0.02:
self.rob.set_tcp((0.0145, 0.0, 0.342, 0, 0, 0)) #need change
time.sleep(0.3)
elif ini_aperture == 0.015:
self.rob.set_tcp((0.012, 0.0, 0.3425, 0, 0, 0)) #need change
time.sleep(0.3)
else:
raise NameError("Wrong ini aperture!")
self.gp_control_distance(ini_aperture, delay_time=0.2)
self.control_exted_thumb.set_thumb_length_int(0)
eefPose = self.rob.get_pose()
time.sleep(0.5)
eefPose = eefPose.get_pose_vector()
self.rob.translate((pos[0] - eefPose[0], pos[1] - eefPose[1], 0),
acc=0.5,
vel=0.8)
self.rob.movel_tool((0, 0, 0, 0, 0, rot_z), acc=0.5, vel=0.8)
self.rob.translate((0, 0, pos[2] + 0.01 - eefPose[2]),
acc=0.05,
vel=0.05)
self.rob.movel_tool((0, 0, 0, 0, pi / 2 - theta, 0),
acc=0.5,
vel=0.8,
wait=True)
self.rob.translate((0, 0, -0.02), acc=0.01, vel=0.05, wait=False)
ini_torque_y = self.getFT300SensorData()[4]
time0 = time.time()
num_large_force = 0
while True:
if num_large_force == 3:
self.rob.stopl()
break
torque_y = self.getFT300SensorData()[4]
if torque_y > ini_torque_y + 0.1:
num_large_force += 1
if time.time() - time0 > 2.8:
break
time.sleep(0.1)
ini_torque_y = self.getFT300SensorData()[4]
shortest_thumb_length = self.control_exted_thumb.shortest_thumb_length
longest_thumb_length = self.finger_length - ini_aperture / tan(theta)
for current_thumb_length in np.arange(
shortest_thumb_length, longest_thumb_length + (1e-8),
(longest_thumb_length - shortest_thumb_length) / 10):
self.control_exted_thumb.set_thumb_length(current_thumb_length)
torque_y = self.getFT300SensorData()[4]
if torque_y > ini_torque_y + 0.2:
break
for aperture_distance in np.arange(ini_aperture, -1e-5, -0.002):
aperture_angle = self.from_aperture_distance_to_angle(
aperture_distance, self.l0, self.l1, self.l2l, self.l2r)
next_aperture_angle = self.from_aperture_distance_to_angle(
aperture_distance - 0.002, self.l0, self.l1, self.l2l,
self.l2r)
translate_dir_dis, extension_distance = self.scooping_parameter_finger_fixed(
aperture_angle, next_aperture_angle, theta * 180 / pi, self.l0,
self.l1, self.l2l, self.l2r, self.finger_length)
current_thumb_length += extension_distance
self.control_exted_thumb.set_thumb_length(current_thumb_length)
self.gp_control_distance(max(aperture_distance, 0), delay_time=0)
self.rob.translate_tool((sin(theta) * translate_dir_dis[0] +
cos(theta) * translate_dir_dis[1], 0,
cos(theta) * translate_dir_dis[0] -
sin(theta) * translate_dir_dis[1]),
acc=0.1,
vel=0.4,
wait=False)
#time.sleep(0.5)
self.rob.translate_tool((0, 0, -0.12), acc=0.3, vel=0.8, wait=True)
self.go_to_home_up()
self.go_to_home()
self.gp_control_distance(0.03)
while True:
whether_successful = input("Whether successful? (y or n)")
if whether_successful == 'n':
return 0
elif whether_successful == 'y':
return 1
def finger_tip_position_wrt_gripper_frame(self, alpha, l0, l1, l2l, l2r,
flength_l, flength_r):
alpha = alpha * pi / 180
left_fingertip_position = [
-l0 - l1 * sin(alpha) + l2l, -l1 * cos(alpha) - flength_l
]
right_fingertip_position = [
l0 + l1 * sin(alpha) - l2r, -l1 * cos(alpha) - flength_r
]
return left_fingertip_position, right_fingertip_position
def from_aperture_distance_to_angle(self, distance, l0, l1, l2l,
l2r): #angle
return asin((distance + l2l + l2r - 2 * l0) / (2 * l1)) * 180 / pi
def scooping_parameter_finger_fixed(self, current_alpha, next_alpha, theta,
l0, l1, l2l, l2r, flength_r):
theta = theta * pi / 180
current_flength_l = flength_r - (2 * l0 + 2 * l1 * sin(
current_alpha * pi / 180) - l2l - l2r) / tan(theta)
current_left_fingertip_pos_g, current_right_fingertip_pos_g = self.finger_tip_position_wrt_gripper_frame(
current_alpha, l0, l1, l2l, l2r, current_flength_l, flength_r)
next_left_fingertip_pos_g, next_right_fingertip_pos_g = self.finger_tip_position_wrt_gripper_frame(
next_alpha, l0, l1, l2l, l2r, current_flength_l, flength_r)
traslating_wst_g = [
current_right_fingertip_pos_g[0] - next_right_fingertip_pos_g[0],
current_right_fingertip_pos_g[1] - next_right_fingertip_pos_g[1]
]
traslating_wst_world = np.array([[sin(theta), -cos(theta)],
[cos(theta), sin(theta)]]).dot(
np.array([[traslating_wst_g[0]],
[traslating_wst_g[1]]
])).ravel().tolist()
next_left_fingertip_pos_g = [
next_left_fingertip_pos_g[0] + traslating_wst_g[0],
next_left_fingertip_pos_g[1] + traslating_wst_g[1]
]
next_right_fingertip_pos_g = [
next_right_fingertip_pos_g[0] + traslating_wst_g[0],
next_right_fingertip_pos_g[1] + traslating_wst_g[1]
]
extension_distance = -(
next_right_fingertip_pos_g[1] +
(2 * l0 + 2 * l1 * sin(next_alpha * pi / 180) - l2l - l2r) /
tan(theta) - next_left_fingertip_pos_g[1])
return traslating_wst_world, extension_distance
def set_joint_positions_collision_check(self, body, joints, values):
assert len(joints) == len(values)
for joint, value in zip(joints, values):
p.resetJointState(body, joint, value)
def collision_check_initialization(self, bowl_position):
self.THUMB_JOINT_INDICES = [0]
physicsClient = p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setPhysicsEngineParameter(enableFileCaching=0)
p.setGravity(0, 0, -9.8)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, False)
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, True)
p.resetDebugVisualizerCamera(cameraDistance=1.400,
cameraYaw=58.000,
cameraPitch=-42.200,
cameraTargetPosition=(0.0, 0.0, 0.0))
self.CLIENT = 0
self.finger_collision_check = p.loadURDF(
'/home/terry/catkin_ws/src/scoop/models/urdf/Finger-2.9-urdf.urdf',
basePosition=[0, 0, 0],
baseOrientation=[0, 0, 1, 0],
useFixedBase=False)
self.thumb_collision_check = p.loadURDF(
'/home/terry/catkin_ws/src/scoop/models/urdf/Assem-1.9-urdf.urdf',
basePosition=[0, 0, 0],
baseOrientation=[0, 0, 1, 0],
useFixedBase=False)
self.bowl_position = bowl_position
self.bowl_circular_collision_check = p.loadURDF(
'/home/terry/catkin_ws/src/scoop/models/urdf/Bowl-circular-urdf.urdf',
basePosition=self.bowl_position,
baseOrientation=[0, 0, 0, 1],
useFixedBase=True)
self.obstacles_collision_check = [self.bowl_circular_collision_check]
def collision_check_scooping(self,
pos,
rot_z,
ini_aperture,
theta=60 * pi / 180):
if Point([pos[0], pos[1]]).within(
Point([self.bowl_position[0],
self.bowl_position[1]]).buffer(0.06)) == False:
return True
if Point([
pos[0] - ini_aperture * sin(rot_z),
pos[1] - ini_aperture * cos(rot_z)
]).within(
Point([self.bowl_position[0],
self.bowl_position[1]]).buffer(0.055)) == False:
return True
start_conf_thumb = [
self.control_exted_thumb.shortest_thumb_length -
(self.finger_length - ini_aperture / tan(theta))
]
#start_conf_thumb = [0.1-(self.finger_length-ini_aperture/tan(theta))]
fingerOrientation_raw = R.from_dcm([[0, 1, 0], [1, 0, 0], [
0, 0, -1
]]) * R.from_euler('z', rot_z, degrees=False) * R.from_euler(
'y', pi / 2 - theta, degrees=False)
fingerOrientation = fingerOrientation_raw.as_quat().tolist()
ThumbTFinger_normal = fingerOrientation_raw.as_dcm()[:, 0].tolist()
fingertipPosition = pos
p.resetBasePositionAndOrientation(self.finger_collision_check,
fingertipPosition, fingerOrientation)
collision_fn = [
len(
p.getClosestPoints(bodyA=self.finger_collision_check,
bodyB=self.bowl_circular_collision_check,
distance=0,
physicsClientId=self.CLIENT)) != 0
]
fingerBasePosition = list(
p.getLinkState(self.finger_collision_check, 0)[0])
thumbOrientation = fingerOrientation
for finger_thumb_distance in np.arange(
ini_aperture + self.l2l + self.l2r,
ini_aperture - 0.0001 + self.l2l + self.l2r,
-ini_aperture / 5):
thumbBasePosition = [
fingerBasePosition[0] -
finger_thumb_distance * ThumbTFinger_normal[0],
fingerBasePosition[1] -
finger_thumb_distance * ThumbTFinger_normal[1],
fingerBasePosition[2] -
finger_thumb_distance * ThumbTFinger_normal[2]
]
self.set_joint_positions_collision_check(
self.thumb_collision_check, self.THUMB_JOINT_INDICES,
start_conf_thumb)
p.resetBasePositionAndOrientation(self.thumb_collision_check,
thumbBasePosition,
thumbOrientation)
collision_fn.append(
len(
p.getClosestPoints(
bodyA=self.thumb_collision_check,
bodyB=self.bowl_circular_collision_check,
distance=0,
physicsClientId=self.CLIENT)) != 0)
result = False
for element in collision_fn:
result = result or element
#print(collision_fn)
return result
def fromResultToRobotParameter(self, MaskRCNNResult, workspace_limits):
random.shuffle(MaskRCNNResult)
for ObjectPoseIndex in range(len(MaskRCNNResult)):
ObjectPose = MaskRCNNResult[ObjectPoseIndex]
if ObjectPose['z'] == 0:
continue
CamTObject = np.array(
[ObjectPose['x'], ObjectPose['y'], ObjectPose['z'], 1])
BaseTObject = np.matmul(self.baseTcam, CamTObject).tolist()[0][0:3]
#print('BaseTObject', BaseTObject)
#print('workspace_limits', self.workspace_limits)
if BaseTObject[0] <= workspace_limits[0][0] or BaseTObject[
0] >= workspace_limits[0][1] or BaseTObject[
1] <= workspace_limits[1][0] or BaseTObject[
1] >= workspace_limits[1][1] or BaseTObject[
2] <= workspace_limits[2][0] or BaseTObject[
2] >= workspace_limits[2][1]:
continue
NormalInCamera = ObjectPose['normal'] / np.linalg.norm(
ObjectPose['normal'])
NormalInTcp = np.matmul(self.eeTcam[0:3, 0:3],
NormalInCamera).tolist()
alpha0 = atan2(NormalInTcp[1], NormalInTcp[0]) # is yaw
if str(alpha0) == 'nan':
continue
beta = -(pi / 2 + atan2(
NormalInTcp[2], sqrt(NormalInTcp[0]**2 + NormalInTcp[1]**2))
) #tune psi
if str(beta) == 'nan':
continue
if abs(beta) < (5 * pi / 180):
alpha_set = [
0, pi / 4, -pi / 4, pi * 3 / 4, -pi * 3 / 4, pi, pi / 2,
-pi / 2
]
rot_z = alpha_set[random.randint(0, len(alpha_set))]
pos = [
BaseTObject[0] + 0.01 * sin(rot_z),
BaseTObject[1] + 0.01 * cos(rot_z), BaseTObject[2]
]
else:
rot_z = round(alpha0 / (pi / 4)) * (pi / 4)
pos = [
BaseTObject[0] + 0.01 * cos(beta) * sin(rot_z),
BaseTObject[1] + 0.01 * cos(beta) * cos(rot_z),
BaseTObject[2]
]
return pos, rot_z
else:
return None, None
rob.movel(snap_pose, a, v) # Move robot arm back to Snap Pose
# 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':
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)
def robot_move(h, v, acc, vel):
robot.movel(h, acc, vel, relative=True)
time.sleep(1)
robot.movel(v, acc, vel, relative=True)
gripper.close_gripper()
time.sleep(3)
gripper.open_gripper()
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]
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
import socket
import time
import urx
from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper
l = 0.1
v = 0.07
a = 0.1
r = 0.05
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
import sys
import urx
from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper
tcp_host_ip = "10.75.15.91"
if __name__ == '__main__':
rob = urx.Robot(tcp_host_ip)
# robotiqgrip = Robotiq_Two_Finger_Gripper(socket_host="/dev/ttyUSB0")
robotiqgrip = Robotiq_Two_Finger_Gripper(rob)
if (len(sys.argv) != 2):
print "false"
sys.exit()
if (sys.argv[1] == "close"):
robotiqgrip.close_gripper()
if (sys.argv[1] == "open"):
robotiqgrip.open_gripper()
rob.close()
print "true"
sys.exit()
import sys
import urx
from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper
import time
rob = urx.Robot("192.168.1.102")
robotiqgrip = Robotiq_Two_Finger_Gripper(rob)
for i in range(0,255,10):
robotiqgrip.gripper_action(i)
time.sleep(0.2)
for i in range(255,0,-10):
robotiqgrip.gripper_action(i)
time.sleep(0.2)
#robotiqgrip.open_gripper()
#robotiqgrip.close_gripper()
if (self.complete_program == ""):
self.logger.debug("robotiq_two_finger_gripper's program is empty")
return ""
prog = self.header
prog += self.complete_program
prog += self.end
return prog
if __name__ == '__main__':
rob = urx.urrobot.URRobot("10.0.1.120", use_rt=True)
rob.set_tcp((0, 0, 0, 0, 0, 0))
rob.set_payload(1.28)
print(rob.getj())
robotiqgrip = Robotiq_Two_Finger_Gripper(type=50)
if len(sys.argv) != 2:
print("false")
sys.exit()
if sys.argv[1] == "close":
# robotiqgrip.close_gripper()
robotiqgrip.close_gripper(speedpercentange=50, forcepercentage=50)
if sys.argv[1] == "open":
robotiqgrip.open_gripper()
# robotiqgrip.open_gripper()
rob.send_program(robotiqgrip.ret_program_to_run())
print(robotiqgrip.ret_program_to_run())
time.sleep(5)
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)
import sys
import urx
from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper
import time
rob = urx.Robot("192.168.1.102")
robotiqgrip = Robotiq_Two_Finger_Gripper(rob)
if(len(sys.argv) != 2):
print("false")
sys.exit()
if(sys.argv[1] == "close") :
robotiqgrip.close_gripper()
if(sys.argv[1] == "open") :
robotiqgrip.open_gripper()
else:
robotiqgrip.gripper_action(int(sys.argv[1]))
# for i in range(25):
# print(i)
# robotiqgrip.gripper_action(i * 10)
# time.sleep(0.01)
rob.close()
