rbprmfullbody.py 51.9 KB
Newer Older
Steve Tonneau's avatar
Steve Tonneau committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
#!/usr/bin/env python
# Copyright (c) 2014 CNRS
# Author: Steve Tonneau
#
# This file is part of hpp-rbprm-corba.
# hpp-rbprm-corba is free software: you can redistribute it
# and/or modify it under the terms of the GNU Lesser General Public
# License as published by the Free Software Foundation, either version
# 3 of the License, or (at your option) any later version.
#
# hpp-manipulation-corba is distributed in the hope that it will be
# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
# General Lesser Public License for more details.  You should have
# received a copy of the GNU Lesser General Public License along with
# hpp-manipulation-corba.  If not, see
# <http://www.gnu.org/licenses/>.

from hpp.corbaserver.rbprm import Client as RbprmClient
20
from hpp.corbaserver.robot import Robot
21
from numpy import array, matrix
22
from hpp_spline import bezier
Steve Tonneau's avatar
Steve Tonneau committed
23

24
## Load and handle a RbprmFullbody robot for rbprm planning
Steve Tonneau's avatar
Steve Tonneau committed
25
26
27
#
#  A RbprmDevice robot is a set of two robots. One for the 
#  trunk of the robot, one for the range of motion
28
class FullBody (Robot):
29
30
31
     ## Constructor
     def __init__ (self, load = True):
          self.tf_root = "base_link"
32
          self.clientRbprm = RbprmClient ()
33
34
35
36
37
38
39
40
41
42
43
44
45
          self.load = load
          self.limbNames = []
     
     ## Virtual function to load the fullBody robot model.
     #
     # \param urdfName urdf description of the fullBody robot
     # \param rootJointType type of root joint among ("freeflyer", "planar",
     #          "anchor"), WARNING. Currently RB-PRM only considerds freeflyer roots
     # \param meshPackageName name of the meshpackage from where the robot mesh will be loaded
     # \param packageName name of the package from where the robot will be loaded
     # \param urdfSuffix optional suffix for the urdf of the robot package
     # \param srdfSuffix optional suffix for the srdf of the robot package
     def loadFullBodyModel (self, urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix):
46
          Robot.__init__(self,urdfName,rootJointType, False)
47
          self.clientRbprm.rbprm.loadFullBodyRobot(urdfName, rootJointType, packageName, urdfName, urdfSuffix, srdfSuffix, self.client.problem.getSelected("problem")[0])
48
          self.client.robot.meshPackageName = meshPackageName
49
50
51
52
53
54
55
56
57
58
          self.meshPackageName = meshPackageName
          self.packageName = packageName
          self.urdfName = urdfName
          self.urdfSuffix = urdfSuffix
          self.srdfSuffix = srdfSuffix
          self.octrees={}
     
     # Virtual function to load the fullBody robot model.
     #
     def loadFullBodyModelFromActiveRobot (self, urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix):
59
          Robot.__init__(self,urdfName,rootJointType, False)
60
          self.clientRbprm.rbprm.loadFullBodyRobotFromExistingRobot()
61
          self.client.robot.meshPackageName = meshPackageName
62
63
64
65
66
67
          self.meshPackageName = meshPackageName
          self.packageName = packageName
          self.urdfName = urdfName
          self.urdfSuffix = urdfSuffix
          self.srdfSuffix = srdfSuffix
          self.octrees={}
68

69
70
71
72
73
74
75
76
77

     ## Add a limb to the model
     #
     # \param id: user defined id for the limb. Must be unique.
     #  The id is used if several contact points are defined for the same limb (ex: the knee and the foot)
     # \param name: name of the joint corresponding to the root of the limb.
     # \param effectorName name of the joint to be considered as the effector of the limb
     # \param offset position of the effector in joint coordinates relatively to the effector joint
     # \param unit normal vector of the contact point, expressed in the effector joint coordinates
78
79
     # \param x half width of the default support polygon of the effector
     # \param y half height of the default support polygon of the effector
80
81
82
83
84
85
86
87
     # \param collisionObjects objects to be considered for collisions with the limb. TODO remove
     # \param nbSamples number of samples to generate for the limb
     # \param resolution, resolution of the octree voxels. The samples generated are stored in an octree data
     # \param resolution, resolution of the octree voxels. The samples generated are stored in an octree data
     # structure to perform efficient proximity requests. The resulution of the octree, in meters, specifies the size
     # of the unit voxel of the octree. The larger they are, the more samples will be considered as candidates for contact.
     # This can be problematic in terms of performance. The default value is 3 cm.
     def addLimb(self, limbId, name, effectorname, offset, normal, x, y, samples):
88
          self.clientRbprm.rbprm.addLimb(limbId, name, effectorname, offset, normal, x, y, samples, "EFORT", 0.03)
89
90
          self.limbNames += [limbId]

91
92
93
94
95
96
97
98
99
     ## Add a limb no used for contact generation to the model
     #
     # \param id: user defined id for the limb. Must be unique.
     #  The id is used if several contact points are defined for the same limb (ex: the knee and the foot)
     # \param name: name of the joint corresponding to the root of the limb.
     # \param effectorName name of the joint to be considered as the effector of the limb
     # \param collisionObjects objects to be considered for collisions with the limb. TODO remove
     # \param nbSamples number of samples to generate for the limb
     def addNonContactingLimb(self, limbId, name, effectorname, samples):
100
          self.clientRbprm.rbprm.addNonContactingLimb(limbId, name, effectorname, samples)
101
102
          self.limbNames += [limbId]

103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
     ## Add a limb to the model
     #
     # \param databasepath: path to the database describing the robot
     # \param limbId: user defined id for the limb. Must be unique.
     #  The id is used if several contact points are defined for the same limb (ex: the knee and the foot)
     # \param heuristicName: name of the selected heuristic for configuration evaluation
     # \param loadValues: whether values computed, other than the static ones, should be loaded in memory
     # \param disableEffectorCollision: whether collision detection should be disabled for end effector bones
     def addLimbDatabase(self, databasepath, limbId, heuristicName, loadValues = True, disableEffectorCollision = False, grasp =False):
          boolVal = 0.
          boolValEff = 0.
          graspv = 0.
          if(loadValues):
                boolVal = 1.
          if(disableEffectorCollision):
                boolValEff = 1.
          if(grasp):
                graspv = 1.
