def create_world(): world = WorldModel() # world.loadElement('robots_urdf/base_link.off') world.readFile('./assets/cardata/car.rob') robot = world.robot(0) robot.setConfig([0, -2, 0]) link = robot.link(2) link_geom = link.geometry() link_geom.scale(4) # make the geometry larger so it's more difficult # world.readFile('./robots_urdf/05-visual.urdf') # load the environment too load_path = './assets/jagged_narrow.xml' xmldoc = minidom.parse(load_path) itemlist = xmldoc.getElementsByTagName('Geom') for i, item in enumerate(itemlist): extends = [ float(x) * 2 for x in item.getElementsByTagName('extents') [0].childNodes[0].data.split() ] translates = [ float(x) for x in item.getElementsByTagName('translation') [0].childNodes[0].data.split() ] create.primitives.box(extends[0], extends[1], extends[2], translates, world=world, name='obstacle%d' % i) return world, robot, [0, 1, 2], [2], [ 0, 1, 2, 3, 4 ] # the last three are link_index, geom_index, obs_index
def main(): w = WorldModel() w.readFile("../data/robots/kinova_gen3_7dof.urdf") robot = w.robot(0) #put a "pen" geometry on the end effector gpen = Geometry3D() res = gpen.loadFile("../data/objects/cylinder.off") assert res gpen.scale(0.01,0.01,0.15) gpen.rotate(so3.rotation((0,1,0),math.pi/2)) robot.link(7).geometry().setCurrentTransform(*se3.identity()) gpen.transform(*robot.link(8).getParentTransform()) robot.link(7).geometry().set(merge_geometry.merge_triangle_meshes(gpen,robot.link(7).geometry())) robot.setConfig(robot.getConfig()) trajs = get_paths() #place on a reasonable height and offset tableh = 0.1 for traj in trajs: for m in traj.milestones: m[0] = m[0]*0.4 + 0.35 m[1] = m[1]*0.4 m[2] = tableh if len(m) == 6: m[3] *= 0.4 m[4] *= 0.4 return run_cartesian(w,trajs)
def create_world(): """Just create the world so geometries are clearly defined""" load_path = './assets/table.xml' xmldoc = minidom.parse(load_path) itemlist = xmldoc.getElementsByTagName('Geom') print('num body = ', len(itemlist)) world = WorldModel() for i, item in enumerate(itemlist): extends = [ float(x) * 2 for x in item.getElementsByTagName('extents') [0].childNodes[0].data.split() ] translates = [ float(x) for x in item.getElementsByTagName('translation') [0].childNodes[0].data.split() ] create.primitives.box(extends[0], extends[1], extends[2], translates, world=world, name='obstacle%d' % i) # load the robot pr2_path = './assets/pr2/pr2.urdf' world.readFile(pr2_path) robot = world.robot(0) print(f'It has {robot.numLinks()} links') # link_names = ['r_shoulder_pan_link', 'r_shoulder_lift_link', 'r_upper_arm_roll_link', 'r_elbow_flex_link', 'r_forearm_roll_link', 'r_wrist_flex_link', 'r_wrist_roll_link'] link_names = [ 'r_shoulder_pan_link', 'r_shoulder_lift_link', 'r_upper_arm_roll_link', 'r_elbow_flex_link', 'r_forearm_roll_link', 'r_wrist_flex_link', 'r_wrist_roll_link', ] geom_names = [ 'r_shoulder_pan_link', 'r_shoulder_lift_link', 'r_upper_arm_roll_link', 'r_upper_arm_link', 'r_elbow_flex_link', 'r_forearm_roll_link', 'r_forearm_link', 'r_wrist_flex_link', 'r_wrist_roll_link', ] link_indexes = [] for link_name in link_names: link = robot.link(link_name) link_indexes.append(link.getIndex()) geom_indexes = [] for link_name in geom_names: link = robot.link(link_name) geom_indexes.append(link.getIndex()) obs_indexes = [0] return world, robot, link_indexes, geom_indexes, obs_indexes
def __init__(self): GLRealtimeProgram.__init__(self, 'World Viewer') self.world = WorldModel() self.fps = 10.0 self.dt = 1 / self.fps self.pre_drawables = [] self.post_drawables = []
def grasp_plan_main(): world = WorldModel() world.readFile("camera.rob") robot = world.robot(0) sensor = robot.sensor(0) world.readFile("table.xml") xform = resource.get("table_camera_00.xform",type='RigidTransform') sensing.set_sensor_xform(sensor,xform) box = GeometricPrimitive() box.setAABB([0,0,0],[1,1,1]) box = resource.get('table.geom','GeometricPrimitive',default=box,world=world,doedit='auto') bmin,bmax = [v for v in box.properties[0:3]],[v for v in box.properties[3:6]] nobj = 5 obj_fns = [] for o in range(nobj): fn = random.choice(ycb_objects) obj = make_ycb_object(fn,world) #TODO: you might want to mess with colors here too obj.appearance().setSilhouette(0) obj_fns.append(fn) for i in range(world.numTerrains()): #TODO: you might want to mess with colors here too world.terrain(i).appearance().setSilhouette(0) problem1.arrange_objects(world,obj_fns,bmin,bmax) intrinsics = dict() w = int(sensor.getSetting('xres')) h = int(sensor.getSetting('yres')) xfov = float(sensor.getSetting('xfov')) yfov = float(sensor.getSetting('yfov')) intrinsics['cx'] = w/2 intrinsics['cy'] = h/2 intrinsics['fx'] = math.tan(xfov*0.5)*h*2 intrinsics['fy'] = math.tan(xfov*0.5)*h*2 print("Intrinsics",intrinsics) planner = ImageBasedGraspPredictor() def do_grasp_plan(event=None,world=world,sensor=sensor,planner=planner,camera_xform=xform,camera_intrinsics=intrinsics): sensor.kinematicReset() sensor.kinematicSimulate(world,0.01) rgb,depth = sensing.camera_to_images(sensor) grasps,scores = planner.generate(camera_xform,camera_intrinsics,rgb,depth) for i,(g,s) in enumerate(zip(grasps,scores)): color = (1-s,s,0,1) g.add_to_vis("grasp_"+str(i),color=color) def resample_objects(event=None,world=world,obj_fns=obj_fns,bmin=bmin,bmax=bmax): problem1.arrange_objects(world,obj_fns,bmin,bmax) vis.add("world",world) vis.add("sensor",sensor) vis.addAction(do_grasp_plan,'Run grasp planner','p') vis.addAction(resample_objects,'Sample new arrangement','s') vis.run()
def __init__(self): self.world = WorldModel() self.robot = self.world.loadRobot( 'franka_panda/panda_model_w_table.urdf') self.robot.setJointLimits([ -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -2.9671, -1.8326, -2.9671, -3.1416, -2.9671, -0.0873, -2.9671, 0.0, 0.0, 0.0 ], [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.9671, 1.8326, 2.9671, 0.0, 2.9671, 3.8223, 2.9671, 0.0, 0.0, 0.0 ]) self.grasptarget_link = self.robot.link(16) time.sleep(1)
def gen_grasp_worlds(N=10): if len(base_boxes) == 0: #edit base boxes for basefn in base_files: world = WorldModel() world.readFile(basefn) base_name = os.path.splitext(os.path.basename(basefn))[0] box = GeometricPrimitive() box.setAABB([0, 0, 0], [1, 1, 1]) base_boxes.append( resource.get(base_name + '.geom', 'GeometricPrimitive', default=box, world=world, doedit='auto')) output_file_pattern = "generated_worlds/world_%04d.xml" #TODO: play around with me, problem 1a num_objects = [3, 3, 4, 5, 6, 6] for i in range(N): nobj = random.choice(num_objects) world = WorldModel() base_world = random.choice(range(len(base_files))) world.readFile(base_files[base_world]) obj_fns = [] for o in range(nobj): fn = random.choice(ycb_objects) obj = make_ycb_object(fn, world) obj_fns.append(fn) bbox = base_boxes[base_world] bmin, bmax = [v for v in bbox.properties[0:3] ], [v for v in bbox.properties[3:6]] #TODO: Problem 1: arrange objects within box bmin,bmax arrange_objects(world, obj_fns, bmin, bmax) save_world(world, output_file_pattern % (i, ))
def main(): #creates a world and loads all the items on the command line world = WorldModel() res = world.readFile("./HRP2_Robot.xml") if not res: raise RuntimeError("Unable to load model") robot = world.robot(0) # coordinates.setWorldModel(world) plugin = MyGLPlugin(world) vis.pushPlugin(plugin) #add the world to the visualizer vis.add("world", world) vis.add("robot", world.robot(0)) vis.show() # ipdb.set_trace() Robotstate_Traj, Contact_Force_Traj = Traj_Loader() h = 0.0068 # 0.0043: vert # 0.0033: flat playspeed = 2.5 norm = 500 while vis.shown(): # This is the main plot program for i in range(0, Robotstate_Traj.shape[1]): vis.lock() Robotstate_Traj_i = Robotstate_Traj[:, i] Robotstate_Traj_Full_i = Dimension_Recovery(Robotstate_Traj_i) # Now it is the plot of the contact force at the contact extremities robot.setConfig(Robotstate_Traj_Full_i) Contact_Force_Traj_i = Contact_Force_Traj[:, i] left_ft, rght_ft, left_hd, rght_hd, left_ft_end, rght_ft_end, left_hd_end, rght_hd_end = Contact_Force_vec( robot, Contact_Force_Traj_i, norm) vis.add("left foot force", Trajectory([0, 1], [left_ft, left_ft_end])) vis.add("right foot force", Trajectory([0, 1], [rght_ft, rght_ft_end])) vis.add("left hand force", Trajectory([0, 1], [left_hd, left_hd_end])) vis.add("right hand force", Trajectory([0, 1], [rght_hd, rght_hd_end])) COMPos_start = robot.getCom() COMPos_end = COMPos_start COMPos_end[2] = COMPos_end[2] + 100 vis.add("Center of Mass", Trajectory([0, 1], [COMPos_start, COMPos_end])) # ipdb.set_trace() vis.unlock() time.sleep(h * playspeed)
def mounted(gripper,klampt_model,base_link,name=None, register=True): """From a standalone gripper, return a GripperInfo such that the link indices are shifted onto a new robot model. """ if name is None: name = gripper.name + "_mounted" w = WorldModel() w.enableGeometryLoading(False) res = w.readFile(klampt_model) if not res: raise IOError("Unable to load file "+str(klampt_model)) robot = w.robot(0) w.enableGeometryLoading(True) if isinstance(base_link,str): base_link = robot.link(base_link).index shifted_finger_links = [l+base_link for l in gripper.finger_links] mount_driver = -1 for i in range(robot.numDrivers()): ls = robot.driver(i).getAffectedLinks() if any(l in ls for l in shifted_finger_links): mount_driver = i break if mount_driver < 0: raise RuntimeError("Can't find the base driver for the mounted gripper?") shifted_finger_drivers = [l+mount_driver for l in gripper.finger_drivers] res = copy.copy(gripper) res.name = name res.base_link = base_link res.klampt_model = klampt_model res.finger_links = shifted_finger_links res.finger_drivers = shifted_finger_drivers if register: GripperInfo.register(res) return res
def _loadWorld(self, world_fn): world = WorldModel() res = world.readFile(world_fn) if not res: print("Unable to read file", world_fn) exit(0) for i in range(world.numRigidObjects()): obj = world.rigidObject(i) #this will perform a reasonable center of mass / inertia estimate m = obj.getMass() m.estimate(obj.geometry(), mass=0.454, surfaceFraction=0.2) obj.setMass(m) self.world = world
class IK(): def __init__(self): self.world = WorldModel() self.robot = self.world.loadRobot( 'franka_panda/panda_model_w_table.urdf') self.robot.setJointLimits([ -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -2.9671, -1.8326, -2.9671, -3.1416, -2.9671, -0.0873, -2.9671, 0.0, 0.0, 0.0 ], [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.9671, 1.8326, 2.9671, 0.0, 2.9671, 3.8223, 2.9671, 0.0, 0.0, 0.0 ]) self.grasptarget_link = self.robot.link(16) time.sleep(1) def __call__(self, from_cfg, to_ee_loc, return_flag=False): if isinstance(to_ee_loc, np.ndarray): to_ee_loc = list(to_ee_loc.flat) from_cfg_full = copy(self.robot.getConfig()) from_cfg_full[7:14] = from_cfg self.robot.setConfig(from_cfg_full) objective = ik.objective(self.grasptarget_link, local=[0, 0, 0], world=to_ee_loc) flag = ik.solve(objective, iters=1000, tol=1e-4) cfg = self.robot.getConfig()[7:14] if not return_flag: return cfg else: return cfg, flag
def __init__(self): self.world = WorldModel() self.robot = self.world.loadRobot( 'franka_panda/panda_model_w_table.urdf') self.robot.setJointLimits([ -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -2.9671, -1.8326, -2.9671, -3.1416, -2.9671, -0.0873, -2.9671, 0.0, 0.0, 0.0 ], [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.9671, 1.8326, 2.9671, 0.0, 2.9671, 3.8223, 2.9671, 0.0, 0.0, 0.0 ]) self.grasptarget_link = self.robot.link(16) self.model_filename = "../dynamical_system_modulation_svm/models/gammaSVM_frankaROCUS_bounded_pyb.pkl" reference_points = np.array([[0, 0.15, 0.975], [0.0, 0.5, 0.80], [0.0, 0, 0.61]]) self.modulator = Modulator(self.model_filename, reference_points)
def klamptModel(self): """Returns the Klamp't RobotModel associated with this robot, either by the robotModel object or loading from the given filename self.fn. The results are cached so this can be called many times. """ if self.robotModel is not None: return self.robotModel if self.fn is None: raise RuntimeError("Can't load robot model for "+self.name+", no file given") from klampt import WorldModel,RobotModel self._worldTemp = WorldModel() self.robotModel = self._worldTemp.loadRobot(self.fn) if self.robotModel.index < 0: raise IOError("Unable to load robot from file "+self.fn) return self.robotModel
def main(): w = WorldModel() w.readFile("../data/robots/kinova_gen3_7dof.urdf") robot = w.robot(0) #put a "pen" geometry on the end effector gpen = Geometry3D() res = gpen.loadFile("../data/objects/cylinder.off") assert res gpen.scale(0.01,0.01,0.15) gpen.rotate(so3.rotation((0,1,0),math.pi/2)) robot.link(7).geometry().setCurrentTransform(*se3.identity()) gpen.transform(*robot.link(8).getParentTransform()) robot.link(7).geometry().set(merge_geometry.merge_triangle_meshes(gpen,robot.link(7).geometry())) robot.setConfig(robot.getConfig()) return run_simulation(w)
def edit_camera_xform(world_fn, xform=None, title=None): """Visual editor of the camera position """ world = WorldModel() world.readFile(world_fn) world.readFile("camera.rob") robot = world.robot(0) sensor = robot.sensor(0) if xform is not None: sensing.set_sensor_xform(sensor, xform) vis.createWindow() if title is not None: vis.setWindowTitle(title) vis.resizeWindow(1024, 768) vis.add("world", world) vis.add("sensor", sensor) vis.add("sensor_xform", sensing.get_sensor_xform(sensor, robot)) vis.edit("sensor_xform") def update_sensor_xform(): sensor_xform = vis.getItemConfig("sensor_xform") sensor_xform = sensor_xform[:9], sensor_xform[9:] sensing.set_sensor_xform(sensor, sensor_xform) vis.loop(callback=update_sensor_xform) sensor_xform = vis.getItemConfig("sensor_xform") return sensor_xform[:9], sensor_xform[9:]
def __init__(self, floor_length=20, floor_width=20): self.w = WorldModel() self.floor = Geometry3D() self.floor.loadFile(model_path + "cube.off") self.floor.transform( [floor_length, 0, 0, 0, floor_width, 0, 0, 0, 0.01], [-floor_length / 2.0, -floor_width / 2.0, -0.01]) floor_terrain = self.w.makeTerrain("floor") floor_terrain.geometry().set(self.floor) floor_terrain.appearance().setColor(0.4, 0.3, 0.2, 1.0) ###colors self.light_blue = [3.0 / 255.0, 140.0 / 255.0, 252.0 / 255.0, 1.0] self.wall_green = [50.0 / 255.0, 168.0 / 255.0, 143.0 / 255.0, 1.0] ###sizes self.table_length = 1.0 self.table_width = 0.5 self.table_top_thickness = 0.03 self.table_height = 0.6 self.leg_width = 0.05 self.cube_width = 0.05
def __init__(self): self.world = WorldModel() self.robot = self.world.loadRobot( 'franka_panda/panda_model_w_table.urdf') self.robot.setJointLimits([ -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -2.9671, -1.8326, -2.9671, -3.1416, -2.9671, -0.0873, -2.9671, 0.0, 0.0, 0.0 ], [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.9671, 1.8326, 2.9671, 0.0, 2.9671, 3.8223, 2.9671, 0.0, 0.0, 0.0 ]) self.initial_cfg_left = [0] * 7 + [ -np.pi / 2, -np.pi / 2, np.pi / 2 + 0.3, -np.pi / 2, 0, np.pi / 2, 3 * np.pi / 4 ] + [0] * 3 self.initial_cfg_right = [0] * 7 + [ -np.pi / 2, np.pi / 2, np.pi / 2 - 0.3, -np.pi / 2, 0, np.pi / 2, 3 * np.pi / 4 ] + [0] * 3 self.grasptarget_link = self.robot.link(16) time.sleep(1)
def load_world(): world = WorldModel() create.primitives.box(0.5, 0.1, 0.5, [0.6, -0.2, 1.6], world=world, name='obstacle') # z is 1.2 for classical setting create.primitives.box( 0.5, 0.1, 0.5, [0.6, 0.25, 1.6], world=world, name='obstacle2') # z used to be 1.6 for classical setting create.primitives.sphere(0.1, [-1, -1, 0], world=world, name='origin') create.primitives.sphere(0.1, [0, -1, 0], world=world, name='X') create.primitives.sphere(0.1, [-1, 1, 0], world=world, name='Y') fn = "./assets/ur5data/ur5Blocks.xml" res = world.readFile(fn) robot = world.robot(0) # change joint limits to avoid self collision qmin, qmax = robot.getJointLimits() qmin[2] = -math.pi / 2 qmax[2] = 0 qmin[3] = 0 qmax[3] = math.pi robot.setJointLimits(qmin, qmax) return world, robot
class IK(): def __init__(self): self.world = WorldModel() self.robot = self.world.loadRobot( 'franka_panda/panda_model_w_table.urdf') # self.table = self.world.loadRobot('franka_panda/table/table_shelf.urdf') # print('done loading') # print(self.robot.getJointLimits()) self.robot.setJointLimits([ -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -2.9671, -1.8326, -2.9671, -3.1416, -2.9671, -0.0873, -2.9671, 0.0, 0.0, 0.0 ], [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.9671, 1.8326, 2.9671, 0.0, 2.9671, 3.8223, 2.9671, 0.0, 0.0, 0.0 ]) self.initial_cfg_left = [0] * 7 + [ -m.pi / 2, -m.pi / 2, m.pi / 2 + 0.3, -m.pi / 2, 0, m.pi / 2, 3 * m.pi / 4 ] + [0] * 3 self.initial_cfg_right = [0] * 7 + [ -m.pi / 2, m.pi / 2, m.pi / 2 - 0.3, -m.pi / 2, 0, m.pi / 2, 3 * m.pi / 4 ] + [0] * 3 self.grasptarget_link = self.robot.link(16) time.sleep(1) # print('initializing collider') # self.collider = WorldCollider(self.world) # print('done') def solve(self, target_loc): if isinstance(target_loc, np.ndarray): target_loc = list(target_loc.flat) if target_loc[0] < 0: initial_cfg = self.initial_cfg_left else: initial_cfg = self.initial_cfg_right self.robot.setConfig(initial_cfg) # return True, self.robot.getConfig()[7:14] objective = ik.objective(self.grasptarget_link, local=[0, 0, 0], world=target_loc) flag = ik.solve(objective, iters=1000, tol=1e-4) cfg = self.robot.getConfig()[7:14] # collisions = list(self.collider.robotSelfCollisions(self.robot)) # print(f'get {len(collisions)} collisions') # for c1, c2 in collisions: # print('collisions', c1.getName(), c2.getName()) return flag, cfg, initial_cfg[7:14]
def setup_robot_and_light( self, robotfile='./data/idealbot.rob', mesh_file='./data/environment_meshes/full_detail_hospital_cad_meters.obj', float_height=0.08): world = WorldModel() res1 = world.loadElement(robotfile) robot = world.robot(0) qmin, qmax = robot.getJointLimits() qmin = np.nan_to_num(qmin, posinf=1000, neginf=-100) qmax = np.nan_to_num(qmax, posinf=1000, neginf=-100) robot.setJointLimits(qmin, qmax) #world.loadElement(robotfile) #a = Geometry3D() # res = a.loadFile(mesh_file) res = world.loadElement(mesh_file) print(res) collider = WorldCollider(world) #we then dilate the base nd ignore collisions between it and the 2 subsequent links: # collider.ignoreCollision((robot.link(self.base_height_link),robot.link(3))) # collider.ignoreCollision((robot.link(self.base_height_link),robot.link(3))) # collider.ignoreCollision((robot.link(8),robot.link(6))) # we now cfig = robot.getConfig() terrain = world.terrain(0) lamp = robot.link(self.lamp_linknum) print('\n\n\nbase height link = {}, lamp linknum = {}\n\n\n'.format( self.base_height_link, self.lamp_linknum)) cfig[self.base_height_link] = float_height robot.setConfig(cfig) robot.link(self.lamp_linknum).appearance().setColor( 210 / 255, 128 / 255, 240 / 255, 1) world.saveFile('disinfection.xml') return world, robot, lamp, collider
class IK(): def __init__(self): self.world = WorldModel() self.robot = self.world.loadRobot( 'franka_panda/panda_model_w_table.urdf') self.robot.setJointLimits([ -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -2.9671, -1.8326, -2.9671, -3.1416, -2.9671, -0.0873, -2.9671, 0.0, 0.0, 0.0 ], [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.9671, 1.8326, 2.9671, 0.0, 2.9671, 3.8223, 2.9671, 0.0, 0.0, 0.0 ]) self.initial_cfg_left = [0] * 7 + [ -np.pi / 2, -np.pi / 2, np.pi / 2 + 0.3, -np.pi / 2, 0, np.pi / 2, 3 * np.pi / 4 ] + [0] * 3 self.initial_cfg_right = [0] * 7 + [ -np.pi / 2, np.pi / 2, np.pi / 2 - 0.3, -np.pi / 2, 0, np.pi / 2, 3 * np.pi / 4 ] + [0] * 3 self.grasptarget_link = self.robot.link(16) time.sleep(1) def solve(self, target_loc): if isinstance(target_loc, np.ndarray): target_loc = list(target_loc.flat) if target_loc[0] < 0: initial_cfg = self.initial_cfg_left else: initial_cfg = self.initial_cfg_right self.robot.setConfig(initial_cfg) objective = ik.objective(self.grasptarget_link, local=[0, 0, 0], world=target_loc) flag = ik.solve(objective, iters=1000, tol=1e-4) cfg = self.robot.getConfig()[7:14] return flag, cfg, initial_cfg[7:14]
def visualize_pc_in_klampt_vis(self, pcloud_fname): title = pcloud_fname + " klampt world" vis_window_id = vis.createWindow(title) vis.setWindowTitle(title) world = WorldModel() vis.add("world", world) pcd = o3d.io.read_point_cloud(pcloud_fname) print(pcd) pc_xyzs = np.asarray(pcd.points) pc_xyzs_as_list = pc_xyzs.flatten().astype("float") # pc_xyzs_as_list = np.array([1,0,0, 1.1, 0, 0, 0, 1, 0]) pcloud = PointCloud() pcloud.setPoints(int(len(pc_xyzs_as_list) / 3), pc_xyzs_as_list) print(pcloud.numPoints()) vis.add("pcloud", pcloud) # vis.setColor("pcloud", 0, 0, 1, a=1) # vis.setAttribute("p1", "size", 5.0) box = klampt.GeometricPrimitive() box.setAABB([-1, -1, 0], [-0.9, -0.9, 0.2]) g = klampt.Geometry3D(box) vis.add("box", g) vis.setColor("box", 0, 0, 1, 0.5) coordinates.setWorldModel(world) vis.add("coordinates", coordinates.manager()) vis.show() while vis.shown(): vis.lock() vis.unlock() time.sleep(0.01) vis.kill()
class WorldViewer(GLRealtimeProgram): def __init__(self): GLRealtimeProgram.__init__(self, 'World Viewer') self.world = WorldModel() self.fps = 10.0 self.dt = 1 / self.fps self.pre_drawables = [] self.post_drawables = [] def display(self): for drawable in self.pre_drawables: drawable.draw() self.world.drawGL() for drawable in self.post_drawables: drawable.draw() # draw poses for all robots and boxes poses = [se3.identity()] for i in range(self.world.numRobots()): robot = self.world.robot(i) poses.append(robot.link(0).getTransform()) poses.append(robot.link(robot.numLinks() - 1).getTransform()) for i in range(self.world.numRigidObjects()): poses.append(self.world.rigidObject(i).getTransform()) for pose in poses: gldraw.xform_widget(pose, 0.1, 0.01, fancy=True) def mousefunc(self, button, state, x, y): return GLRealtimeProgram.mousefunc(self, button, state, x, y) def motionfunc(self, x, y, dx, dy): return GLRealtimeProgram.motionfunc(self, x, y, dx, dy)
def add_to_vis(self,robot=None,animate=True,base_xform=None): """Adds the gripper to the klampt.vis scene.""" from klampt import vis from klampt import WorldModel,Geometry3D,GeometricPrimitive from klampt.model.trajectory import Trajectory prefix = "gripper_"+self.name if robot is None and self.klampt_model is not None: w = WorldModel() if w.readFile(self.klampt_model): robot = w.robot(0) vis.add(prefix+"_gripper",w) robotPath = (prefix+"_gripper",robot.getName()) elif robot is not None: vis.add(prefix+"_gripper",robot) robotPath = prefix+"_gripper" if robot is not None: assert self.base_link >= 0 and self.base_link < robot.numLinks() robot.link(self.base_link).appearance().setColor(1,0.75,0.5) if base_xform is None: base_xform = robot.link(self.base_link).getTransform() else: if robot.link(self.base_link).getParent() >= 0: print("Warning, setting base link transform for an attached gripper base") #robot.link(self.base_link).setParent(-1) robot.link(self.base_link).setParentTransform(*base_xform) robot.setConfig(robot.getConfig()) for l in self.finger_links: assert l >= 0 and l < robot.numLinks() robot.link(l).appearance().setColor(1,1,0.5) if robot is not None and animate: q0 = robot.getConfig() for i in self.finger_drivers: if isinstance(i,(list,tuple)): for j in i: robot.driver(j).setValue(1) else: robot.driver(i).setValue(1) traj = Trajectory([0],[robot.getConfig()]) for i in self.finger_drivers: if isinstance(i,(list,tuple)): for j in i: robot.driver(j).setValue(0) traj.times.append(traj.endTime()+0.5) traj.milestones.append(robot.getConfig()) else: robot.driver(i).setValue(0) traj.times.append(traj.endTime()+1) traj.milestones.append(robot.getConfig()) traj.times.append(traj.endTime()+1) traj.milestones.append(traj.milestones[0]) traj.times.append(traj.endTime()+1) traj.milestones.append(traj.milestones[0]) assert len(traj.times) == len(traj.milestones) traj.checkValid() vis.animate(robotPath,traj) robot.setConfig(q0) if self.center is not None: vis.add(prefix+"_center",se3.apply(base_xform,self.center)) center_point = (0,0,0) if self.center is None else self.center outer_point = (0,0,0) if self.primary_axis is not None: length = 0.1 if self.finger_length is None else self.finger_length outer_point = vectorops.madd(self.center,self.primary_axis,length) line = Trajectory([0,1],[self.center,outer_point]) line.milestones = [se3.apply(base_xform,m) for m in line.milestones] vis.add(prefix+"_primary",line,color=(1,0,0,1)) if self.secondary_axis is not None: width = 0.1 if self.maximum_span is None else self.maximum_span line = Trajectory([0,1],[vectorops.madd(outer_point,self.secondary_axis,-0.5*width),vectorops.madd(outer_point,self.secondary_axis,0.5*width)]) line.milestones = [se3.apply(base_xform,m) for m in line.milestones] vis.add(prefix+"_secondary",line,color=(0,1,0,1)) elif self.maximum_span is not None: #assume vacuum gripper? p = GeometricPrimitive() p.setSphere(outer_point,self.maximum_span) g = Geometry3D() g.set(p) vis.add(prefix+"_opening",g,color=(0,1,0,0.25))
class RobotInfo: """Stores common semantic properties for a robot. Attributes: - name (str): a string identifying this robot - fn (fn, optional): a file pointing to the Klamp't model. - endEffectors (dict str->EndEffectorInfo): a set of named end effectors. - parts (dict str->list): a set of named parts """ def __init__(self, name, fn=None, freeBase=False, parts=None, endEffectors=None, properties=None): self.name = name self.fn = fn self.robotModel = None self._worldTemp = None self.freeBase = freeBase if parts is None: parts = dict() else: if not isinstance(parts, dict): raise ValueError( "`parts` must be a dict from names to link lists") for (k, v) in parts: if not hasattr(v, '__iter__'): raise ValueError( "`parts` must be a dict from names to link lists") if endEffectors is None: endEffectors = dict() else: if not isinstance(endEffectors, dict): raise ValueError( "`endEffectors` must be a dict from names to links") if properties is None: properties = dict() else: if not isinstance(properties, dict): raise ValueError("`properties` must be a dict") self.parts = parts self.endEffectors = endEffectors self.properties = properties def klamptModel(self): """Returns the Klamp't RobotModel associated with this robot, either by the robotModel object or loading from the given filename self.fn. The results are cached so this can be called many times. """ if self.robotModel is not None: return self.robotModel if self.fn is None: raise RuntimeError("Can't load robot model for " + self.name + ", no file given") from klampt import WorldModel, RobotModel self._worldTemp = WorldModel() self.robotModel = self._worldTemp.loadRobot(self.fn) if self.robotModel.index < 0: raise IOError("Unable to load robot from file " + self.fn) return self.robotModel def partLinks(self, part): return self.parts[part] def partLinkIndices(self, part): res = self.parts[part] return self.toIndices(res) def partLinkNames(self, part): res = self.parts[part] return self.toNames(res) def partAsSubrobot(self, part): from klampt.model.subrobot import SubRobotModel partLinks = self.partLinkIndices(part) model = self.klamptModel() return SubRobotModel(model, partLinks) def eeSolver(self, endEffector, target): """Given a named end effector and a target point, transform, or set of parameters from config.setConfig(ikgoal) / config.getConfig(ikgoal), returns the :class:`~klampt.robotsim.IKSolver` for the end effector and that target. """ ee = self.endEffectors[endEffector] from klampt import IKSolver, IKObjective from ..model import config, ik robot = self.klamptModel() link = robot.link(ee.link) s = IKSolver(robot) q = config.getConfig(target) obj = ee.ikObjective if ee.ikObjective is None: if len(q) == 3: obj = ik.objective(link, local=[0, 0, 0], world=q) elif len(q) == 12: obj = ik.objective(link, R=q[:9], t=q[9:]) else: raise ValueError( "Invalid # of elements given in target, must be either a 3-vector or SE3 element" ) s.add(obj) s.setActiveDofs(self.toIndices(ee.activeLinks)) return s def toIndices(self, items): """Returns link identifiers as link indices""" if all(isinstance(v, int) for v in items): return items else: robot = self.klamptModel() for i, v in enumerate(items): if not isinstance(v, int): assert isinstance( v, str), "Link identifiers must be int or str" items[i] = robot.link(v).getIndex() assert items[ i] >= 0, "Link %s doesn't exist in robot %s" % ( v, self.name) return items def toNames(self, items): """Returns link identifiers as link names""" if all(isinstance(v, str) for v in items): return items else: robot = self.klamptModel() for i, v in enumerate(items): if not isinstance(v, str): assert isinstance( v, int), "Link identifiers must be int or str" assert v >= 0 and v < robot.numLinks( ), "Link %d is invalid for robot %s" % (v, self.name) items[i] = robot.link(v).getName() return items def load(self, f): """Loads the info from a JSON file. f is a file object.""" import json from ..io import loader jsonobj = json.load(f) for attr in [ 'name', 'fn', 'parts', 'freeBase', 'endEffectors', 'properties' ]: if attr not in jsonobj: raise IOError("Loaded JSON object doesn't contain '" + attr + "' key") setattr(self, attr, jsonobj[attr]) ees = dict() for (k, v) in self.endEffectors.items(): obj = None if v['ikObjective'] is None else loader.fromJson( v['ikObjective'], 'IKObjective') ees[k] = EndEffectorInfo(v['link'], v['activeLinks'], obj) self.endEffectors = ees def save(self, f): """Saves the info to a JSON file. f is a file object.""" import json from ..io import loader jsonobj = dict() for attr in ['name', 'fn', 'parts', 'freeBase', 'properties']: jsonobj[attr] = getattr(self, attr) ees = dict() for k, v in self.endEffectors.items(): obj = None if v.ikObjective is None else loader.toJson( v.ikObjective) ees[k] = { 'link': v.link, 'activeLinks': v.activeLinks, 'ikObjective': obj } jsonobj['endEffectors'] = ees json.dump(jsonobj, f)
from klampt import WorldModel, vis world = WorldModel() world.readFile("world.xml") vis.init("IPython") vis.add("world", world) vis.show() #string = vis.nativeWindow().page() #with open("vis.html", "w") as f: # f.write(string)
from klampt import WorldModel from klampt import vis import klampt import time world = WorldModel() world_file = "../data/simulation_test_worlds/drc_rough_terrain_world.xml" if not world.readFile(world_file): raise RuntimeError("Unable to load terrain model") robot_file = "../data/robot_model/robosimian_caesar_new.rob" if not world.readFile(robot_file): raise RuntimeError("Unable to load robot model") vis_window_id = vis.createWindow(world_file) vis.setWindow(vis_window_id) vis.add("world", world) vp = vis.getViewport() vis.setViewport(vp) vis.autoFitCamera() world.robot(0).setConfig([ 12.621508747630084, 1.3060978650033888, 0.7271994997360561, -0.18389666460947365, -0.2336561986984183, 0.23915345995072382, 0.0,
import numpy as np import time import copy import json sys.path.append("../common") import grasp import grasp_database from known_grippers import * # physics simulation doesn't work for now, set it to False! PHYSICS_SIMULATION = False if __name__ == '__main__': #load the robot / world file fn = "world.xml" world = WorldModel() res = world.readFile(fn) if not res: print("Unable to read file", fn) exit(0) for i in range(world.numRigidObjects()): obj = world.rigidObject(i) # this will perform a reasonable center of mass / inertia estimate m = obj.getMass() m.estimate(obj.geometry(), mass=0.454, surfaceFraction=0.2) obj.setMass(m) plate_mass = world.rigidObject("plate").getMass() plate_mass.setInertia([1, 0, 0, 0, 1, 0, 0, 0, 1]) world.rigidObject("plate").setMass(plate_mass)
from klampt import WorldModel from klampt.io import resource from klampt import vis from klampt.model import trajectory import planning import math # PROBLEM = '1a' PROBLEM = '1b' if __name__ == '__main__': #load the robot / world file fn = "problem1.xml" world = WorldModel() res = world.readFile(fn) if not res: print("Unable to read file", fn) exit(0) robot = world.robot(0) #need to fix the spin joints somewhat qmin, qmax = robot.getJointLimits() for i in range(len(qmin)): if qmax[i] - qmin[i] > math.pi * 2: qmin[i] = -float('inf') qmax[i] = float('inf') robot.setJointLimits(qmin, qmax) qstart = resource.get("start.config", world=world) #add the world elements individually to the visualization
import sys import time import numpy as np from Robot import Robot from klampt import WorldModel from Simmulation import Simulation from functions import visualize if len(sys.argv)<=1: print "USAGE: kinematicSim.py [world_file]" exit() world = WorldModel() world_file = sys.argv[1] res = world.readFile(world_file) if not res: raise RuntimeError("Unable to load model " + world_file) # np.random.seed(11) np.random.seed(1711) simulation = Simulation(world) start_pc = [-0, 0, 0.05, 0, 0, 0] goal_pc = [0.7, -0.5, 0.05, 0, 0, 0] simulation.create(start_pc, goal_pc) gamma = 0.99 agent = Robot(simulation, gamma) if len(sys.argv) > 2 and sys.argv[2] == 'train': epochs = 600 decay = 0.99 max_step = 3000