Example #1
0
if not res[0]:
    raise Exception('Goal configuration could not be projected.')
q_goal = res[1]

import datetime as dt
totalTime = dt.timedelta(0)
totalNumberNodes = 0
for i in range(args.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 = ps.numberNodes()
    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:
    v = vf.createViewer()
    pp = PathPlayer(v, robot.client.basic)
    if args.run:
        pp(0)
Example #2
0
    },
    lLegId: {
        'file': "hrp2/LL_com.ineq",
        'effector': 'LLEG_JOINT5'
    },
    rarmId: {
        'file': "hrp2/RA_com.ineq",
        'effector': rHand
    }
}
#~ larmId : {'file': "hrp2/LA_com.ineq", 'effector' : lHand} }

#~ fullBody.limbRRTFromRootPath(0,len(configs)-1,0,2)
from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean, stats, saveAllData, play_traj
from hpp.gepetto import PathPlayer
pp = PathPlayer(fullBody.client.basic, r)


def act(i,
        numOptim=0,
        use_window=0,
        friction=0.5,
        optim_effectors=True,
        verbose=False,
        draw=False):
    return step(fullBody,
                configs,
                i,
                numOptim,
                pp,
                limbsCOMConstraints,
# Choosing RBPRM shooter and path validation methods.
ps.selectConfigurationShooter("RbprmShooter")
ps.selectPathValidation("RbprmPathValidation",0.05)
# Choosing kinodynamic methods :
#ps.selectSteeringMethod("RBPRMKinodynamic")
#ps.selectDistance("Kinodynamic")
#ps.selectPathPlanner("DynamicPlanner")

# Solve the planning problem :
t = ps.solve ()
print "Guide planning time : ",t

try :
    # display solution : 
    from hpp.gepetto import PathPlayer
    pp = PathPlayer (v)
    pp.dt=0.1
    pp.displayPath(0)#pp.displayVelocityPath(0) #
    #v.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
    pp.dt=0.01
    #pp(0)
except Exception:
    pass

# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
v(q_far)

r = Viewer(ps)

q_init = rbprmBuilder.getCurrentConfig()
q_init[0:3] = [-1, -0.82, 0.5]
rbprmBuilder.setCurrentConfig(q_init)
r(q_init)

q_goal = q_init[::]
q_goal[3:7] = [0., 0., 0.14943813, 0.98877108]
q_goal[0:3] = [1.49, -0.65, 1.2]
r(q_goal)
ps.addPathOptimizer("RandomShortcut")
#~ ps.setInitialConfig (q_init)
#~ ps.addGoalConfig (q_goal)

from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
#~ afftool.loadObstacleModel (packageName, "stair_bauzil", "planning", r)
afftool.loadObstacleModel(packageName, "bauzil_stairs_10cm", "planning", r)
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])

ps.client.problem.selectConfigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation", 0.05)
#~ t = ps.solve ()

from hpp.gepetto import PathPlayer
pp = PathPlayer(r)
#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~
Example #5
0
ps.selectConfigurationShooter("RbprmShooter")
ps.addPathOptimizer("RandomShortcutDynamic")
ps.selectPathValidation("RbprmPathValidation", 0.05)
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("Kinodynamic")
ps.selectPathPlanner("DynamicPlanner")

# Solve the planning problem :

t = ps.solve()
print "Guide planning time : ", t
#pId = ps.numberPaths()-1
pId = 0

# display solution :
try:
    from hpp.gepetto import PathPlayer
    pp = PathPlayer(v)
    pp.dt = 0.1
    pp.displayVelocityPath(pId)
    v.client.gui.setVisibility("path_0_root", "ALWAYS_ON_TOP")
    pp.dt = 0.01
    pp(pId)
except Exception:
    pass
# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
v(q_far)
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver

robot = Robot ('puzzle') # object5
robot.setJointBounds('base_joint_xyz', [-0.9, 0.9, -0.9, 0.9, -1., 1.])
#robot.setJointBounds('base_joint_xyz', [-0.6, 0.6, -0.6, 0.6, -0.3, 1.0])
ps = ProblemSolver (robot)
cl = robot.client

q1 = [0.0, 0.0, 0.8, 1.0, 0.0, 0.0, 0.0]; q2 = [0.0, 0.0, -0.8, 1.0, 0.0, 0.0, 0.0]
#q1 = [0.0, 0.0, 0.8, 1.0, 0.0, 0.0, 0.0]; q2 = [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0] # simpler
#q1 = [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]; q2 = [0.0, 0.0, -0.8, 1.0, 0.0, 0.0, 0.0] # simpler2

from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
#r.loadObstacleModel ("puzzle_description","decor_very_easy","decor_very_easy")
r.loadObstacleModel ("puzzle_description","decor_easy","decor_easy")
r(q2) # r(q1)

#q1bis = q2; q2bis = [0.0, 0.0, -0.8, 1.0, 0.0, 0.0, 0.0]
#ps.resetGoalConfigs (); ps.setInitialConfig (q1bis); ps.addGoalConfig (q2bis); ps.solve ()
# ps.resetGoalConfigs (); ps.setInitialConfig (q1); ps.addGoalConfig (q2bis); ps.solve ()
#i = ps.numberPaths()-1

ps.setInitialConfig (q1); ps.addGoalConfig (q2)

ps.selectPathPlanner ("VisibilityPrmPlanner")
#ps.selectPathValidation ("Dichotomy", 0.)
#ps.saveRoadmap ('/local/mcampana/devel/hpp/data/puzzle_veryEasy_PRM.rdm')
#ps.readRoadmap ('/local/mcampana/devel/hpp/data/puzzle_easy_RRT.rdm')
ps.selectPathPlanner("DynamicPlanner")

