r.client.gui.setCameraTransform(0,camera)
"""
"""
r.client.gui.removeFromGroup("rm",r.sceneName)
r.client.gui.removeFromGroup("rmstart",r.sceneName)
r.client.gui.removeFromGroup("rmgoal",r.sceneName)
for i in range(0,ps.numberNodes()):
  r.client.gui.removeFromGroup("vecRM"+str(i),r.sceneName)

"""

# Playing the computed path
from hpp.gepetto import PathPlayer
pp = PathPlayer(rbprmBuilder.client.basic, r)
pp.dt = 0.03
pp.displayVelocityPath(0)
r.client.gui.setVisibility("path_0_root", "ALWAYS_ON_TOP")
#display path
pp.speed = 0.3
#pp (0)

#display path with post-optimisation

ps.client.problem.extractPath(0, 0, 1.95)
r.client.gui.removeFromGroup("path_0_root", r.sceneName)
pp.displayVelocityPath(1)
#pp (1)
"""
q_far = q_init[::]
q_far[2] = -3
r(q_far)
print "done planning, optimize path ..."
#v.solveAndDisplay('rm',2,0.005)
#for i in range(5):
#  ps.optimizePath(ps.numberPaths() -1)

pId_end = ps.numberPaths() - 1
### END after rubbles #####
pathId = pId_begin
ps.concatenatePath(pathId, pId_rubbles)
ps.concatenatePath(pathId, pId_end)

print "done optimizing."
from hpp.gepetto import PathPlayer
pp = PathPlayer(v)
pp.dt = 0.1
pp.displayVelocityPath(pathId)
v.client.gui.setVisibility("path_" + str(pathId) + "_root", "ALWAYS_ON_TOP")
pp.dt = 0.01

q_far = q_goal[::]
q_far[2] = -5
v(q_far)
q_init = q_init_0[::]

from numpy import arange

pathLength = ps.pathLength(pathId)  #length of the path
configs = []
#get configuration along the path
for s in arange(0, pathLength, 1.3):
    configs.append(ps.configAtParam(pathId, s))
예제 #3
0
 10.504343032836914,
 0.9323806762695312,
 0.36073973774909973,
 0.008668755181133747,
 0.02139890193939209]
r.client.gui.setCameraTransform(0,camera)
"""
"""
r.client.gui.removeFromGroup("rm",r.sceneName)
r.client.gui.removeFromGroup("rmstart",r.sceneName)
r.client.gui.removeFromGroup("rmgoal",r.sceneName)
for i in range(0,ps.numberNodes()):
  r.client.gui.removeFromGroup("vecRM"+str(i),r.sceneName)

"""
"""
# for seed 1486657707
ps.client.problem.extractPath(0,0,2.15) 

