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)
Exemplo n.º 2
0
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
Exemplo n.º 3
0
 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)
Exemplo n.º 4
0
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
Exemplo n.º 5
0
 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)
Exemplo n.º 10
0
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)
Exemplo n.º 11
0
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]
Exemplo n.º 13
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")
Exemplo n.º 14
0
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
Exemplo n.º 15
0
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)
Exemplo n.º 18
0
    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)
Exemplo n.º 19
0
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)
Exemplo n.º 20
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()
Exemplo n.º 21
0
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)
Exemplo n.º 22
0
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)
Exemplo n.º 28
0
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")
Exemplo n.º 29
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
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)
Exemplo n.º 30
0
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)
Exemplo n.º 31
0
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,
Exemplo n.º 32
0
# 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)
Exemplo n.º 34
0
        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)
Exemplo n.º 35
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)
Exemplo n.º 36
0
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", "")
Exemplo n.º 37
0
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)
Exemplo n.º 38
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)
Exemplo n.º 39
0
# 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 = ""
Exemplo n.º 40
0
# 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]