rbprmfullbody.py 45 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
20
#!/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
from hpp.corbaserver import Client as BasicClient
Steve Tonneau's avatar
Steve Tonneau committed
21
import hpp.gepetto.blender.exportmotion as em
22
from numpy import array
Steve Tonneau's avatar
Steve Tonneau committed
23
24
25
26
27
28
29
30
31
32
33

## Corba clients to the various servers
#
class CorbaClient:
    """
    Container for corba clients to various interfaces.
    """
    def __init__ (self):
        self.basic = BasicClient ()
        self.rbprm = RbprmClient ()

34
## Load and handle a RbprmFullbody robot for rbprm planning
Steve Tonneau's avatar
Steve Tonneau committed
35
36
37
#
#  A RbprmDevice robot is a set of two robots. One for the 
#  trunk of the robot, one for the range of motion
38
class FullBody (object):
Steve Tonneau's avatar
Steve Tonneau committed
39
40
41
42
43
44
    ## Constructor
    def __init__ (self, load = True):
        self.tf_root = "base_link"
        self.rootJointType = dict()
        self.client = CorbaClient ()
        self.load = load
t steve's avatar
t steve committed
45
46
47
48
        self.limbNames = []
    
    ## Virtual function to load the fullBody robot model.
    #
Steve Tonneau's avatar
doc    
Steve Tonneau committed
49
50
51
52
53
54
55
    # \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
Steve Tonneau's avatar
Steve Tonneau committed
56
    def loadFullBodyModel (self, urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix):
t steve's avatar
t steve committed
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
        self.client.rbprm.rbprm.loadFullBodyRobot(urdfName, rootJointType, packageName, urdfName, urdfSuffix, srdfSuffix)
        self.name = urdfName
        self.displayName = urdfName
        self.tf_root = "base_link"
        self.rootJointType = rootJointType
        self.jointNames = self.client.basic.robot.getJointNames ()
        self.allJointNames = self.client.basic.robot.getAllJointNames ()
        self.client.basic.robot.meshPackageName = meshPackageName
        self.meshPackageName = meshPackageName
        self.rankInConfiguration = dict ()
        self.rankInVelocity = dict ()
        self.packageName = packageName
        self.urdfName = urdfName
        self.urdfSuffix = urdfSuffix
        self.srdfSuffix = srdfSuffix
        self.octrees={}
        rankInConfiguration = rankInVelocity = 0
        for j in self.jointNames:
            self.rankInConfiguration [j] = rankInConfiguration
            rankInConfiguration += self.client.basic.robot.getJointConfigSize (j)
            self.rankInVelocity [j] = rankInVelocity
            rankInVelocity += self.client.basic.robot.getJointNumberDof (j)
    
    # Virtual function to load the fullBody robot model.
    #
82
    def loadFullBodyModelFromActiveRobot (self, urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix):
t steve's avatar
t steve committed
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
        self.client.rbprm.rbprm.loadFullBodyRobotFromExistingRobot()
        self.name = urdfName
        self.displayName = urdfName
        self.tf_root = "base_link"
        self.rootJointType = rootJointType
        self.jointNames = self.client.basic.robot.getJointNames ()
        self.allJointNames = self.client.basic.robot.getAllJointNames ()
        self.client.basic.robot.meshPackageName = meshPackageName
        self.meshPackageName = meshPackageName
        self.rankInConfiguration = dict ()
        self.rankInVelocity = dict ()
        self.packageName = packageName
        self.urdfName = urdfName
        self.urdfSuffix = urdfSuffix
        self.srdfSuffix = srdfSuffix
        self.octrees={}
        rankInConfiguration = rankInVelocity = 0
        for j in self.jointNames:
            self.rankInConfiguration [j] = rankInConfiguration
            rankInConfiguration += self.client.basic.robot.getJointConfigSize (j)
            self.rankInVelocity [j] = rankInVelocity
            rankInVelocity += self.client.basic.robot.getJointNumberDof (j)
Steve Tonneau's avatar
Steve Tonneau committed
105

t steve's avatar
t steve committed
106
107
108
    ## Add a limb to the model
    #
    # \param id: user defined id for the limb. Must be unique.