# Playing the computed path
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
pp.dt=0.03
pp.displayVelocityPath(1)
r.client.gui.setVisibility("path_1_root","ALWAYS_ON_TOP")
#display path
pp.speed=0.3
#pp (0)
"""
#display path with post-optimisation
"""
class AbstractPathPlanner:

    rbprmBuilder = None
    ps = None
    v = None
    afftool = None
    pp = None
    extra_dof_bounds = None
    robot_node_name = None  # name of the robot in the node list of the viewer

    def __init__(self):
        self.v_max = -1  # bounds on the linear velocity for the root, negative values mean unused
        self.a_max = -1  # bounds on the linear acceleration for the root, negative values mean unused
        self.root_translation_bounds = [
            0
        ] * 6  # bounds on the root translation position (-x, +x, -y, +y, -z, +z)
        self.root_rotation_bounds = [
            -3.14, 3.14, -0.01, 0.01, -0.01, 0.01
        ]  # bounds on the rotation of the root (-z, z, -y, y, -x, x)
        # The rotation bounds are only used during the random sampling, they are not enforced along the path
        self.extra_dof = 6  # number of extra config appended after the joints configuration, 6 to store linear root velocity and acceleration
        self.mu = 0.5  # friction coefficient between the robot and the environment
        self.used_limbs = [
        ]  # names of the limbs that must be in contact during all the motion
        self.size_foot_x = 0  # size of the feet along the x axis
        self.size_foot_y = 0  # size of the feet along the y axis
        self.q_init = []
        self.q_goal = []

    @abstractmethod
    def load_rbprm(self):
        """
        Build an rbprmBuilder instance for the correct robot and initialize it's extra config size
        """
        pass

    def set_configurations(self):
        self.rbprmBuilder.client.robot.setDimensionExtraConfigSpace(
            self.extra_dof)
        self.q_init = self.rbprmBuilder.getCurrentConfig()
        self.q_goal = self.rbprmBuilder.getCurrentConfig()
        self.q_init[2] = self.rbprmBuilder.ref_height
        self.q_goal[2] = self.rbprmBuilder.ref_height

    def compute_extra_config_bounds(self):
        """
        Compute extra dof bounds from the current values of v_max and a_max
        By default, set symmetrical bounds on x and y axis and bounds z axis values to 0
        """
        # bounds for the extradof : by default use v_max/a_max on x and y axis and 0 on z axis
        self.extra_dof_bounds = [
            -self.v_max, self.v_max, -self.v_max, self.v_max, 0, 0,
            -self.a_max, self.a_max, -self.a_max, self.a_max, 0, 0
        ]

    def set_joints_bounds(self):
        """
        Set the root translation and rotation bounds as well as the the extra dofs bounds
        """
        self.rbprmBuilder.setJointBounds("root_joint",
                                         self.root_translation_bounds)
        self.rbprmBuilder.boundSO3(self.root_rotation_bounds)
        self.rbprmBuilder.client.robot.setExtraConfigSpaceBounds(
            self.extra_dof_bounds)

    def set_rom_filters(self):
        """
        Define which ROM must be in collision at all time and with which kind of affordances
        By default it set all the roms in used_limbs to be in contact with 'support' affordances
        """
        self.rbprmBuilder.setFilter(self.used_limbs)
        for limb in self.used_limbs:
            self.rbprmBuilder.setAffordanceFilter(limb, ['Support'])

    def init_problem(self):
        """
        Load the robot, set the bounds and the ROM filters and then
        Create a ProblemSolver instance and set the default parameters.
        The values of v_max, a_max, mu, size_foot_x and size_foot_y must be defined before calling this method
        """
        self.load_rbprm()
        self.set_configurations()
        self.compute_extra_config_bounds()
        self.set_joints_bounds()
        self.set_rom_filters()
        self.ps = ProblemSolver(self.rbprmBuilder)
        # define parameters used by various methods :
        if self.v_max >= 0:
            self.ps.setParameter("Kinodynamic/velocityBound", self.v_max)
        if self.a_max >= 0:
            self.ps.setParameter("Kinodynamic/accelerationBound", self.a_max)
        if self.size_foot_x > 0:
            self.ps.setParameter("DynamicPlanner/sizeFootX", self.size_foot_x)
        if self.size_foot_y > 0:
            self.ps.setParameter("DynamicPlanner/sizeFootY", self.size_foot_y)
        self.ps.setParameter("DynamicPlanner/friction", 0.5)
        # sample only configuration with null velocity and acceleration :
        self.ps.setParameter("ConfigurationShooter/sampleExtraDOF", False)

    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 init_planner(self, kinodynamic=True, optimize=True):
        """
        Select the rbprm methods, and the kinodynamic ones if required
        :param kinodynamic: if True, also select the kinodynamic methods
        :param optimize: if True, add randomShortcut path optimizer (or randomShortcutDynamic if kinodynamic is also True)
        """
        self.ps.selectConfigurationShooter("RbprmShooter")
        self.ps.selectPathValidation("RbprmPathValidation", 0.05)
        if kinodynamic:
            self.ps.selectSteeringMethod("RBPRMKinodynamic")
            self.ps.selectDistance("Kinodynamic")
            self.ps.selectPathPlanner("DynamicPlanner")
        if optimize:
            if kinodynamic:
                self.ps.addPathOptimizer("RandomShortcutDynamic")
            else:
                self.ps.addPathOptimizer("RandomShortcut")

    def solve(self):
        """
        Solve the path planning problem.
        q_init and q_goal must have been defined before calling this method
        """
        if len(self.q_init) != self.rbprmBuilder.getConfigSize():
            raise ValueError(
                "Initial configuration vector do not have the right size")
        if len(self.q_goal) != self.rbprmBuilder.getConfigSize():
            raise ValueError(
                "Goal configuration vector do not have the right size")
        self.ps.setInitialConfig(self.q_init)
        self.ps.addGoalConfig(self.q_goal)
        self.v(self.q_init)
        t = self.ps.solve()
        print("Guide planning time : ", t)

    def display_path(self, path_id=-1, dt=0.1):
        """
        Display the path in the viewer, if no path specified display the last one
        :param path_id: the Id of the path specified, default to the most recent one
        :param dt: discretization step used to display the path (default to 0.1)
        """
        if self.pp is not None:
            if path_id < 0:
                path_id = self.ps.numberPaths() - 1
            self.pp.dt = dt
            self.pp.displayVelocityPath(path_id)

    def play_path(self, path_id=-1, dt=0.01):
        """
        play the path in the viewer, if no path specified display the last one
        :param path_id: the Id of the path specified, default to the most recent one
        :param dt: discretization step used to display the path (default to 0.01)
        """
        self.show_rom()
        if self.pp is not None:
            if path_id < 0:
                path_id = self.ps.numberPaths() - 1
            self.pp.dt = dt
            self.pp(path_id)

    def hide_rom(self):
        """
        Remove the current robot from the display
        """
        self.v.client.gui.setVisibility(self.robot_node_name, "OFF")

    def show_rom(self):
        """
        Add the current robot to the display
        """
        self.v.client.gui.setVisibility(self.robot_node_name, "ON")

    @abstractmethod
    def run(self):
        """
        Must be defined in the child class to run all the methods with the correct arguments.
        """
        # example of definition:
        """
        self.init_problem()
        # define initial and goal position
        self.q_init[:2] = [0, 0]
        self.q_goal[:2] = [1, 0]
        
        self.init_viewer("multicontact/ground", visualize_affordances=["Support"])
        self.init_planner()
        self.solve()
        self.display_path()
        self.play_path()
        """
        pass