121
          self.clientRbprm.rbprm.addLimbDatabase(databasepath, limbId, heuristicName, boolVal,boolValEff,graspv)
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
          self.limbNames += [limbId]     

     ## Add a limb to the model
     #
     # \param id: user defined id for the limb. Must be unique.
     #  The id is used if several contact points are defined for the same limb (ex: the knee and the foot)
     # \param name: name of the joint corresponding to the root of the limb.
     # \param effectorName name of the joint to be considered as the effector of the limb
     # \param offset position of the effector in joint coordinates relatively to the effector joint
     # \param unit normal vector of the contact point, expressed in the effector joint coordinates
     # \param x width of the default support polygon of the effector
     # \param y height of the default support polygon of the effector
     # \param collisionObjects objects to be considered for collisions with the limb. TODO remove
     # \param nbSamples number of samples to generate for the limb
     # \param heuristicName: name of the selected heuristic for configuration evaluation
     # \param resolution, resolution of the octree voxels. The samples generated are stored in an octree data
     # structure to perform efficient proximity requests. The resulution of the octree, in meters, specifies the size
     # of the unit voxel of the octree. The larger they are, the more samples will be considered as candidates for contact.
     # This can be problematic in terms of performance. The default value is 3 cm.
     # \param contactType whether the contact is punctual ("_3_DOF") or surfacic ("_6_DOF")
     # \param disableEffectorCollision: whether collision detection should be disabled for end effector bones
Pierre Fernbach's avatar
Pierre Fernbach committed
143
     # \param limbOffset the offset between the limb joint and it's link
144
145
146
147
     # \param kinematicConstraintsPath : path that point to the .obj file containing the kinematic constraints of the limb,
     # if not set the default is "package://hpp-rbprm-corba/com_inequalities/"+name+"_com_constraints.obj"
     # \param kinematicConstraintsMin : add an additionnal inequalities on the kinematicConstraints, of normal (0,0,1) and origin (0,0,kinematicConstraintsMin)
     def addLimb(self, limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution, contactType="_6_DOF",disableEffectorCollision = False, grasp =False,limbOffset=[0,0,0],kinematicConstraintsPath = "", kinematicConstraintsMin = 0.):
148
149
150
151
152
153
          boolValEff = 0.
          if(disableEffectorCollision):
                boolValEff = 1.
          graspv = 0.
          if(grasp):
                graspv = 1.
154
          self.clientRbprm.rbprm.addLimb(limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution,contactType, boolValEff,graspv,limbOffset,kinematicConstraintsPath,kinematicConstraintsMin)
155
156
157
158
159
160
161
          self.limbNames += [limbId]

     ## Returns the configuration of a limb described by a sample
     #
     # \param name name of the limb considered
     # \param idsample identifiant of the sample considered
     def getSample(self, name, idsample):
162
          return self.clientRbprm.rbprm.getSampleConfig(name,idsample)
163
164
165
166
167
168
169
          
     ## Returns the end effector position of en effector,
     # given the current root configuration of the robot and a limb sample
     #
     # \param name name of the limb considered
     # \param idsample identifiant of the sample considered
     def getSamplePosition(self, name, idsample):
170
          return self.clientRbprm.rbprm.getSamplePosition(name,idsample)
171
172
173
174
175
176
177
          
     ## Get the end effector position for a given configuration, assuming z normal
     # \param limbName name of the limb from which to retrieve a sample
     # \param configuration configuration of the robot
     # \return world position of the limb end effector given the current robot configuration.
     # array of size 4, where each entry is the position of a corner of the contact surface
     def getEffectorPosition(self, limbName, configuration):
178
          return self.clientRbprm.rbprm.getEffectorPosition(limbName,configuration)
179
180
181
182
183
184
185
          
     ##compute the distance between all effectors replaced between two states
     # does not account for contact not present in both states
     # \param state1 from state
     # \param state2 destination state
     # \return the value computed for the given sample and analytics
     def getEffectorDistance(self, state1, state2):
186
          return self.clientRbprm.rbprm.getEffectorDistance(state1,state2)
187
188
189
190
191
192
193

     ## Generates a balanced contact configuration, considering the
     #  given current configuration of the robot, and a direction of motion.
     #  Typically used to generate a start and / or goal configuration automatically for a planning problem.
     #
     # \param configuration the initial robot configuration
     # \param direction a 3d vector specifying the desired direction of motion
194
     # \return the configuration in contact
195
     def generateContacts(self, configuration, direction,acceleration = [0,0,0], robustnessThreshold = 0):
196
          return self.clientRbprm.rbprm.generateContacts(configuration, direction, acceleration, robustnessThreshold)
197

198
199
200
201
202
203
204
205
     ## Generates a balanced contact configuration, considering the
     #  given current configuration of the robot, and a direction of motion.
     #  Typically used to generate a start and / or goal configuration automatically for a planning problem.
     #
     # \param configuration the initial robot configuration
     # \param direction a 3d vector specifying the desired direction of motion
     # \return the Id of the new state
     def generateStateInContact(self, configuration, direction,acceleration = [0,0,0], robustnessThreshold = 0):
206
          return self.clientRbprm.rbprm.generateStateInContact(configuration, direction, acceleration, robustnessThreshold)
207
208


209
210
211
212
     ## Generate an autocollision free configuration with limbs in contact with the ground
     # \param contactLimbs name of the limbs to project in contact
     # \return a sampled contact configuration
     def generateGroundContact(self, contactLimbs):
213
          return self.clientRbprm.rbprm.generateGroundContact(contactLimbs)
214
215
216
217
218
219
          
     ## Create a state and push it to the state array
     # \param q configuration
     # \param names list of effectors in contact
     # \return stateId
     def createState(self, q, contactLimbs):
220
          return self.clientRbprm.rbprm.createState(q, contactLimbs)