Steve Tonneau's avatar
doc    
Steve Tonneau committed
109
110
111
112
113
114
115
116
117
118
    #  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 resolution, resolution of the octree voxels. The samples generated are stored in an octree data
119
    # \param resolution, resolution of the octree voxels. The samples generated are stored in an octree data
Steve Tonneau's avatar
doc    
Steve Tonneau committed
120
121
122
    # 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.
123
    def addLimb(self, limbId, name, effectorname, offset, normal, x, y, samples):
t steve's avatar
t steve committed
124
125
        self.client.rbprm.rbprm.addLimb(limbId, name, effectorname, offset, normal, x, y, samples, "EFORT", 0.03)
        self.limbNames += [limbId]
126

t steve's avatar
t steve committed
127
128
129
130
    ## 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.
Steve Tonneau's avatar
Steve Tonneau committed
131
132
    #  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
133
    # \param loadValues: whether values computed, other than the static ones, should be loaded in memory
134
    # \param disableEffectorCollision: whether collision detection should be disabled for end effector bones
t steve's avatar
t steve committed
135
    def addLimbDatabase(self, databasepath, limbId, heuristicName, loadValues = True, disableEffectorCollision = False, grasp =False):
t steve's avatar
t steve committed
136
137
138
139
140
141
142
143
144
145
146
        boolVal = 0.
        boolValEff = 0.
        graspv = 0.
        if(loadValues):
            boolVal = 1.
        if(disableEffectorCollision):
            boolValEff = 1.
        if(grasp):
            graspv = 1.
        self.client.rbprm.rbprm.addLimbDatabase(databasepath, limbId, heuristicName, boolVal,boolValEff,graspv)    
        self.limbNames += [limbId]    
Steve Tonneau's avatar
Steve Tonneau committed
147

t steve's avatar
t steve committed
148
149
150
    ## Add a limb to the model
    #
    # \param id: user defined id for the limb. Must be unique.
151
152
153
154
155
156
157
158
159
    #  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
160
    # \param heuristicName: name of the selected heuristic for configuration evaluation
161
162
163
164
    # \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.
165
    # \param contactType whether the contact is punctual ("_3_DOF") or surfacic ("_6_DOF")
166
    # \param disableEffectorCollision: whether collision detection should be disabled for end effector bones
t steve's avatar
t steve committed
167
    def addLimb(self, limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution, contactType="_6_DOF",disableEffectorCollision = False, grasp =False):
t steve's avatar
t steve committed
168
169
170
171
172
173
174
175
        boolValEff = 0.
        if(disableEffectorCollision):
            boolValEff = 1.
        graspv = 0.
        if(grasp):
            graspv = 1.
        self.client.rbprm.rbprm.addLimb(limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution,contactType, boolValEff,graspv)
        self.limbNames += [limbId]
176

t steve's avatar
t steve committed
177
178
    ## Returns the configuration of a limb described by a sample
    #
Steve Tonneau's avatar
doc    
Steve Tonneau committed
179
180
    # \param name name of the limb considered
    # \param idsample identifiant of the sample considered
181
    def getSample(self, name, idsample):
t steve's avatar
t steve committed
182
183
184
185
186
        return self.client.rbprm.rbprm.getSampleConfig(name,idsample)
        
    ## Returns the end effector position of en effector,
    # given the current root configuration of the robot and a limb sample
    #
Steve Tonneau's avatar
doc    
Steve Tonneau committed
187
188
    # \param name name of the limb considered
    # \param idsample identifiant of the sample considered
Steve Tonneau's avatar
Steve Tonneau committed
189
    def getSamplePosition(self, name, idsample):
t steve's avatar
t steve committed
190
191
192
        return self.client.rbprm.rbprm.getSamplePosition(name,idsample)
        
    ## Get the end effector position for a given configuration, assuming z normal
193
194
195
196
197
    # \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):
t steve's avatar
t steve committed
198
199
200
        return self.client.rbprm.rbprm.getEffectorPosition(limbName,configuration)
        
    ##compute the distance between all effectors replaced between two states
201
202
203
204
205
    # 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):
