talos_flatGround.py 3.54 KB
Newer Older
1
from hpp.corbaserver.rbprm.talos import Robot
2
from hpp.gepetto import Viewer
3
from tools.display_tools import *
4
5
6
7
8
9
10
import time
print "Plan guide trajectory ..."
import talos_flatGround_path as tp
print "Done."
import time

pId = tp.ps.numberPaths() -1
11
12
fullBody = Robot ()

13
# Set the bounds for the root
14
fullBody.setJointBounds ("root_joint",  [-5,5, -1.5, 1.5, 0.95, 1.05])
15
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
16
17
18
19
20
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,0,0])
ps = tp.ProblemSolver( fullBody )
ps.setParameter("Kinodynamic/velocityBound",tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
21
#load the viewer
22
23
v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)

24
# load a reference configuration
25
q_ref = fullBody.referenceConfig[::]+[0]*6
26
27
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
28
fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
29
30
31
32
33
34
fullBody.setCurrentConfig (q_init)

print "Generate limb DB ..."
tStart = time.time()
# generate databases : 

35
36
nbSamples = 100000
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep08", 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints,kinematicConstraintsMin = 0.75)
37
fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
38
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep08", 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.75)
39
fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
40
41
42
43
44
45


tGenerate =  time.time() - tStart
print "Done."
print "Databases generated in : "+str(tGenerate)+" s"

46
#define initial and final configurations : 
47
48
configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace()

49
q_init[0:7] = tp.ps.configAtParam(pId,0.001)[0:7] # use this to get the correct orientation
50
q_goal = q_init[::]; q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[0:7]
51
52
53
vel_init = tp.ps.configAtParam(pId,0)[tp.indexECS:tp.indexECS+3]
acc_init = tp.ps.configAtParam(pId,0)[tp.indexECS+3:tp.indexECS+6]
vel_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[tp.indexECS:tp.indexECS+3]
54
55
56
57
acc_goal = [0,0,0]

robTreshold = 3
# copy extraconfig for start and init configurations
58
q_init[configSize:configSize+3] = vel_init[::]
59
q_init[configSize+3:configSize+6] = acc_init[::]
60
q_goal[configSize:configSize+3] = vel_goal[::]
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
q_goal[configSize+3:configSize+6] = [0,0,0]


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)

78
# specify the full body configurations as start and goal state of the problem
79
80
fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lLegId])
fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lLegId])
81
82
83
84


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