Skip to content

Commit 453571a

Browse files
pre-commit-ci[bot]florent-lamiraux
pre-commit-ci[bot]
authored and
florent-lamiraux
committed
[pre-commit.ci] auto fixes from pre-commit.com hooks
for more information, see https://pre-commit.ci
1 parent 9e90e34 commit 453571a

11 files changed

+803
-456
lines changed

‎tests/benchmarks/baxter-manipulation-boxes-spf.py‎

Lines changed: 47 additions & 43 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,6 @@
99
from pyhpp.manipulation import Device, Graph, Problem, urdf, StatesPathFinder
1010

1111
from pyhpp.manipulation import (
12-
RandomShortcut as ManipRandomShortcut,
1312
GraphRandomShortcut,
1413
GraphPartialShortcut,
1514
)
@@ -22,12 +21,12 @@
2221
from pyhpp.constraints import (
2322
LockedJoint,
2423
)
25-
from pinocchio import SE3, Quaternion
24+
from pinocchio import SE3
2625

2726
parser = ArgumentParser()
28-
parser.add_argument('-N', default=0, type=int)
29-
parser.add_argument('--display', action='store_true')
30-
parser.add_argument('--run', action='store_true')
27+
parser.add_argument("-N", default=0, type=int)
28+
parser.add_argument("--display", action="store_true")
29+
parser.add_argument("--run", action="store_true")
3130
args = parser.parse_args()
3231

3332
# Robot and object file paths
@@ -70,8 +69,7 @@
7069
box_pose,
7170
)
7271
robot.setJointBounds(
73-
boxes[i] + '/root_joint',
74-
[-1, 0.5, -1, 2, 0.6, 1.9, -1, 1, -1, 1, -1, 1, -1, 1]
72+
boxes[i] + "/root_joint", [-1, 0.5, -1, 2, 0.6, 1.9, -1, 1, -1, 1, -1, 1, -1, 1]
7573
)
7674

7775
model = robot.model()
@@ -88,47 +86,48 @@
8886
# Calculate box positions
8987
rankB = list()
9088
for i in range(K):
91-
joint_id = robot.model().getJointId(boxes[i] + '/root_joint')
92-
rankB.append (robot.model().idx_qs[joint_id])
89+
joint_id = robot.model().getJointId(boxes[i] + "/root_joint")
90+
rankB.append(robot.model().idx_qs[joint_id])
9391

94-
bb = [0.7, 0.8, 0., 0.1]
92+
bb = [0.7, 0.8, 0.0, 0.1]
9593
c = sqrt(2) / 2
9694
xstep = (bb[1] - bb[0]) / (nBoxPerLine - 1) if nBoxPerLine > 1 else (bb[1] - bb[0])
97-
nbCols = int(K * 1. / nBoxPerLine + 0.5)
95+
nbCols = int(K * 1.0 / nBoxPerLine + 0.5)
9896
ystep = (bb[3] - bb[2]) / (nbCols - 1) if nbCols > 1 else (bb[3] - bb[2])
9997
for i in range(K):
10098
iL = i % nBoxPerLine
10199
iC = (i - iL) / nBoxPerLine
102100
x = bb[0] + xstep * iL
103101
y = bb[2] + xstep * iC
104-
q_init[rankB[i]:rankB[i]+7] = [x, y, 0.746, 0, -c, 0, c]
102+
q_init[rankB[i] : rankB[i] + 7] = [x, y, 0.746, 0, -c, 0, c]
105103

106104
q_goal = q_init[::].copy()
107105
for i in range(K):
108106
r = rankB[i]
109107
rn = rankB[goal[i]]
110-
q_goal[r:r+7] = q_init[rn:rn+7]
108+
q_goal[r : r + 7] = q_init[rn : rn + 7]
111109

112110
constraints = dict()
113111
graphConstraints = dict()
114112

115113
jointNames = dict()
116-
jointNames['all'] = robot.model().names
117-
jointNames['baxterRightSide'] = list()
118-
jointNames['baxterLeftSide'] = list()
114+
jointNames["all"] = robot.model().names
115+
jointNames["baxterRightSide"] = list()
116+
jointNames["baxterLeftSide"] = list()
119117