예제 #5
0
 0.008668755181133747,
 0.02139890193939209]
r.client.gui.setCameraTransform(0,camera)
"""

q = [
    1.0220000000000002, -0.01, 0.6955999999999999, 1.0, 0.0, 0.0, 0.0, 0.0,
    0.0, 0.0, 0.0, 0.3, 0.0, -0.17000000000000023, 0.0, 0.0, 0.0
]  # BUG

# Playing the computed path
pi = ps.numberPaths() - 1
from hpp.gepetto import PathPlayer
pp = PathPlayer(rbprmBuilder.client.basic, r)
pp.dt = 0.03
pp.displayVelocityPath(pi)
r.client.gui.setVisibility("path_" + str(pi) + "_root", "ALWAYS_ON_TOP")
#display path
pp.speed = 1
#pp (pi)
"""
import parse_bench

parse_bench.parseBenchmark(t)
"""
print("path lenght = ",
      str(ps.client.problem.pathLength(ps.numberPaths() - 1)))

###########################
#display path with post-optimisation
"""
ps.selectDistance("KinodynamicDistance")
ps.selectPathPlanner("DynamicPlanner")
ps.addPathOptimizer("RandomShortcutDynamic")
ps.addPathOptimizer("OrientedPathOptimizer")

#solve the problem :
r(q_init)

#r.solveAndDisplay("rm",1,radiusSphere=0.01)

t = ps.solve()

from hpp.gepetto import PathPlayer
pp = PathPlayer(rbprmBuilder.client.basic, r)
pp.dt = 0.03
pp.displayVelocityPath(2)
r.client.gui.setVisibility("path_2_root", "ALWAYS_ON_TOP")

q_far = q_init[::]
q_far[2] = -3
r(q_far)
"""
camera = [0.6293167471885681,
 -9.560577392578125,
 10.504343032836914,
 0.9323806762695312,
 0.36073973774909973,
 0.008668755181133747,
 0.02139890193939209]
r.client.gui.setCameraTransform(0,camera)
"""
예제 #7
0
"""

t = ps.solve()

#r.displayRoadmap('rm',radiusSphere=0.01)
#r.displayPathMap("pm",0)

#tf=r.solveAndDisplay("rm",1,0.01)
#t = [0,0,tf,0]
#r.client.gui.removeFromGroup("rm_group",r.sceneName)

# Playing the computed path
from hpp.gepetto import PathPlayer
pp = PathPlayer(rbprmBuilder.client.basic, r)
pp.dt = 0.03
pp.displayVelocityPath(ps.numberPaths() - 1)
#r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
#display path
pp.speed = 0.5
#pp (0)

import parse_bench

parse_bench.parseBenchmark(t)
print "path lenght = ", str(ps.client.problem.pathLength(ps.numberPaths() - 1))

###########################
#display path with post-optimisation
"""
print("Press Enter to display the optimization ...")
raw_input()
# 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")

#Alternatively, use the following line instead of ps.solve() to display the roadmap as it's computed (note that it slow down a lot the computation)
#v.solveAndDisplay('rm',1)

# Highlight the solution path used in the roadmap
v.displayPathMap('pm',0)

# remove the roadmap for the scene : 
#v.client.gui.removeFromGroup("rm",v.sceneName)
#v.client.gui.removeFromGroup("pm",v.sceneName)


# Connect to a viewer server and play solution paths
from hpp.gepetto import PathPlayer
pp = PathPlayer (v)
#play path before optimization
pp (0)

# Display the optimized path, with a color variation depending on the velocity along the path (green for null velocity, red for maximal velocity)
pp.dt=0.1
pp.displayVelocityPath(1)
# play path after random shortcut
pp.dt=0.001
pp (1)