221
222
223
224
225
226
227
          
     ## Retrieves the contact candidates configurations given a configuration and a limb
     #
     # \param name id of the limb considered
     # \param configuration the considered robot configuration
     # \param direction a 3d vector specifying the desired direction of motion
     def getContactSamplesIds(self, name, configuration, direction):
228
          return self.clientRbprm.rbprm.getContactSamplesIds(name, configuration, direction)
229
230
231
232
233
234
235
          
     ## Retrieves the contact candidates configurations given a configuration and a limb
     #
     # \param name id of the limb considered
     # \param configuration the considered robot configuration
     # \param direction a 3d vector specifying the desired direction of motion
     def getContactSamplesProjected(self, name, configuration, direction, numSamples = 10):
236
          return self.clientRbprm.rbprm.getContactSamplesProjected(name, configuration, direction, numSamples)
237
238
239
240
241
242
243
          
     ## Retrieves the samples IDs In a given octree cell
     #
     # \param name id of the limb considered
     # \param configuration the considered robot configuration
     # \param direction a 3d vector specifying the desired direction of motion
     def getSamplesIdsInOctreeNode(self, limbName, octreeNodeId):
244
          return self.clientRbprm.rbprm.getSamplesIdsInOctreeNode(limbName, octreeNodeId)
245
246
247
248
249
          
     ## Get the number of samples generated for a limb
     #
     # \param limbName name of the limb from which to retrieve a sample
     def getNumSamples(self, limbName):
250
          return self.clientRbprm.rbprm.getNumSamples(limbName)
251
252
253
254
255
          
     ## Get the number of octreeNodes
     #
     # \param limbName name of the limb from which to retrieve a sample
     def getOctreeNodeIds(self, limbName):
256
          return self.clientRbprm.rbprm.getOctreeNodeIds(limbName)
257
258
259
260
261
262
263
          
     ## Get the sample value for a given analysis
     #
     # \param limbName name of the limb from which to retrieve a sample
     # \param valueName name of the analytic measure desired
     # \param sampleId id of the considered sample
     def getSampleValue(self, limbName, valueName, sampleId):
264
          return self.clientRbprm.rbprm.getSampleValue(limbName, valueName, sampleId)
265
          
266
     ## Initialize the first configuration of the path interpolation
267
268
269
270
271
     # with a balanced configuration for the interpolation problem;
     #
     # \param configuration the desired start configuration
     # \param contacts the array of limbs in contact
     def setStartState(self, configuration, contacts):
272
          return self.clientRbprm.rbprm.setStartState(configuration, contacts)
273

274
275
276
     ## Initialize the first state of the path interpolation
     # \param stateId the Id of the desired start state in fullBody
     def setStartStateId(self,stateId):
277
          return self.clientRbprm.rbprm.setStartStateId(stateId)
278
279
280
281

     ## Initialize the goal state of the path interpolation
     # \param stateId the Id of the desired start state in fullBody
     def setEndStateId(self,stateId):
282
          return self.clientRbprm.rbprm.setEndStateId(stateId)
283
284


285
286
287
288
289
290
     ## Create a state given a configuration and contacts
     #
     # \param configuration the desired start configuration
     # \param contacts the array of limbs in contact
     # \return id of created state
     def createState(self, configuration, contacts):
291
          return self.clientRbprm.rbprm.createState(configuration, contacts)
292
293
294
295
296
297
298
                
     ## Initialize the last configuration of the path discretization 
     # with a balanced configuration for the interpolation problem;
     #
     # \param configuration the desired end configuration
     # \param contacts the array of limbs in contact          
     def setEndState(self, configuration, contacts):
299
          return self.clientRbprm.rbprm.setEndState(configuration, contacts)
300
301
302
303
304
     
     ## Saves a computed contact sequence in a given filename
     #
     # \param The file where the configuration must be saved
     def saveComputedStates(self, filename):
305
          return self.clientRbprm.rbprm.saveComputedStates(filename)
306
307
308
309
310
311
     
     ## Saves a limb database
     #
     # \param limb name of the limb for which the DB must be saved
     # \param The file where the configuration must be saved
     def saveLimbDatabase(self, limbName, filename):
312
          return self.clientRbprm.rbprm.saveLimbDatabase(limbName, filename)
313
314
315
316
317
318
319
320
321
     
     ## Discretizes a path return by a motion planner into a discrete
     # sequence of balanced, contact configurations and returns
     # the sequence as an array of configurations
     #
     # \param stepSize discretization step
     # \param pathId Id of the path to compute from
     # \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
     # \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
322
323
324
     # \param testReachability : if true, check each contact transition with our reachability criterion
     # \param quasiStatic : if True, use our reachability criterion with the quasiStatic constraint
     def interpolate(self, stepsize, pathId = 1, robustnessTreshold = 0, filterStates = False,testReachability = True, quasiStatic = False):
325
326
327
328
          if(filterStates):
                filt = 1
          else:
                filt = 0
329
          return self.clientRbprm.rbprm.interpolate(stepsize, pathId, robustnessTreshold, filt,testReachability, quasiStatic)
330
331
332
333
334
335
336
     
     ## Provided a discrete contact sequence has already been computed, computes
     # all the contact positions and normals for a given state, the next one, and the intermediate between them.
     #
     # \param stateId normalized step for generation along the path (ie the path has a length of 1).
     # \return list of 2 or 3 lists of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
     def computeContactPoints(self, stateId):
337
          rawdata = self.clientRbprm.rbprm.computeContactPoints(stateId)
338
339
          return [[b[i:i+3] for i in range(0, len(b), 6)] for b in rawdata], [[b[i+3:i+6] for i in range(0, len(b), 6)] for b in rawdata]
     
340
341
342
343
344
345
     ### Provided a discrete contact sequence has already been computed, computes
     # all the contact positions and normals for a given state
     # \param stateId iD of the considered state
     # \param isIntermediate whether the intermediate state should be considerred rather than this one
     # \return list of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
     def computeContactPointsAtState(self, stateId,isIntermediate=False):
346
          rawdata =  self.clientRbprm.rbprm.computeContactPointsAtState(stateId,isIntermediate)