t steve's avatar
t steve committed
206
        return self.client.rbprm.rbprm.getEffectorDistance(state1,state2)        
207

t steve's avatar
t steve committed
208
    ## Generates a balanced contact configuration, considering the
Steve Tonneau's avatar
doc    
Steve Tonneau committed
209
210
    #  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.
t steve's avatar
t steve committed
211
    #
Steve Tonneau's avatar
doc    
Steve Tonneau committed
212
213
    # \param configuration the initial robot configuration
    # \param direction a 3d vector specifying the desired direction of motion
Steve Tonneau's avatar
Steve Tonneau committed
214
    def generateContacts(self, configuration, direction):
t steve's avatar
t steve committed
215
216
217
        return self.client.rbprm.rbprm.generateContacts(configuration, direction)
        
    ## Generate an autocollision free configuration with limbs in contact with the ground
218
219
220
    # \param contactLimbs name of the limbs to project in contact
    # \return a sampled contact configuration
    def generateGroundContact(self, contactLimbs):
t steve's avatar
t steve committed
221
222
223
224
225
226
        return self.client.rbprm.rbprm.generateGroundContact(contactLimbs)
        
    ## Create a state and push it to the state array
    # \param q configuration
    # \param names list of effectors in contact
    # \return stateId
Steve Tonneau's avatar
Steve Tonneau committed
227
    def createState(self, q, contactLimbs):
t steve's avatar
t steve committed
228
229
230
231
        return self.client.rbprm.rbprm.createState(q, contactLimbs)
        
    ## Retrieves the contact candidates configurations given a configuration and a limb
    #
Steve Tonneau's avatar
doc    
Steve Tonneau committed
232
233
234
    # \param name id of the limb considered
    # \param configuration the considered robot configuration
    # \param direction a 3d vector specifying the desired direction of motion
235
    def getContactSamplesIds(self, name, configuration, direction):
t steve's avatar
t steve committed
236
237
238
239
        return self.client.rbprm.rbprm.getContactSamplesIds(name, configuration, direction)
        
    ## Retrieves the contact candidates configurations given a configuration and a limb
    #
Steve Tonneau's avatar
Steve Tonneau committed
240
241
242
243
    # \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):
t steve's avatar
t steve committed
244
245
246
247
        return self.client.rbprm.rbprm.getContactSamplesProjected(name, configuration, direction, numSamples)
        
    ## Retrieves the samples IDs In a given octree cell
    #
248
249
250
251
    # \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):
t steve's avatar
t steve committed
252
253
254
255
        return self.client.rbprm.rbprm.getSamplesIdsInOctreeNode(limbName, octreeNodeId)
        
    ## Get the number of samples generated for a limb
    #
Steve Tonneau's avatar
Steve Tonneau committed
256
257
    # \param limbName name of the limb from which to retrieve a sample
    def getNumSamples(self, limbName):
t steve's avatar
t steve committed
258
259
260
261
        return self.client.rbprm.rbprm.getNumSamples(limbName)
        
    ## Get the number of octreeNodes
    #
262
263
    # \param limbName name of the limb from which to retrieve a sample
    def getOctreeNodeIds(self, limbName):
t steve's avatar
t steve committed
264
265
266
267
        return self.client.rbprm.rbprm.getOctreeNodeIds(limbName)
        
    ## Get the sample value for a given analysis
    #
Steve Tonneau's avatar
Steve Tonneau committed
268
269
270
271
    # \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):
t steve's avatar
t steve committed
272
273
274
275
276
        return self.client.rbprm.rbprm.getSampleValue(limbName, valueName, sampleId)
        
    ## Initialize the first configuration of the path discretization 
    # with a balanced configuration for the interpolation problem;
    #
Steve Tonneau's avatar
doc    
Steve Tonneau committed
277
278
    # \param configuration the desired start configuration
    # \param contacts the array of limbs in contact
Steve Tonneau's avatar
Steve Tonneau committed
279
    def setStartState(self, configuration, contacts):
t steve's avatar
t steve committed
280
281
282
283
        return self.client.rbprm.rbprm.setStartState(configuration, contacts)
        
    ## Create a state given a configuration and contacts
    #