r(q_init)

ps.client.problem.prepareSolveStepByStep()

ps.client.problem.finishSolveStepByStep()

#i = 0
#r.displayRoadmap("rm"+str(i),0.02)
#ps.client.problem.executeOneStep() ;i = i+1; r.displayRoadmap("rm"+str(i),0.02) ; r.client.gui.removeFromGroup("rm"+str(i-1),r.sceneName) ;
#t = ps.solve ()

# Playing the computed path
from hpp.gepetto import PathPlayer
pp = PathPlayer(rbprmBuilder.client.basic, r)
pp.dt = 1. / 30.
#r.client.gui.removeFromGroup("rm0",r.sceneName)
pp.displayVelocityPath(0)
pp.speed = 0.2
pp(0)
"""

#r.client.gui.removeFromGroup("rm",r.sceneName)
r.client.gui.removeFromGroup("rmPath",r.sceneName)
r.client.gui.removeFromGroup("path_1_root",r.sceneName)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")


math.sqrt((np.linalg.norm(u)*np.linalg.norm(u)) * (np.linalg.norm(v)*np.linalg.norm(v))
# You can use the colors defined in viewer.color or give an array of 4 float which define a normalized RGBA color 
v.displayRoadmap("rmR",0.02,1,v.color.blue,v.color.lightBlue,'r_gripper_tool_joint')
v.displayRoadmap("rmL",0.02,1,v.color.green,v.color.lightGreen,'l_gripper_tool_joint')


# hide roadmap in the scene
#v.client.gui.removeFromGroup("rmL",v.sceneName)
#v.client.gui.removeFromGroup("rmR",v.sceneName)



# alternative method : replace ps.solve() and v.displayRoadmap() with :
# v.solveAndDisplay("rmL1",2,0.02,1,v.color.green,v.color.lightGreen,'l_gripper_tool_joint')
# v.displayRoadmap( "rmR1"  ,0.02,1,v.color.blue ,v.color.lightBlue ,'r_gripper_tool_joint')
# solveAndDisplay show the construction of the roadmap, the first int set the refresh rate of the display (here, every 2 iteration of the algorithm)
################################################################

pp = PathPlayer (robot.client, v)
#display path
# The edge of the roadmap are always straight line, if you want to display the accurate trajectory of a body of the robot, you should use the following method :
#  displayPath(pathId, color(default = green), jointName (default = root)
pp.displayPath(0     , v.color.lightBlack    ,'r_gripper_tool_joint')
pp (0)
#display path after optimisation
pp.displayPath(1,v.color.lightRed,'r_gripper_tool_joint')
pp (1)




def generateConstrainedBezierTraj(cfg,
                                  time_interval,
                                  placement_init,
                                  placement_end,
                                  numTry,
                                  q_t=None,
                                  phase_previous=None,
                                  phase=None,
                                  phase_next=None,
                                  fullBody=None,
                                  eeName=None,
                                  viewer=None):
    if numTry == 0:
        return generateSmoothBezierTraj(cfg, time_interval, placement_init,
                                        placement_end)
    else:
        if not q_t or not phase_previous or not phase or not phase_next or not fullBody or not eeName:
            raise ValueError(
                "Cannot compute LimbRRTOptimizedTraj for try >= 1 without optionnal arguments"
            )

    predef_curves = generatePredefBeziers(cfg, time_interval, placement_init,
                                          placement_end)
    bezier_takeoff = predef_curves.curves[predef_curves.idFirstNonZero()]
    bezier_landing = predef_curves.curves[predef_curves.idLastNonZero()]
    id_middle = int(math.floor(len(predef_curves.curves) / 2.))
    pos_init = bezier_takeoff(bezier_takeoff.max())
    pos_end = bezier_landing(0)
    if VERBOSE:
        print("bezier takeoff end : ", pos_init)
        print("bezier landing init : ", pos_end)
    t_begin = predef_curves.times[id_middle]
    t_middle = predef_curves.curves[id_middle].max()
    t_end = t_begin + t_middle
    if VERBOSE:
        print("t begin : ", t_begin)
        print("t end   : ", t_end)
    q_init = q_t[:, int(t_begin / cfg.IK_dt)]  # after the predef takeoff
    q_end = q_t[:, int(t_end / cfg.IK_dt)]
    # compute limb-rrt path :
    pathId = limb_rrt.generateLimbRRTPath(q_init, q_end, phase_previous, phase,
                                          phase_next, fullBody)

    if viewer and cfg.DISPLAY_FEET_TRAJ and DISPLAY_RRT:
        from hpp.gepetto import PathPlayer
        pp = PathPlayer(viewer)
        if DISPLAY_JOINT_LEVEL:
            pp.displayPath(pathId, jointName=fullBody.getLinkNames(eeName)[0])
        else:
            #TODO
            pp.displayPath(
                pathId,
                jointName=fullBody.getLinkNames(eeName)[0],
                offset=cfg.Robot.dict_offset[eeName].translation.tolist())

    # compute constraints for the end effector trajectories :
    pData = bezier_com.ProblemData()
    pData.c0_ = bezier_takeoff(bezier_takeoff.max())
    pData.dc0_ = bezier_takeoff.derivate(bezier_takeoff.max(), 1)
    pData.ddc0_ = bezier_takeoff.derivate(bezier_takeoff.max(), 2)
    pData.j0_ = bezier_takeoff.derivate(bezier_takeoff.max(), 3)
    pData.c1_ = bezier_landing(0)
    pData.dc1_ = bezier_landing.derivate(0, 1)
    pData.ddc1_ = bezier_landing.derivate(0, 2)
    pData.j1_ = bezier_landing.derivate(0, 3)
    pData.constraints_.flag_ = bezier_com.ConstraintFlag.INIT_POS | bezier_com.ConstraintFlag.INIT_VEL | bezier_com.ConstraintFlag.INIT_ACC | bezier_com.ConstraintFlag.END_ACC | bezier_com.ConstraintFlag.END_VEL | bezier_com.ConstraintFlag.END_POS | bezier_com.ConstraintFlag.INIT_JERK | bezier_com.ConstraintFlag.END_JERK

    # now compute additional inequalities around the rrt path :
    pDef = computeProblemConstraints(pData, fullBody, pathId, t_middle, eeName,
                                     viewer)
    solved = False
    # loop and increase the number of variable control point until a solution is found
    flagData = pData.constraints_.flag_
    flags = [
        bezier_com.ConstraintFlag.ONE_FREE_VAR, None,
        bezier_com.ConstraintFlag.THREE_FREE_VAR, None,
        bezier_com.ConstraintFlag.FIVE_FREE_VAR
    ]
    numVars = 1
    while not solved and numVars <= 5:
        pData.constraints_.flag_ = flagData | flags[numVars - 1]
        ineqEff = bezier_com.computeEndEffectorConstraints(pData, t_middle)
        Hg = bezier_com.computeEndEffectorVelocityCost(pData, t_middle)
        res = bezier_com.computeEndEffector(
            pData, t_middle)  #only used for comparison/debug ?
        bezier_unconstrained = res.c_of_t
        pDef.degree = bezier_unconstrained.degree
        ineqConstraints = curves.generate_problem(pDef)

        # _ prefix = previous notation (in bezier_com_traj)
        # min        x' H x + 2 g' x
        # subject to A*x <= b
        _A = np.vstack([ineqEff.A, ineqConstraints.A])
        _b = np.vstack([ineqEff.b, ineqConstraints.b])
        _H = Hg.A
        _g = Hg.b
        #_H = ineqConstraints.cost.A
        #_g = ineqConstraints.cost.b

        # quadprog notation :
        #min (1/2)x' P x + q' x
        #subject to  G x <= h
        #subject to  C x  = d
        G = _A
        h = _b.reshape((-1))
        P = _H * 2.
        q = (_g * 2.).flatten()
        try:
            if VERBOSE:
                print("try to solve quadprog with " + str(numVars) +
                      " variable")
            res = quadprog_solve_qp(P, q, G, h)
            solved = True
        except ValueError:
            if VERBOSE:
                print("Failed.")
            numVars += 2
    if solved:
        if VERBOSE:
            print("Succeed.")
    else:
        print(
            "Constrained End effector Bezier method failed for all numbers of variables control points."
        )
        print("Return predef trajectory (may be in collision).")
        return

    # retrieve the result of quadprog and create a bezier curve :
    vars = np.split(res, numVars)
    wps = bezier_unconstrained.waypoints()
    id_firstVar = 4
    i = id_firstVar
    for x in vars:
        wps[:, i] = x
        i += 1

    bezier_middle = curves.bezier(wps, 0., t_middle)
    # create concatenation with takeoff/landing
    curves = predef_curves.curves[::]
    curves[id_middle] = bezier_middle
    pBezier = PolyBezier(curves)
    if VERBOSE:
        print("time interval     = ", time_interval[1] - time_interval[0])
        print("polybezier length = ", pBezier.max())
    ref_traj = trajectories.BezierTrajectory(pBezier, placement_init,
                                             placement_end, time_interval)
    return ref_traj
Example #10
0
ps.addPathOptimizer("RandomShortcutDynamic")
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("Kinodynamic")
ps.selectPathPlanner("DynamicPlanner")

t = ps.solve()
print("Guide planning time : ", t)
#v.solveAndDisplay("rm",10,0.01)

for i in range(20):
    print("Optimize path, " + str(i + 1) + "/20 ... ")
    ps.optimizePath(ps.numberPaths() - 1)
pathId = ps.numberPaths() - 1

# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer(v)
pp.dt = 0.01
pp.displayPath(pathId)
#v.client.gui.setVisibility("path_"+str(pathId)+"_root","ALWAYS_ON_TOP")
pp.dt = 0.01
#pp(pathId)

#v.client.gui.writeNodeFile("path_"+str(pathId)+"_root","guide_path_maze_hard.obj")

# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
v(q_far)
ps.addGoalConfig (q_goal)

# Choosing RBPRM shooter and path validation methods.
ps.selectConfigurationShooter("RbprmShooter")
ps.addPathOptimizer ("RandomShortcutDynamic")
ps.selectPathValidation("RbprmPathValidation",0.05)
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("Kinodynamic")
ps.selectPathPlanner("DynamicPlanner")

# Solve the planning problem :
t = ps.solve ()
print "Guide planning time : ",t


# display solution : 
from hpp.gepetto import PathPlayer
pp = PathPlayer (v)
pp.dt=0.1
#pp.displayVelocityPath(0)
v.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
pp.dt=0.01
#pp(0)

# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
v(q_far)

Example #12
0
 0.02139890193939209]
r.client.gui.setCameraTransform(0,camera)
"""

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
"""
#ps.client.problem.executeOneStep() ;i = i+1; r.displayRoadmap("rm"+str(i),0.02) ; r.client.gui.removeFromGroup("rm"+str(i-1),r.sceneName) ;
#t = ps.solve ()

r.solveAndDisplay("rm",1,0.02)


#t = ps.solve ()

#r.displayRoadmap("rm",0.02)


r.displayPathMap("rmPath",0,0.02)


from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)

pp.displayPath(0,r.color.lightGreen)
pp(0)


pp.displayPath(1,blue)
r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")

pp.displayPath(1,black)
pp (1)

#r.client.gui.removeFromGroup("rm",r.sceneName)
r.client.gui.removeFromGroup("rmPath",r.sceneName)
r.client.gui.removeFromGroup("path_1_root",r.sceneName)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
Example #14
0
afftool.loadObstacleModel(packageName, "plane_hole", "planning", r)
#~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])

# Choosing RBPRM shooter and path validation methods.
# Note that the standard RRT algorithm is used.
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation", 0.05)

# Solve the problem
t = ps.solve()
if isinstance(t, list):
    t = t[0] * 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]

# Playing the computed path
from hpp.gepetto import PathPlayer
pp = PathPlayer(rbprmBuilder.client.basic, r)

q_far = q_init[::]
q_far[0:3] = [-2, -3, 0.63]
r(q_far)

for i in range(1, 10):
    rbprmBuilder.client.basic.problem.optimizePath(i)

from hpp.corbaserver import Client
from hpp.corbaserver.robot import Robot as Parent


class Robot(Parent):
    rootJointType = 'freeflyer'
    packageName = 'hpp-rbprm-corba'
Example #15
0
def generateLimbRRTOptimizedTraj(time_interval,
                                 placement_init,
                                 placement_end,
                                 numTry,
                                 q_t=None,
                                 phase_previous=None,
                                 phase=None,
                                 phase_next=None,
                                 fullBody=None,
                                 eeName=None,
                                 viewer=None):
    if numTry == 0:
        return generateSmoothBezierTraj(time_interval, placement_init,
                                        placement_end)
    else:
        if q_t is None or phase_previous is None or phase is None or phase_next is None or not fullBody or not eeName:
            raise ValueError(
                "Cannot compute LimbRRTOptimizedTraj for try >= 1 without optionnal arguments"
            )
    if cfg.EFF_T_PREDEF > 0:
        predef_curves = generatePredefBeziers(time_interval, placement_init,
                                              placement_end)
    else:
        predef_curves = generateSmoothBezierTraj(time_interval, placement_init,
                                                 placement_end).curves
    id_middle = int(math.floor(len(predef_curves.curves) / 2.))
    predef_middle = predef_curves.curves[id_middle]
    pos_init = predef_middle(0)
    pos_end = predef_middle(predef_middle.max())
    if VERBOSE:
        print "generateLimbRRTOptimizedTraj, try number " + str(numTry)
        print "bezier takeoff end : ", pos_init
        print "bezier landing init : ", pos_end
    t_begin = predef_curves.times[id_middle]
    t_middle = predef_middle.max()
    t_end = t_begin + t_middle
    if VERBOSE:
        print "t begin : ", t_begin
        print "t end   : ", t_end
    q_init = q_t[:, int(math.floor(t_begin /
                                   cfg.IK_dt))]  # after the predef takeoff
    id_end = int(math.ceil(t_end / cfg.IK_dt)) - 1
    if id_end >= q_t.shape[
            1]:  # FIXME : why does it happen ? usually it's == to the size when the bug occur
        id_end = q_t.shape[1] - 1
    q_end = q_t[:, id_end]
    global current_limbRRT_id
    # compute new limb-rrt path if needed:
    if not current_limbRRT_id or (numTry in recompute_rrt_at_tries):
        current_limbRRT_id = generateLimbRRTPath(q_init, q_end, phase_previous,
                                                 phase, phase_next, fullBody)
        if viewer and cfg.DISPLAY_FEET_TRAJ and DISPLAY_RRT_PATH:
            from hpp.gepetto import PathPlayer
            pp = PathPlayer(viewer)
            pp.displayPath(
                current_limbRRT_id,
                jointName=fullBody.getLinkNames(eeName)[0],
                offset=cfg.Robot.dict_offset[eeName].translation.T.tolist()[0])

    # find weight and number of variable to use from the numTry :
    for offset in reversed(recompute_rrt_at_tries):
        if numTry >= offset:
            id = numTry - offset
            break
    if VERBOSE:
        print "weights_var id = ", id
    if id >= len(weights_vars):
        raise ValueError(
            "Max number of try allow to find a collision-end effector trajectory reached."
        )
    weight = weights_vars[id][0]
    varFlag = weights_vars[id][1]
    numVars = weights_vars[id][2]
    if VERBOSE:
        print "use weight " + str(weight) + " with num free var = " + str(
            numVars)
    # compute constraints for the end effector trajectories :
    pData = bezier_com.ProblemData()
    pData.c0_ = predef_middle(0)
    pData.dc0_ = predef_middle.derivate(0, 1)
    pData.ddc0_ = predef_middle.derivate(0, 2)
    pData.j0_ = predef_middle.derivate(0, 3)
    pData.c1_ = predef_middle(predef_middle.max())
    pData.dc1_ = predef_middle.derivate(predef_middle.max(), 1)
    pData.ddc1_ = predef_middle.derivate(predef_middle.max(), 2)
    pData.j1_ = predef_middle.derivate(predef_middle.max(), 3)
    pData.constraints_.flag_ = bezier_com.ConstraintFlag.INIT_POS | bezier_com.ConstraintFlag.INIT_VEL | bezier_com.ConstraintFlag.INIT_ACC | bezier_com.ConstraintFlag.END_ACC | bezier_com.ConstraintFlag.END_VEL | bezier_com.ConstraintFlag.END_POS | bezier_com.ConstraintFlag.INIT_JERK | bezier_com.ConstraintFlag.END_JERK | varFlag
    Constraints = bezier_com.computeEndEffectorConstraints(pData, t_middle)
    Cost_smooth = bezier_com.computeEndEffectorVelocityCost(pData, t_middle)
    Cost_distance = computeDistanceCostMatrices(fullBody, current_limbRRT_id,
                                                pData, t_middle, eeName)
    # formulate QP matrices :
    # _ prefix = previous notation (in bezier_com_traj)
    # min        x' H x + 2 g' x
    # subject to A*x <= b
    _A = Constraints.A
    _b = Constraints.b
    _H = ((1. - weight) * Cost_smooth.A + weight * Cost_distance.A)
    _g = ((1. - weight) * Cost_smooth.b + weight * Cost_distance.b)
    if VERBOSE > 1:
        print "A = ", _A
        print "b = ", _b
        print "H = ", _H
        print "h = ", _g
    """  
    _A = np.array(_A)
    _b = np.array(_b)
    _H = np.array(_H)
    _g = np.array(_g)
    """

    # quadprog notation :
    #min (1/2)x' P x + q' x
    #subject to  G x <= h
    #subject to  C x  = d
    G = _A
    h = _b.flatten().T  # remove the transpose when working with array
    P = _H * 2.
    q = (_g * 2.).flatten().T

    if VERBOSE > 1:
        print "G = ", G
        print "h = ", h
        print "P = ", P
        print "q = ", q
        print "Shapes : "
        print "G : ", G.shape
        print "h : ", h.shape
        print "P : ", P.shape
        print "q : ", q.shape

    # solve the QP :
    solved = False
    try:
        res = quadprog_solve_qp(P, q, G, h)
        solved = True
    except ValueError, e:
        print "Quadprog error : "
        print e.message
        raise ValueError(
            "Quadprog failed to solve QP for optimized limb-RRT end-effector trajectory, for try number "
            + str(numTry))
# Script which goes with gravity.launch, to simulate Hrp2 in the space with a spaceship from a movie and an emu character from Nasa.gov. See gravity_description package.

from hpp.gepetto import Viewer, PathPlayer
from hpp.corbaserver.hrp2 import Robot
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver import Client

#Robot.urdfSuffix = '_capsule'
#Robot.srdfSuffix= '_capsule'
robot = Robot ('hrp2_14')
#robot.setJointBounds('base_joint_xyz', [-5, 10, -10, 10, -5, 5])
robot.setJointBounds('base_joint_xyz', [-3, 10, -4, 4, -3, 5])
ps = ProblemSolver (robot)
cl = robot.client
r = Viewer (ps)
pp = PathPlayer (cl, r)
#r.loadObstacleModel ("gravity_description","emu","emu")
r.loadObstacleModel ("gravity_description","gravity_decor","gravity_decor")

# Difficult init config
q1 = [1.45, 1.05, -0.8, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.8, 1.0, -1.0, -0.85, 0.0, -0.65, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -1.9, 0.0, -0.6, -0.3, 0.7, -0.4, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.1, -0.15, -0.1, 0.3, -0.418879, 0.0, 0.0, 0.3, -0.8, 0.3, 0.0, 0.0]

q2 = [6.55, -2.91, 1.605, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.2, 1.0, -0.4, -1.0, 0.0, -0.2, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -1.5, -0.2, 0.1, -0.3, 0.1, 0.1, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -0.2, 0.6, -0.453786, 0.872665, -0.418879, 0.2, -0.4, 0.0, -0.453786, 0.1, 0.7, 0.0]

q2hard = [7.60, -2.41, 0.545, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.8, 0.0, -0.4, -0.55, 0.0, -0.6, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -2.8, 0.0, 0.1, -0.2, -0.1, 0.4, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -0.2, 0.6, -0.1, 1.2, -0.4, 0.2, -0.3, 0.0, -0.4, 0.2, 0.7, 0.0]

robot.isConfigValid(q1)
robot.isConfigValid(q2)

# qf should be invalid
qf = [1, -3, 3, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.2, 1.0, -0.4, -1.0, 0.0, -0.2, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -1.5, -0.2, 0.1, -0.3, 0.1, 0.1, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -0.2, 0.6, -0.453786, 0.872665, -0.418879, 0.2, -0.4, 0.0, -0.453786, 0.1, 0.7, 0.0]
Example #17
0
#~ V0list = tp.V0list
#~ Vimplist = tp.Vimplist
base_joint_xyz_limits = tp.base_joint_xyz_limits

fullBody = FullBody()
robot = fullBody.client.basic.robot
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName,
                           packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds("base_joint_xyz", base_joint_xyz_limits)

#psf = ProblemSolver(fullBody); rr = Viewer (psf); gui = rr.client.gui
ps = path_planner.ProblemSolver(fullBody)
r = path_planner.Viewer(ps, viewerClient=path_planner.r.client)
rr = r
#~ psf = tp.ProblemSolver( fullBody ); rr = tp.Viewer (psf); gui = rr.client.gui
pp = PathPlayer(fullBody.client.basic, rr)
pp.speed = 0.6
q_0 = fullBody.getCurrentConfig()
rr(q_0)

rLegId = 'RFoot'
lLegId = 'LFoot'
rarmId = 'RHand'
larmId = 'LHand'
rfoot = 'SpidermanRFootSphere'
lfoot = 'SpidermanLFootSphere'
lHand = 'SpidermanLHandSphere'
rHand = 'SpidermanRHandSphere'
nbSamples = 50000
x = 0.03
y = 0.08
success1, q_touch_3, residual_error1 = graph.applyNodeConstraints("talos/left_gripper grasps desk/touchpoint_left_front", q_init) # Collision between legs
success2, q_touch_4, residual_error2 = graph.applyNodeConstraints("talos/left_gripper grasps desk/touchpoint_left_drawer", q_init) # fail

# q_touch_1 redefinition
q_touch_1 = coda.q_touch_1

# q_touch_2 redefinition
q_touch_2 = coda.q_touch_2

# q_touch_4 redefinition
q_touch_4 = coda.q_touch_4

ps.addPathOptimizer("Graph-RandomShortcut")

from hpp.gepetto import PathPlayer
pp = PathPlayer(v, ps.client.basic)

def solveAll():
	# trajectory 1
	ps.setInitialConfig(q_init)
	ps.addGoalConfig(q_touch_1)
	graph.setWeight("talos/right_gripper > desk/touchpoint_right_front | f", 0)
	graph.setWeight("talos/left_gripper > desk/touchpoint_left_front | f", 0)
	graph.setWeight("talos/left_gripper > desk/touchpoint_left_drawer | f", 0)
	graph.setWeight("talos/right_gripper > desk/upper_drawer_spherical_handle | f", 0)
	print ps.solve()

	# trajectory 2
	ps.clearRoadmap()
	ps.resetGoalConfigs()
	ps.setInitialConfig(q_touch_1)
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("KinodynamicDistance")
ps.selectPathPlanner("DynamicPlanner")
ps.selectPathProjector('Progressive', 0.05)
#solve the problem :
r(q_init)

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

t = ps.solve()

tPlanning = time.time() - tStart

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")
"""
if isinstance(t, list):
	t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
