def makeRobotProblemAndViewerFactory(clients, rolling_table=True, rosParam=None): if rosParam is not None: import rospy, os, hpp HumanoidRobot.urdfString = rospy.get_param(rosParam) srdfFilename = hpp.retrieveRosResource(HumanoidRobot.srdfFilename) with open(srdfFilename, 'r') as f: HumanoidRobot.srdfString = f.read() objects = list() robot = HumanoidRobot("talos", "talos", rootJointType="freeflyer", client=clients) robot.leftAnkle = "talos/leg_left_6_joint" robot.rightAnkle = "talos/leg_right_6_joint" robot.setJointBounds("talos/root_joint", [-2, 2, -2, 2, 0, 2]) shrinkJointRange(robot, 0.95) ps = ProblemSolver(robot) ps.setErrorThreshold(1e-3) ps.setMaxIterProjection(40) ps.addPathOptimizer("EnforceTransitionSemantic") ps.addPathOptimizer("SimpleTimeParameterization") ps.selectPathValidation('Graph-Progressive', 0.01) vf = ViewerFactory(ps) objects.append(Box(name="box", vf=vf)) robot.setJointBounds("box/root_joint", [-2, 2, -2, 2, 0, 2]) # Loaded as an object to get the visual tags at the right position. # vf.loadEnvironmentModel (Table, 'table') if rolling_table: table = RollingTable(name="table", vf=vf) else: table = Table(name="table", vf=vf) robot.setJointBounds("table/root_joint", [-2, 2, -2, 2, -2, 2]) return robot, ps, vf, table, objects
q_goal_monotone = q_init[:] q_goal_monotone[rank:rank+3] = [ -0.545, 0.268, 0.7460100959532432] rank = robot.rankInConfiguration ['box2/base_joint_xyz'] q_goal_monotone[rank:rank+3] = [ -0.445, 0.268, 0.7460100959532432] q_goal_inverted=q_init[:] b1 = q_goal_inverted[rankB1:rankB1+7] q_goal_inverted[rankB1:rankB1+7] = q_goal_inverted[rankB2:rankB2+7] q_goal_inverted[rankB2:rankB2+7] = b1 # 3}}} robot.client.basic.problem.resetRoadmap () robot.client.basic.problem.setErrorThreshold (1e-3) robot.client.basic.problem.setMaxIterations (20) ps.selectPathValidation ('Discretized', 0.05) ps.selectPathProjector ('Progressive', 0.1) # ps.selectPathProjector ('Global', 0.2) # Create constraints. {{{3 from hpp.corbaserver.manipulation import ConstraintGraph cg = ConstraintGraph (robot, 'graph') # Create passive DOF lists {{{4 jointNames = dict () jointNames['all'] = robot.getJointNames () jointNames['baxter'] = list () jointNames['baxterRightSide'] = list () jointNames['baxterLeftSide'] = list () jointNames['box1'] = list () jointNames['box2'] = list ()
except: print("Did not find viewer") joint_bounds = {} def setRobotJointBounds(which): for jn, bound in joint_bounds[which]: robot.setJointBounds(jn, bound) joint_bounds["default"] = [ (jn, robot.getJointBounds(jn)) for jn in robot.jointNames ] shrinkJointRange(robot, 0.6) joint_bounds["grasp-generation"] = [ (jn, robot.getJointBounds(jn)) for jn in robot.jointNames ] setRobotJointBounds("default") shrinkJointRange(robot, 0.95) joint_bounds["planning"] = [ (jn, robot.getJointBounds(jn)) for jn in robot.jointNames ] ps.selectPathValidation("Graph-Discretized", 0.05) #ps.selectPathValidation("Graph-Dichotomy", 0) #ps.selectPathValidation("Graph-Progressive", 0.02) ps.selectPathProjector("Progressive", 0.2) ps.addPathOptimizer("EnforceTransitionSemantic") ps.addPathOptimizer("SimpleTimeParameterization") if isSimulation: ps.setMaxIterProjection (1) ps.setParameter("SimpleTimeParameterization/safety", 0.25) ps.setParameter("SimpleTimeParameterization/order", 2) ps.setParameter("SimpleTimeParameterization/maxAcceleration", 1.0) ps.setParameter("ManipulationPlanner/extendStep", 0.7) ps.setParameter("SteeringMethod/Carlike/turningRadius", 0.05) q0 = robot.getCurrentConfig()
vf.loadObstacleModel("package://gerard_bauzil/urdf/gerard_bauzil.urdf", "room") #vf.loadObstacleModel ("package://agimus_demos/urdf/P72-table.urdf", "table") # Display Tiago Field of view. #vf.guiRequest.append( (tiago_fov.loadInGui, {'self':None})) # Display visibility cones. vf.addCallback(tiago_fov_gui) try: v = vf.createViewer() except: print("Did not find viewer") shrinkJointRange(robot, 0.95) ps.selectPathValidation("Graph-Dichotomy", 0) ps.selectPathProjector("Progressive", 0.2) ps.addPathOptimizer("EnforceTransitionSemantic") ps.addPathOptimizer("SimpleTimeParameterization") if isSimulation: ps.setMaxIterProjection(1) ps.setParameter("SimpleTimeParameterization/safety", 0.25) ps.setParameter("SimpleTimeParameterization/order", 2) ps.setParameter("SimpleTimeParameterization/maxAcceleration", 1.0) ps.setParameter("ManipulationPlanner/extendStep", 0.7) ps.setParameter("SteeringMethod/Carlike/turningRadius", 0.05) q0 = robot.getCurrentConfig() q0[:4] = [0, -0.9, 0, 1] #q0[robot.rankInConfiguration['tiago/torso_lift_joint']] = 0.15
cg = ConstraintGraph.buildGenericGraph(robot, "graph", grippers, [ placard.name, ], handlesPerObjects, [ [], ], [], rules) # cg = ConstraintGraph (robot, "graph") # factory = ConstraintGraphFactory (cg) # factory.setGrippers (grippers) # factory.setObjects ([placard.name,], handlesPerObjects, [[],]) # factory.setRules (rules) # factory.generate () cg.addConstraints(graph=True, constraints=commonConstraints) ps.selectPathProjector("Progressive", .05) ps.selectPathValidation("Progressive", 0.025) cg.initialize() # Define initial and final configurations q_goal = [ -0.003429678026293006, 7.761615492429529e-05, 0.8333148411182841, -0.08000440760954532, 0.06905332841243099, -0.09070086400314036, 0.9902546570793265, 0.2097693637044623, 0.19739743868699455, -0.6079135018296973, 0.8508704420155889, -0.39897628829947995, -0.05274298289004072, 0.20772797293264825, 0.1846394290733244, -0.49824886682709824, 0.5042013065348324, -0.16158420369261683, -0.039828502509861335, -0.3827070014985058, -0.24118425356319423, 1.0157846623463191, 0.5637424355124602, -1.3378817283780955, -1.3151786907256797, -0.392409481224193, 0.11332560818107676, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.35936687035487364, -0.32595302056157444, -0.33115291290191723,