def makeRobotProblemAndViewerFactory(corbaclient): robot = Robot("dev", "talos", rootJointType="freeflyer", client=corbaclient) robot.leftAnkle = "talos/leg_left_6_joint" robot.rightAnkle = "talos/leg_right_6_joint" robot.setJointBounds("talos/root_joint", [-1, 1, -1, 1, 0.5, 1.5]) ps = ProblemSolver(robot) ps.setRandomSeed(123) ps.selectPathProjector("Progressive", 0.2) ps.setErrorThreshold(1e-3) ps.setMaxIterProjection(40) ps.addPathOptimizer("SimpleTimeParameterization") vf = ViewerFactory(ps) vf.loadObjectModel(Object, "box") robot.setJointBounds("box/root_joint", [-1, 1, -1, 1, 0, 2]) vf.loadEnvironmentModel(Table, "table") return robot, ps, vf
rootJointType = 'freeflyer' packageName = 'hpp_tutorial' meshPackageName = 'hpp_tutorial' urdfName = 'cup' urdfSuffix = "" srdfSuffix = "" handles = [ "box/top", "box/bottom" ] robot = Robot ('dev', 'talos', rootJointType = "freeflyer") robot. leftAnkle = "talos/leg_left_6_joint" robot.rightAnkle = "talos/leg_right_6_joint" robot.setJointBounds ("talos/root_joint", [-1, 1, -1, 1, 0.5, 1.5]) ps = ProblemSolver (robot) ps.setRandomSeed(123) ps.selectPathProjector("Progressive", 0.2) ps.setErrorThreshold (1e-3) ps.setMaxIterProjection (40) ps.addPathOptimizer("SimpleTimeParameterization") vf = ViewerFactory (ps) Object = Box vf.loadObjectModel (Object, 'box') robot.setJointBounds ("box/root_joint", [-1, 1, -1, 1, 0, 2]) qq = [-0.7671778026566639, 0.0073267002287253635, 1.0035168727631776, -0.003341673452654457, -0.021566597515109524, 0.0002183620894239602, 0.9997618053357284, -0.00020053128587844821, -0.0021365695604276275, -0.4415951773681094, 0.9659230706255528, -0.48119003672520416, 0.007109157982067145, -0.00020095991543181877, -0.002126639473414498, -0.4382848597339842, 0.9589221865248464, -0.4774994711722908, 0.007099218648561522, 0.0, 0.025235347910697536, -0.06985947194357875, 0.0, -0.12446173084176845, -1.5808415926365578, 0.014333078135875619, -0.0806417043955706, -0.37124401660668394, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.25955282922987977, -0.1618313202464181, 0.002447883426630002, -0.5149037074691503, -0.00010703274362664899, 0.0008742582163227642, 0.10168585913285667, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.785398163397, 0.32250041955468695, -0.2569883469655496, 0.18577095561452217, 1.164388709412583, 0.0694401264431558, 0.5475575114527793, -0.11842286843715424, 0.8254301089264399] half_sitting = [ -0.74,0,1.0192720229567027,0,0,0,1, # root_joint
constraints=Constraints(numConstraints=com_constraint + foot_placement + arm_locked)) graph.initialize() ps.setParameter("SimpleTimeParameterization/safety", 0.5) ps.setParameter("SimpleTimeParameterization/order", 2) ps.setParameter("SimpleTimeParameterization/maxAcceleration", .25) ps.addPathOptimizer("RandomShortcut") ps.addPathOptimizer("SimpleTimeParameterization") res, q, err = graph.generateTargetConfig("starting_motion", initConf, initConf) if not res: raise RuntimeError('Failed to project initial configuration') ps.setRandomSeed(54) ps.setInitialConfig(initConf) ps.addGoalConfig(q) ps.solve() # Delete intermediate non optimized paths for i in range(ps.numberPaths() - 1): ps.erasePath(0) # Generate N random configurations N = args.N if N != 0: configs = [q[::]] configs += shootRandomConfigs(ps, graph, configs[0], N) configs = orderConfigurations(ps, configs) configs.append(initConf) else: