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()