Steve Tonneau's avatar
Steve Tonneau committed
284
285
286
287
    # \param configuration the desired start configuration
    # \param contacts the array of limbs in contact
    # \return id of created state
    def createState(self, configuration, contacts):
t steve's avatar
t steve committed
288
289
290
291
292
        return self.client.rbprm.rbprm.createState(configuration, contacts)
            
    ## Initialize the last configuration of the path discretization 
    # with a balanced configuration for the interpolation problem;
    #
Steve Tonneau's avatar
doc    
Steve Tonneau committed
293
    # \param configuration the desired end configuration
t steve's avatar
t steve committed
294
    # \param contacts the array of limbs in contact        
Steve Tonneau's avatar
Steve Tonneau committed
295
    def setEndState(self, configuration, contacts):
t steve's avatar
t steve committed
296
297
298
299
        return self.client.rbprm.rbprm.setEndState(configuration, contacts)
    
    ## Saves a computed contact sequence in a given filename
    #
Steve Tonneau's avatar
doc    
Steve Tonneau committed
300
    # \param The file where the configuration must be saved
Steve Tonneau's avatar
Steve Tonneau committed
301
    def saveComputedStates(self, filename):
t steve's avatar
t steve committed
302
303
304
305
        return self.client.rbprm.rbprm.saveComputedStates(filename)
    
    ## Saves a limb database
    #
Steve Tonneau's avatar
Steve Tonneau committed
306
307
308
    # \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):
t steve's avatar
t steve committed
309
310
311
312
313
314
        return self.client.rbprm.rbprm.saveLimbDatabase(limbName, filename)
    
    ## 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
    #
Steve Tonneau's avatar
doc    
Steve Tonneau committed
315
    # \param stepSize discretization step
316
    # \param pathId Id of the path to compute from
317
    # \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
318
319
    # \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
    def interpolate(self, stepsize, pathId = 1, robustnessTreshold = 0, filterStates = False):
t steve's avatar
t steve committed
320
321
322
323
324
325
326
        if(filterStates):
            filt = 1
        else:
            filt = 0
        return self.client.rbprm.rbprm.interpolate(stepsize, pathId, robustnessTreshold, filt)
    
    ## Provided a discrete contact sequence has already been computed, computes
327
328
329
330
    # 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]
331
    def computeContactPoints(self, stateId):
t steve's avatar
t steve committed
332
333
334
335
        rawdata = self.client.rbprm.rbprm.computeContactPoints(stateId)
        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]
    
    ## Provided a discrete contact sequence has already been computed, computes
336
337
338
339
340
    # 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):
t steve's avatar
t steve committed
341
342
343
344
        rawdata = self.client.rbprm.rbprm.computeContactPointsForLimb(stateId, limb)
        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
345
346
347
348
349
    # 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]
Steve Tonneau's avatar
Steve Tonneau committed
350
    def computeContactForConfig(self, q, limbName):
t steve's avatar
t steve committed
351
352
        rawdata = self.client.rbprm.rbprm.computeContactForConfig(q, limbName)
        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)]
Steve Tonneau's avatar
rebase    
Steve Tonneau committed
353

t steve's avatar
t steve committed
354
    ## Provided a discrete contact sequence has already been computed, computes
355
356
357
358
    # 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]
359
    def computeContactPointsPerLimb(self, stateId, limbs, dicEffector = {}):
t steve's avatar
t steve committed
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
        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
    #
Steve Tonneau's avatar
Steve Tonneau committed
378
379
    # \param stepSize discretization step
    # \param pathId Id of the path to compute from
380
381
382
    # \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
    def interpolateConfigs(self, configs, robustnessTreshold = 0, filterStates = False):
t steve's avatar
t steve committed
383
384
385
386
387
388
389
        if(filterStates):
            filt = 1
        else:
            filt = 0
        return self.client.rbprm.rbprm.interpolateConfigs(configs, robustnessTreshold, filt)
        
    ##
390
391
392
393
394
    # 
    # \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):
t steve's avatar
t steve committed
395
396
397
        return self.client.rbprm.rbprm.isLimbInContact(limbname, state1) >0
        
    ##
398
399
400
401
402
    # 
    # \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):