f = open('log.txt', 'a')
f.write("path computation " + str(t) + "\n")
f.close()
"""
"""
for i in range(0,9):
  t = ps.solve()
  if isinstance(t, list):
    ts = t[0]* 3600. + t[1] * 60. + t[2] + t[3]/1000.	
Example #20
0
q_init = fullBody.generateContacts(q_init, [0, 0, 1])
q_goal = fullBody.generateContacts(q_goal, [0, 0, 1])

# specifying the full body configurations as start and goal state of the problem
fullBody.setStartState(
    q_init,
    [fullBody.rLegId, fullBody.lArmId, fullBody.lLegId, fullBody.rArmId])
fullBody.setEndState(
    q_goal,
    [fullBody.rLegId, fullBody.lArmId, fullBody.lLegId, fullBody.rArmId])

v(q_init)
configs = []

from hpp.gepetto import PathPlayer
pp = PathPlayer(fullBody.client, v)

import time

#DEMO METHODS


def initConfig():
    v.client.gui.setVisibility("hyq", "ON")
    tp.cl.problem.selectProblem("default")
    tp.v.client.gui.setVisibility("toto", "OFF")
    tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
    v(q_init)


def endConfig():
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)
with open('roadmap.txt', 'w') as f:
    for i in a[0]:
        json.dump(i, f)
        f.write("\n")