120-
for n in jointNames['all']:
118+
for n in jointNames["all"]:
121119
if n.startswith("baxter"):
122120
if n.startswith("baxter/left_"):
123-
jointNames['baxterLeftSide'].append(n)
121+
jointNames["baxterLeftSide"].append(n)
124122
if n.startswith("baxter/right_"):
125-
jointNames['baxterRightSide'].append(n)
123+
jointNames["baxterRightSide"].append(n)
126124
# Lock finger joints
127-
lockFingers = ["r_gripper_l_finger",
128-
"r_gripper_r_finger",
129-
"l_gripper_l_finger",
130-
"l_gripper_r_finger",
131-
]
125+
lockFingers = [
126+
"r_gripper_l_finger",
127+
"r_gripper_r_finger",
128+
"l_gripper_l_finger",
129+
"l_gripper_r_finger",
130+
]
132131
for side in ["r", "l"]:
133132
joint_name = "baxter/" + side + "_gripper_r_finger_joint"
134133
cs = LockedJoint(robot, joint_name, np.array([-0.02]))
@@ -141,18 +140,18 @@
141140

142141

143142
# Lock head
144-
lockHead = 'head_pan'
145-
joint_name = 'baxter/head_pan'
143+
lockHead = "head_pan"
144+
joint_name = "baxter/head_pan"
146145
joint_id = robot.model().getJointId(joint_name)
147146
cs = LockedJoint(robot, joint_name, np.array([q_init[robot.model().idx_qs[joint_id]]]))
148147
constraints[lockHead] = cs
149148
graphConstraints[lockHead] = cs
150149
for n in jointNames["baxterRightSide"]:
151-
cs = LockedJoint(robot, n, np.array([0.]))
150+
cs = LockedJoint(robot, n, np.array([0.0]))
152151
constraints[n] = cs
153152

154153
for n in jointNames["baxterLeftSide"]:
155-
cs = LockedJoint(robot, n, np.array([0.]))
154+
cs = LockedJoint(robot, n, np.array([0.0]))
156155
constraints[n] = cs
157156

158157
# Define handles and contact surfaces
@@ -168,20 +167,20 @@
168167

169168
factory = ConstraintGraphFactory(cg, constraints)
170169
factory.setGrippers(grippers)
171-
factory.environmentContacts(['table/pancake_table_table_top'])
170+
factory.environmentContacts(["table/pancake_table_table_top"])
172171
factory.setObjects(boxes, handlesPerObject, objContactSurfaces)
173172
factory.setRules(rules)
174173
factory.generate()
175-
cg.addNumericalConstraintsToGraph( list(graphConstraints.values()))
174+
cg.addNumericalConstraintsToGraph(list(graphConstraints.values()))
176175
cg.initialize()
177176

178-
res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init)
177+
res, q_init_proj, err = cg.applyStateConstraints(cg.getState("free"), q_init)
179178
if not res:
180-
raise Exception('Init configuration could not be projected.')
179+
raise Exception("Init configuration could not be projected.")
181180

182-
res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal)
181+
res, q_goal_proj, err = cg.applyStateConstraints(cg.getState("free"), q_goal)
183182
if not res:
184-
raise Exception('Goal configuration could not be projected.')
183+
raise Exception("Goal configuration could not be projected.")
185184

186185
problem.initConfig(q_init_proj)
187186

@@ -200,14 +199,19 @@
200199
problem.addConfigValidation("CollisionValidation")
201200

202201
optimizers = {
203-
'GraphPartialShortcut': GraphPartialShortcut(problem),
204-
'GraphRandomShortcut': GraphRandomShortcut(problem),
205-
'PartialShortcut': PartialShortcut(problem),
206-
'RandomShortcut': RandomShortcut(problem),
207-
'SimpleShortcut': SimpleShortcut(problem),
202+
"GraphPartialShortcut": GraphPartialShortcut(problem),
203+
"GraphRandomShortcut": GraphRandomShortcut(problem),
204+
"PartialShortcut": PartialShortcut(problem),
205+
"RandomShortcut": RandomShortcut(problem),
206+
"SimpleShortcut": SimpleShortcut(problem),
208207
}
209-
optimizerNames = ['GraphPartialShortcut', 'GraphRandomShortcut', 'PartialShortcut',
210-
'RandomShortcut', 'SimpleShortcut']
208+
optimizerNames = [
209+
"GraphPartialShortcut",
210+
"GraphRandomShortcut",
211+
"PartialShortcut",
212+
"RandomShortcut",
213+
"SimpleShortcut",
214+
]
211215
iOpt = 0
212216

213217
totalTime = dt.timedelta(0)
@@ -219,7 +223,7 @@
219223
iOpt += 1
220224
if iOpt == len(optimizerNames):
221225
iOpt = 0
222-
226+
223227
try:
224228
planner.roadmap().clear()
225229
problem.resetGoalConfigs()
@@ -245,4 +249,4 @@
245249
print(f"Success rate: {success / args.N * 100}%")
246250
if success > 0:
247251
print(f"Average time per success: {totalTime.total_seconds() / success}")
248-
print(f"Average number nodes per success: {totalNumberNodes / success}")
252+
print(f"Average number nodes per success: {totalNumberNodes / success}")

