anymal_modular_palet_low.py 4.63 KB
Newer Older
1
2
from hpp.corbaserver.rbprm.anymal import Robot
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
import scenarios.sandbox.ANYmal.anymal_modular_palet_low_obstacles_path as tp
7

8
9
10
11
12
#Robot.urdfSuffix += "_safeFeet"
statusFilename = tp.statusFilename
pId = tp.pid
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
33
34
  f.write("Planning_success: False"+"\n")
  f.close()
  import sys
  sys.exit(1)

fullBody = Robot ()

# Set the bounds for the root
rootBounds = tp.rootBounds[::]

rootBounds[0] -= 0.2
rootBounds[1] += 0.2
rootBounds[2] -= 0.5
rootBounds[3] += 0.5
rootBounds[4] -= 0.2
rootBounds[5] += 0.2
fullBody.setJointBounds ("root_joint",  rootBounds)
35
fullBody.setConstrainedJointsBounds()
36
37
38
39
40
41
42
43
44
45
46
47

# 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,-tp.vMax,tp.vMax,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,-tp.aMaxZ,tp.aMaxZ])
ps = tp.ProblemSolver( fullBody )
ps.setParameter("Kinodynamic/velocityBound",tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
ps.setParameter("FullBody/frictionCoefficient",tp.mu)
#load the viewer
try :
    v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
except Exception:
48
    print("No viewer started !")
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
    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_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
#fullBody.usePosturalTaskContactCreation(True)

fullBody.setCurrentConfig (q_init)

68
print("Generate limb DB ...")
69
tStart = time.time()
Guilhem Saurel's avatar
Guilhem Saurel committed
70
# generate databases :
71

72
fullBody.loadAllLimbs("static","ReferenceConfiguration",nbSamples=100000,disableEffectorCollision=False)
73
tGenerate =  time.time() - tStart
74
75
print("Done.")
print("Databases generated in : "+str(tGenerate)+" s")
76
77
v.addLandmark('anymal/base_0',0.2)

Guilhem Saurel's avatar
Guilhem Saurel committed
78
#define initial and final configurations :
79
80
81
82
83
84
85
86
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]
87
vel_init = [0,0,0]
88
robTreshold = 3
89
90
91
92
93
94
95
# 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]


96
97
q_init[2] = q_ref[2] + 0.13
q_goal[2] = q_ref[2] + 0.15
98
99
100
101
102
103
104
105
106
107
108
109

normals = [[0.,0.,1.] for _ in range(4)]
fullBody.setStaticStability(True)
fullBody.setCurrentConfig (q_init)
v(q_init)
fullBody.setStartState(q_init,Robot.limbs_names,normals)
fullBody.setEndState(q_goal,Robot.limbs_names,normals)

fullBody.setCurrentConfig (q_goal)
v(q_goal)


110
print("Generate contact plan ...")
111
112
113
tStart = time.time()
configs = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=True,quasiStatic=True)
tInterpolateConfigs = time.time() - tStart
114
115
116
print("Done.")
print("Contact plan generated in : "+str(tInterpolateConfigs)+" s")
print("number of configs :", len(configs))
117
118
119
120
121
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)

if len(configs) < 2 :
  cg_success = False
122
  print("Error during contact generation.")
123
124
else:
  cg_success = True
125
  print("Contact generation Done.")
126
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):
127
  print("Contact generation successful.")
128
129
  cg_reach_goal = True
else:
130
  print("Contact generation failed to reach the goal.")
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
  cg_reach_goal = False

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

if (not cg_success):
  import sys
  sys.exit(1)

#beginId = 2

# put back original bounds for wholebody methods
fullBody.resetJointsBounds()
#displayContactSequence(v,configs)