def init_viewer(self, env_name, env_package="hpp_environments", reduce_sizes=[0, 0, 0], visualize_affordances=[]): """ Build an instance of hpp-gepetto-viewer from the current problemSolver :param env_name: name of the urdf describing the environment :param env_package: name of the package containing this urdf (default to hpp_environments) :param reduce_sizes: Distance used to reduce the affordances plan toward the center of the plane (in order to avoid putting contacts closes to the edges of the surface) :param visualize_affordances: list of affordances type to visualize, default to none """ vf = ViewerFactory(self.ps) self.afftool = AffordanceTool() self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) self.afftool.loadObstacleModel("package://" + env_package + "/urdf/" + env_name + ".urdf", "planning", vf, reduceSizes=reduce_sizes) self.v = vf.createViewer(ghost=True, displayArrows=True) self.pp = PathPlayer(self.v) for aff_type in visualize_affordances: self.afftool.visualiseAffordances(aff_type, self.v, self.v.color.lightBrown)
def initScene(Robot, envName="multicontact/ground", genLimbsDB=True): from hpp.gepetto import Viewer, ViewerFactory from hpp.corbaserver.rbprm.rbprmfullbody import FullBody from hpp.corbaserver import ProblemSolver fullBody = Robot() fullBody.client.robot.setDimensionExtraConfigSpace(6) fullBody.setJointBounds("root_joint", [-100, 100, -100, 100, -100, 100]) fullBody.client.robot.setExtraConfigSpaceBounds( [-100, 100, -100, 100, -100, 100, -100, 100, -100, 100, -100, 100]) fullBody.setReferenceConfig(fullBody.referenceConfig[::] + [0] * 6) fullBody.setPostureWeights(fullBody.postureWeights[::] + [0] * 6) try: if genLimbsDB: fullBody.loadAllLimbs("static", nbSamples=100) else: fullBody.loadAllLimbs("static", nbSamples=1) except AttributeError: print( "WARNING initScene : fullBody do not have loadAllLimbs, some scripts may fails." ) ps = ProblemSolver(fullBody) vf = ViewerFactory(ps) vf.loadObstacleModel( "package://hpp_environments/urdf/" + envName + ".urdf", "planning") v = vf.createViewer(ghost=True, displayCoM=True) v(fullBody.getCurrentConfig()) return fullBody, v
def createViewer(self, ViewerClass=Viewer, viewerClient=None, host=None, collisionURDF=False): if host is not None and viewerClient is None: from gepetto.corbaserver import Client as GuiClient viewerClient = GuiClient(host=host) v = Parent.createViewer(self, ViewerClass, viewerClient, host, collisionURDF) return v
def initScene(Robot, envName="multicontact/ground"): from hpp.gepetto import Viewer, ViewerFactory from hpp.corbaserver.rbprm.rbprmfullbody import FullBody from hpp.corbaserver import ProblemSolver fullBody = Robot() fullBody.client.robot.setDimensionExtraConfigSpace(6) try: fullBody.loadAllLimbs("static", nbSamples=1) except AttributeError: print "WARNING initScene : fullBody do not have loadAllLimbs, some scripts may fails." ps = ProblemSolver(fullBody) vf = ViewerFactory(ps) vf.loadObstacleModel("hpp_environments", envName, "planning") v = vf.createViewer(displayCoM=True) v(fullBody.getCurrentConfig()) return fullBody, v
def initScene(Robot, envName="multicontact/ground", genLimbsDB=True, context=None): """ Create and initialize a hpp.gepetto.Viewer object with a hpp.corbaserver.rbprm.FullBody object :param Robot: a Robot configuration class (eg. the class defined in talos_rbprm.talos.Robot) :param envName: the name of the environment file to load. Load a flat floor at z = 0 by default Currently only work with environments from the hpp-environment package :param genLimbsDB: If True (default value) generate the limbs database of the fullbody object :param context: An optional string that give a name to a corba context instance :return: a Fullbody object and a Viewer object """ from hpp.gepetto import ViewerFactory fullBody, ps = build_fullbody(Robot, genLimbsDB, context) vf = ViewerFactory(ps) vf.loadObstacleModel( "package://hpp_environments/urdf/" + envName + ".urdf", "planning") v = vf.createViewer(ghost=True, displayCoM=True) v(fullBody.getCurrentConfig()) return fullBody, v
def init_viewer(self, env_name, env_package="hpp_environments", reduce_sizes=[0, 0, 0], visualize_affordances=[], min_area=None): """ Build an instance of hpp-gepetto-viewer from the current problemSolver :param env_name: name of the urdf describing the environment :param env_package: name of the package containing this urdf (default to hpp_environments) :param reduce_sizes: Distance used to reduce the affordances plan toward the center of the plane (in order to avoid putting contacts closes to the edges of the surface) :param visualize_affordances: list of affordances type to visualize, default to none :param min_area: list of couple [affordanceType, size]. If provided set the minimal area for each affordance """ vf = ViewerFactory(self.ps) if self.context: self.afftool = AffordanceTool(context=self.context) self.afftool.client.affordance.affordance.resetAffordanceConfig( ) # FIXME: this should be called in afftool constructor else: self.afftool = AffordanceTool() self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) if min_area is not None: for (aff_type, min_size) in min_area: self.afftool.setMinimumArea(aff_type, min_size) self.afftool.loadObstacleModel("package://" + env_package + "/urdf/" + env_name + ".urdf", "planning", vf, reduceSizes=reduce_sizes) self.v = vf.createViewer(ghost=True, displayArrows=True) self.pp = PathPlayer(self.v) for aff_type in visualize_affordances: self.afftool.visualiseAffordances(aff_type, self.v, self.v.color.lightBrown)
raise RuntimeError ("Failed to apply constraint.") ps.selectPathValidation ("Progressive", 0.025) import datetime as dt totalTime = dt.timedelta (0) totalNumberNodes = 0 N = 20 for i in range (N): ps.client.problem.clearRoadmap () ps.resetGoalConfigs () ps.setInitialConfig (q1proj) ps.addGoalConfig (q2proj) t1 = dt.datetime.now () ps.solve () t2 = dt.datetime.now () totalTime += t2 - t1 print (t2-t1) n = len (ps.client.problem.nodes ()) totalNumberNodes += n print ("Number nodes: " + str(n)) print ("Average time: " + str ((totalTime.seconds+1e-6*totalTime.microseconds)/float (N))) print ("Average number nodes: " + str (totalNumberNodes/float (N))) from hpp.gepetto import ViewerFactory vf = ViewerFactory (ps) r = vf.createViewer() from hpp.gepetto import PathPlayer pp = PathPlayer (robot.client, r)
q_goal [rank] = -0.5 vf.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen") ps.selectPathValidation ("Progressive", 0.025) import datetime as dt totalTime = dt.timedelta (0) totalNumberNodes = 0 N = 20 for i in range (N): ps.clearRoadmap () ps.resetGoalConfigs () ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) t1 = dt.datetime.now () ps.solve () t2 = dt.datetime.now () totalTime += t2 - t1 print (t2-t1) n = len (ps.client.problem.nodes ()) totalNumberNodes += n print ("Number nodes: " + str(n)) print ("Average time: " + str ((totalTime.seconds+1e-6*totalTime.microseconds)/float (N))) print ("Average number nodes: " + str (totalNumberNodes/float (N))) r = vf.createViewer(); r (q_init) pp = PathPlayer (robot.client, r)
class HppServerInitializer(HppClient): subscribersDict = { "hpp": { "reset": [Bool, "reset"], "create_viewer": [Empty, "_createViewer"], "configure": [Empty, "configure"], "load_environment": [String, "loadEnvironment"], }, } def __init__(self): super(HppServerInitializer, self).__init__() self.subscribers = self._createTopics("", self.subscribersDict, True) self.setHppUrl() self.robot = None self.ps = None self.tfListener = TransformListener() self.world_frame = "/world" def reset(self, msg): self.initialize() self.loadRobot() if msg.data: self.loadEnvironment(param="/environment/description", paramName="/environment/name") self.configure() try: self.createViewer() except: rospy.loginfo("Could not reach the Gepetto-viewer") def initialize(self): self.setHppUrl() hpp = self._hpp() # Reset the problem if necessary if self.robot is not None: hpp.problem.resetProblem() self.robot = None self.ps = None self.vf = None def loadRobot(self): hppClient = self._hpp() # Load the robot robotName = rospy.get_param("/robot_name", "robot") rootJointType = rospy.get_param("/robot_root_joint_type", "anchor") urdfString = rospy.get_param("/robot_description") srdfString = rospy.get_param("/robot_description_semantic", "") hppClient.robot.loadRobotModelFromString(robotName, rootJointType, urdfString, srdfString) self.robot = hpp.corbaserver.robot.Robot(robotName, rootJointType, client=hppClient, load=False) self.ps = hpp.corbaserver.ProblemSolver(self.robot) self.vf = ViewerFactory(self.ps) self.vf.loadRobot(urdfString) # Set the root joint position in the world. base = self.robot.getLinkNames("root_joint")[0] p, q = self.tfListener.lookupTransform(self.world_frame, base, rospy.Time(0)) self.robot.setJointPosition("root_joint", p + q) def loadEnvironment(self, param=None, xmlString=None, paramName=None, name=None): if name is None: name = rospy.get_param( paramName) if paramName is not None else "obstacle" if param is not None: self.vf.loadObstacleModel("", rospy.get_param(param), name) elif xmlString is not None: self.vf.loadObstacleModel("", xmlString, name) def configure(self): hpp = self._hpp() # Setup the planner setParameters(hpp, "", rospy.get_param("/hpp/parameters", dict())) if rospy.has_param("/hpp/path_validation/method"): ps.selectPathValidation( rospy.get_param("/hpp/path_validation/method"), rospy.get_param("/hpp/path_validation/tolerance", 0.05)) self.ps.clearPathOptimizers() if rospy.has_param("/hpp/path_optimizers"): pathOpts = rospy.get_param("/hpp/path_optimizers", ["RandomShortcut"]) if isinstance(pathOpts, list): for n in pathOpts: self.ps.addPathOptimizer(n) else: rospy.logerr( "Parameter /hpp/path_optimizers shoud be a list of strings." ) def _createViewer(self, msg): self.createViewer() def createViewer(self, *args, **kwargs): host = rospy.get_param("/gepetto_viewer/host", "localhost") try: return self.vf.createViewer(host=host, *args, **kwargs) except: rospy.loginfo("Could not reach the Gepetto-viewer")
def createViewer(self, ViewerClass=Viewer, *args, **kwargs): return Parent.createViewer(self, ViewerClass, *args, **kwargs)
from hpp.corbaserver.practicals.ur5 import Robot from hpp.corbaserver import ProblemSolver from hpp.gepetto import ViewerFactory, PathPlayer robot = Robot('ur5') ps = ProblemSolver(robot) vf = ViewerFactory(ps) vf.loadObstacleModel("hpp_practicals", "ur_benchmark/obstacles", "obstacles") vf.loadObstacleModel("hpp_practicals", "ur_benchmark/table", "table") vf.loadObstacleModel("hpp_practicals", "ur_benchmark/wall", "wall") v = vf.createViewer() q1 = [0, -1.57, 1.57, 0, 0, 0] q2 = [0.2, -1.57, -1.8, 0, 0.8, 0] q3 = [1.57, -1.57, -1.8, 0, 0.8, 0] v(q2) v(q3) ps.setInitialConfig(q2) ps.addGoalConfig(q3) from motion_planner import MotionPlanner m = MotionPlanner(robot, ps) pathId = m.solveBiRRT(maxIter=1000) pp = PathPlayer(v)
def end(): gameWidget.end() sys.argv = [] gui = ViewerClient() gui.gui.createWindow("ia") gui.gui.createWindow("player") iaRobot = Buggy("buggy_ia") ips = ProblemSolver(iaRobot) ivf = ViewerFactory(ips) ivf.loadObstacleModel('hpp_environments', "scene", "scene_ia") iv = ivf.createViewer(viewerClient=gui) iaRobot.client.problem.selectProblem("player") playerRobot = Buggy("buggy_player") pps = ProblemSolver(playerRobot) pvf = ViewerFactory(pps) pvf.loadObstacleModel('hpp_environments', "scene", "scene_player") pvf(playerRobot.client.robot.getCurrentConfig()) pv = pvf.createViewer(viewerClient=gui) gui.gui.addSceneToWindow("buggy_player", 1) gui.gui.addSceneToWindow("scene_player", 1) gui.gui.addSceneToWindow("buggy_ia", 0) gui.gui.addSceneToWindow("scene_ia", 0) # To launch the game gameTimer = QtCore.QTimer()
def createViewer (self, ViewerClass = Viewer, **kwargs): return Parent.createViewer (self, ViewerClass, **kwargs)
q_init [0:3] = [6.5,-4,0.4] #root position q_init[3:7] = [0,0,0,1] #root rotation #set initial velocity (along x,y,z axis) : q_init[-6:-3]=[0,0,0] #q_goal[0:3] = [6.5,-1,0.4] # straight line q_goal [0:3] = [3,-4,0.4] # easy goal position #q_goal[0:3]=[-4.5,-4.8,0.4]# harder goal position #set goal velocity (along x,y,z axis) : q_goal[-6:-3]=[0,0,0] vf.loadObstacleModel ("iai_maps", "room", "room") # with displayArrow parameter the viewer will display velocity and acceleration of the center of the robot with 3D arrow. The length scale with the amplitude with a length of 1 for the maximal amplitude v = vf.createViewer(displayArrows = True) ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) ps.addPathOptimizer ("RandomShortcut") #select kinodynamic methods : ps.selectSteeringMethod("Kinodynamic") ps.selectDistance("Kinodynamic") # the Kinodynamic steering method require a planner that build directionnal roadmap (with oriented edges) as the trajectories cannot always be reversed. ps.selectPathPlanner("BiRRTPlanner") print (ps.solve ()) # display the computed roadmap. Note that the edges are all represented as straight line and may not show the real motion of the robot between the nodes : v.displayRoadmap("rm")
# add the 6 extraDof for velocity and acceleration (see *_path.py script) fullBody.client.robot.setDimensionExtraConfigSpace(6) vMax = 0.5 # linear velocity bound for the root aMax = 0.5 # linear acceleration bound for the root fullBody.client.robot.setExtraConfigSpaceBounds( [-vMax, vMax, -vMax, vMax, 0, 0, -aMax, aMax, -aMax, aMax, 0, 0]) ps = ProblemSolver(fullBody) vf = ViewerFactory(ps) ps.setParameter("Kinodynamic/velocityBound", vMax) ps.setParameter("Kinodynamic/accelerationBound", aMax) ps.setRandomSeed(random.SystemRandom().randint(0, 999999)) #load the viewer try: v = vf.createViewer(displayCoM=True) except Exception: print("No viewer started !") class FakeViewer(): sceneName = "" def __init__(self): return def __call__(self, q): return def addLandmark(self, a, b): return
ps.lockJoint ('base_joint_xy', [0,0]) ps.lockJoint ('base_joint_rz', [1,0]) res = ps.applyConstraints (q_init) if res [0]: q_init = res [1] else: raise RuntimeError ('Failed to apply constraints on init config.') res = ps.applyConstraints (q_goal) if res [0]: q_goal = res [1] else: raise RuntimeError ('Failed to apply constraints on goal config.') robot.setCurrentConfig (q_init) r = vf.createViewer (collisionURDF = True) r (q_init) ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) ps.selectPathPlanner ("DiffusingPlanner") ps.selectPathValidation("Progressive", 0.01) #ps.selectPathValidation("Progressive", 0.05) #ps.readRoadmap ('/home/florent/devel/fiad/data/roadmap.rdm') # ps.solve () pp = PathPlayer (ps.client, r)
q_goal [rank] = -0.5 vf.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen") ps.selectPathValidation ("Dichotomy", 0) import datetime as dt totalTime = dt.timedelta (0) totalNumberNodes = 0 N = 20 for i in range (N): ps.clearRoadmap () ps.resetGoalConfigs () ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) t1 = dt.datetime.now () ps.solve () t2 = dt.datetime.now () totalTime += t2 - t1 print (t2-t1) n = len (ps.client.problem.nodes ()) totalNumberNodes += n print ("Number nodes: " + str(n)) print ("Average time: " + str ((totalTime.seconds+1e-6*totalTime.microseconds)/float (N))) print ("Average number nodes: " + str (totalNumberNodes/float (N))) r = vf.createViewer(); r (q_init) pp = PathPlayer (robot.client, r)
def play(): gameWidget.update() def end(): gameWidget.end() sys.argv = [] gui = ViewerClient() gui.gui.createWindow("ia") gui.gui.createWindow("player") iaRobot = Buggy("buggy_ia") ips = ProblemSolver(iaRobot) ivf = ViewerFactory(ips) ivf.loadObstacleModel('hpp_environments', "scene", "scene_ia") iv = ivf.createViewer(viewerClient = gui) iaRobot.client.problem.selectProblem("player") playerRobot = Buggy("buggy_player") pps = ProblemSolver(playerRobot) pvf = ViewerFactory(pps) pvf.loadObstacleModel('hpp_environments', "scene", "scene_player") pvf(playerRobot.client.robot.getCurrentConfig()) pv = pvf.createViewer(viewerClient = gui) gui.gui.addSceneToWindow("buggy_player", 1) gui.gui.addSceneToWindow("scene_player", 1) gui.gui.addSceneToWindow("buggy_ia", 0) gui.gui.addSceneToWindow("scene_ia", 0) # To launch the game gameTimer = QtCore.QTimer()
# sample only configuration with null velocity and acceleration : ps.setParameter("ConfigurationShooter/sampleExtraDOF", False) # initialize the viewer : from hpp.gepetto import ViewerFactory vf = ViewerFactory(ps) # load the module to analyse the environnement and compute the possible contact surfaces from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) afftool.loadObstacleModel("hpp_environments", "multicontact/ground", "planning", vf) #load the viewer try: v = vf.createViewer(displayArrows=True) except Exception: print "No viewer started !" class FakeViewer(): sceneName = "" def __init__(self): return def __call__(self, q): return def addLandmark(self, a, b): return
class Platform(): # main_agent = None agents = [] # problem solver ps = None # path player pp = None # view factory vf = None # viewer r = None env = None # a dictionary to get the agent's index index_dic = {} #for tree searching tree = None # the root of the three current_node = None # pp = PathPlayer (rbprmBuilder.client.basic,ls r) def __init__(self, agents): self.agents = agents for i in range(len(agents)): a = agents[i] self.index_dic[agents[i].robot.name] = i self.agents[i].registerPlatform(self, i) print 'the agent ', agents[ i].robot.name, ' is now registered with the index ', self.getInidex( agents[i].robot.name) self.tree = self.getStartNode() self.current_node = self.tree def getStartNode(self): init_configs = [] for a in self.agents: init_configs.append(a.start_config) return Node(init_configs) def start(self): # self.problem.selectProblem(0) self.ps = ProblemSolver(self.agents[0].robot) self.vf = ViewerFactory(self.ps) if self.env != None: self.vf.loadObstacleModel(self.env.packageName, self.env.urdfName, self.env.name) self.r = self.vf.createViewer() for a in self.agents: a.startDefaultSolver() a.setBounds() a.setEnvironment() a.solve() a.storePath() # self.loadAgentView(a.index) # self.r(a.start_config) print 'the agent ', a.robot.name, ' now has a backup plan of length', a.getProposedPlanLength( ) # self.pp = PathPlayer (self.agents[0], self.r) def loadAgentView(self, index, default=False): #default position or not self.ps = self.agents[index - 1].ps self.vf = ViewerFactory(self.ps) # self.vf.loadObstacleModel(self.env.packageName, self.env.urdfName, self.env.name) self.r = self.vf.createViewer() # print '---------------->', len(self.agents[index - 1].init_config) if default: self.r(self.agents[index - 1].current_config) # self.r.computeObjectPosition() def getInidex(self, robot_name): return self.index_dic[robot_name] def setEnvironment(self, env): self.env = env # self.ps.moveObstacle('airbase_link_0', [0,0, -3, 1,0,0,0]) # self.r = self.vf.createViewer() # def startViewer(self): # self.r = vf.createViewer() def updateViewAtTime(self, t): config = [] for a in self.agents: config.append(a.getConfigOfProposedPlanAtTime(t)) self.r(config) def playAllProposedPath(self): print 'play proposed path' max_time = 0 for a in self.agents: l = a.getProposedPlanLength() if l > max_time: max_time = l for t in range(max_time): # print 'time is ', t for i in range(len(self.agents)): a = self.agents[i] if a.getProposedPlanLength() > t: # print 'agent ', a.index, self.loadAgentView(i + 1) # and then set the agent to its current configuration self.r(a.getConfigOfProposedPlanAtTime(t)) # sleep(0.003) def playAllPermittedPath(self): max_time = 0 for a in self.agents: l = a.getPermittedPlanLength() if l > max_time: max_time = l for t in range(max_time): # print 'time is ', t for i in range(len(self.agents)): a = self.agents[i] if a.getPermittedPlanLength() > t: # print 'agent ', a.index, self.loadAgentView(i + 1) # and then set the agent to its current configuration self.r(a.getConfigOfPermittedPlanAtTime(t)) # sleep(0.003) def validateAllPaths(self, agents_remained): print '******* start validation **********' # print agents_remained max_time = 0 for i in agents_remained: a = self.agents[i] a.startDefaultSolver() a.setBounds() a.setEnvironment() a.loadOtherAgents() l = a.getProposedPlanLength() if l > max_time: max_time = l for t in range(max_time): print '\n this is time ', t for i in agents_remained: a = self.agents[i] a.startDefaultSolver() a.setBounds() a.setEnvironment() a.loadOtherAgents() # print 'this is robot ', a.robot.name # a1.obstacle.getObstacleNames(False, 1000) if a.getProposedPlanLength() > t: # myconfig = a.getConfigOfProposedPlanAtTime(t) # myspec = a.getMoveSpecification(myconfig) # print 'the agent is at ', myspec[0], myspec[1] # first of all, move all the obstacles for oa in self.agents: # other agents if a.index != oa.index: # print '\t and moving the ghost of ', oa.robot.name if not (oa.index in agents_remained ) or oa.getProposedPlanLength() <= t: config = oa.end_config else: config = oa.getConfigOfProposedPlanAtTime(t) spec = oa.getMoveSpecification(config) a.obstacle.moveObstacle( oa.robot.name + 'base_link_0', spec) print '\tmove ghost', oa.robot.name, ' to ', spec[ 0], spec[1] # secondly, test if the configuration is valid (result, _) = a.robot.isConfigValid( a.getConfigOfProposedPlanAtTime(t)) if not result: return t # if everything is fine at each time slot, return -1 return -1 def construct_tree(self, iteration): print '******************* this is iternation ', iteration, ' ***********************' self.current_node.printInformation() if iteration > 0: #expand the tree by doing planning for each agent and find the collision momment for i in self.current_node.getAgentsRemained(): a = self.agents[i] print '>>>>>>>>>>>>this is agent', a.robot.name, ' computing ' if (a.computePlan(self.current_node) == -1): print 'the agent is now using its backup plan/plan from last time' # line = input() # self.playAllProposedPath() t = self.validateAllPaths(self.current_node.getAgentsRemained()) print 'in this iteration, the collision appears at time ', t if t == -1: # the path is valid! we terminate the process paths = [] indexes_and_paths = [] for i in self.current_node.agent_remained: a = self.agents[i] indexes_and_paths.append((a.index, a.obtainProposedPlan())) child = self.current_node.expand(indexes_and_paths, []) return (True, child.paths) else: reached = [] indexes_and_paths = [] for i in self.current_node.getAgentsRemained(): a = self.agents[i] if a.getProposedPlanLength( ) > t - 1: # up to t, because t is the moment of collision! path = a.obtainProposedPlan()[:t - 1] indexes_and_paths.append((i, path)) else: reached.append( i ) # reached, therefore remove from the remaining list indexes_and_paths.append((i, a.obtainProposedPlan())) self.current_node = self.current_node.expand( indexes_and_paths, reached) return self.construct_tree(iteration - 1) else: # can not find a path for each agent within limited iteration return (False, None) # remove those who has already got to where they suppose to be # at least one step ----------- frangment