347
348
          return [[b[i:i+6] for i in range(0, len(b), 6)] for b in rawdata][0]

349
350
351
352
353
354
     ## Provided a discrete contact sequence has already been computed, computes
     # all the contact positions and normals for a given state, the next one, and the intermediate between them.
     #
     # \param stateId normalized step for generation along the path (ie the path has a length of 1).
     # \return list of 2 or 3 lists of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
     def computeContactPointsForLimb(self, stateId, limb):
355
          rawdata = self.clientRbprm.rbprm.computeContactPointsForLimb(stateId, limb)
356
357
358
359
360
361
362
363
364
          return [[b[i:i+3] for i in range(0, len(b), 6)] for b in rawdata], [[b[i+3:i+6] for i in range(0, len(b), 6)] for b in rawdata]
     
     ## Compute effector contact points and normals for a given configuration
     # in local coordinates
     #
     # \param q configuration of the robot
     # \param limbName ids of the limb in contact
     # \return list 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
     def computeContactForConfig(self, q, limbName):
365
          rawdata = self.clientRbprm.rbprm.computeContactForConfig(q, limbName)
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
          return [rawdata[i:i+3] for i in range(0, len(rawdata), 6)], [rawdata[i+3:i+6] for i in range(0, len(rawdata), 6)]

     ## Provided a discrete contact sequence has already been computed, computes
     # all the contact positions and normals for a given state, the next one, and the intermediate between them.
     #
     # \param stateId normalized step for generation along the path (ie the path has a length of 1).
     # \return list of 2 or 3 lists of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
     def computeContactPointsPerLimb(self, stateId, limbs, dicEffector = {}):
          Ps = []; Ns = []
          for limb in limbs:
                P, N = self.computeContactPointsForLimb(stateId, limb)
                for i in range(len(P)):
                     if i > len(Ps) - 1 :
                          Ps.append({})
                          Ns.append({})
                     if(len(P[i]) > 0):
                          targetName = limb
                          if(dicEffector.has_key(limb)):
                                targetName = dicEffector[limb]['effector']
                          Ps[i][targetName] = P[i]
                          Ns[i][targetName] = N[i]
          return Ps, Ns
          
     ## Given start and goal states
     #  generate a contact sequence over a list of configurations
     #
     # \param stepSize discretization step
     # \param pathId Id of the path to compute from
     # \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
     # \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
396
397
398
     # \param testReachability : if true, check each contact transition with our reachability criterion
     # \param quasiStatic : if True, use our reachability criterion with the quasiStatic constraint
     def interpolateConfigs(self, configs, robustnessTreshold = 0, filterStates = False, testReachability = True, quasiStatic = False):
399
400
401
402
          if(filterStates):
                filt = 1
          else:
                filt = 0
403
          return self.clientRbprm.rbprm.interpolateConfigs(configs, robustnessTreshold, filt,testReachability, quasiStatic)
404
405
406
407
408
409
410
          
     ##
     # 
     # \param state1 current state considered
     # \param limb name of the limb for which the request aplies
     # \return whether the limb is in contact at this state
     def isLimbInContact(self, limbname, state1):
411
          return self.clientRbprm.rbprm.isLimbInContact(limbname, state1) >0
412
413
414
415
416
417
418
          
     ##
     # 
     # \param state1 current state considered
     # \param limb name of the limb for which the request aplies
     # \return whether the limb is in contact at this state
     def isLimbInContactIntermediary(self, limbname, state1):
419
          return self.clientRbprm.rbprm.isLimbInContactIntermediary(limbname, state1) >0
420
421
422
423
424
425
426
          
     ##
     # 
     # \param state1 current state considered
     # \param limb name of the limb for which the request aplies
     # \return all limbs in contact at this state
     def getLimbsInContact(self, limbNames, state1):
Pierre Fernbach's avatar
Pierre Fernbach committed
427
          return [limbName for limbName in limbNames if self.isLimbInContact(limbName, state1)]
428
429
430
431
432
433
434
          
     ##
     # 
     # \param state1 current state considered
     # \param limb name of the limb for which the request aplies
     # \return all limbs in contact at this state
     def getLimbsInContactIntermediary(self, limbNames, state1):
Pierre Fernbach's avatar
Pierre Fernbach committed
435
          return [limbName for limbName in limbNames if self.isLimbInContactIntermediary(limbName, state1)]
436
437
438
439
440
441
     
     ## returns the CWC of the robot at a given state
     #
     # \param stateId The considered state
     # \return H matrix and h column, such that H w <= h
     def getContactCone(self, stateId, friction = 0.5):
442
          H_h =  array(self.clientRbprm.rbprm.getContactCone(stateId, friction))
443
444
445
446
447
448
449
450
451
452
          #~ print "H_h", H_h.shape 
          #~ print "norm h", ( H_h[:, -1] != 0).any()
          # now decompose cone 
          return H_h[:,:-1], H_h[:, -1]
          
     ## returns the CWC of the robot  between two states
     #
     # \param stateId The first considered state
     # \return H matrix and h column, such that H w <= h
     def getContactIntermediateCone(self, stateId, friction = 0.5):
453
          H_h =  array(self.clientRbprm.rbprm.getContactIntermediateCone(stateId, friction))
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
          #~ print "H_h", H_h.shape 
          #~ print "norm h", ( H_h[:, -1] != 0).any()
          # now decompose cone 
          return H_h[:,:-1], H_h[:, -1]
          
     ## Create a path for the root given
     #  a set of 3d key points
     #  The path is composed of n+1 linear interpolations
     #  between the n positions.
     #  The rotation is linearly interpolated as well,
     #  between a start and a goal rotation. The resulting path
     #  is added to the problem solver
     #  \param positions array of positions
     #  \param quat_1 quaternion of 1st rotation
     #  \param quat_2 quaternion of 2nd rotation
     def generateRootPath(self, positions, quat_1, quat_2):
470
          return self.clientRbprm.rbprm.generateRootPath(positions, quat_1, quat_2)
