def sampling_9(serial_num): world_name = "flatworld" world_file = "../../data/robot_model_files/worlds/" + world_name + ".xml" robot_file = "../../data/robot_model_files/robots/irb120_icp.rob" world = get_world(world_file, robot_file, visualize_robot=True) robot = world.robot(0) est_config_list = cal_est_config_list(robot, serial_num) path = collision_checking(world, est_config_list, 0.001) if path is False: sys.exit("The calibration path exist self-collision.") else: print("The calibration path is collision free.") vis.clear() vis.add("world", world) for i in range(len(est_config_list)): vis.add("estimated configure" + str(i), est_config_list[i]) vis.setColor("estimated configure" + str(i), 1.0, 0.0, 0.1 * i, 0.5) vis.spin(float('inf')) df = pd.DataFrame(np.asarray(est_config_list)[:, 7:13] * (180. / np.pi)) df.to_csv("../../data/robot_configuration_files/configuration_record_" + serial_num + ".csv", header=False, index=False) time.sleep(1.) est_config_list = est_config_list[0:] controller = SampleController(est_config_list, robot, '192.168.1.178') controller.start()
def main(): world, robot, link_index, geom_index, obs_index = create_world() q0 = [0, -2, 0] qf = [3, 1, 3.14] config = TrajOptSettings(cvxpy_args={'solver': 'GUROBI'}, limit_joint=False) trajopt = KineTrajOpt(world, robot, q0=q0, qf=qf, config=config, link_index=link_index, geom_index=geom_index, obs_index=obs_index) # create guess and solve n_grid = 10 guess = np.linspace(q0, qf, n_grid) rst = trajopt.optimize(guess) print(rst) traj = rst['sol'] # show the results vis.add('world', world) robot.setConfig(traj[-1]) for i, qi in enumerate(traj[:-1]): name = 'ghost%d' % i vis.add(name, qi) vis.setAttribute(name, 'type', 'Config') vis.setColor(name, 0, 1, 0, 0.4) vis.spin(100)
def qt_template(world): """Runs a custom Qt frame around a visualization window""" if not glinit.available('PyQt5'): print( "PyQt5 is not available on your system, try sudo apt-get install python-qt5" ) return #Qt objects must be explicitly deleted for some reason in PyQt5... g_mainwindow = None #All Qt functions must be called in the vis thread. #To hook into that thread, you will need to pass a window creation function into vis.customUI. def makefunc(gl_backend): global g_mainwindow g_mainwindow = MyQtMainWindow(gl_backend) return g_mainwindow vis.customUI(makefunc) vis.add("world", world) vis.setWindowTitle("Klamp't Qt test") vis.spin(float('inf')) vis.kill() #Safe cleanup of all Qt objects created in makefunc. #If you don't have this, PyQt5 complains about object destructors being called from the wrong thread del g_mainwindow
def viewport_template(world): """Changes the parameters of the viewport and main window""" #add the world to the visualizer vis.add("world",world) vp = vis.getViewport() vp.w,vp.h = 800,800 vis.setViewport(vp) #this auto-sizes the camera vis.autoFitCamera() vis.setWindowTitle("Viewport modification test") vis.spin(float('inf')) vis.kill()
def main(): world, robot, link_index, geom_index, obs_index = create_world() # get start and target for the joints joint_start = [ 0.115, 0.6648, -0.3526, 1.6763, -1.9242, 2.9209, -1.2166, 1.3425, -0.6365, 0.0981, -1.226, -2.0264, -3.0125, -1.3958, -1.9289, 2.9295, 0.5748, -3.137 ] joint_target = [ 0.115, 0.6808, -0.3535, 1.4343, -1.8516, 2.7542, -1.2005, 1.5994, -0.6929, -0.3338, -1.292, -1.9048, -2.6915, -1.2908, -1.7152, 1.3155, 0.6877, -0.0041 ] assert (len(link_index) == len(joint_start) == len(joint_target)) # generate guess n_grid = 10 # resolution guess = np.linspace(joint_start, joint_target, n_grid) # create trajopt instance config = TrajOptSettings(cvxpy_args={'solver': 'GUROBI'}, limit_joint=False) trajopt = KineTrajOpt(world, robot, joint_start, joint_target, config=config, link_index=link_index, geom_index=geom_index, obs_index=obs_index) # create some stuff for visualization config = np.array(robot.getConfig()) def update_config(q): config[link_index] = q def get_config(q): cq = copy.copy(config) cq[link_index] = q return cq rst = trajopt.optimize(guess) sol = rst['sol'] vis.add('world', world) dists = [] for i in range(1, n_grid - 1): # set to -2 so the robot stops there with collision vis.add('ghost%d' % i, list(get_config(sol[i]))) vis.setColor('ghost%d' % i, 0, 1, 0, 0.3) robot.setConfig(get_config(sol[i])) update_config(sol[-1]) robot.setConfig(config) vis.spin(float('inf'))
def main(): world, robot, link_index, geom_index, obs_index = create_world() joint_start = [-1.832, -0.332, -1.011, -1.437, -1.1, -1.926, 3.074] joint_target = [0.062, 1.287, 0.1, -1.554, -3.011, -0.268, 2.988] # compute the goal pose config0 = robot.getConfig() for lid, theta in zip(link_index, joint_target): config0[lid] = theta robot.setConfig(config0) tR, tt = robot.link(link_index[-1]).getTransform() print(f'target is {tR}, {tt}') # create the trajopt instance config = TrajOptSettings(cvxpy_args={'solver': 'GUROBI'}, limit_joint=False) trajopt = KineTrajOpt(world, robot, q0=joint_start, qf=None, config=config, link_index=link_index, geom_index=geom_index, obs_index=obs_index) FINAL_POSE = True if FINAL_POSE: trajopt.add_pose_constraint(-1, link_index[-1], (tR, tt)) # add keep constraint trajopt.add_orientation_constraint(None, link_index[-1], tR) # generate guess n_grid = 10 # resolution guess = np.linspace(joint_start, joint_target, n_grid) # create some stuff for visualization config = np.array(robot.getConfig()) def update_config(q): config[link_index] = q def get_config(q): cq = copy.copy(config) cq[link_index] = q return cq rst = trajopt.optimize(guess) sol = rst['sol'] vis.add('world', world) for i in range(1, n_grid - 2): # set to -2 so the robot stops there with collision vis.add('ghost%d' % i, list(get_config(sol[i]))) vis.setColor('ghost%d' % i, 0, 1, 0, 0.5) update_config(sol[-1]) vis.spin(float('inf'))
def vis_interaction_test(world): """Run some tests of visualization module interacting with the resource module""" print("Showing robot in modal dialog box") vis.add("robot", world.robot(0)) vis.add("ee", world.robot(0).link(11).getTransform()) vis.dialog() config = resource.get("resourcetest1.config", description="Should show up without a hitch...", doedit=True, editor='visual', world=world) import time if MULTITHREADED: print( "Showing threaded visualization (this will fail on GLUT or Mac OS)" ) vis.show() for i in range(3): vis.lock() q = world.robot(0).getConfig() q[9] = 3.0 world.robot(0).setConfig(q) vis.unlock() time.sleep(1.0) if not vis.shown(): break vis.lock() q = world.robot(0).getConfig() q[9] = -1.0 world.robot(0).setConfig(q) vis.unlock() time.sleep(1.0) if not vis.shown(): break print("Hiding visualization window") vis.show(False) else: print("Showing single-threaded visualization for 5s") vis.spin(5.0) config = resource.get("resourcetest1.config", description="Should show up without a hitch...", doedit=True, editor='visual', world=world)
def multiwindow_template(world): """Tests multiple windows and views.""" vis.add("world", world) vp = vis.getViewport() vp.w, vp.h = 400, 600 vis.setViewport(vp) vis.addText("label1", "This is Window 1", (20, 20)) vis.setWindowTitle("Window 1") vis.show() id1 = vis.getWindow() print("First window ID:", id1) id2 = vis.createWindow("Window 2") vis.add("Lone point", [0, 0, 0]) vis.setViewport(vp) vis.addText("label1", "This is Window 2", (20, 20)) print("Second window ID:", vis.getWindow()) vis.setWindow(id2) vis.spin(float('inf')) #restore back to 1 window, clear the text vis.setWindow(id1) vis.clearText() vp = vis.getViewport() vp.w, vp.h = 800, 800 vis.setViewport(vp) vis.setWindowTitle("vis.spin test: will close in 5 seconds...") vis.spin(5.0) #Now testing ability to re-launch windows vis.setWindowTitle("Shown again. Close me to proceed.") vis.spin(float('inf')) vis.setWindowTitle("Dialog test. Close me to proceed.") vp = vis.getViewport() vp.w, vp.h = 400, 600 vis.setViewport(vp) vis.dialog() vp.w, vp.h = 640, 480 vis.setViewport(vp) for i in range(3): widgets = GLWidgetPlugin() widgets.addWidget(RobotPoser(world.robot(0))) vis.addPlugin(widgets) vis.setWindowTitle("Split screen test") vis.spin(float('inf')) vis.setPlugin(None) vis.setWindowTitle("Back to normal. Close me to quit.") vis.dialog() vis.kill()
def visualize(world, robot, path=None, start=None, goal=None): """Visualize a path""" # visualize start/goal as spheres if exist r = 0.04 if start != None: sphere = create.primitives.sphere(r, start, world=world, name='start') # vis.add('start', sphere) if goal != None: create.primitives.sphere(r, goal, world=world, name='goal') vis.add("world", world) # animate path if exist if path != None: traj = trajectory.RobotTrajectory(robot, range(len(path)), path) vis.animate(("world", robot.getName()), path, speed=0.5) vis.add("trajectory", traj) vis.spin(float('inf'))
def main(): world, robot = load_world() space = makeSpace(world, robot) collision_check = lambda: (space.selfCollision(robot.getConfig( ))) == False and (space.envCollision(robot.getConfig()) == False) # q0, qf = compute_initial_final(world, robot) q0 = np.array([ 0.0, 0.04143683792522413, -1.183867130629673, 2.0680756463305556, 3.745524327531242, 3.6939852943821956e-06, 0.08270468308944955, 0.0 ]) qf = np.array([ 0.0, 3.590769443639893, -0.8366423979290207, 1.5259988654642873, 5.59380779741848, 0.8157228966102285, 4.712401453217654, 0.0 ]) robot.setConfig(q0) q0 = q0[1:-1] qf = qf[1:-1] link_index = [1, 2, 3, 4, 5, 6] geom_index = link_index obs_index = [0, 1] n_grid = 10 # resolution guess = np.linspace(q0, qf, n_grid) # guess += np.random.uniform(-0.1, 0.1, size=guess.shape) vis.add("world", world) # this one has no additional constraint so it should be easy to solve... config = TrajOptSettings(cvxpy_args={'solver': 'GUROBI'}) trajopt = KineTrajOpt(world, robot, q0, qf, config=config, link_index=link_index, obs_index=obs_index, geom_index=geom_index) rst = trajopt.optimize(guess) sol = rst['sol'] for i in range(1, n_grid): robot.setConfig([0] + sol[i].tolist() + [0]) print(collision_check()) vis.add('ghost%d' % i, [0] + list(sol[i]) + [0]) vis.setColor('ghost%d' % i, 0, 1, 0, 0.5) vis.spin(float('inf'))
def main(): world, robot, link_index, geom_index, obs_index = create_world() joint_start = [-1.832, -0.332, -1.011, -1.437, -1.1, -1.926, 3.074] joint_target = [0.062, 1.287, 0.1, -1.554, -3.011, -0.268, 2.988] # generate guess n_grid = 10 # resolution guess = np.linspace(joint_start, joint_target, n_grid) # create trajopt instance config = TrajOptSettings(cvxpy_args={'solver': 'GUROBI'}, limit_joint=False) trajopt = KineTrajOpt(world, robot, joint_start, joint_target, config=config, link_index=link_index, geom_index=geom_index, obs_index=obs_index) # create some stuff for visualization config = np.array(robot.getConfig()) def update_config(q): config[link_index] = q def get_config(q): cq = copy.copy(config) cq[link_index] = q return cq rst = trajopt.optimize(guess) sol = rst['sol'] vis.add('world', world) for i in range(1, n_grid - 2): # set to -2 so the robot stops there with collision vis.add('ghost%d' % i, list(get_config(sol[i]))) vis.setColor('ghost%d' % i, 0, 1, 0, 0.5) # dists = compute_distance(world, robot, link_index, obs_index) # print(dists) update_config(sol[-1]) vis.spin(float('inf'))
def multiwindow_template(world): """Tests multiple windows and views.""" vis.add("world",world) vp = vis.getViewport() vp.w,vp.h = 800,800 vis.setViewport(vp) vis.setWindowTitle("vis.spin test: will close in 5 seconds...") vis.spin(5.0) #Now testing ability to re-launch windows vis.setWindowTitle("Shown again. Close me to proceed.") vis.spin(float('inf')) vis.setWindowTitle("Dialog test. Close me to proceed.") vp = vis.getViewport() vp.w,vp.h = 400,600 vis.setViewport(vp) vis.dialog() vp.w,vp.h = 640,480 vis.setViewport(vp) for i in range(3): widgets = GLWidgetPlugin() widgets.addWidget(RobotPoser(world.robot(0))) vis.addPlugin(widgets) vis.setWindowTitle("Split screen test") vis.spin(float('inf')) vis.setPlugin(None) vis.setWindowTitle("Back to normal. Close me to quit.") vis.dialog() vis.kill()
print(plan.space.cspace.feasibilityQueryOrder()) print("Plan stats:") print(plan.getStats()) print("CSpace stats:") print(plan.space.getStats()) #to be nice to the C++ module, do this to free up memory plan.space.close() plan.close() if len(wholepath) > 1: #print "Path:" #for q in wholepath: # print " ",q #if you want to save the path to disk, uncomment the following line #wholepath.save("test.path") #draw the path as a RobotTrajectory (you could just animate wholepath, but for robots with non-standard joints #the results will often look odd). Animate with 5-second duration times = [i * 5.0 / (len(wholepath) - 1) for i in range(len(wholepath))] traj = trajectory.RobotTrajectory(robot, times=times, milestones=wholepath) #show the path in the visualizer, repeating for 60 seconds vis.animate("robot", traj) vis.spin(60) else: print("Failed to generate a plan") vis.kill()
glColor3f(0, 0, 0) glBegin(GL_POINTS) for pt in self.pc: if len(pt) == 6: glColor3f(*pt[3:6]) if read_local: glVertex3fv(se3.apply(T, pt[0:3])) else: glVertex3fv(pt[0:3]) glEnd() #print "Draw PC time",time.time()-t0 return True vis.pushPlugin(SensorTestWorld()) vis.spin(float('inf')) vis.kill() """ #Note: GLEW doesn't work outside of the main thread, hence the use of the GLPluginInterface. #The below code falls back to the non-accelerated sensor simulation vis.show() time.sleep(0.5) for sample in range(10): vis.lock() robot.randomizeConfig() #sensor.kinematicSimulate(world,0.01) sim.controller(0).setPIDCommand(robot.getConfig(),[0.0]*7) vis.unlock() for i in range(100):
#obj.geometry().loadFile(OBJECT_DIR+"ycb-select/011_banana/nontextured.ply"); #obj.geometry().loadFile(OBJECT_DIR+"ycb-select/021_bleach_cleanser/nontextured.ply") obj.geometry().loadFile(OBJECT_DIR + "ycb-select/048_hammer/nontextured.ply") #obj.geometry().loadFile(OBJECT_DIR+"cube.off"); obj.geometry().scale(0.2) #weird bug in Qhull -- cylinder can't be converted to ConvexHull #obj.geometry().loadFile(OBJECT_DIR+"cylinder.off") #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) obj.appearance().setColor(0.3, 0.3, 0.3, 1) #debugging the stable faces sides = stable_faces(obj, stability_tol=0.0, merge_tol=0.05) vis.add("world", world) vis.setBackgroundColor(1, 1, 1) vis.add("COM", m.getCom(), color=(1, 0, 0, 1), size=5, hide_label=True) for i, f in enumerate(sides): gf = GeometricPrimitive() gf.setPolygon(np.stack(f).flatten()) color = (0.5 + 0.5 * random.random(), 0.5 + 0.5 * random.random(), 0.5 + 0.5 * random.random(), 0.5) vis.add("face{}".format(i), Geometry3D(gf), color=color, hide_label=True) vis.show() vis.spin(float("inf"))
def main(): print(""" =============================================================================== A program to quickly browse Klamp't objects. USAGE: %s [item1 item2 ...] where the given items are world, robot, terrain, object, or geometry files. Run it without arguments %s for an empty reference world. You may add items to the reference world using the `Add to World` button. If you know what items to use in the reference world, run it with %s world.xml or %s item1 item2 ... where the items are world, robot, terrain, object, or geometry files. =============================================================================== """ % (sys.argv[0], sys.argv[0], sys.argv[0], sys.argv[0])) #must be explicitly deleted for some reason in PyQt5... g_browser = None def makefunc(gl_backend): global g_browser browser = ResourceBrowser(gl_backend) g_browser = browser dw = QtWidgets.QDesktopWidget() x = dw.width() * 0.8 y = dw.height() * 0.8 browser.setFixedSize(x, y) for fn in sys.argv[1:]: res = browser.world.readFile(fn) if not res: print("Unable to load model", fn) print("Quitting...") sys.exit(1) print("Added", fn, "to world") if len(sys.argv) > 1: browser.emptyVisPlugin.add("world", browser.world) return browser vis.customUI(makefunc) vis.show() vis.setWindowTitle("Klamp't Resource Browser") vis.spin(float('inf')) vis.kill() del g_browser return #this code below is incorrect... app = QtWidgets.QApplication(sys.argv) browser = ResourceBrowser() for fn in sys.argv[1:]: res = browser.world.readFile(fn) if not res: print("Unable to load model", fn) print("Quitting...") sys.exit(1) print("Added", fn, "to world") if len(sys.argv) > 1: browser.emptyVisPlugin.add("world", browser.world) dw = QtWidgets.QDesktopWidget() x = dw.width() * 0.8 y = dw.height() * 0.8 browser.setFixedSize(x, y) #browser.splitter.setWindowState(QtCore.Qt.WindowMaximized) browser.setWindowTitle("Klamp't Resource Browser") browser.show() # Start the main loop. res = app.exec_() return res
#print some interpolated points print 0.5,":",traj.eval(0.5) print 2.5,":",traj.eval(2.5) #print some stuff after the end of trajectory print 7,":",traj.eval(6) print 100.3,":",traj.eval(100.3) print -2,":",traj.eval(-2) from klampt import vis vis.add("point",[0,0,0]) vis.animate("point",traj) vis.add("traj",traj) #vis.spin(float('inf')) #show the window until you close it traj2 = trajectory.HermiteTrajectory() traj2.makeSpline(traj) vis.animate("point",traj2) vis.hide("traj") vis.add("traj2",traj2.configTrajectory().discretize(0.1)) #vis.spin(float('inf')) traj_timed = trajectory.path_to_trajectory(traj,vmax=2,amax=4) #next, try this line instead #traj_timed = trajectory.path_to_trajectory(traj,timing='sqrt-L2',speed='limited',vmax=2,amax=4) #or this line #traj_timed = trajectory.path_to_trajectory(traj2.configTrajectory().discretize(0.1),timing='sqrt-L2',speed=0.3) vis.animate("point",traj_timed) vis.spin(float('inf'))
def mousefunc(self, button, state, x, y): print("Mouse button", button, "state", state, "at point", x, y) def motionfunc(self, x, y, dx, dy): if 'shift' in self.modifiers(): self.q[2] = float(y) / 400 self.q[3] = float(x) / 400 self.world.robot(0).setConfig(self.q) return True return False if __name__ == "__main__": print("""================================================================ mouse_capture.py: A simple program where the mouse motion, when shift-clicking, gets translated into joint values for an animated robot. ======================================================================== """) world = klampt.WorldModel() res = world.readFile("../../data/tx90blocks.xml") if not res: raise RuntimeError("Unable to load world") plugin = MouseCapture(world) vis.add("world", world) vis.pushPlugin(plugin) vis.setWindowTitle("mouse_capture.py") vis.spin(float('inf')) #shows the window vis.kill()
def optimizeGrasping(robot, objs, init_config, gridres, pcres, score_oracle=ORACLE, collision_links=None): print(f"Oracle used: {score_oracle}") robot.setConfig(init_config) robot_geometry = RobotKinematicsCache(robot, gridres, pcres) object_geometry = PenetrationDepthGeometry(objs[0].geometry(), gridres, pcres) q_init = robot.getConfig() grasping_problem = SIPCCProblem() grasping_problem.dim_z = 7 grasping_problem.z_proj = np.array([0, 0, 0, 0, 0, 0, 1]) grasping_problem.z_lb = np.array([0, 0, 0, 0, 0, 0, 0]) grasping_problem.domain = LinkAndGeometryDomain() grasping_problem.set_objective(ConstantObjectiveFunction(0)) grasping_problem.set_complementarity( RobotCollisionConstraint(robot, object_geometry, gridres, pcres, robot_geometry, collision_links)) grasping_problem.add_ineq(FrictionConstraint()) grasping_problem.add_eq(Equilibrium3DConstraint(objs[0], object_geometry)) grasping_problem.colliding_links = collision_links if score_oracle == 'LSO': grasping_problem.oracle = local_score_oracle elif score_oracle == 'MVO': grasping_problem.oracle = maximum_violation_oracle elif score_oracle == 'GSO': grasping_problem.oracle = global_score_oracle else: raise ValueError("score_oracle must be LSO, GSO, or MVO") x_init = q_init x_lb = grasping_problem.complementarity.robot.robot.getJointLimits()[0] x_ub = grasping_problem.complementarity.robot.robot.getJointLimits()[1] dim_z = 7 from klampt import vis wdisplay = WorldModel() wdisplay.add('robot', robot) display_robot = wdisplay.robot(0) for i, o in enumerate(objs): wdisplay.add('obj_{}'.format(i), o) def update_robot_viz(x, y, z): vis.clear() vis.add("robot", display_robot) for i, o in enumerate(objs): odisplay = wdisplay.rigidObject(i) vis.add("obj_{}".format(i), odisplay) try: display_robot.setConfig(x) for i, yi in enumerate(y): vis.add("pt_{}".format(i), yi[1], color=(1, 0, 0, 1), hide_label=True) f_normal = z[i * 7 + 6] f_friction = z[i * 7:i * 7 + 6] n = np.array(normal(grasping_problem.xyz_eq.env_geom, yi[1])) f = n * f_normal n1 = so3.canonical(n)[3:6] n2 = so3.canonical(n)[6:9] for j in range(6): n_tmp = (math.cos( (math.pi / 3) * j) * np.array(n1) + math.sin( (math.pi / 3) * j) * np.array(n2)) f += n_tmp * f_friction[j] # f is the force pointing inward vis.add( "n_{}".format(i), Trajectory([0, 1], [yi[1], vectorops.madd(yi[1], n, 0.05)]), color=(1, 1, 0, 1), hide_label=True) vis.add( "f_{}".format(i), Trajectory( [0, 1], [yi[1], vectorops.madd(yi[1], f.tolist(), -1.0)]), color=(1, 0.5, 0, 1), hide_label=True) finally: pass time.sleep(0.01) vis.show() settings = SIPCCOptimizationSettings() settings.callback = lambda *args: vis.threadCall(lambda: update_robot_viz( *args)) settings.min_index_point_distance = MINIMUM_INDEX_DISTANCE t_start = time.time() res = optimizeSIPCC(grasping_problem, x_init, x_lb, x_ub, settings=settings) t_end = time.time() res.time_opt = t_end - t_start vis.spin(float('inf')) res.number_of_points = object_geometry.pcdata.numPoints() return res
def optimization(self): """ :return: None """ time_stamp = time.time() min_error = 10e5 for k in range(25): random.seed(time.time()) est_input = [random.uniform(-2 * np.pi, 2 * np.pi) for i in range(3)] + \ [random.uniform(-0.8, 0.8) for i in range(3)] + \ [random.uniform(-2 * np.pi, 2 * np.pi) for i in range(3)] + \ [random.uniform(-0.2, 0.2) for i in range(3)] print("We will start " + str(k + 1) + "th minimize!") res = root(opt_error_fun, np.array(est_input), args=(self.Tlink_set, self.trans_list), method='lm', options={}) print("The reason for termination: \n", res.message, "\nAnd the number of the iterations are: ", res.nfev) error = np.sum(np.absolute(res.fun)) print("The minimize is end, and the error is: ", error) if error <= min_error: min_error = error self.min_res = res print("The optimization uses: ", time.time() - time_stamp, "seconds") print("The error is: ", np.sum(np.absolute(self.min_res.fun))) print("The optimized T_oct is: ", (self.min_res.x[0:3], self.min_res.x[3:6])) print( "And the matrix form is: \n", np.array( se3.homogeneous( (so3.from_rpy(self.min_res.x[0:3]), self.min_res.x[3:6])))) print("The optimized T_needle_end is: ", (self.min_res.x[6:9], self.min_res.x[9:12])) print( "And the matrix form is: \n", np.array( se3.homogeneous((so3.from_rpy(self.min_res.x[6:9]), self.min_res.x[9:12])))) # os.makedirs("../../data/suture_experiment/calibration_result_files/" + self.serial_num + "/", exist_ok=True) # np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num + # "/calibration_result.npy", self.min_res.x, allow_pickle=True) vis.spin(float('inf')) vis.clear() self.robot.setConfig(self.qstart) vis.add("world", self.world) World_base = model.coordinates.Frame("Base Frame", worldCoordinates=(so3.from_rpy( [0, 0, 0]), [0, 0, 0])) vis.add('World_base', World_base) est_OCT_coordinate = model.coordinates.Frame( "est OCT Frame", worldCoordinates=(so3.from_rpy(self.min_res.x[0:3]), self.min_res.x[3:6])) vis.add("est OCT Frame", est_OCT_coordinate) Tlink_6 = model.coordinates.Frame( "Link 6 Frame", worldCoordinates=self.robot.link('link_6').getTransform()) vis.add('Link 6 Frame', Tlink_6) est_Tneedle_world = se3.mul( self.robot.link('link_6').getTransform(), (so3.from_rpy(self.min_res.x[6:9]), self.min_res.x[9:12])) est_needle_coordinate = model.coordinates.Frame( "est needle Frame", worldCoordinates=est_Tneedle_world) vis.add("est needle Frame", est_needle_coordinate) vis.spin(float('inf'))
""" Try to load the urdf of pr2. """ from klampt import WorldModel, vis world = WorldModel() world.readFile('pr2.urdf') vis.add('world', world) vis.spin(100)