t steve's avatar
t steve committed
403
404
405
        return self.client.rbprm.rbprm.isLimbInContactIntermediary(limbname, state1) >0
        
    ##
406
407
408
409
410
    # 
    # \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):
t steve's avatar
t steve committed
411
412
413
        return [limbName for limbName in limbNames if self.isLimbInContact(limbname, state1)]
        
    ##
414
415
416
417
418
    # 
    # \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):
t steve's avatar
t steve committed
419
420
421
        return [limbName for limbName in limbNames if self.isLimbInContactIntermediary(limbname, state1)]
    
    ## returns the CWC of the robot at a given state
422
423
424
    #
    # \param stateId The considered state
    # \return H matrix and h column, such that H w <= h
425
    def getContactCone(self, stateId, friction = 0.5):
t steve's avatar
t steve committed
426
427
428
429
430
431
432
        H_h =  array(self.client.rbprm.rbprm.getContactCone(stateId, friction))
        #~ 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
433
434
435
    #
    # \param stateId The first considered state
    # \return H matrix and h column, such that H w <= h
436
    def getContactIntermediateCone(self, stateId, friction = 0.5):
t steve's avatar
t steve committed
437
438
439
440
441
442
443
        H_h =  array(self.client.rbprm.rbprm.getContactIntermediateCone(stateId, friction))
        #~ 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
444
445
446
447
448
449
450
451
452
453
    #  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):
t steve's avatar
t steve committed
454
455
456
        return self.client.rbprm.rbprm.generateRootPath(positions, quat_1, quat_2)
    
    ## Create a com trajectory given list of positions, velocities and accelerations
457
458
459
460
461
462
463
    # 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
t steve's avatar
t steve committed
464
465
    # \return id of the root path computed
    def straightPath(self, positions):
t steve's avatar
t steve committed
466
        return self.client.rbprm.rbprm.straightPath(positions)
t steve's avatar
t steve committed
467
468
469
470
471
472
473
474
475
        
    ## 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):
t steve's avatar
t steve committed
476
        return self.client.rbprm.rbprm.generateCurveTraj(positions)
t steve's avatar
t steve committed
477
478
479
480
481
482
483
484
485
486
        
    ## 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):
t steve's avatar
t steve committed
487
488
489
        return self.client.rbprm.rbprm.generateCurveTrajParts(positions, partition)
            
    ## Create a com trajectory given list of positions, velocities and accelerations
t steve's avatar
t steve committed
490
491
492
493
494
495
496
    # 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
497
498
499
500
501
    # \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):
t steve's avatar
t steve committed
502
        return self.client.rbprm.rbprm.generateComTraj(positions, velocities, accelerations, dt)
t steve's avatar
t steve committed
503
        
t steve's avatar
t steve committed
504
    ## Create a path for the root given
505
506
507
508
509
510
511
512
513
514
    #  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
515
    #  \return id of the resulting path
516
    def generateRootPathStates(self, positions, configState1, configState2):
t steve's avatar
t steve committed
517
518
519
520
        return self.client.rbprm.rbprm.generateRootPath(positions, configState1[3:7], configState2[3:7])
        
    ## Given start and goal states
    #  Provided a path has already been computed and interpolated, generate a continuous path
521
    #  between two indicated states. Will fail if the index of the states do not exist
t steve's avatar
t steve committed
522
    #
523
524
    # \param index of first state.
    # \param index of second state.
525
    # \param numOptim Number of iterations of the shortcut algorithm to apply between each states
526
    #  \return id of the resulting path
Steve Tonneau's avatar
Steve Tonneau committed
527
    def limbRRT(self, state1, state2, numOptim = 10):
t steve's avatar
t steve committed
528
529
530
531
532
533
534
535
536
537
538
539
        return self.client.rbprm.rbprm.limbRRT(state1, state2, numOptim)
        
    ## 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 
540
    #  \return id of the resulting path
Steve Tonneau's avatar
Steve Tonneau committed
541
    def limbRRTFromRootPath(self, state1, state2, path, numOptim = 10):
t steve's avatar
t steve committed
542
543
544
545
546
547
548
549
550
551
552
553
554
        return self.client.rbprm.rbprm.limbRRTFromRootPath(state1, state2, path, numOptim)        
        
        
    ## 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 