‎tests/benchmarks/baxter-manipulation-boxes.py‎

Lines changed: 46 additions & 43 deletions
Original file line numberDiff line numberDiff line change
@@ -10,8 +10,6 @@
1010
from pyhpp.manipulation import Device, Graph, Problem, urdf, ManipulationPlanner
1111

1212
from pyhpp.manipulation import (
13-
RandomShortcut as ManipRandomShortcut,
14-
EnforceTransitionSemantic,
1513
GraphRandomShortcut,
1614
GraphPartialShortcut,
1715
)
@@ -24,12 +22,12 @@
2422
from pyhpp.constraints import (
2523
LockedJoint,
2624
)
27-
from pinocchio import SE3, Quaternion
25+
from pinocchio import SE3
2826

2927
parser = ArgumentParser()
30-
parser.add_argument('-N', default=0, type=int)
31-
parser.add_argument('--display', action='store_true')
32-
parser.add_argument('--run', action='store_true')
28+
parser.add_argument("-N", default=0, type=int)
29+
parser.add_argument("--display", action="store_true")
30+
parser.add_argument("--run", action="store_true")
3331
args = parser.parse_args()
3432

3533
# Robot and object file paths
@@ -72,8 +70,7 @@
7270
box_pose,
7371
)
7472
robot.setJointBounds(
75-
boxes[i] + '/root_joint',
76-
[-1, 0.5, -1, 2, 0.6, 1.9, -1, 1, -1, 1, -1, 1, -1, 1]
73+
boxes[i] + "/root_joint", [-1, 0.5, -1, 2, 0.6, 1.9, -1, 1, -1, 1, -1, 1, -1, 1]
7774
)
7875

7976
tmp = boxes[0]
@@ -95,47 +92,48 @@
9592
# Calculate box positions
9693
rankB = list()
9794
for i in range(K):
98-
joint_id = robot.model().getJointId(boxes[i] + '/root_joint')
99-
rankB.append (robot.model().idx_qs[joint_id])
95+
joint_id = robot.model().getJointId(boxes[i] + "/root_joint")
96+
rankB.append(robot.model().idx_qs[joint_id])
10097

101-
bb = [0.7, 0.8, 0., 0.1]
98+
bb = [0.7, 0.8, 0.0, 0.1]
10299
c = sqrt(2) / 2
103100
xstep = (bb[1] - bb[0]) / (nBoxPerLine - 1) if nBoxPerLine > 1 else (bb[1] - bb[0])
104-
nbCols = int(K * 1. / nBoxPerLine + 0.5)
101+
nbCols = int(K * 1.0 / nBoxPerLine + 0.5)
105102
ystep = (bb[3] - bb[2]) / (nbCols - 1) if nbCols > 1 else (bb[3] - bb[2])
106103
for i in range(K):
107104
iL = i % nBoxPerLine
108105
iC = (i - iL) / nBoxPerLine
109106
x = bb[0] + xstep * iL
110107
y = bb[2] + xstep * iC
111-
q_init[rankB[i]:rankB[i]+7] = [x, y, 0.746, 0, -c, 0, c]
108+
q_init[rankB[i] : rankB[i] + 7] = [x, y, 0.746, 0, -c, 0, c]
112109

113110
q_goal = q_init[::].copy()
114111
for i in range(K):
115112
r = rankB[i]
116113
rn = rankB[goal[i]]
117-
q_goal[r:r+7] = q_init[rn:rn+7]
114+
q_goal[r : r + 7] = q_init[rn : rn + 7]
118115

119116
constraints = dict()
120117
graphConstraints = dict()
121118

122119
jointNames = dict()
123-
jointNames['all'] = robot.model().names
124-
jointNames['baxterRightSide'] = list()
125-
jointNames['baxterLeftSide'] = list()
120+
jointNames["all"] = robot.model().names
121+
jointNames["baxterRightSide"] = list()
122+
jointNames["baxterLeftSide"] = list()
126123