471
472
473
474
475
476
477
478
479
480
481
     
     ## Create a com trajectory given list of positions, velocities and accelerations
     # accelerations list contains one less element because it does not have an initial state.
     # a set of 3d key points
     # The path is composed of n+1 integrations
     # between the n positions.
     # The resulting path
     # is added to the problem solver
     # \param positions array of positions
     # \return id of the root path computed
     def straightPath(self, positions):
482
          return self.clientRbprm.rbprm.straightPath(positions)
483
484
485
486
487
488
489
490
491
          
     ## Create a com trajectory given a polynomial trajectory.
     # The path is composed of n+1 integrations
     # between the n waypoints.
     # The resulting path
     # is added to the problem solver
     # \param positions array of positions
     # \return id of the root path computed
     def generateCurveTraj(self, positions):
492
          return self.clientRbprm.rbprm.generateCurveTraj(positions)
493
494
495
496
497
498
499
500
501
502
          
     ## Create a com trajectory given a polynomial trajectory.
     # The path is composed of n+1 integrations
     # between the n waypoints. Then splits the curve into several sub curves
     # The resulting paths
     # are added to the problem solver
     # \param positions array of positions
     # \param partition array of times [0,1.] that describe the beginning or end of each phase
     # \return id of the complete path computed
     def generateCurveTrajParts(self, positions, partition):
503
          return self.clientRbprm.rbprm.generateCurveTrajParts(positions, partition)
504
505
506
507
508
509
510
511
512
513
514
515
516
517
                
     ## Create a com trajectory given list of positions, velocities and accelerations
     # accelerations list contains one less element because it does not have an initial state.
     # a set of 3d key points
     # The path is composed of n+1 integrations
     # between the n positions.
     # The resulting path
     # is added to the problem solver
     # \param positions array of positions
     # \param velocities array of velocities
     # \param accelerations array of accelerations
     # \param dt time between two points
     # \return id of the root path computed
     def generateComTraj(self, positions, velocities, accelerations, dt):
518
          return self.clientRbprm.rbprm.generateComTraj(positions, velocities, accelerations, dt)
519
520
521
522
523
524
525
526
527
528
529
530
531
532
          
     ## Create a path for the root given
     #  a set of 3d key points
     #  The path is composed of n+1 linear interpolations
     #  between the n positions.
     #  The rotation is linearly interpolated as well,
     #  between a start and a goal configuration. Assuming the robot is a
     #  free-flyer, rotations are extracted automatically. The resulting path
     #  is added to the problem solver
     #  \param positions array of positions
     #  \param configState1 configuration of 1st rotation
     #  \param configState2 configuration of 2nd rotation
     #  \return id of the resulting path
     def generateRootPathStates(self, positions, configState1, configState2):
533
          return self.clientRbprm.rbprm.generateRootPath(positions, configState1[3:7], configState2[3:7])
534
535
536
537
538
539
540
541
542
543
          
     ## Given start and goal states
     #  Provided a path has already been computed and interpolated, generate a continuous path
     #  between two indicated states. Will fail if the index of the states do not exist
     #
     # \param index of first state.
     # \param index of second state.
     # \param numOptim Number of iterations of the shortcut algorithm to apply between each states
     #  \return id of the resulting path
     def limbRRT(self, state1, state2, numOptim = 10):
544
          return self.clientRbprm.rbprm.limbRRT(state1, state2, numOptim)
545
546
547
548
549
550
551
552
553
554
555
556
557
          
     ## Provided a path has already been computed and interpolated, generate a continuous path                                
     # between two indicated states. The states do not need to be consecutive, but increasing in Id.                      
     # Will fail if the index of the states do not exist                                                                                 
     # The path of the root and limbs not considered by the contact transitions between                                        
     # two states is assumed to be already computed, and registered in the solver under the id specified by the user.
     # It must be valid in the sense of the active PathValidation.                                                                    
     # \param state1 index of first state.                                                                                                    
     # \param state2 index of second state.                                                                                                  
     # \param path index of the path considered for the generation                                                                    
     # \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states 
     #  \return id of the resulting path
     def limbRRTFromRootPath(self, state1, state2, path, numOptim = 10):
558
          return self.clientRbprm.rbprm.limbRRTFromRootPath(state1, state2, path, numOptim)
559
560
561
562
563
564
565
566
567
568
569
570
571
572
          
          
     ## Provided a center of mass trajectory has already been computed and interpolated, generate a continuous full body path                                
     # between two indicated states. The states do not need to be consecutive, but increasing in Id.                      
     # Will fail if the index of the states do not exist                                                                                 
     # The path of the COM  between                                        
     # two states is assumed to be already computed, and registered in the solver under the id specified by the user.
     # It must be valid in the sense of the active PathValidation.                                                                    
     # \param state1 index of first state.                                                                                                    
     # \param state2 index of second state.                                                                                                  
     # \param path index of the com path considered for the generation                                                                    
     # \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states 
     #  \return id of the resulting path
     def comRRT(self, state1, state2, path, numOptim = 10):
573
          return self.clientRbprm.rbprm.comRRT(state1, state2, path, numOptim)
574
575
576
          
                
     ## Provided a path has already been computed and interpolated, generate a continuous path
577
578
     # between two indicated states. The states do not need to be consecutive, but increasing in Id.
     # Will fail if the index of the states do not exist
579
580
     # The path of the COM of thr robot and limbs not considered by the contact transitions between
     # two states is assumed to be already computed, and registered in the solver under the id specified by the user.
581
582
     # It must be valid in the sense of the active PathValidation.
     # \param state1 index of first state.
583
584
585
     # \param rootPositions1 com positions to track for 1st phase
     # \param rootPositions1 com positions to track for 2nd phase
     # \param rootPositions1 com positions to track for 3rd phase
586
     # \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
587
588
     # \return id of the root path computed
     def comRRTFromPos(self, state1, comPos1, comPos2, comPos3, numOptim = 10):
589
          return self.clientRbprm.rbprm.comRRTFromPos(state1, comPos1, comPos2, comPos3, numOptim)