ps.clearPathOptimizers()
ps.addPathOptimizer("GradientBased")
ps.optimizePath(0)
ps.numberPaths()
ps.pathLength(ps.numberPaths() - 1)

pp(ps.numberPaths() - 1)

ps.clearPathOptimizers()

len(ps.getWaypoints(0))

from hpp.gepetto import Viewer, PathPlayer
r = Viewer(ps)
pp = PathPlayer(cl, r)
r.loadObstacleModel("potential_description", "obstacles_concaves",
                    "obstacles_concaves")

import numpy as np
dt = 0.1
nPath = 0
points = []
for t in np.arange(0., cl.problem.pathLength(nPath), dt):
    points.append([
        cl.problem.configAtParam(nPath, t)[0],
        cl.problem.configAtParam(nPath, t)[1], 0
    ])

r.client.gui.addCurve("initCurvPath", points, [1, 0.3, 0, 1])
r.client.gui.addToGroup("initCurvPath", r.sceneName)
Example #23
0
q_goal_proj = res[1]

ps.setInitialConfig(q_init_proj)
ps.addGoalConfig(q_goal_proj)

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_proj)
    ps.addGoalConfig(q_goal_proj)
    t1 = dt.datetime.now()
    ps.solve()
    t2 = dt.datetime.now()
    totalTime += t2 - t1
    print(t2 - t1)
    n = ps.numberNodes()
    totalNumberNodes += n
    print("Number nodes: " + str(n))