555
    #  \return id of the resulting path
Steve Tonneau's avatar
Steve Tonneau committed
556
    def comRRT(self, state1, state2, path, numOptim = 10):
t steve's avatar
t steve committed
557
558
559
560
        return self.client.rbprm.rbprm.comRRT(state1, state2, path, numOptim)        
        
            
    ## Provided a path has already been computed and interpolated, generate a continuous path
Steve Tonneau's avatar
Steve Tonneau committed
561
562
563
564
565
566
567
568
569
570
571
572
    # 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 comRRTFromPos(self, state1, comPos1, comPos2, comPos3, numOptim = 10):
t steve's avatar
t steve committed
573
574
575
576
        return self.client.rbprm.rbprm.comRRTFromPos(state1, comPos1, comPos2, comPos3, numOptim)
        
            
    ## Provided a path has already been computed and interpolated, generate a continuous path
577
578
579
580
581
582
583
584
585
586
587
588
    # 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):
t steve's avatar
t steve committed
589
590
591
592
        return self.client.rbprm.rbprm.effectorRRT(state1, comPos1, comPos2, comPos3, numOptim)
        
                    
    ## Provided a path has already been computed and interpolated, generate a continuous path
593
594
595
596
597
598
599
600
601
602
603
604
    # 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 = []):
t steve's avatar
t steve committed
605
606
607
        return self.client.rbprm.rbprm.effectorRRTFromPath(state1, refPathId, path_start, path_to, comPos1, comPos2, comPos3, numOptim, trackedEffectors)
        
    ## Project a given state into a given COM position
608
609
610
611
612
613
    # 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
    def projectToCom(self, state, targetCom):
t steve's avatar
t steve committed
614
615
616
        return self.client.rbprm.rbprm.projectToCom(state, targetCom)    
        
    ## Returns the configuration at a given state
617
618
619
620
    # Will fail if the index of the state does not exist.
    # \param state index of state.
    # \return state configuration
    def getConfigAtState(self, state):
t steve's avatar
t steve committed
621
622
623
        return self.client.rbprm.rbprm.getConfigAtState(state)        
        
    ## Project a given state into a given COM position
624
625
626
627
628
    # 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 projectStateToCOM(self, state, targetCom):
t steve's avatar
t steve committed
629
630
631
        return self.client.rbprm.rbprm.projectStateToCOM(state, targetCom)    > 0
        
    ## Project a given state into a given COM position
Steve Tonneau's avatar
Steve Tonneau committed
632
633
634
635
636
    # 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):
t steve's avatar
t steve committed
637
638
639
640
641
        return self.client.rbprm.rbprm.setConfigAtState(state, targetCom)    > 0
        
    ## Given start and goal states
    #  generate a contact sequence over a list of configurations
    #
642
643
644
    # \param stepSize discretization step
    # \param pathId Id of the path to compute from
    def isConfigBalanced(self, config, names, robustness = 0):
t steve's avatar
t steve committed
645
646
647
648
649
650
651
652
        if (self.client.rbprm.rbprm.isConfigBalanced(config, names, robustness) == 1):
            return True
        else:
            return False
        
    ## Given start and goal states
    #  generate a contact sequence over a list of configurations
    #
653
654
655
    # \param stepSize discretization step
    # \param pathId Id of the path to compute from
    def isStateBalanced(self, stateId, robustness = 0):
t steve's avatar
t steve committed
656
657
658
659
        return self.client.rbprm.rbprm.isStateBalanced(stateId) > robustness
                    
    ## Updates limb databases with a user chosen computation
    #
660
661
662
    # \param analysis name of computation
    # \param isstatic whether the computation should be used to sort samples by default
    def runSampleAnalysis(self, analysis, isstatic):
t steve's avatar
t steve committed
663
664
665
666
667
668
669
        isStatic = 0.
        if(isstatic):
            isStatic = 1.
        self.client.rbprm.rbprm.runSampleAnalysis(analysis,isStatic)
        
    ## Updates a limb database with a user chosen computation
    #
670
671
672
673
    # \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):