590
591
592
593
594
595
596
597
598
599
600
601
602
          
     ## Provided a path has already been computed and interpolated, generate a continuous path
     # between two indicated states. The states do not need to be consecutive, but increasing in Id.
     # Will fail if the index of the states do not exist
     # The path of the COM of thr robot and limbs not considered by the contact transitions between
     # two states is assumed to be already computed, and registered in the solver under the id specified by the user.
     # It must be valid in the sense of the active PathValidation.
     # \param state1 index of first state.
     # \param rootPositions1 com positions to track for 1st phase
     # \param rootPositions1 com positions to track for 2nd phase
     # \param rootPositions1 com positions to track for 3rd phase
     # \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
     # \return id of the root path computed
Steve Tonneau's avatar
Steve Tonneau committed
603
     def effectorRRTFromPosBetweenState(self, state1, state2, comPos1, comPos2, comPos3, numOptim = 10):
604
          return self.clientRbprm.rbprm.effectorRRTFromPosBetweenState(state1, state2, comPos1, comPos2, comPos3, numOptim)
Steve Tonneau's avatar
Steve Tonneau committed
605
          
606
     def comRRTFromPosBetweenState(self, state1, state2, comPos1, comPos2, comPos3, numOptim = 10):
607
          return self.clientRbprm.rbprm.comRRTFromPosBetweenState(state1, state2, comPos1, comPos2, comPos3, numOptim)
608
          
609
610
611
612
613
614
615
616
617
618
619
620
621
622
                
     ## Provided a path has already been computed and interpolated, generate a continuous path
     # between two indicated states. The states do not need to be consecutive, but increasing in Id.
     # Will fail if the index of the states do not exist
     # The path of the COM of thr robot and limbs not considered by the contact transitions between
     # two states is assumed to be already computed, and registered in the solver under the id specified by the user.
     # It must be valid in the sense of the active PathValidation.
     # \param state1 index of first state.
     # \param rootPositions1 com positions to track for 1st phase
     # \param rootPositions1 com positions to track for 2nd phase
     # \param rootPositions1 com positions to track for 3rd phase
     # \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
     # \return id of the root path computed
     def effectorRRT(self, state1, comPos1, comPos2, comPos3, numOptim = 10):
623
          return self.clientRbprm.rbprm.effectorRRT(state1, comPos1, comPos2, comPos3, numOptim)
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
          
                          
     ## Provided a path has already been computed and interpolated, generate a continuous path
     # between two indicated states. The states do not need to be consecutive, but increasing in Id.
     # Will fail if the index of the states do not exist
     # The path of the COM of thr robot and limbs not considered by the contact transitions between
     # two states is assumed to be already computed, and registered in the solver under the id specified by the user.
     # It must be valid in the sense of the active PathValidation.
     # \param state1 index of first state.
     # \param rootPositions1 com positions to track for 1st phase
     # \param rootPositions1 com positions to track for 2nd phase
     # \param rootPositions1 com positions to track for 3rd phase
     # \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
     # \return id of the root path computed
     def effectorRRTFromPath(self, state1, refPathId, path_start, path_to, comPos1, comPos2, comPos3, numOptim = 10, trackedEffectors = []):
639
          return self.clientRbprm.rbprm.effectorRRTFromPath(state1, refPathId, path_start, path_to, comPos1, comPos2, comPos3, numOptim, trackedEffectors)
640
641


642
643
644
645
646
647
648
649
    ## Provided a path has already been computed and interpolated, generate a continuous path
    # between two indicated states. The states need to be consecutive with no contact variation between them
    # (the free limbs can move, but there should be no contact creation/break)
    # \param state1 index of the first state
    # \param state2 index of the second state
    # \param comPos com position to track
    # \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
     def effectorRRTOnePhase(self,state1,state2,comPos,numOptim=10):
650
         return self.clientRbprm.rbprm.effectorRRTOnePhase(state1,state2,comPos,numOptim)
651

652
653
654
655
656
657
658
659
660
661
    ## Provided a path has already been computed and interpolated, generate an array of bezier curves,
    ## with varying weightRRT (see effector-rrt.cc::fitBezier)
    # (the free limbs can move, but there should be no contact creation/break)
    # \param state1 index of the first state
    # \param state2 index of the second state
    # \param comPos com position to track
    # \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
    # \return array of pathIds : first index is the trajectorie, second index is the curve inside this trajectory
    # (there should be 3 curves per trajectories : takeoff / mid / landing)
     def generateEffectorBezierArray(self,state1,state2,comPos,numOptim=10):
662
         return self.clientRbprm.rbprm.generateEffectorBezierArray(state1,state2,comPos,numOptim)
663
664
665
666
667
668
669
670
671
672
673



    ## Provided a path has already been computed and interpolated, generate a continuous path
    # between two indicated states. The states need to be consecutive with no contact variation between them
    # (the free limbs can move, but there should be no contact creation/break)
    # \param state1 index of the first state
    # \param state2 index of the second state
    # \param comPos com position to track
    # \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
     def comRRTOnePhase(self,state1,state2,comPos,numOptim=10):
674
         return self.clientRbprm.rbprm.comRRTOnePhase(state1,state2,comPos,numOptim)
675
676


677
678
679
680
681
682
683
    # compute and add a trajectory for the end effector between the 2 states
    # represented as a bezier curve.
    # Do not check the kinematic feasability of this trajectory
    # \param state1 index of first state.
    # \param state2 index of end state.
    # \param rootPositions com positions to track
     def generateEndEffectorBezier(self, state1, state2, comPos):
684
          return self.clientRbprm.rbprm.generateEndEffectorBezier(state1, state2, comPos)
685

686
687
688
689
690
691
692
          
     ## Project a given state into a given COM position
     # between two indicated states. The states do not need to be consecutive, but increasing in Id.
     # Will fail if the index of the state does not exist.
     # \param state index of first state.
     # \param targetCom 3D vector for the com position
     # \return projected configuration
693
     def projectToCom(self, state, targetCom, maxNumSample = 0):
694
          return self.clientRbprm.rbprm.projectToCom(state, targetCom, maxNumSample)
695
696
697
698
699
700
          
     ## Returns the configuration at a given state
     # Will fail if the index of the state does not exist.
     # \param state index of state.
     # \return state configuration
     def getConfigAtState(self, state):