if N != 0:
    print("Average time: " +
          str((totalTime.seconds + 1e-6 * totalTime.microseconds) / float(N)))
    print("Average number nodes: " + str(totalNumberNodes / float(N)))

v = vf.createViewer()
pp = PathPlayer(robot.client.basic, v)
Example #24
0
        loopFound = True
      if edgeFound and loopFound:
        break
    if not edgeFound : raise RuntimeError \
       ('cannot find edge from node "{0}" to "{1}"'.format (n1, n2))
    if not loopFound : raise RuntimeError \
       ('cannot find edge from node "{0}" to "{1}"'.format (n1, n1))
  return edges, loops

# infinite norm between vectors
dC = lambda q1,q2: reduce (lambda x,y : x if fabs (y [0]- y [1]) < x \
                           else fabs (y [0]- y [1]), zip (q1, q2), 0)

if args.display:
  v = vf.createViewer ()
  pp = PathPlayer (v)
else:
  v = lambda x: None

## Initial configuration of manipulator arms
q0_r0 = [pi/6, -pi/2, pi/2, 0, 0, 0,]
q0_r1 = q0_r0 [::]

## Generate initial configurations of spheres
q0_spheres = list ()
i = 0
y = 0.04
while i < nSphere:
  q0_spheres.append ([- .1*(i//2), -.12 + y, 0.025, 0, 0, 0, 1])
  i+=1; y = -y
# Randomly generating a contact configuration at q_end
fullBody.setCurrentConfig (q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1])

# specifying the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init,[])
fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])


