talos_circle.py 6.72 KB
Newer Older
1
2
from hpp.corbaserver.rbprm.talos import Robot
from hpp.gepetto import Viewer
Guilhem Saurel's avatar
Guilhem Saurel committed
3
from hpp.corbaserver.rbprm.tools.display_tools import *
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
import time
print "Plan guide trajectory ..."
import talos_circle_path as tp
print "Done."
import time
statusFilename = tp.statusFilename
pId = 0
f = open(statusFilename,"a")
if tp.ps.numberPaths() > 0 :
  print "Path planning OK."
  f.write("Planning_success: True"+"\n")
  f.close()
else :
  print "Error during path planning"
  f.write("Planning_success: False"+"\n")
  f.close()
  import sys
  sys.exit(1)


fullBody = Robot ()

# Set the bounds for the root
fullBody.setJointBounds ("root_joint",  [-2,2, -2, 2, 0.9, 1.05])
## reduce bounds on joints along x, to put conservative condition on the contact generation for sideway steps
joint6L_bounds_prev=fullBody.getJointBounds('leg_left_6_joint')
joint2L_bounds_prev=fullBody.getJointBounds('leg_left_2_joint')
joint6R_bounds_prev=fullBody.getJointBounds('leg_right_6_joint')
joint2R_bounds_prev=fullBody.getJointBounds('leg_right_2_joint')
33
34
35
36
fullBody.setJointBounds('leg_left_6_joint',[-0.25,0.25])
fullBody.setJointBounds('leg_left_2_joint',[-0.25,0.25])
fullBody.setJointBounds('leg_right_6_joint',[-0.25,0.25])
fullBody.setJointBounds('leg_right_2_joint',[-0.25,0.25])
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
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)
#load the viewer
try :
    v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
except Exception:
    print "No viewer started !"
    class FakeViewer():
        def __init__(self):
            return
        def __call__(self,q):
            return
        def addLandmark(self,a,b):
            return
    v = FakeViewer()


# load a reference configuration
q_ref = fullBody.referenceConfig[::]+[0]*6
#q_ref = fullBody.referenceConfig_legsApart[::]+[0]*6
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
63

64

Guilhem Saurel's avatar
Guilhem Saurel committed
65
if abs(tp.q_goal[1]) <= abs(tp.q_goal[0]) :
66
67
68
69
  fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
  heuristicR = "fixedStep08"
  heuristicL = "fixedStep08"
  print "Use weight for straight walk"
70
  fullBody.usePosturalTaskContactCreation(True)
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
else :
  fullBody.setPostureWeights(fullBody.postureWeights_straff[::]+[0]*6)
  print "Use weight for straff walk"
  if tp.q_goal[1] < 0 :
    print "start with right leg"
    heuristicL = "static"
    heuristicR = "fixedStep06"
  else:
    print "start with left leg"
    heuristicR = "static"
    heuristicL = "fixedStep06"


fullBody.setCurrentConfig (q_init)

print "Generate limb DB ..."
tStart = time.time()
Guilhem Saurel's avatar
Guilhem Saurel committed
88
# generate databases :
89
90
91
92
93
94
95
96
97
98
99
100

nbSamples = 100000
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, heuristicR, 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints,kinematicConstraintsMin = 0.85)
fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, heuristicL, 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.85)
fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)


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

Guilhem Saurel's avatar
Guilhem Saurel committed
101
#define initial and final configurations :
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace()

q_init[0:7] = tp.ps.configAtParam(pId,0)[0:7] # use this to get the correct orientation
q_goal = q_init[::]; q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[0:7]
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]
acc_goal = [0,0,0]

robTreshold = 3
# copy extraconfig for start and init configurations
q_init[configSize:configSize+3] = vel_init[::]
q_init[configSize+3:configSize+6] = acc_init[::]
q_goal[configSize:configSize+3] = vel_goal[::]
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)
#fullBody.setReferenceConfig(fullBody.referenceConfig_legsApart[::]+[0]*6)


# 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()
146
configs = fullBody.interpolate(0.005,pathId=pId,robustnessTreshold = 3, filterStates = True,quasiStatic=True)
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
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."
if abs(configs[-1][0] - tp.q_goal[0]) < 0.01 and abs(configs[-1][1]- tp.q_goal[1]) < 0.01  and (len(fullBody.getContactsVariations(len(configs)-2,len(configs)-1))==1):
  print "Contact generation successful."
  cg_reach_goal = True
else:
  print "Contact generation failed to reach the goal."
  cg_reach_goal = False
if len(configs) > 5 :
  cg_too_many_states = True
  cg_success = False
  print "Discarded contact sequence because it was too long."
else:
  cg_too_many_states = False

f = open(statusFilename,"a")
f.write("cg_success: "+str(cg_success)+"\n")
f.write("cg_reach_goal: "+str(cg_reach_goal)+"\n")
f.write("cg_too_many_states: "+str(cg_too_many_states)+"\n")
f.close()

if (not cg_success) or cg_too_many_states or (not cg_reach_goal):
  import sys
  sys.exit(1)

# put back original bounds for wholebody methods
fullBody.setJointBounds('leg_left_6_joint',joint6L_bounds_prev)
fullBody.setJointBounds('leg_left_2_joint',joint2L_bounds_prev)
fullBody.setJointBounds('leg_right_6_joint',joint6R_bounds_prev)
fullBody.setJointBounds('leg_right_2_joint',joint2R_bounds_prev)