127-
for n in jointNames['all']:
124+
for n in jointNames["all"]:
128125
if n.startswith("baxter"):
129126
if n.startswith("baxter/left_"):
130-
jointNames['baxterLeftSide'].append(n)
127+
jointNames["baxterLeftSide"].append(n)
131128
if n.startswith("baxter/right_"):
132-
jointNames['baxterRightSide'].append(n)
129+
jointNames["baxterRightSide"].append(n)
133130
# Lock finger joints
134-
lockFingers = ["r_gripper_l_finger",
135-
"r_gripper_r_finger",
136-
"l_gripper_l_finger",
137-
"l_gripper_r_finger",
138-
]
131+
lockFingers = [
132+
"r_gripper_l_finger",
133+
"r_gripper_r_finger",
134+
"l_gripper_l_finger",
135+
"l_gripper_r_finger",
136+
]
139137
for side in ["r", "l"]:
140138
joint_name = "baxter/" + side + "_gripper_r_finger_joint"
141139
cs = LockedJoint(robot, joint_name, np.array([-0.02]))
@@ -148,18 +146,18 @@
148146

149147

150148
# Lock head
151-
lockHead = 'head_pan'
152-
joint_name = 'baxter/head_pan'
149+
lockHead = "head_pan"
150+
joint_name = "baxter/head_pan"
153151
joint_id = robot.model().getJointId(joint_name)
154152
cs = LockedJoint(robot, joint_name, np.array([q_init[robot.model().idx_qs[joint_id]]]))
155153
constraints[lockHead] = cs
156154
graphConstraints[lockHead] = cs
157155
for n in jointNames["baxterRightSide"]:
158-
cs = LockedJoint(robot, n, np.array([0.]))
156+
cs = LockedJoint(robot, n, np.array([0.0]))
159157
constraints[n] = cs
160158

161159
for n in jointNames["baxterLeftSide"]:
162-
cs = LockedJoint(robot, n, np.array([0.]))
160+
cs = LockedJoint(robot, n, np.array([0.0]))
163161
constraints[n] = cs
164162

165163
# Define handles and contact surfaces
@@ -175,20 +173,20 @@
175173

176174
factory = ConstraintGraphFactory(cg, constraints)
177175
factory.setGrippers(grippers)
178-
factory.environmentContacts(['table/pancake_table_table_top'])
176+
factory.environmentContacts(["table/pancake_table_table_top"])
179177
factory.setObjects(boxes, handlesPerObject, objContactSurfaces)
180178
factory.setRules(rules)
181179
factory.generate()
182-
cg.addNumericalConstraintsToGraph( list(graphConstraints.values()))
180+
cg.addNumericalConstraintsToGraph(list(graphConstraints.values()))
183181
cg.initialize()
184182

185-
res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init)
183+
res, q_init_proj, err = cg.applyStateConstraints(cg.getState("free"), q_init)
186184
if not res:
187-
raise Exception('Init configuration could not be projected.')
185+
raise Exception("Init configuration could not be projected.")
188186

189-
res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal)
187+
res, q_goal_proj, err = cg.applyStateConstraints(cg.getState("free"), q_goal)
190188
if not res:
191-
raise Exception('Goal configuration could not be projected.')
189+
raise Exception("Goal configuration could not be projected.")
192190

193191
problem.initConfig(q_init_proj)
194192

@@ -200,14 +198,19 @@
200198
problem.clearConfigValidations()
201199
problem.addConfigValidation("CollisionValidation")
202200
optimizers = {
203-
'GraphPartialShortcut': GraphPartialShortcut(problem),
204-
'GraphRandomShortcut': GraphRandomShortcut(problem),
205-
'PartialShortcut': PartialShortcut(problem),
206-
'RandomShortcut': RandomShortcut(problem),
207-
'SimpleShortcut': SimpleShortcut(problem),
201+
"GraphPartialShortcut": GraphPartialShortcut(problem),
202+
"GraphRandomShortcut": GraphRandomShortcut(problem),
203+
"PartialShortcut": PartialShortcut(problem),
204+
"RandomShortcut": RandomShortcut(problem),
205+
"SimpleShortcut": SimpleShortcut(problem),
208206
}
209-
optimizerNames = ['GraphPartialShortcut', 'GraphRandomShortcut', 'PartialShortcut',
210-
'RandomShortcut', 'SimpleShortcut']
207+
optimizerNames = [
208+
"GraphPartialShortcut",
209+
"GraphRandomShortcut",
210+
"PartialShortcut",
211+
"RandomShortcut",
212+
"SimpleShortcut",
213+
]
211214
iOpt = 0
212215

213216
totalTime = dt.timedelta(0)
@@ -219,7 +222,7 @@
219222
iOpt += 1
220223
if iOpt == len(optimizerNames):
221224
iOpt = 0
222-
225+
223226
try:
224227
planner.roadmap().clear()
225228
problem.resetGoalConfigs()

0 commit comments

Comments
 (0)