r(q_init)
# computing the contact sequence
configs = fullBody.interpolate(0.1, 1, 0)

r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")

# calling draw with increasing i will display the sequence
i = 0;
fullBody.draw(configs[i],r); i=i+1; i-1

#~ collisions = 0;
#~ for config in configs:
  #~ if fullBody.isConfigValid(config)[0] == False:
    #~ collisions += 1
#~ 
#~ collisions

from hpp.gepetto import PathPlayer
pp = PathPlayer (ps.robot.client.basic, r)

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, context=None):
        """
        Constructor
        :param context: An optional string that give a name to a corba context instance
        """
        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 = []
        self.context = context
        if context:
            createContext(context)
            loadServerPlugin(context, 'rbprm-corba.so')
            loadServerPlugin(context, 'affordance-corba.so')
            self.hpp_client = Client(context=context)
            self.hpp_client.problem.selectProblem(context)
            self.rbprm_client = RbprmClient(context=context)
        else:
            self.hpp_client = None
            self.rbprm_client = None

    @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)
        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])
        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, display_roadmap=False):
        """
        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)
        if display_roadmap and self.v.client.gui.getNodeList() is not None:
            t = self.v.solveAndDisplay("roadmap", 5, 0.001)
        else:
            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
Example #27
0
#robot.setJointBounds('base_joint_xyz', [1.6, 6.9, -2.2, 1.5, 0, 3]) # first goal
#robot.setJointBounds('base_joint_xyz', [-0.3, 6.9, -2.2, 2.4, 0, 3]) # second goal
#robot.setJointBounds('base_joint_xyz', [-2.6, 6.9, -2.2, 2.4, 0, 3]) # third goal
robot.setJointBounds('base_joint_xyz', [-6, 6.9, -2.8, 3.2, 0, 3]) # start to bottom
#robot.setJointBounds('base_joint_xyz', [-6, -2.2, -2.4, 3, 0, 8]) # bottom to ultimate
#robot.setJointBounds('base_joint_xyz', [-5, -2.2, -0.1, 2.8, 0, 6]) # bottom to middle column
#robot.setJointBounds('base_joint_xyz', [-5, -2.2, -0.1, 2.8, 0, 3]) # bottom to bottom 1
#robot.setJointBounds('base_joint_xyz', [-6, 6.9, -2.5, 3.2, 0, 3]) # first to bottom

ps = ProblemSolver (robot)
cl = robot.client
#cl.obstacle.loadObstacleModel('animals_description','inclined_plane_3d','inclined_plane_3d')

from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
r.loadObstacleModel ("animals_description","scene_jump_harder","scene_jump_harder")

# Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta]
q11 = [6.4, 0.5, 0.5, 0, 0, 0, 1, 0, 0, 1, Pi] # start
#q11 = [-3.5, 1.7, 0.4, 0, 0, 0, 1, 0, 0, 1, Pi] # bottom of column
r(q11)
#q22 = [2.6, -1.4, 0.35, 0, 0, 0, 1, 0, 0, 1, Pi] # first plateform
#q22 = [0.7, 1.55, 0.4, 0, 0, 0, 1, 0, 0, 1, Pi] # second plateform
#q22 = [-1.7, -1.5, 0.4, 0, 0, 0, 1, 0, 0, 1, Pi] # third plateform
q22 = [-3.5, 1.7, 0.4, 0, 0, 0, 1, 0, 0, 1, Pi] # bottom of column
#q22 = [-3.3, 1.5, 3.4, 0, 0, 0, 1, 0, 0, 1, Pi] # in column
#q22 = [-4.2, 0.9, 1.7, 0, 0, 0, 1, 0, 0, 1, Pi] # bottom 1 of column
#q22 = [-4.4, 0.9, 4.1, 0, 0, 0, 1, 0, 0, 1, Pi] # bottom 3 of column
#q22 = [-4.4, -1.8, 6.5, 0, 0, 0, 1, 0, 0, 1, Pi] # ultimate goal!
r(q22)
Example #28
0
ps = ProblemSolver (robot)
cl = robot.client
#cl.obstacle.loadObstacleModel('animals_description','inclined_plane_3d','inclined_plane_3d')

# Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta]
#q1 = [-1.5, -1.5, 3.41, 0, 0, 0, 1, 0, 0, 1, 0]; q2 = [2.6, 3.7, 3.41, 0, 0, 0, 1, 0, 0, 1, 0]
#q11 = [2.5, 3, 4, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [-2.5, 3, 4, 0, 0, 0, 1, 0, 0, 1, 0] # plane with theta = Pi
#q11 = [-2.5, 3, 4, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [2.5, 2.7, 8, 0, 0, 0, 1, 0, 0, 1, 0] # plane with theta ~= 0
#q11 = [-2.5, 3, 4, 0, 0, 0, 1,-0.1, 0, 0, 1, 0]; q22 = [2.5, 2.7, 8, 0, 0, 0, 1, -0.1, 0, 0, 1, 0] # plane with theta ~= 0 MONOPOD
#q11 = [-2.5, 3, 4, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [2.5, 2.7, 9, 0, 0, 0, 1, 0, 0, 1, 0] # multiple-planes (3 planes)
q11 = [-5, 3.1, 4.2, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [5.2, -5.2, 4, 0, 0, 0, 1, 0, 0, 1, 0] # environment_3d
#r(q22)

from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
#r.loadObstacleModel ("animals_description","plane_3d","plane_3d")
#r.loadObstacleModel ("animals_description","multiple_planes_3d","multiple_planes_3d")
#r.loadObstacleModel ("animals_description","inclined_plane_3d","inclined_plane_3d")
r.loadObstacleModel ("animals_description","environment_3d","environment_3d")
#r.loadObstacleModel ("animals_description","environment_3d_with_window","environment_3d_with_window")
addLight (r, [-3,3,7,1,0,0,0], "li"); addLight (r, [3,-3,7,1,0,0,0], "li1")
r(q11)

q1 = cl.robot.projectOnObstacle (q11, 0.001); q2 = cl.robot.projectOnObstacle (q22, 0.001)
robot.isConfigValid(q1); robot.isConfigValid(q2)

ps.setInitialConfig (q1); ps.addGoalConfig (q2)
ps.solve () 
# PROBLEM !! not finding solution for environment_3d_window with mu=0.5 V0max=6.5 Projectionshooter ....  V0 or Vimp too much limiting ?? 'cause V0max=7 gives a too "easy" solution ...
Example #29
0
q_goal = s_goal.q()

# copy extraconfig for start and init configurations
q_init[configSize:configSize + 3] = dir_init[::]
q_init[configSize + 3:configSize + 6] = acc_init[::]
q_goal[configSize:configSize + 3] = dir_goal[::]
q_goal[configSize + 3:configSize + 6] = acc_goal[::]
# specifying the full body configurations as start and goal state of the problem
fullBody.setStartStateId(s_init.sId)
fullBody.setEndStateId(s_goal.sId)

q_far = q_init[::]
q_far[2] = -5

from hpp.gepetto import PathPlayer
pp = PathPlayer(fullBody.client.basic, r)
pp.dt = 0.0001

r(q_init)
# computing the contact sequence

#~ configs = fullBody.interpolate(0.08,pathId=1,robustnessTreshold = 2, filterStates = True)
configs = fullBody.interpolate(0.001,
                               pathId=0,
                               robustnessTreshold=1,
                               filterStates=True)
r(configs[-1])

print "number of configs =", len(configs)
r(configs[-1])
Example #30
0
 0.9323806762695312,
 0.36073973774909973,
 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)))

###########################
Example #31
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)
from viewer_display_library import normalizeDir, plotCone, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath, contactPosition, addLight, plotSphere, plotSpheresWaypoints
import math
import numpy as np

robot = Robot ('sphere')
robot.setJointBounds('base_joint_xyz', [-5, 5, -5, 5, -1, 7])
ps = ProblemSolver (robot)
cl = robot.client

# Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta]
q11 = [-3.8, 2.4, 1.2, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [3.5, -3.3, 0.4, 0, 0, 0, 1, 0, 0, 1, 0]
#cl.obstacle.loadObstacleModel('animals_description','envir3d_window_mesh','')

from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
r.loadObstacleModel ("animals_description","envir3d_window_mesh","envir3d_window_mesh")
addLight (r, [-3,3,4,1,0,0,0], "li"); addLight (r, [3,-3,4,1,0,0,0], "li1")
r(q11)

q1 = cl.robot.projectOnObstacle (q11, 0.001); q2 = cl.robot.projectOnObstacle (q22, 0.001)
robot.isConfigValid(q1); robot.isConfigValid(q2)
r(q1)

ps.setInitialConfig (q1); ps.addGoalConfig (q2)

plotSphere (q2, cl, r, "sphere_q2", [0,1,0,1], 0.02) # same as robot
nPointsPlot = 50
offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2
plotFrame (r, "_", [0,0,2.8], 0.5)
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")

# 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

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)
# b hpp::core::pathOptimization::GradientBased::initialize (const PathVectorPtr_t& path)


from hpp.corbaserver.puzzle import Robot
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
import time
import math

robot = Robot ('puzzle')
ps = ProblemSolver (robot)
cl = robot.client

from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
#r.loadObstacleModel ("puzzle_description","decor_very_easy","decor_very_easy")

robot.setJointBounds('base_joint_xyz', [-2.2, +2.2, -0.2, 2.2, -0.5, 0.5])
Q = []
Q.append ( [0, 0, 0, 1.0, 0.0, 0.0, 0.0])
"""
Q.append ( [0.0, 0.0, 0.0, 0.8573289792052967, 0.5110043077178016, 0.0474382998474562, 0.04014009987092448])
Q.append ( [0.0, 0.0, 0.0, 0.47002595717039686, 0.8761976030104256, 0.08134045836690892, 0.06882654169507678])
Q.append ( [0.0, 0.0, 0.0, -0.05139523108351973, 0.9913748854243123, 0.09203276443212992, 0.07787387759641762]) # !
Q.append ( [0.0, 0.0, 0.0, -0.5581511991721069, 0.8236712340507641, 0.07646425360117025, 0.06470052227791329])
Q.append ( [0.0, 0.0, 0.0, -0.9056431645733478, 0.420939551154707, 0.03907727653904272, 0.033065387840728454]) # ! one turn
Q.append ( [0.0, 0.0, 0.0, -0.7409238777634497, -0.6666775704281703, 0.06188998802924064, 0.05236845140935746])
Q.append ( [0.0, 0.0, 0.0, 0.2445263320841038, -0.9625514882482619, 0.08935698863687985, 0.07560975961582142])
Q.append ( [0.0, 0.0, 0.0, 0.708781393086984, -0.7002692759274627, 0.0650084224020931, 0.05500712664792493])
Q.append ( [0.0, 0.0, 0.0, 0.9707913243458437, -0.23817079875118724, 0.02211022019858673, 0.01870864786034262])
# 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)