def receiveRigidBodyFrame(id, position, rotation):
global tcp_to_target_T, global_to_arm_T, tcp_to_arm_T, counter
trace("Received frame for rigid body", id, position, rotation)
if (id == 1):
trans_matrix = math3d.Transform()
quaternion = math3d.UnitQuaternion(rotation[3], rotation[0],
rotation[1], rotation[2])
trans_matrix.set_pos(position)
trans_matrix.set_orient(quaternion.orientation)
global_to_arm_T = trans_matrix.get_inverse()
trace("arm_to_global_T", trans_matrix.matrix)
trace("global_to_arm_T", global_to_arm_T.matrix)
elif (id == 3):
# frame counter, exec each 10 frame
# if(counter < 2):
# counter += 1
# return
# counter = 0
trans_matrix = math3d.Transform()
quaternion = math3d.UnitQuaternion(rotation[3], rotation[0],
rotation[1], rotation[2])
trans_matrix.set_pos(position)
trans_matrix.set_orient(quaternion.orientation)
obj_to_global_T = trans_matrix
temp_matrix = global_to_arm_T.matrix * obj_to_global_T.matrix * tcp_to_target_T.matrix
tcp_to_arm_T = math3d.Transform()
tcp_to_arm_T.set_pos(temp_matrix[:3, 3].A1)
orient = math3d.Orientation(temp_matrix[:3, :3].A1)
tcp_to_arm_T.set_orient(orient)
trace("obj_to_global_T", obj_to_global_T.matrix)
def test_transform(self):
p = hcn.HPose()
t = m3d.Transform()
t.pos = m3d.Vector(2, 3, 1)
t.orient.rotate_zb(math.pi / 2)
p = hcn.HPose(*t.pose_vector)
t2 = m3d.Transform(p.to_list()[:-1])
self.assertEqual(t, t2)
def movec(self, pose_via, pose_to, acc=0.01, vel=0.01, radius=0, wait=True):
"""
Move Circular: Move to position (circular in tool-space)
see UR documentation
"""
via = self.csys * m3d.Transform(pose_via)
to = self.csys * m3d.Transform(pose_to)
return URRobot.movec(self, via.pose_vector, to.pose_vector, acc=acc, vel=vel, radius=radius, wait=wait)
def get_end_pose(coords, angle):
r = 0.535 # 门半径（m）
print "door radius (m): ", r
T1 = m3d.Transform(get_world_coords(coords)) # 门把手中心相对世界坐标系的位姿
# RZ
trans = m3d.Transform()
trans.set_orient(m3d.Orientation.new_euler((0, 0, angle), encoding='xyz'))
T2 = trans * T1
T2.set_pos(
m3d.Vector(coords[0] - r + r * cos(angle), coords[1] + r * sin(angle),
coords[2]))
return get_base_coords(T2.pose_vector)
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 transform_scanlines(self):
print('Transforming pointcloud')
l2t = self.get_laser_to_turntable_transform()
w = self.angspeed
st = self.start_scan_time
sl = self.scanlines
slt = self.scanline_times
self.pc = np.empty((len(sl)*len(sl[0]), 3))
pci = 0
for i in range(len(sl)):
udp = self.undistort_points(sl[i])
# x,z in laser coordinates
self.ptp = self.perspective_transform(udp)
angle = w * (slt[i] - st)
rot_trf = m3d.Transform(m3d.Orientation.new_rot_z(-angle), m3d.Vector()) * l2t
for p in self.ptp:
# create m3d Vector of each point in scanline
p = m3d.Vector(p[0], 0, p[1])
# transform to turntable coordinates
tp = rot_trf * p
self.pc[pci] = tp.data
pci += 1
def generate_point_cloud(image_number):
intrinsics = np.loadtxt("intrinsics/"+str(image_number)+".txt")
extrinsics = np.loadtxt("extrinsics/"+str(image_number)+".txt", delimiter=", ")
depth = np.load("depth/"+str(image_number)+".npy")
rgb = cv2.imread("rgb/"+str(image_number)+".png")
width = rgb.shape[0]
height = rgb.shape[1]
print(rgb.shape)
ex_transform = m3d.Transform(extrinsics)
ex_transform.invert()
# Slower way but did this originally
# point_cloud = np.zeros((rgb.shape[0],rgb.shape[1],3))
# point_cloud[:,:,3:] = rgb
# for i in range(rgb.shape[0]):
# for j in range(rgb.shape[1]):
# point_cloud[i][j][0] = depth[i][j]/intrinsics[0][0]*(i-intrinsics[0][2])
# point_cloud[i][j][1] = depth[i][j]/intrinsics[1][1]*(j-intrinsics[1][2])
# point_cloud[i][j][2] = depth[i][j]
# Faster list comprehension. Used equations from: https://github.com/RobotLocomotion/drake/blob/master/perception/depth_image_to_point_cloud.cc
# Also includes the extrinsic properties by multiplying by inverse of extrinsic transformation matrix
point_cloud = np.array([[ex_transform*np.array([depth[i][j]/intrinsics[0][0]*(i-intrinsics[0][2]), depth[i][j]/intrinsics[1][1]*(j-intrinsics[1][2]), depth[i][j]]) for j in range(height)]for i in range(width)])
# Use this for debugging and visualizing point cloud
# pcd = open3d.PointCloud()
# pcd.points = open3d.Vector3dVector(point_cloud.reshape(width*height,3))
# open3d.draw_geometries([pcd])
return point_cloud
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 convert_to_transform(data):
quaternion = math3d.UnitQuaternion(data['rot3'], data['rot0'],
data['rot1'], data['rot2'])
transform = math3d.Transform()
transform.set_pos((data['pos0'], data['pos1'], data['pos2']))
transform.set_orient(quaternion.orientation)
return transform
def get_tool_transform(self):
"""Returns the tool transformation"""
if self._tool_segment is None:
return m3d.Transform()
else:
frame = self._tool_segment.getFrameToTip()
return kdl_frame_to_m3d_transform(frame)
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 get_transform(self, wait=False):
"""
get current transform from base to to tcp
"""
pose = URRobot.getl(self, wait)
trans = self.csys.inverse * m3d.Transform(pose)
return trans
def __init__(self, host, useRTInterface=False, logLevel = logging.WARN, parserLogLevel=logging.WARN):
URRobot.__init__(self, host, useRTInterface, logLevel=logLevel, parserLogLevel=parserLogLevel)
self.default_linear_acceleration = 0.01
self.default_linear_velocity = 0.01
self.csys_dict = {}
self.csys = None
self.set_csys("Robot", m3d.Transform()) #identity
def move_paper(self):
drag_dist = 0.10 # 10 cm
plunge_dist = 0.1273
print("moving paper")
self.robot.set_csys(
m3d.Transform()) # reset csys otherwise weird things happen...
self.robot.movel((0.02, -0.548, 0.1980, 0.0, -3.14, 0), self.a, self.v)
self.robot.movel((0.0, 0.0, -plunge_dist, 0.0, 0, 0),
self.a,
self.v,
relative=True)
self.robot.movel((-drag_dist, 0.0, 0.0, 0.0, 0, 0),
self.a,
self.v,
relative=True)
time.sleep(3)
self.robot.movel((0.0, 0.0, plunge_dist, 0.0, 0, 0),
self.a,
self.v,
relative=True)
self.robot.movel((0.0, 0.0, -plunge_dist / 2, 0.0, 0, 0),
self.a * 2.5,
self.v * 2.6,
relative=True)
self.robot.movel((0.0, 0.0, plunge_dist / 2, 0.0, 0, 0),
self.a * 2.7,
self.v * 2.7,
relative=True)
time.sleep(3)
print("paper moved, hopefully it fell off the pen again... ")
return True
def set_pose(self,
trans,
acc=0.01,
vel=0.01,
wait=True,
command="movel",
threshold=None,
radius=0):
"""
move tcp to point and orientation defined by a transformation
UR robots have several move commands, by default movel is used but it can be changed
using the command argument
"""
self.logger.debug("Setting pose to %s", trans.pose_vector)
t = self.csys * trans
pose = URRobot.movex(self,
command,
t.pose_vector,
acc=acc,
vel=vel,
wait=wait,
threshold=threshold,
radius=radius)
if pose is not None:
return self.csys.inverse * m3d.Transform(pose)
def move_to_write(self):
#self.move_between()
#self.robot.movej((-1.198425594960348, -1.518754784260885, -1.8426645437823694, -0.7939837614642542,1.5331677198410034, 0.15597468614578247), self.a, self.v *0.8)
self.robot.movej((radians(-69), radians(-97), radians(-108),
radians(-64), radians(89.5), radians(0)), self.a,
self.v)
# über dem papier 01 self.robot.movej((-1.186561409627096, -1.9445274511920374, -1.7661479155169886, -1.006078068410055, 1.5503629446029663, 0.3756316900253296), self.a, self.v)
self.robot.movej(
(-1.2749927679644983, -1.9379289785968226, -2.09098464647402,
-0.6840408484088343, 1.5629680156707764, 0.28495118021965027),
self.a, self.v)
# if movel should be used csys has to be reset to base before move
# #self.robot.movel((0.6000000521429313, 0.15000001308942848, 0.09000014933946439, -2.221440281881211, -2.2214404854075736, -1.74503212199567e-07), self.a, self.v)
self.robot.csys = m3d.Transform(
) # reset csys otherwise weird things happen...
write_csys = self.robot.get_pose()
time.sleep(0.1)
self.robot.set_csys(write_csys)
time.sleep(0.3)
#print("write_csys: ", write_csys)
print("Csys set to current pose: Write")
self.robot.movel((0, 0, -0.02, 0, 0, 0))
return None
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 get_tcp_ft(self, wait=True):
"""
get force and torque of tcp in tcp frame
"""
b2force = self.get_tcp_force(wait)
tcp2b = m3d.Transform(self.rtmon.getTCF(wait).tolist()).orient.inverse
ft = (tcp2b * m3d.Vector(b2force[:3])).list + (tcp2b * m3d.Vector(b2force[3:])).list
return ft
def set_pos(self, vect, acc=None, vel=None, radius=0, wait=True):
"""
set tool to given pos, keeping constant orientation
"""
if not type(vect) is m3d.Vector:
vect = m3d.Vector(vect)
trans = m3d.Transform(self.get_orientation(), m3d.Vector(vect))
return self.apply_transform(trans, acc, vel, radius, 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 get_QR_pose(pose):
global QR_pose
pos = [pose.pose.position.x,pose.pose.position.y,pose.pose.position.z]
orient = m3d.UnitQuaternion(pose.pose.orientation.w,
m3d.Vector(pose.pose.orientation.x,
pose.pose.orientation.y,
pose.pose.orientation.z))
QR_pose = m3d.Transform(orient,pos).pose_vector
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()