701
          return self.clientRbprm.rbprm.getConfigAtState(state)
702
703
704
705
706
707
          
     ## Project a given state into a given COM position
     # and update the state configuration.
     # \param state index of first state.
     # \param targetCom 3D vector for the com position
     # \return whether the projection was successful
708
     def projectStateToCOM(self, state, targetCom, maxNumSample = 0):
709
          return self.clientRbprm.rbprm.projectStateToCOM(state, targetCom, maxNumSample)     > 0
710
711
712
713
714
715
716
          
     ## Project a given state into a given COM position
     # and update the state configuration.
     # \param state index of first state.
     # \param targetCom 3D vector for the com position
     # \return whether the projection was successful
     def setConfigAtState(self, state, targetCom):
717
          return self.clientRbprm.rbprm.setConfigAtState(state, targetCom)     > 0
718

719
720
721
722
723
     ## Given start and goal states
     #  generate a contact sequence over a list of configurations
     #
     # \param stepSize discretization step
     # \param pathId Id of the path to compute from
724
     def isConfigBalanced(self, config, names, robustness = 0,CoM = [0,0,0]):
725
          if (self.clientRbprm.rbprm.isConfigBalanced(config, names, robustness,CoM) == 1):
726
727
728
                return True
          else:
                return False
729
730

     ## Check if the state at the given index is balanced for a given robustness
731
     #
732
     def isStateBalanced(self, stateId, robustnessThreshold = 0, robustnessFound = 0):
733
          robustnessFound = self.clientRbprm.rbprm.isStateBalanced(stateId)
734
735
          return robustnessFound > robustnessThreshold

736
737
738
739
740
741
742
743
     ## Updates limb databases with a user chosen computation
     #
     # \param analysis name of computation
     # \param isstatic whether the computation should be used to sort samples by default
     def runSampleAnalysis(self, analysis, isstatic):
          isStatic = 0.
          if(isstatic):
                isStatic = 1.
744
          self.clientRbprm.rbprm.runSampleAnalysis(analysis,isStatic)
745
746
747
748
749
750
751
752
753
754
          
     ## Updates a limb database with a user chosen computation
     #
     # \param limbname name of the limb chosen for computation
     # \param analysis name of computation
     # \param isstatic whether the computation should be used to sort samples by default
     def runLimbSampleAnalysis(self, limbname, analysis, isstatic):
          isStatic = 0.
          if(isstatic):
                isStatic = 1.
755
          return self.clientRbprm.rbprm.runLimbSampleAnalysis(limbname, analysis,isStatic)
756
757
758
759
760
          
     ## if the preprocessor variable PROFILE is active
     # dump the profiling data into a logFile
     # \param logFile name of the file where to dump the profiling data
     def dumpProfile(self, logFile="log.txt"):
761
          return self.clientRbprm.rbprm.dumpProfile(logFile)
762
763
764
765
766
767
768
769
770

     ## Create octree nodes representation for a given limb
     #
     # \param gui gepetoo viewer instance discretization step
     # \param winId window to draw to step
     # \param limbName name of the limb considered
     # \param config initial configuration of the robot, used when created octree
     # \param color of the octree boxes
     def createOctreeBoxes(self, gui, winId, limbName, config, color = [1,1,1,0.3]):
771
          boxes = self.clientRbprm.rbprm.getOctreeBoxes(limbName, config)
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
          scene = "oct"+limbName
          gui.createScene(scene)
          resolution = boxes[0][3]
          i = 0
          for box in boxes:
                boxname = scene+"/b"+str(i)
                gui.addBox(boxname,resolution,resolution,resolution, color)
                gui.applyConfiguration(boxname,[box[0],box[1],box[2],1,0,0,0])
                i = i+1
          self.octrees[limbName] = scene
          gui.addSceneToWindow(scene,winId)
          gui.refresh()
          
     ## Create octree nodes representation for a given limb
     #
     # \param limbName name of the limb considered
     # \param ocId of the octree box
     def getOctreeBox(self, limbName, ocId):
790
          return self.clientRbprm.rbprm.getOctreeBox(limbName, ocId)
791
792
793
794
795
796
797
     
     ## Draws robot configuration, along with octrees associated
     #
     # \param viewer gepetto viewer instance
     def draw(self, configuration, viewer):
          viewer(configuration)
          for limb, groupid in self.octrees.iteritems():
798
                transform = self.clientRbprm.rbprm.getOctreeTransform(limb, configuration)
799
800
801
802
803
804
805
806
807
808
809
810
                viewer.client.gui.applyConfiguration(groupid,transform)
          viewer.client.gui.refresh()
          
          
    ## Computes the closest projection matrix that will bring a limb's effector
    # from its current configuration to a specified location
    # \param limbName name of the considered limb
    # \param configuration considered configuration of the robot
    # \param p target position
    # \param n target normal
    # \return 7D Transform of effector in contact (position + quaternion)
     def computeTargetTransform(self, limbName, configuration, p, n):         
811
          return self.clientRbprm.rbprm.computeTargetTransform(limbName, configuration, p, n)
812
813
814
815
816
817
818

     ## Export motion to a format readable by the blender
     # importFromGepetto script
     # \param viewer gepetto viewer instance
     # \param configurations list of configurations to save
     # \param filename outputfile where to export the motion
     def exportMotion(self, viewer, configurations, filename):
819
          import hpp.gepetto.blender.exportmotion as em
820
          em.exportStates(viewer, self.client.robot, configurations, filename)
821
822
823
824
825
826
827
828
829
830
831
832
833
          
     ## Export motion to a format readable by the blender
     # importFromGepetto script
     # \param viewer gepetto viewer instance
     # \param configurations list of configurations to save
     # \param filename outputfile where to export the motion
     def exportAll(self, viewer, configurations, testname):
          self.exportMotion(viewer, configurations, testname+"_motion.txt")
          self.saveComputedStates(testname+"_states.txt")
          f1 = open(testname+"_configs.txt","w+")
          f1.write(str(configurations))
          f1.close()

