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 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)
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 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( )
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", 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)
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)
from hpp.environments import Buggy from hpp.gepetto import PathPlayer robot = Buggy("buggy") robot.setJointBounds ("root_joint", [-5, 16, -4.5, 4.5, -1.01, 1.01, -1.01, 1.01]) from hpp.corbaserver import ProblemSolver ps = ProblemSolver (robot) from hpp.gepetto import ViewerFactory gui = ViewerFactory (ps) gui.loadObstacleModel ('hpp_environments', "scene", "scene") q_init = robot.getCurrentConfig () q_goal = q_init [::] q_init[0:2] = [-3.7, -4]; gui (q_init) q_goal [0:2] = [15,2] gui (q_goal) ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) ps.selectSteeringMethod ("ReedsShepp") ps.selectPathPlanner ("DiffusingPlanner") ps.addPathOptimizer ("RandomShortcut") t = ps.solve () print ("solving time", t)
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) #pp (pathId)
robot.client.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,-aMax,aMax,-aMax,aMax,0,0]) from hpp.corbaserver import ProblemSolver ps = ProblemSolver (robot) # define the velocity and acceleration bounds used by the steering method. This bounds will be stastified along the whole trajectory. ps.setParameter("Kinodynamic/velocityBound",vMax) ps.setParameter("Kinodynamic/accelerationBound",aMax) ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",100) # Uncomment the following line if you want to constraint the orientation of the base of the robot to follow the direction of the motion. Note that in this case the initial and final orientation are not considered. #ps.setParameter("Kinodynamic/forceOrientation",True) # The following line constraint the random sampling method to fix all the extraDOF at 0 during sampling. Comment it if you want to sample states with non-null velocity and acceleration. Note that it increase the complexity of the problem and greatly increase the computation time. ps.setParameter("ConfigurationShooter/sampleExtraDOF",False) from hpp.gepetto import ViewerFactory vf = ViewerFactory (ps) q_init = robot.getCurrentConfig () q_goal = q_init [::] 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]
from hpp.corbaserver.pr2 import Robot robot = Robot('pr2') robot.setJointBounds("root_joint", [-4, -3, -5, -3]) from hpp.corbaserver import ProblemSolver ps = ProblemSolver(robot) from hpp.gepetto import ViewerFactory vf = ViewerFactory(ps) q_init = robot.getCurrentConfig() q_goal = q_init[::] q_init[0:2] = [-3.2, -4] rank = robot.rankInConfiguration['torso_lift_joint'] q_init[rank] = 0.2 q_goal[0:2] = [-3.2, -4] rank = robot.rankInConfiguration['l_shoulder_lift_joint'] q_goal[rank] = 0.5 rank = robot.rankInConfiguration['l_elbow_flex_joint'] q_goal[rank] = -0.5 rank = robot.rankInConfiguration['r_shoulder_lift_joint'] q_goal[rank] = 0.5 rank = robot.rankInConfiguration['r_elbow_flex_joint'] q_goal[rank] = -0.5 vf.loadObstacleModel("package://hpp_tutorial/urdf/kitchen_area.urdf", "kitchen")
fullBody.setJointBounds("RF_KFE", [-1.4, 0.0]) fullBody.setJointBounds("LH_KFE", [0.0, 1.4]) fullBody.setJointBounds("RH_KFE", [0.0, 1.4]) fullBody.setJointBounds("root_joint", [-20, 20, -20, 20, -20, 20]) dict_heuristic = { fullBody.rLegId: "static", fullBody.lLegId: "static", fullBody.rArmId: "fixedStep04", fullBody.lArmId: "fixedStep04", } fullBody.loadAllLimbs(dict_heuristic, "ReferenceConfiguration", nbSamples=12) nbSamples = 1 ps = ProblemSolver(fullBody) vf = ViewerFactory(ps) v = vf.createViewer() rootName = "root_joint" zero = [0.0, 0.0, 0.0] rLegId = fullBody.rLegId rLeg = fullBody.rleg rfoot = fullBody.rfoot rLegOffset = fullBody.offset[:] lLegOffset = fullBody.offset[:] rArmOffset = fullBody.offset[:] lArmOffset = fullBody.offset[:] lLegId = fullBody.lLegId lLeg = fullBody.lleg lfoot = fullBody.lfoot
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 createRealClient(self, ViewerClass=Viewer, viewerClient=None): v = Parent.createRealClient(self, ViewerClass, viewerClient) return v
def __init__ (self, problemSolver) : Parent.__init__ (self, problemSolver)
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)
import datetime as dt totalTime = dt.timedelta(0) totalNumberNodes = 0 for i in range(args.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(args.N))) print("Average number nodes: " + str(totalNumberNodes / float(args.N))) if args.display: from hpp.gepetto import ViewerFactory vf = ViewerFactory(ps) #v = vf.createViewer() from hpp.gepetto import PathPlayer pp = PathPlayer(v, robot.client) if args.run: pp(0)
import CORBA robot = Robot ('ur5') cl = robot.client ps = ProblemSolver (robot) # q = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] 6 DoF# q1 = [0, -1.57, 1.57, 0, 0, 0]; q2 = [0.2, -1.57, -1.8, 0, 0.8, 0] q3 = [0, -1.57, 1.57, 3.267256451, 0, 0] q_init = q1; # q_goal = q2 q_goal = q3 from hpp.gepetto import ViewerFactory, PathPlayerGui vf = ViewerFactory (ps) # vf.loadObstacleModel ("ur_description","obstacles","obstacles") vf.loadObstacleModel ("ur_description","table","table") # vf.loadObstacleModel ("ur_description","wall","wall") vf(q1) ps.lockJoint("elbow_joint", [q1[2]]) ps.selectPathValidation("Progressive", .05) ps.setParameter("SplineGradientBased/alphaInit", CORBA.Any(CORBA.TC_double, 0.3)) ps.setParameter("SplineGradientBased/alwaysStopAtFirst", CORBA.Any(CORBA.TC_boolean, True)) ps.setParameter("SplineGradientBased/linearizeAtEachStep", CORBA.Any(CORBA.TC_boolean, False)) print "Optimizer parameters are:" print "alphaInit:", ps.getParameter("SplineGradientBased/alphaInit").value()
from hpp.environments import Buggy robot = Buggy("buggy") robot.setJointBounds ("base_joint_xy", [-5, 16, -4.5, 4.5]) from hpp.corbaserver import ProblemSolver ps = ProblemSolver (robot) from hpp.gepetto import ViewerFactory gui = ViewerFactory (ps) gui.loadObstacleModel ('hpp_environments', "scene", "scene") q_init = robot.getCurrentConfig () q_goal = q_init [::] q_init[0:2] = [-3.7, -4]; gui (q_init) q_goal [0:2] = [15,2] gui (q_goal) ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) ps.selectSteeringMethod ("ReedsShepp") ps.selectPathPlanner ("DiffusingPlanner") ps.addPathOptimizer ("RandomShortcut") t = ps.solve () print ("solving time", t)
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)
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")
joint3L_bounds_prev = fullBody.getJointBounds('leg_left_3_joint') joint1R_bounds_prev = fullBody.getJointBounds('leg_right_1_joint') joint3R_bounds_prev = fullBody.getJointBounds('leg_right_3_joint') fullBody.setJointBounds('leg_left_1_joint', [-0.2, 0.7]) fullBody.setJointBounds('leg_left_3_joint', [-1.3, 0.4]) fullBody.setJointBounds('leg_right_1_joint', [-0.7, 0.2]) fullBody.setJointBounds('leg_right_3_joint', [-1.3, 0.4]) # 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 createViewer (self, ViewerClass = Viewer, **kwargs): return Parent.createViewer (self, ViewerClass, **kwargs)
def __init__(self, problemSolver): Parent.__init__(self, problemSolver)
from hpp.corbaserver.pr2 import Robot robot = Robot('pr2') robot.setJointBounds("root_joint", [-4, -3, -5, -3]) from hpp.corbaserver import ProblemSolver ps = ProblemSolver(robot) from hpp.gepetto import ViewerFactory vf = ViewerFactory(ps) q_init = robot.getCurrentConfig() q_goal = q_init[::] q_init[0:2] = [-3.2, -4] rank = robot.rankInConfiguration['torso_lift_joint'] q_init[rank] = 0.2 q_goal[0:2] = [-3.2, -4] rank = robot.rankInConfiguration['l_shoulder_lift_joint'] q_goal[rank] = 0.5 rank = robot.rankInConfiguration['l_elbow_flex_joint'] q_goal[rank] = -0.5 rank = robot.rankInConfiguration['r_shoulder_lift_joint'] q_goal[rank] = 0.5 rank = robot.rankInConfiguration['r_elbow_flex_joint'] q_goal[rank] = -0.5 vf.loadObstacleModel("package://iai_maps/urdf/kitchen_area.urdf", "kitchen") ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal)
from hpp.corbaserver import * from hpp.corbaserver.nassime import RobotL #from hpp.corbaserver.nassime import RobotCursor robot = RobotL('robot_L', True) robot.setJointBounds("base_joint_xyz", [-2.5, 12, -7, 7, -7, 7]) robot.tf_root = 'base_link' from hpp.corbaserver import ProblemSolver ps = ProblemSolver(robot) from hpp.gepetto import ViewerFactory vf = ViewerFactory(ps) q_init = robot.getCurrentConfig() q_goal = q_init[::] q_init[0:3] = [0, 0, 0] vf(q_init) q_goal[0:7] = [10, 6, 0.11, 0.707460067, 0.706753314, 0, 0] q_goal[0:7] = [9.7, 6, 0.11, 0.707460067, 0.706753314, 0, 0] vf(q_goal) vf.loadObstacleModel("iai_maps", "env_complexe", "simple") ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) ps.clearRoadmap() ps.selectPathPlanner("interactive")
from hpp.corbaserver.pr2 import Robot robot = Robot('pr2') robot.setJointBounds("base_joint_xy", [-4, -3, -5, -3]) from hpp.corbaserver import ProblemSolver ps = ProblemSolver(robot) from hpp.gepetto import ViewerFactory vf = ViewerFactory(ps) q_init = robot.getCurrentConfig() q_goal = q_init[::] q_init[0:2] = [-3.2, -4] rank = robot.rankInConfiguration['torso_lift_joint'] q_init[rank] = 0.2 vf(q_init) q_goal[0:2] = [-3.2, -4] rank = robot.rankInConfiguration['l_shoulder_lift_joint'] q_goal[rank] = 0.5 rank = robot.rankInConfiguration['l_elbow_flex_joint'] q_goal[rank] = -0.5 rank = robot.rankInConfiguration['r_shoulder_lift_joint'] q_goal[rank] = 0.5 rank = robot.rankInConfiguration['r_elbow_flex_joint'] q_goal[rank] = -0.5 vf(q_goal) vf.loadObstacleModel("iai_maps", "kitchen_area", "kitchen") ps.setInitialConfig(q_init)
from hpp.corbaserver.practicals.ur5 import Robot from hpp.corbaserver import ProblemSolver, Client from hpp.gepetto import ViewerFactory, PathPlayer Client().problem.resetProblem() import readline import rlcompleter readline.parse_and_bind("tab: complete") robot = Robot('ur5') ps = ProblemSolver(robot) vf = ViewerFactory(ps) vf.loadObstacleModel( "package://hpp_practicals/urdf/ur_benchmark/obstacles.urdf", "obstacles") vf.loadObstacleModel("package://hpp_practicals/urdf/ur_benchmark/table.urdf", "table") vf.loadObstacleModel("package://hpp_practicals/urdf/ur_benchmark/wall.urdf", "wall") 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] ps.setInitialConfig(q2) ps.addGoalConfig(q3) from motion_planner import MotionPlanner m = MotionPlanner(robot, ps) pathId = m.solveBiRRT(maxIter=1000)
from talos_rbprm.talos import Robot from hpp.gepetto import Viewer import time from hpp.corbaserver import ProblemSolver from hpp.corbaserver.rbprm.rbprmstate import State, StateHelper import time fullBody = Robot() fullBody.setJointBounds("root_joint", [-5, 5, -1.5, 1.5, 0.95, 1.05]) fullBody.client.robot.setDimensionExtraConfigSpace(6) fullBody.client.robot.setExtraConfigSpaceBounds([0] * 12) ps = ProblemSolver(fullBody) from hpp.gepetto import ViewerFactory vf = ViewerFactory(ps) vf.loadObstacleModel("hpp_environments", "multicontact/table_140_70_73", "planning") q_ref = [ 0.0, 0.0, 1.0232773, 0.0, 0.0, 0.0, 1, #Free flyer 0.0, 0.0, -0.411354, 0.859395, -0.448041,
# hpp-tom If not, see # <http://www.gnu.org/licenses/>. from math import sqrt from hpp.corbaserver.tom import Robot from hpp.gepetto import PathPlayer #Robot.urdfName = "tom_full" robot = Robot ('tom') robot.setJointBounds ('base_joint_xy', [-1,1,-1,1]) from hpp.corbaserver import ProblemSolver ps = ProblemSolver (robot) from hpp.gepetto import ViewerFactory vf = ViewerFactory (ps) #r.loadObstacleModel ("tom_description", "table", "table") vf.loadObstacleModel ("hpp_tutorial", "box", "box") box_position = [.5,0,.5,1,0,0,0] table_position = (-0.90, -0.91, -0.42, .5, .5, .5, .5) #r.moveObstacle ("table/table_tom_link_0", table_position) #r.moveObstacle ("tom/box_tom_link_0", table_position) vf.moveObstacle ("box/base_link_0", box_position) q_init = robot.halfSitting [::] dict_goal = { 'l_shoulder_pan_joint': -1.62236446982, 'l_shoulder_lift_joint': -0.741943917854,
from hpp.corbaserver.pr2 import Robot robot = Robot ('pr2') robot.setJointBounds ("root_joint", [-4, -3, -5, -3]) from hpp.corbaserver import ProblemSolver ps = ProblemSolver (robot) from hpp.gepetto import ViewerFactory vf = ViewerFactory (ps) q_init = robot.getCurrentConfig () q_goal = q_init [::] q_init [0:2] = [-3.2, -4] rank = robot.rankInConfiguration ['torso_lift_joint'] q_init [rank] = 0.2 vf (q_init) q_goal [0:2] = [-3.2, -4] rank = robot.rankInConfiguration ['l_shoulder_lift_joint'] q_goal [rank] = 0.5 rank = robot.rankInConfiguration ['l_elbow_flex_joint'] q_goal [rank] = -0.5 rank = robot.rankInConfiguration ['r_shoulder_lift_joint'] q_goal [rank] = 0.5 rank = robot.rankInConfiguration ['r_elbow_flex_joint'] q_goal [rank] = -0.5 vf (q_goal) vf.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen") ps.setInitialConfig (q_init)
self.button.connect("clicked()", launch) 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)
from hpp.corbaserver.pr2 import Robot robot = Robot('pr2') robot.setJointBounds("base_joint_xy", [-4, -3, -5, -3]) from hpp.corbaserver import ProblemSolver ps = ProblemSolver(robot) from hpp.gepetto import ViewerFactory r = ViewerFactory(ps) q_init = robot.getCurrentConfig() q_goal = q_init[::] q_init[0:2] = [-3.2, -4] rank = robot.rankInConfiguration['torso_lift_joint'] q_init[rank] = 0.2 r(q_init) q_goal[0:2] = [-3.2, -4] rank = robot.rankInConfiguration['l_shoulder_lift_joint'] q_goal[rank] = 0.5 rank = robot.rankInConfiguration['l_elbow_flex_joint'] q_goal[rank] = -0.5 rank = robot.rankInConfiguration['r_shoulder_lift_joint'] q_goal[rank] = 0.5 rank = robot.rankInConfiguration['r_elbow_flex_joint'] q_goal[rank] = -0.5 r(q_goal)
from hpp.quadcopter import Robot robot = Robot("quad") robot.client.robot.setDimensionExtraConfigSpace(9) # robot.client.robot.setDimensionExtraConfigSpace(6) robot.setJointBounds ("base_joint_xyz", [-5, 16, -4.5, 4.5, 0, 0.6]) # robot.client.robot.setExtraConfigSpaceBounds([-1,1,-1,1,-1,1,-1,1,-1,1,-1,1]) robot.client.robot.setExtraConfigSpaceBounds([-1,1,-1,1,-1,1,-1,1,-1,1,-1,1,-1,1,-1,1,-1,1]) from hpp.corbaserver import ProblemSolver ps = ProblemSolver (robot) from hpp.gepetto import ViewerFactory, PathPlayer vf = ViewerFactory (ps) vf.loadObstacleModel ('hpp-quadcopter', "scene", "scene") q_init = robot.getCurrentConfig () q_init[2] = 0.5 q_goal = q_init [::] q_init [0:2] = [-3.7, -4]; vf (q_init) q_goal [0:2] = [15,2] vf (q_goal) q = q_init[:] q[0:2] = [2,0] vf(q) #~ ps.loadObstacleFromUrdf ("iai_maps", "kitchen_area", "")
from hpp.corbaserver.pr2 import Robot from hpp.gepetto import PathPlayer robot = Robot ('pr2') robot.setJointBounds ("base_joint_xy", [-4, -3, -5, -3]) from hpp.corbaserver import ProblemSolver ps = ProblemSolver (robot) from hpp.gepetto import ViewerFactory vf = ViewerFactory (ps) q_init = robot.getCurrentConfig () q_goal = q_init [::] q_init [0:2] = [-3.2, -4] rank = robot.rankInConfiguration ['torso_lift_joint'] q_init [rank] = 0.2 q_goal [0:2] = [-3.2, -4] rank = robot.rankInConfiguration ['l_shoulder_lift_joint'] q_goal [rank] = 0.5 rank = robot.rankInConfiguration ['l_elbow_flex_joint'] q_goal [rank] = -0.5 rank = robot.rankInConfiguration ['r_shoulder_lift_joint'] q_goal [rank] = 0.5 rank = robot.rankInConfiguration ['r_elbow_flex_joint'] q_goal [rank] = -0.5 vf.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen") ps.selectPathValidation ("Dichotomy", 0)
from hpp.corbaserver.fleche import Robot robot = Robot('fleche') robot.setJointBounds("root_joint", [-70, 4.375, -5, 4.375, -5, 7]) from hpp.corbaserver import ProblemSolver ps = ProblemSolver(robot) from hpp.gepetto import ViewerFactory vf = ViewerFactory(ps) q_init = robot.getCurrentConfig() q_goal = q_init[::] q_init[0:2] = [-60, -4] vf.loadObstacleModel("iai_maps", "ville1", "ville1_link") v = vf.createViewer() ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) ps.addPathOptimizer("RandomShortcut") print(ps.solve()) ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) ps.addPathOptimizer("RandomShortcut") from hpp.gepetto import PathPlayer pp = PathPlayer(v) pp.displayPath(1) #pp (0) import json a = ps.getWaypoints(1)
# Creating an instance of HPP problem solver ps = ProblemSolver(rbprmBuilder) # define parameters used by various methods : ps.setParameter("Kinodynamic/velocityBound", vMax) ps.setParameter("Kinodynamic/accelerationBound", aMax) ps.setParameter("DynamicPlanner/sizeFootX", 0.2) ps.setParameter("DynamicPlanner/sizeFootY", 0.12) ps.setParameter("DynamicPlanner/friction", 0.5) ps.setParameter("Kinodynamic/forceYawOrientation", True) # 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 = ""
# will be stastified along the whole trajectory. ps.setParameter("Kinodynamic/velocityBound", vMax) ps.setParameter("Kinodynamic/accelerationBound", aMax) ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 100) # Uncomment the following line if you want to constraint the orientation of the base of # the robot to follow the direction of the motion. Note that in this case the initial # and final orientation are not considered. # ps.setParameter("Kinodynamic/forceOrientation",True) # The following line constraint the random sampling method to fix all the extraDOF at 0 # during sampling. Comment it if you want to sample states with non-null velocity and # acceleration. Note that it increase the complexity of the problem and greatly increase # the computation time. ps.setParameter("ConfigurationShooter/sampleExtraDOF", False) vf = ViewerFactory(ps) q_init = robot.getCurrentConfig() q_goal = q_init[::] 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]