talos_circle.py 6.74 KB
Newer Older
1
from talos_rbprm.talos import Robot
2
from hpp.gepetto import Viewer
Guilhem Saurel's avatar
Guilhem Saurel committed
3
from hpp.corbaserver.rbprm.tools.display_tools import *
4
import time
5
print("Plan guide trajectory ...")
6
from . import talos_circle_path as tp
7
print("Done.")
8
9
10
11
12
import time
statusFilename = tp.statusFilename
pId = 0
f = open(statusFilename,"a")
if tp.ps.numberPaths() > 0 :
13
  print("Path planning OK.")
14
15
16
  f.write("Planning_success: True"+"\n")
  f.close()
else :
17
  print("Error during path planning")
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
  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
# 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:
47
    print("No viewer started !")
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
    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
  fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
  heuristicR = "fixedStep08"
  heuristicL = "fixedStep08"
69
  print("Use weight for straight walk")
70
  fullBody.usePosturalTaskContactCreation(True)
71
72
else :
  fullBody.setPostureWeights(fullBody.postureWeights_straff[::]+[0]*6)
73
  print("Use weight for straff walk")
74
  if tp.q_goal[1] < 0 :
75
    print("start with right leg")
76
77
78
    heuristicL = "static"
    heuristicR = "fixedStep06"
  else:
79
    print("start with left leg")
80
81
82
83
84
85
    heuristicR = "static"
    heuristicL = "fixedStep06"


fullBody.setCurrentConfig (q_init)

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

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
98
99
print("Done.")
print("Databases generated in : "+str(tGenerate)+" s")
100

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
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])

144
print("Generate contact plan ...")
145
tStart = time.time()
146
configs = fullBody.interpolate(0.005,pathId=pId,robustnessTreshold = 3, filterStates = True,quasiStatic=True)
147
tInterpolateConfigs = time.time() - tStart
148
149
150
print("Done.")
print("Contact plan generated in : "+str(tInterpolateConfigs)+" s")
print("number of configs :", len(configs))
151
152
153
154
155
156
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)


if len(configs) < 2 :
  cg_success = False
157
  print("Error during contact generation.")
158
159
else:
  cg_success = True
160
  print("Contact generation Done.")
161
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):
162
  print("Contact generation successful.")
163
164
  cg_reach_goal = True
else:
165
  print("Contact generation failed to reach the goal.")
166
167
168
169
  cg_reach_goal = False
if len(configs) > 5 :
  cg_too_many_states = True
  cg_success = False
170
  print("Discarded contact sequence because it was too long.")
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
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)