Pierre Fernbach's avatar
Pierre Fernbach committed
834
835
836
837
838
      ## set a boolean in rbprmFullBody
      # if true, the acceleration doesn't account for the stability check
      #
        # \param staticStability boolean
     def setStaticStability(self,staticStability):
839
          return self.clientRbprm.rbprm.setStaticStability(staticStability)
Pierre Fernbach's avatar
Pierre Fernbach committed
840
841
842
843

     ## set a reference configuration in FullBody
     # \param referenceConfig dofArray
     def setReferenceConfig(self,referenceConfig):
844
          return self.clientRbprm.rbprm.setReferenceConfig(referenceConfig)
Pierre Fernbach's avatar
Pierre Fernbach committed
845
846
847
848

      ## return the time at the given state index (in the path computed during the first phase)
      # \param stateId : index of the state
     def getTimeAtState(self,stateId):
849
          return self.clientRbprm.rbprm.getTimeAtState(stateId)
Pierre Fernbach's avatar
Pierre Fernbach committed
850

851
852
853
854
855
856
      ## return the duration for the given state index
      # note : it depend on the implementation of interpolate : which state do we add to the list : the first one or the last or one in the middle
      # current implementation : the last one
      # \param stateId : index of the state
     def getDurationForState(self,stateId):
          if stateId == 0:
857
               return self.clientRbprm.rbprm.getTimeAtState(stateId)
858
          else:
859
               return (self.clientRbprm.rbprm.getTimeAtState(stateId) - self.clientRbprm.rbprm.getTimeAtState(stateId-1))
860
861


862
863
864
865
     ## Return the names of all the effector for which a trajectory have been computed for a given path index.
     #  \param pathId : index of the path, the same index as the wholeBody path stored in problem-solver
     #  \return the list of all the end-effector (joint names) for which a trajectory have been defined
     def getEffectorsTrajectoriesNames(self, pathId):
866
         return self.clientRbprm.rbprm.getEffectorsTrajectoriesNames(pathId)
867
868
869
870
871
872
873

     ## Return the waypoints of the bezier curve for a given pathIndex and effector name
     #  \param pathId : index of the path, the same index as the wholeBody path stored in problem-solver
     #  \param effectorName : the name of the desired effector (Joint name)
     #  \return the waypoints of the bezier curve followed by this end effector
     #  Throw an error if there is no trajectory computed for the given id/name
     def getEffectorTrajectoryWaypoints(self,pathId,effectorName):
874
         return self.clientRbprm.rbprm.getEffectorTrajectoryWaypoints(pathId,effectorName)
875
876
877
878
879


     ## Return the bezier curve for a given pathIndex and effector name
     #  \param pathId : index of the path, the same index as the wholeBody path stored in problem-solver
     #  \param effectorName : the name of the desired effector (Joint name)
880
     #  \return a list of bezier curve (from spline library) followed by the end effector.
881
882
     #  Throw an error if there is no trajectory computed for the given id/name
     def getEffectorTrajectory(self,pathId,effectorName):
883
         curvesWp =  self.clientRbprm.rbprm.getEffectorTrajectoryWaypoints(pathId,effectorName)
884
885
886
887
888
889
890
891
892
893
894
         res=[]
         for cid in range(len(curvesWp)):
             wp=curvesWp[cid]
             if len(wp[0]) == 1: # length is provided :
                waypoints = matrix(wp[1:]).transpose()
                curve = bezier(waypoints,wp[0][0])
             else :
                waypoints=matrix(wp).transpose()
                curve = bezier(waypoints)
             res +=[curve]
         return res
895

896
897
    ## Return the bezier curve corresponding to a given path index
     def getPathAsBezier(self,pathId):
898
         l = self.clientRbprm.rbprm.getPathAsBezier(pathId)
899
900
901
902
903
904
         t = l[0][0]
         wps = matrix(l[1:]).transpose()
         curve = bezier(wps,t)
         return curve


Pierre Fernbach's avatar
Pierre Fernbach committed
905
906
907
908
      ## return the contacts variation between two states
      # \param stateIdFrom : index of the first state
      # \param stateIdTo : index of the second state
     def getContactsVariations(self,stateIdFrom,stateIdTo):
909
          return self.clientRbprm.rbprm.getContactsVariations(stateIdFrom,stateIdTo)
Pierre Fernbach's avatar
Pierre Fernbach committed
910
911
912

      ## return a list of all the limbs names
     def getAllLimbsNames(self):
913
          return self.clientRbprm.rbprm.getAllLimbsNames()
Pierre Fernbach's avatar
Pierre Fernbach committed
914
915
916
917
918

      ## return a list of all the limbs in contact
      # \param stateId : index of the state
     def getAllLimbsInContact(self,stateId):
          return self.getLimbsInContact(self.getAllLimbsNames(),stateId)
919

920
     ## check the kinematics constraints for the given point, assuming all contact are established
921
     # \param point : test if the point is inside the kinematics constraints for the current configuration
922
     def areKinematicsConstraintsVerified(self,point):
923
          return self.clientRbprm.rbprm.areKinematicsConstraintsVerified(point)
924
925

     def areKinematicsConstraintsVerifiedForState(self,stateFrom, point):
926
         return self.clientRbprm.rbprm.areKinematicsConstraintsVerifiedForState(stateFrom,point)
927

928
     def isReachableFromState(self,stateFrom,stateTo,computePoint=False):
929
          raw =  self.clientRbprm.rbprm.isReachableFromState(stateFrom,stateTo)
930
931
932
933
934
935
936
937
938
          if computePoint :
                res = []
                res += [raw[0]>0.]
                res += [[raw[1],raw[2],raw[3]]]
                if len(raw) > 4:
                    res += [[raw[4],raw[5],raw[6]]]
                return res
          else :
                return raw[0] > 0.
939

940
     def isDynamicallyReachableFromState(self,stateFrom,stateTo,addPathPerPhase = False,timings=[],numPointsPerPhases=5):
941
          return self.clientRbprm.rbprm.isDynamicallyReachableFromState(stateFrom,stateTo,addPathPerPhase,timings,numPointsPerPhases)
942