def __init__(self, xml: str, stats: Stats, args: MagicDict): """ :param xml: the xml filename for Klampt to read the world settings :param stats: the Stats object to keep track of stats :param args: an instance of the input arguments """ super().__init__(stats) import klampt from klampt.plan import robotplanning world = klampt.WorldModel() world.readFile(xml) # very cluttered robot = world.robot(0) # this is the CSpace that will be used. # Standard collision and joint limit constraints will be checked space = robotplanning.makeSpace(world, robot, edgeCheckResolution=0.1) self.space = space self.robot = robot self.world = world import copy self.template_pos = copy.copy(robot.getConfig()) self.template_pos[1:7] = [0] * 6
def getSim(simfile): if simfile in simTable: return simTable[simfile][1] world = klampt.WorldModel() if not world.readFile(simfile): raise ValueError("Invalid simulation file " + simfile) sim = klampt.Simulator(world) simTable[simfile] = (world, sim) return sim
def __init__(self, robot, bulbId): if isinstance(robot, str): self.world = klampt.WorldModel() self.world.loadRobot(robot) robot = self.world.robot(0) self.robot = robot self.bulbId = bulbId self.localPos = ShaderMesh(robot.link(bulbId).geometry()).centroid() self.frag = '''
def __init__(self, random_plan = False): self.world = klampt.WorldModel() self.world.readFile(world_config_file) self.robot = self.world.robot(0) self.obstacles = [] for i in range(self.world.numTerrains()): self.obstacles.append(self.world.terrain(i)) self.initialize(random_plan) while not self.test(self.start_conf) or not self.test(self.goal_conf): self.initialize(random_plan)
def __init__(self,worldfn = None): GLRealtimeProgram.__init__(self,"Haptic UI Viewer") self.statelock = threading.Lock() self.state = None if worldfn == None: self.world = None self.robot = None else: self.world = klampt.WorldModel() if not self.world.readFile(worldfn): print "HapticUIViewer: Warning, could not load world file",worldfn print "Proceeding without robot feedback" self.robot = None self.world = None else: self.robot = self.world.robot(0) self.qcmdGetter = TopicListener(system_state_addr,'.robot.command.qcmd') self.qdestGetter = TopicListener(system_state_addr,'.controller.traj_q_end')
def print_config(self): config = self.robotWidget.get() print("Config:",config) self.world.robot(0).setConfig(config) def save_world(self): fn = "widgets_test_world.xml" print("Saving file to",fn) self.world.saveFile(fn) if __name__ == "__main__": print("==============================================================================") print("gl_vis_widgets.py: This example demonstrates how to manually add visualization") print("widgets to pose a robot using the GLWidgetPlugin interface") if len(sys.argv)<=1: print() print("USAGE: gl_vis_widgets.py [world_file]") print("==============================================================================") if len(sys.argv)<=1: exit() world = klampt.WorldModel() for fn in sys.argv[1:]: res = world.readFile(fn) if not res: raise RuntimeError("Unable to load model "+fn) viewer = MyGLViewer(world) vis.run(viewer)
def main(): parser = argparse.ArgumentParser(description="Get imgs") parser.add_argument( '-w', type=str, default="/home/motion/TRINA/Motion/data/TRINA_world_cholera.xml", help="world file") parser.add_argument('-n', type=str, default="cholera", help="codename") parser.add_argument( '-c', nargs='*', default=['zed_slam_left', 'zed_slam_right', 'realsense_slam_l515'], help="cameras to use") parser.add_argument('--components', nargs='*', default=['left_limb', 'right_limb', 'base'], help="components to use") parser.add_argument('-m', type=str, default='Kinematic', help='mode to operate in') args = parser.parse_args() mc = Motion.MotionClient() mc.restartServer(mode=args.m, components=args.components, codename=args.n) world = klampt.WorldModel() world.loadFile(args.w) sensor_module = Camera_Robot(mc, world=world, cameras=args.c) time.sleep(1) camera_names = ["realsense_slam_l515", "zed_slam_left", "zed_slam_right"] topic_names = [] rgb_suf = "_rgb" d_suf = "_d" for cn in camera_names: topic_names.append(cn + rgb_suf) topic_names.append(cn + d_suf) rate = rospy.Rate(30) pubs = {} topic_pref = "images/" for name in topic_names: full_name = topic_pref + name pub = rospy.Publisher(full_name, Image, queue_size=10) pubs[full_name] = pub #rospy.init_node("sim_pipe", anonymous=True) bridge = CvBridge() max_d = 6.0 min_d = 0.0 while not rospy.is_shutdown(): img_dict = sensor_module.get_rgbd_images() for name in camera_names: img = img_dict[name] if len(img) > 0: color = img[0] # Normalize depth to be 0 <= d < 1 depth = (img[1] - min_d) / (max_d - min_d) # Fit in maximum range of 16 (unsigned) bits depth *= (2**16 - 1) depth = depth.astype("uint16") depth = bridge.cv2_to_imgmsg(depth, "16UC1") pubs[topic_pref + name + rgb_suf].publish( bridge.cv2_to_imgmsg(color, "8UC3")) pubs[topic_pref + name + d_suf].publish(depth) rate.sleep()