Python Robot.interpolate示例

示例#1

文件： talos_navBauzil.py 项目： pFernbach/hpp-rbprm-corba

q_init[2] = q_ref[2]
q_goal[2] = q_ref[2]

fullBody.setStaticStability(True)
fullBody.setCurrentConfig(q_init)
v(q_init)

fullBody.setCurrentConfig(q_goal)
v(q_goal)

v.addLandmark('talos/base_link', 0.3)
v(q_init)

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

print "Generate contact plan ..."
tStart = time.time()
configs = fullBody.interpolate(0.01,
                               pathId=pId,
                               robustnessTreshold=2,
                               filterStates=True,
                               testReachability=False)
tInterpolateConfigs = time.time() - tStart
print "Done."
print "number of configs :", len(configs)
raw_input("Press Enter to display the contact sequence ...")
displayContactSequence(v, configs)

示例#2

文件： talos_platform.py 项目： teguhSL/hpp-rbprm-corba

# specify the full body configurations as start and goal state of the problem

if q_goal[
        1] < 0:  # goal on the right side of the circle, start motion with right leg first
    fullBody.setStartState(q_init, [fullBody.rLegId, fullBody.lLegId])
    fullBody.setEndState(q_goal, [fullBody.rLegId, fullBody.lLegId])
else:
    fullBody.setStartState(q_init, [fullBody.lLegId, fullBody.rLegId])
    fullBody.setEndState(q_goal, [fullBody.lLegId, fullBody.rLegId])

print "Generate contact plan ..."
tStart = time.time()
configs = fullBody.interpolate(0.005,
                               pathId=pId,
                               robustnessTreshold=3,
                               filterStates=True,
                               quasiStatic=True)
tInterpolateConfigs = time.time() - tStart
print "Done."
print "Contact plan generated in : " + str(tInterpolateConfigs) + " s"
print "number of configs :", len(configs)
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)

if len(configs) < 2:
    cg_success = False
    print "Error during contact generation."
else:
    cg_success = True
    print "Contact generation Done."

示例#3

    if q_goal[1] > q_init[1]:  # turn right
        gait = [fullBody.lLegId, fullBody.rLegId]
    else:  # turn left
        gait = [fullBody.rLegId, fullBody.lLegId]

v(q_init)
fullBody.setStartState(q_init, gait)
v(q_goal)
fullBody.setEndState(q_goal, gait)

print "Generate contact plan ..."
tStart = time.time()
v(q_init)
configs = fullBody.interpolate(0.005,
                               pathId=pId,
                               robustnessTreshold=1,
                               filterStates=True,
                               testReachability=True,
                               quasiStatic=True)
tInterpolateConfigs = time.time() - tStart
print "Done."
print "Contact plan generated in : " + str(tInterpolateConfigs) + " s"
print "number of configs :", len(configs)
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)

if len(configs) < 2:
    cg_success = False
    print "Error during contact generation."
else:
    cg_success = True
    print "Contact generation Done."

示例#4

文件： talos_flatGround.py 项目： pFernbach/hpp-rbprm-corba

q_init[2] = q_ref[2]
q_goal[2] = q_ref[2]

fullBody.setStaticStability(True)
fullBody.setCurrentConfig(q_init)
v(q_init)

fullBody.setCurrentConfig(q_goal)
v(q_goal)

v.addLandmark('talos/base_link', 0.3)
v(q_init)

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

print "Generate contact plan ..."
tStart = time.time()
configs = fullBody.interpolate(0.01,
                               pathId=pId,
                               robustnessTreshold=2,
                               filterStates=True)
tInterpolateConfigs = time.time() - tStart
print "Done."
print "Contact plan generated in : " + str(tInterpolateConfigs) + " s"
print "number of configs :", len(configs)
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)