t steve's avatar
t steve committed
674
675
676
677
678
679
        isStatic = 0.
        if(isstatic):
            isStatic = 1.
        return self.client.rbprm.rbprm.runLimbSampleAnalysis(limbname, analysis,isStatic)
        
    ## if the preprocessor variable PROFILE is active
680
681
682
    # 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"):
t steve's avatar
t steve committed
683
        return self.client.rbprm.rbprm.dumpProfile(logFile)
684

t steve's avatar
t steve committed
685
686
    ## Create octree nodes representation for a given limb
    #
687
688
689
690
691
692
    # \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]):
t steve's avatar
t steve committed
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
        boxes = self.client.rbprm.rbprm.getOctreeBoxes(limbName, config)
        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
    #
709
710
711
    # \param limbName name of the limb considered
    # \param ocId of the octree box
    def getOctreeBox(self, limbName, ocId):
t steve's avatar
t steve committed
712
713
714
715
        return self.client.rbprm.rbprm.getOctreeBox(limbName, ocId)
    
    ## Draws robot configuration, along with octrees associated
    #
716
717
    # \param viewer gepetto viewer instance
    def draw(self, configuration, viewer):
t steve's avatar
t steve committed
718
719
720
721
722
        viewer(configuration)
        for limb, groupid in self.octrees.iteritems():
            transform = self.client.rbprm.rbprm.getOctreeTransform(limb, configuration)
            viewer.client.gui.applyConfiguration(groupid,transform)
        viewer.client.gui.refresh()
723

Steve Tonneau's avatar
Steve Tonneau committed
724

t steve's avatar
t steve committed
725
726
    ## Export motion to a format readable by the blender
    # importFromGepetto script
Steve Tonneau's avatar
Steve Tonneau committed
727
728
729
730
    # \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):
t steve's avatar
t steve committed
731
732
733
734
        em.exportStates(viewer, self.client.basic.robot, configurations, filename)
        
    ## Export motion to a format readable by the blender
    # importFromGepetto script
735
736
737
738
    # \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):
t steve's avatar
t steve committed
739
740
741
742
743
        self.exportMotion(viewer, configurations, testname+"_motion.txt")
        self.saveComputedStates(testname+"_states.txt")
        f1 = open(testname+"_configs.txt","w+")
        f1.write(str(configurations))
        f1.close()
Steve Tonneau's avatar
Steve Tonneau committed
744

