def ready(self, side):
if side == 'left':
self.robot['left'] = urx.Robot("192.168.1.66")
self.gripper = Robotiq_Two_Finger_Gripper(self.robot['left'],
speed=200,
force=20)
self.gripper.force = 100
# self.gripper = Robotiq_Two_Finger_Gripper(self.robot['left'], payload=0.25, speed=50, force=10)
if side == 'right':
self.robot['right'] = urx.Robot("192.168.1.109")
self.hand = HYHandController()
self.hand.connect()
print 'Robot tcp:', np.array(self.robot[side].getl())
print 'Robot joints:', np.array(self.robot[side].getj())
def __init__(self):
rospy.init_node('ur_driver', anonymous=True)
rospy.logwarn('SIMPLE_UR DRIVER LOADING')
self.dyn_reconfigure_server = Server(ConfigType,
self.dynamic_callback) #edit
# TF
self.broadcaster_ = tf.TransformBroadcaster()
self.listener_ = tf.TransformListener()
# SERVICES
self.set_stop_service = rospy.Service('simple_ur_msgs/SetStop',
SetStop, self.set_stop_call)
self.set_servo_mode_service = rospy.Service(
'simple_ur_msgs/SetServoMode', SetServoMode,
self.set_servo_mode_call)
# PUBLISHERS AND SUBSCRIBERS
self.driver_status_publisher = rospy.Publisher(
'/ur_robot/driver_status', String, queue_size=5)
self.robot_state_publisher = rospy.Publisher('/ur_robot/robot_state',
String,
queue_size=5)
self.joint_state_publisher = rospy.Publisher('joint_states',
JointState,
queue_size=5)
self.desired_joint_state_subscriber = rospy.Subscriber(
'joint_trajectory_real', JointState, self.callback)
#self.desired_joint_state_subscriber = rospy.Subscriber('joint_trajectory_real', JointState, self.callback, queue_size = 1)
### Set Up Robot ###
#self.rob = urx.Robot("192.168.1.155", logLevel=logging.INFO)
logging.basicConfig(level=logging.INFO)
#self.rob = urx.Robot("10.162.43.69")
self.rob = urx.Robot("127.0.0.1")
if not self.rob:
rospy.logwarn('SIMPLE UR - ROBOT NOT CONNECTED')
self.driver_status = 'DISCONNECTED'
self.robot_state = 'POWER OFF'
else:
rospy.logwarn('SIMPLE UR - ROBOT CONNECTED SUCCESSFULLY')
self.rtm = self.rob.get_realtime_monitor()
rospy.logwarn('SIMPLE UR - GOT REAL TIME INTERFACE TO ROBOT')
self.driver_status = 'IDLE'
self.robot_state = 'RUNNING_IDLE'
### Set Up PID ###
while not rospy.is_shutdown():
self.update()
self.check_driver_status()
self.check_robot_state() # problematic now
self.publish_status()
self.dynamic_check()
# rospy.spin()
# Unlike in roscpp, rospy.spin() doesn't affect the subscriber callback functions
rospy.sleep(.01)
# Finish
rospy.logwarn('SIMPLE UR - ROBOT INTERFACE CLOSING')
self.rob.close()
def __init__(self,
rob_id="192.168.1.233",
ini_y=0.1,
ini_z=-0.2,
work_z=0.254,
work_y=0.10,
press_z=0.003,
press_x=0.02,
grasp_times=2,
press_times=5,
a=0.15,
v=0.25):
self.ini_y = ini_y
self.ini_z = ini_z
self.work_z = work_z
self.work_y = work_y
self.press_z = press_z
self.press_x = press_x
self.work_a = a
self.work_v = v
self.grasptimes = grasp_times
self.press_times = press_times
self.rob = urx.Robot(rob_id)
self.inipose = utils.safe_setzero(self.rob.getj())
self.rob.set_tcp((0, 0, 0, 0, 0, 0))
self.rob.set_payload(0.5, (0, 0, 0))
def test_movep(self):
print(ip)
rob = urx.Robot(ip)
try:
l = 0.1
v = 0.07
a = 0.1
r = 0.05
pose = rob.getl()
pose[2] += l
rob.movep(pose, acc=a, vel=v)
while True:
p = rob.getl(wait=True)
if p[2] > pose[2] - 0.05:
break
pose[1] += l
rob.movep(pose, acc=a, vel=v)
while True:
p = rob.getl(wait=True)
if p[1] > pose[1] - 0.05:
break
pose[2] -= l
rob.movep(pose, acc=a, vel=v)
while True:
p = rob.getl(wait=True)
if p[2] < pose[2] + 0.05:
break
pose[1] -= l
rob.movep(pose, acc=a, vel=v)
finally:
rob.close()
def __init__(self, config):
"""Initialize class."""
super().__init__('ur_interface')
# Store parameters
self._acc = config['acceleration']
self._vel = config['velocity']
# Create trajectory process
self._trajectory_thread = th.Thread()
# Connect to robot
self.get_logger().info("Connecting to robot at {}".format(config['robot_ip']))
connected = False
while not connected:
try:
self._robot = urx.Robot(config['robot_ip'], True)
connected = True
except OSError as ex:
self.get_logger().warn("Unable to connect: {}. Retrying...".format(str(ex)))
# Create subscribers/publishers
self._joint_state_timer = self.create_timer(1.0 / config['state_update_rate'],
self._joint_state_callback)
self._joint_state_pub = self.create_publisher(JointState, config['state_topic'])
self._trajectory_sub = self.create_subscription(JointTrajectory, config['command_topic'],
self._trajectory_callback)
return
def main():
rob = urx.Robot(args.ip)
rob.set_tcp((0, 0, 0, 0, 0, 0))
rob.set_payload(0.5, (0, 0, 0))
pose = rob.getl()
rob.movel(pose, acc=args.a, vel=args.v, wait=False)
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
pipeline.start(config)
aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_250)
try:
while True:
frames = pipeline.wait_for_frames()
color_frame = frames.get_color_frame()
if not color_frame:
continue
img = np.asanyarray(color_frame.get_data())
cv2.imshow('RealSense', img)
cv2.waitKey(1)
markerCorners, markerIds, DetectionParameters = cv2.aruco.detectMarkers(img, aruco_dict)
finally:
pipeline.stop()
def __init__(self):
try:
self.bot = urx.Robot("192.168.0.11")
self.bot.secmon.running = True
self.bot.secmon._parser.version = (3, 0)
print(self.bot.secmon._parser.version)
# bot.set_tcp((0, 0, 0.999, 0, 0, 0))
trx = m3d.Transform()
trx.orient.rotate_zb(math.radians(-20 + 45))
self.bot.set_csys(trx)
# self.bot.set_pos(self.tower_pos, self.a, self.v)
except urx.urrobot.RobotException as e:
print(e)
except Exception as e:
self.running = False
logging.error(traceback.format_exc())
if self.bot:
print("BOT STOPPED")
self.bot.stop()
self.bot.close()
raise Exception("Could not connect to robot")
return
self.open_gripper()
self.move_to(list(self.init_pos),
list(self.grip_angle_straight))
return
def test_matrix(self):
global ip
print(ip)
rob = urx.Robot(ip)
#rob = urx.Robot("localhost")
rob.set_tcp((0,0,0,0,0,0))
rob.set_payload(0.5, (0,0,0))
l = 0.05
v = 0.05
a = 0.3
j = rob.getj()
print("Initial joint configuration is ", j)
t = rob.get_pose()
print("Transformation from base to tcp is: ", t)
print("Translating in x")
rob.translate((l, 0, 0), acc=a, vel=v)
pose = rob.getl()
print("robot tcp is at: ", pose)
print("moving in z")
pose[2] += l
rob.movel(pose, acc=a, vel=v)
print("Translate in -x and rotate")
t.orient.rotate_zb(math.pi/4)
t.pos[0] -= l
rob.set_pose(t, vel=v, acc=a)
print("Sending robot back to original position")
rob.movej(j, acc=0.8, vel=0.2)
rob.close()
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 connect(self):
try:
self.robot = urx.Robot(self.IP)
except:
print("retrying to connect to robot")
time.sleep(1)
self.connect()
def InitBaseVariables(self):
self.pub = rospy.Publisher('SocialReturnValues', String, queue_size=10)
self.ImageIndex = 0
self.ExtraContours = []
self.StateOfWait = False
self.StepsTaken = [
'Idle', 'Greeting', 'ExecuteDrawing', 'ContemplateAnimation',
'ExecuteDrawing', 'EncourageDrawing', 'ObserveUserDrawing',
'SignDrawing', 'EncourageSigining', 'ObserveUserSigning', 'GoodBye'
]
self.RunningSocialAction = False
self.IdleCon = False
self.withdrawPose = rospy.get_param('WithdrawPose')
self.NodPose = rospy.get_param('NodDifferentials')
self.initHoverPos = [0, 0, 0, 0, 0, 0]
self.md = [0, 0]
self.zDraw = 0
self.zHover = 0
self.xZero = 0
self.yZero = 0
self.xMax = 0
self.yMax = 0
self.a = rospy.get_param('speedParameters')
self.v = rospy.get_param('speedParameters')
self.rob = urx.Robot(rospy.get_param('roboIP'))
self.leftwithDraw = rospy.get_param('leftRetreat')
self.rightwithDraw = rospy.get_param('RightRetreat')
self.Interruption = False
self.StopCommand = False
self.InteruptedPosition = self.rob.getl()
self.secmon = ursecmon.SecondaryMonitor(self.rob.host)
return
def main():
# Leap motion
controller = Leap.Controller()
controller.config.save()
# UR3 robot config
robot = urx.Robot("192.168.0.2")
mytcp = m3d.Transform() # create a matrix for our tool tcp
mytcp.pos.x = -0.0002
mytcp.pos.y = -0.144
mytcp.pos.z = 0.05
time.sleep(1)
robot.set_tcp(mytcp)
time.sleep(1)
while 1:
try:
tool_pose = get_tool_pose(robot)
T = convert_tool_pose_to_transformation_matrix(tool_pose)
# print(T)
frame = controller.frame()
if len(frame.hands):
extended_finger_list = frame.fingers.extended()
number_extended_fingers = len(extended_finger_list)
if (number_extended_fingers != 5):
pass
else:
relative_palm_postion = read_hand_position(frame)
absolute_hand_position = calculate_hand_position(
T, relative_palm_postion)
required_robot_position = calculate_required_robot_position(
absolute_hand_position)
final_pose = list(required_robot_position)
final_pose.extend(tool_pose[3:])
pose_difference = np.linalg.norm(
np.array(tool_pose[:3]) -
np.array(required_robot_position))
# Only moves robot if the move is greater than 0.5cm; reduces jitter this way
if pose_difference > 0.005:
print('\ncurrent_pose: %s' % (tool_pose))
print('\nabsolute_hand_position: %s' %
(absolute_hand_position))
print('required_pose: %s' % (final_pose))
print('pose_difference: %s' % (pose_difference))
# Only moves robot if move is smaller than 8cm, minimizes robot moving in strange directions
if pose_difference < 0.08:
# print(T)
robot.movep(list(final_pose),
acc=0.1,
vel=0.2,
wait=False)
except:
robot.close()
sys.exit(0)
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 __init__(self):
super(MainWindow, self).__init__()
self.DE2R = math.pi / 180
self.M = 1000
# # 机器人驱动
# self.ur = UR()
self.robot = urx.Robot("192.168.36.26")
# # 连接
# self.ur.connect()
# 设置标题
self.setWindowTitle('机器人绘制')
# 设置窗口大小
self.setFixedSize(640, 480)
# 创建整体布局
wholeLayout = QVBoxLayout()
# 设置布局
self.setLayout(wholeLayout)
# 创建上部布局
topLayout = QHBoxLayout()
# 添加布局到整体布局中
wholeLayout.addLayout(topLayout)
# 创建两个按钮
clearBtn = QPushButton('清理')
paintBtn = QPushButton('画板写字')
paintBtn1 = QPushButton('svg图片写字')
paintBtn2 = QPushButton('识别预存图片写字')
paintBtn3 = QPushButton('摄像头识别图片写标准字')
paintBtn4 = QPushButton('摄像头识别图片写原字')
# 按钮添加到布局中
topLayout.addWidget(clearBtn)
topLayout.addWidget(paintBtn)
topLayout.addWidget(paintBtn1)
topLayout.addWidget(paintBtn2)
topLayout.addWidget(paintBtn3)
topLayout.addWidget(paintBtn4)
## 获取手写板数据
# 创建自定义控件
self.paintWidget = PaintWidget()
# 添加到整体布局中
wholeLayout.addWidget(self.paintWidget)
##获取svg数据
self.svg = svgPain()
# 设置按钮点击事件
# 槽函数可以是普通函数 也可以是类的方法
clearBtn.clicked.connect(self.clear)
# 通过画板写字 绘制事件
paintBtn.clicked.connect(self.paint)
#通过svg图片写字
paintBtn1.clicked.connect(self.paint_svg)
# 识别手写字图片写字 识别预存图片写字
paintBtn2.clicked.connect(self.paint_svg_byImgae)
# 摄像头识别写标准字
paintBtn3.clicked.connect(self.paint_svg_byImgae_Video)
# 摄像头识别写原字
paintBtn4.clicked.connect(self.paint_svg_byImgae_Video_withSVG)
def rightArmCord():
r = urx.Robot('192.168.1.11')
r.new_csys_from_xpy()
fp = open('right_ref_cfg.pkl', 'wb')
pickle.dump(r.csys, fp)
fp.close()
r.close()
def rob_init():
rob = urx.Robot("192.168.1.10")
rob.set_tcp((0, 0, 0.19, 0, 0, 0))
rob.set_payload(0.65, (0, 0, 0.1))
rob.set_gravity((0, 0, 9.82))
rob.set_tool_voltage(24)
rob.set_digital_out(7, 1)
return rob
def leftArmCord():
l = urx.Robot('192.168.1.10')
l.new_csys_from_xpy()
fp = open('left_ref_cfg.pkl', 'wb')
pickle.dump(l.csys, fp)
fp.close()
l.close()
def move_to_pose_target(pose_target):
rob = urx.Robot("192.168.80.2")
rob.set_tcp((0, 0, 0, 0, 0, 0))
rob.set_payload(0.5, (0, 0, 0))
v = 0.02
a = 0.1
rob.movel(pose_target, acc=a, vel=v)
rob.close()
def test2(self):
robot = urx.Robot("192.168.56.102")
robot.set_digital_out(0, 1)
time.sleep(1)
robot.set_digital_out(0, 0)
robot.set_tcp((0, 0, 0.05, 0, 0, 0)) #ckeck Installation tab
robot.translate((0, 0, -0.05), acc=0.05, vel=0.05) #acceleration, velocity
robot.close()
def run(self):
print("Chess Planner running")
tcp_ip = "10.42.0.54"
self.coordinator = urx.Robot(tcp_ip)
switch = 3
stop_switch = 0
while stop_switch == 0 and not rospy.is_shutdown():
try:
if switch == -1:
print("moving the arm to hovering position in 5 second")
time.sleep(5)
self.joints_pub_castor.publish("chess_hover")
time.sleep(10)
switch += 1
if switch == 0:
print("moving the arm to the origin in 5 second")
x_coord = origin[0]
y_coord = origin[1]
z_coord = origin[2]
msg = str(x_coord) + ' ' + str(y_coord) + ' ' + str(
z_coord)
print('Sending Castor:' + msg)
time.sleep(5)
self.coordinates_pub_castor.publish(msg)
time.sleep(10)
switch += 3
if switch == 1:
print("moving chess piece to coordinate 4,4")
self.wait(5)
self.move(1, 1)
switch += 1
if switch == 2:
print("Gripping in 5 second")
time.sleep(5)
self.grip()
stop_switch += 1
if switch == 3:
self.wait(5)
self.move_ordinary('c3a4')
self.wait(10)
if switch == 4:
self.ur5_arm_node.g
except KeyboardInterrupt:
self.coordinator.close()
print("interrupting")
break
print("the program ended!")
self.coordinator.close()
def __init__(self, robot_ip):
self.vel = 0.15
self.acc = 0.5
self.stop_acc = 0.3
self.cmd_velocity_vector = []
self.move_vel = False
self.item_height = 0.11
self.robot = urx.Robot(robot_ip, True)
self.my_tcp = m3d.Transform() # create a matrix for our tool tcp
self.current_TCP = 'TCP'
self.set_TCP('pressure_ft')
# self.robot.set_payload(4.25)
# self.robot.set_gravity([0, 0, 0.15])
time.sleep(0.2)
self.ros_node = rospy.init_node('ur10_node', anonymous=True)
self.pose_publisher = rospy.Publisher('tcp_pose', PoseStamped, queue_size=1)
self.velocity_publisher = rospy.Publisher('/dvs/vel', Twist, queue_size=1)
self.speed_publisher = rospy.Publisher('/dvs/spd', Float64, queue_size=1)
self.cam_pose_publisher = rospy.Publisher('/dvs/pose', PoseStamped, queue_size=1)
self.cmd_vel_subs = rospy.Subscriber("ur_cmd_vel", Twist, self.move_robot_callback, queue_size=1)
self.cmd_pose_subs = rospy.Subscriber("ur_cmd_pose", Pose, self.move_pose_callback)
self.cmd_adjust_pose_subs = rospy.Subscriber("ur_cmd_adjust_pose", Pose, self.adjust_pose_callback)
self.rotate_ee_cmd = rospy.Subscriber("ur_rotate_ee", Float64, self.angle_callback)
self.rotate_ee_cmd = rospy.Subscriber("ur_rotate_ee_x", Float64, self.angle_callback_x)
self.pressure_movement_subs = rospy.Subscriber("move_pressure_to_cam", Bool, self.move_PF_to_cam)
self.pickup_service = rospy.Service("ur_pickup", Empty, self.pick_item)
self.set_tcp_service = rospy.Service("set_TCP", desiredTCP, self.set_TCP_cb)
self.move_service = rospy.Service('move_ur', moveUR, self.moveUR_cb)
self.move_service = rospy.Service('fire_drill', fireDrill, self.fire_drill_cb)
self.rate = rospy.Rate(100)
#TF
self.tfBuffer = tf2_ros.Buffer()
self.tl = tf2_ros.TransformListener(self.tfBuffer)
self.br = tf2_ros.TransformBroadcaster()
self.brs = tf2_ros.StaticTransformBroadcaster()
self.robot_pose = PoseStamped()
self.camera_pose = PoseStamped()
self.prev_camera_pose = PoseStamped()
self.pressure_ft_pose = PoseStamped()
self.cam_vel = Twist()
self.cam_speed = Float64()
self.seq = 1
self.pose = []
self.initial_pose = []
self.center_pose = []
self.static_transform_list = []
self.setup_tf()
def print_robot_tcp():
# ptint robot current tcp
rob = urx.Robot("192.168.80.2")
rob.set_tcp((0, 0, 0, 0, 0, 0))
rob.set_payload(0.5, (0, 0, 0))
pose = rob.getl()
# print("robot current tcp is at: ", pose) # in the world coordinate position and rotation
print("pose = ", pose)
rob.close()
def print_robot_joints():
# print robot joint value
rob = urx.Robot("192.168.80.2")
rob.set_tcp((0, 0, 0, 0, 0, 0))
rob.set_payload(0.5, (0, 0, 0))
joints = rob.getj()
# print("robot current tcp is at: ", pose) # in the world coordinate position and rotation
print("joints = ", joints)
rob.close()
def listener():
robot = urx.Robot(args['robot_ip_adress'],
use_rt=args['realtime_controller'])
rospy.init_node('ur_listener', anonymous=True)
rospy.Subscriber("Hello", String, callback)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def __init__(self):
#Setting up the connection to UR5 server
rospy.init_node("arm_node", anonymous=True, disable_signals=True)
self.client = actionlib.SimpleActionClient(
'follow_joint_trajectory', FollowJointTrajectoryAction)
print "Waiting for server..."
self.client.wait_for_server()
print "Connected to server"
self.JOINT_NAMES = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
parameters = rospy.get_param(None)
index = str(parameters).find('prefix')
if (index > 0):
prefix = str(parameters)[index + len("prefix': '"):(
index + len("prefix': '") +
str(parameters)[index + len("prefix': '"):-1].find("'"))]
for i, name in enumerate(self.JOINT_NAMES):
self.JOINT_NAMES[i] = prefix + name
#HOME position of the arm
self.HOME = [0, -90, 0, -90, 90, 0]
#Gesture dictionary, and building it
self.gestures = {}
self.build_gesture_dict()
#Setup TCP
tcp_ip = rospy.get_param("~robot_ip")
self.coordinator = urx.Robot(tcp_ip)
arm_dict = {'10.42.0.175': '_pollux', '10.42.0.54': '_castor'}
#Setting up subscriber
self.joints_sub = rospy.Subscriber(
"behaviors_cmd%s" % arm_dict[tcp_ip], String,
self.behaviors_callback)
self.coordinates_sub = rospy.Subscriber(
"coordinates_cmd%s" % arm_dict[tcp_ip], String,
self.coordinates_callback)
self.status_pub = rospy.Publisher("arm_status%s" % arm_dict[tcp_ip],
String,
queue_size=0)
self.query_sub = rospy.Subscriber("query_cmd%s" % arm_dict[tcp_ip],
String,
self.query_callback,
queue_size=10)
self.info_pub = rospy.Publisher("arm_info%s" % arm_dict[tcp_ip],
String,
queue_size=10)
print "Running startup sequence"
self.run_gesture("begin")
print "Initialized, listening..."
def robot_move_joint6_sub(N):
rob = urx.Robot("192.168.80.2")
rob.set_tcp((0, 0, 0, 0, 0, 0))
rob.set_payload(0.5, (0, 0, 0))
joint_pose = rob.getj()
# print("joints = ", joints)
print("joint6 sub 180/pi")
joint_pose[5] -= delta_joint_value * N
rob.movej(joint_pose, acc=0.8, vel=0.2)
rob.close()
def robot_move_joint4_add(N): #wrist 1
rob = urx.Robot("192.168.80.2")
rob.set_tcp((0, 0, 0, 0, 0, 0))
rob.set_payload(0.5, (0, 0, 0))
joint_pose = rob.getj()
# print("joints = ", joints)
print("joint4 add 180/pi")
joint_pose[3] += delta_joint_value * N
rob.movej(joint_pose, acc=0.8, vel=0.2)
rob.close()
def connect(self, ip="169.254.204.33"):
try:
self.rob = urx.Robot(ip)
self.rob.set_tcp(
(0.00022, -0.00265, 0.12321, 0, 0, 0)) # pre measurement
self.rob.set_payload(0.5, (0, 0, 0))
self.is_connected = True
except Exception as e:
print(traceback.format_exc())
print("Begin simulation....")
def run(self):
print("Chess Movement running")
tcp_ip = "10.42.0.54"
self.coordinator = urx.Robot(tcp_ip)
board = chess.Board()
engine = chess.uci.popen_engine("stockfish")
engine.uci()
engine.position(board)
side = "w"
num_b = 0
num_w = 0
switch = 0
while not board.is_game_over() and not rospy.is_shutdown():
try:
if switch == 0:
self.to_hovering()
print("Next Move in 5 second")
engine.position(board)
move = engine.go(movetime=1000)
capture_status = board.is_capture(move.bestmove)
board.push(move.bestmove)
print(board)
time.sleep(5)
if capture_status == True and side == "w":
self.move_capture(str(move.bestmove), side, num_w)
num_w += 1
elif capture_status == True and side == "b":
self.move_capture(str(move.bestmove), side, num_b)
num_b += 1
else:
self.move_ordinary(str(move.bestmove))
if side == "b":
side = "w"
else:
side = "b"
elif switch == 1:
#self.wait(5)
#self.to_hovering()
self.move_capture("a1b4", "w", 0)
except KeyboardInterrupt:
self.coordinator.close()
print("interrupting")
break
print("the program ended!")
self.coordinator.close()
def test_delta_robot_tcp(delta_robot_tcp):
rob = urx.Robot("192.168.80.2")
rob.set_tcp((0, 0, 0, 0, 0, 0))
rob.set_payload(0.5, (0, 0, 0))
pose = rob.getl()
print("robot tcp is at: ",
pose) # in the world coordinate position and rotation
pose = np.array(pose) + np.array(delta_robot_tcp)
rob.movel(pose, acc=0.1, vel=0.01)
rob.close()