Steve Tonneau's avatar
Steve Tonneau committed
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
   ## \name Degrees of freedom
    #  \{

    ## Get size of configuration
    # \return size of configuration
    def getConfigSize (self):
        return self.client.basic.robot.getConfigSize ()

    # Get size of velocity
    # \return size of velocity
    def getNumberDof (self):
        return self.client.basic.robot.getNumberDof ()
    ## \}

    ## \name Joints
    #\{

    ## Get joint names in the same order as in the configuration.
    def getJointNames (self):
        return self.client.basic.robot.getJointNames ()

    ## Get joint names in the same order as in the configuration.
    def getAllJointNames (self):
        return self.client.basic.robot.getAllJointNames ()

    ## Get joint position.
    def getJointPosition (self, jointName):
        return self.client.basic.robot.getJointPosition (jointName)

    ## Set static position of joint in its parent frame
    def setJointPosition (self, jointName, position):
        return self.client.basic.robot.setJointPosition (jointName, position)

    ## Get joint number degrees of freedom.
    def getJointNumberDof (self, jointName):
        return self.client.basic.robot.getJointNumberDof (jointName)

    ## Get joint number config size.
    def getJointConfigSize (self, jointName):
        return self.client.basic.robot.getJointConfigSize (jointName)

    ## set bounds for the joint
    def setJointBounds (self, jointName, inJointBound):
        return self.client.basic.robot.setJointBounds (jointName, inJointBound)

    ## Set bounds on the translation part of the freeflyer joint.
    #
    #  Valid only if the robot has a freeflyer joint.
    def setTranslationBounds (self, xmin, xmax, ymin, ymax, zmin, zmax):
        self.client.basic.robot.setJointBounds \
            (self.displayName + "base_joint_x", [xmin, xmax])
        self.client.basic.robot.setJointBounds \
            (self.displayName + "base_joint_y", [ymin, ymax])
        self.client.basic.robot.setJointBounds \
            (self.displayName + "base_joint_z", [zmin, zmax])

    ## Get link position in joint frame
    #
    # Joints are oriented in a different way as in urdf standard since
    # rotation and uni-dimensional translation joints act around or along
    # their x-axis. This method returns the position of the urdf link in
    # world frame.
    #
    # \param jointName name of the joint
    # \return position of the link in world frame.
    def getLinkPosition (self, jointName):
        return self.client.basic.robot.getLinkPosition (jointName)

    ## Get link name
    #
    # \param jointName name of the joint,
    # \return name of the link.
    def getLinkName (self, jointName):
        return self.client.basic.robot.getLinkName (jointName)
    ## \}

    ## \name Access to current configuration
    #\{

    ## Set current configuration of composite robot
    #
    #  \param q configuration of the composite robot
    def setCurrentConfig (self, q):
        self.client.basic.robot.setCurrentConfig (q)

    ## Get current configuration of composite robot
    #
    #  \return configuration of the composite robot
    def getCurrentConfig (self):
        return self.client.basic.robot.getCurrentConfig ()

    ## Shoot random configuration
    #  \return dofArray Array of degrees of freedom.
    def shootRandomConfig(self):
        return self.client.basic.robot.shootRandomConfig ()

    ## \}

    ## \name Bodies
    #  \{

    ##  Get the list of objects attached to a joint.
    #  \param inJointName name of the joint.
    #  \return list of names of CollisionObject attached to the body.
    def getJointInnerObjects (self, jointName):
        return self.client.basic.robot.getJointInnerObjects (jointName)


    ##  Get list of collision objects tested with the body attached to a joint
    #  \param inJointName name of the joint.
    #  \return list of names of CollisionObject
    def getJointOuterObjects (self, jointName):
        return self.client.basic.robot.getJointOuterObjects (jointName)

    ## Get position of robot object
    #  \param objectName name of the object.
    #  \return transformation as a hpp.Transform object
    def getObjectPosition (self, objectName):
        return Transform (self.client.basic.robot.getObjectPosition
                          (objectName))

    ## \brief Remove an obstacle from outer objects of a joint body
    #
    #  \param objectName name of the object to remove,
    #  \param jointName name of the joint owning the body,
    #  \param collision whether collision with object should be computed,
    #  \param distance whether distance to object should be computed.
    def removeObstacleFromJoint (self, objectName, jointName, collision,
                                 distance):
        return self.client.basic.obstacle.removeObstacleFromJoint \
            (objectName, jointName, collision, distance)


    ## \}

    ## \name Collision checking and distance computation
    # \{

    ## Test collision with obstacles and auto-collision.
    #
    # Check whether current configuration of robot is valid by calling
    # CkwsDevice::collisionTest ().
    # \return whether configuration is valid
    # \note Deprecated. Use isConfigValid instead.
    def collisionTest (self):
        print "Deprecated. Use isConfigValid instead"
        return self.client.basic.robot.collisionTest ()

    ## Check the validity of a configuration.
    #
    # Check whether a configuration of robot is valid.
    # \param cfg a configuration
    # \return whether configuration is valid
    def isConfigValid (self, cfg):
        return self.client.basic.robot.isConfigValid (cfg)

    ## Compute distances between bodies and obstacles
    #
    # \return list of distances,
    # \return names of the objects belonging to a body
    # \return names of the objects tested with inner objects,
    # \return  closest points on the body,
    # \return  closest points on the obstacles
    # \note outer objects for a body can also be inner objects of another
    # body.
    def distancesToCollision (self):
        return self.client.basic.robot.distancesToCollision ()
    ## \}

    ## \}
    ## \name Mass and inertia
    # \{

    ## Get mass of robot
    def getMass (self):
        return self.client.basic.robot.getMass ()

    ## Get position of center of mass
    def getCenterOfMass (self):
        return self.client.basic.robot.getCenterOfMass ()
    ## Get Jacobian of the center of mass
    def getJacobianCenterOfMass (self):
        return self.client.basic.robot.getJacobianCenterOfMass ()
    ##\}