rbprmfullbody.py 24.5 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
Steve Tonneau's avatar
Steve Tonneau committed
22
23
24
25
26
27
28
29
30
31
32

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

33
## Load and handle a RbprmFullbody robot for rbprm planning
Steve Tonneau's avatar
Steve Tonneau committed
34
35
36
#
#  A RbprmDevice robot is a set of two robots. One for the 
#  trunk of the robot, one for the range of motion
37
class FullBody (object):
Steve Tonneau's avatar
Steve Tonneau committed
38
39
40
41
42
43
    ## Constructor
    def __init__ (self, load = True):
        self.tf_root = "base_link"
        self.rootJointType = dict()
        self.client = CorbaClient ()
        self.load = load
Steve Tonneau's avatar
doc    
Steve Tonneau committed
44
45
46
47
48
49
50
51
52
53
	
	## 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
Steve Tonneau's avatar
Steve Tonneau committed
54
    def loadFullBodyModel (self, urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix):
55
		self.client.rbprm.rbprm.loadFullBodyRobot(urdfName, rootJointType, packageName, urdfName, urdfSuffix, srdfSuffix)
Steve Tonneau's avatar
Steve Tonneau committed
56
57
58
59
60
61
62
63
64
65
66
67
68
69
		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
70
		self.octrees={}
Steve Tonneau's avatar
Steve Tonneau committed
71
72
73
74
75
76
77
		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
doc    
Steve Tonneau committed
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
	## 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 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.
94
95
96
    def addLimb(self, limbId, name, effectorname, offset, normal, x, y, samples):
		self.client.rbprm.rbprm.addLimb(limbId, name, effectorname, offset, normal, x, y, samples, "EFORT", 0.03)

Steve Tonneau's avatar
Steve Tonneau committed
97
98
99
100
101
102
	## 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
103
    # \param loadValues: whether values computed, other than the static ones, should be loaded in memory
104
105
    # \param disableEffectorCollision: whether collision detection should be disabled for end effector bones
    def addLimbDatabase(self, databasepath, limbId, heuristicName, loadValues = True, disableEffectorCollision = False):		
106
		boolVal = 0.
107
		boolValEff = 0.
108
109
		if(loadValues):
			boolVal = 1.
110
111
112
		if(disableEffectorCollision):
			boolValEff = 1.
		self.client.rbprm.rbprm.addLimbDatabase(databasepath, limbId, heuristicName, boolVal,boolValEff)		
Steve Tonneau's avatar
Steve Tonneau committed
113

114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
	## 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 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.
130
    # \param contactType whether the contact is punctual ("_3_DOF") or surfacic ("_6_DOF")
131
132
133
134
135
136
    # \param disableEffectorCollision: whether collision detection should be disabled for end effector bones
    def addLimb(self, limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution, contactType="_6_DOF",disableEffectorCollision = False):
		boolValEff = 0.
		if(disableEffectorCollision):
			boolValEff = 1.
		self.client.rbprm.rbprm.addLimb(limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution,contactType, boolValEff)
137

Steve Tonneau's avatar
doc    
Steve Tonneau committed
138
139
140
141
	## Returns the configuration of a limb described by a sample
	#
    # \param name name of the limb considered
    # \param idsample identifiant of the sample considered
142
143
    def getSample(self, name, idsample):
		return self.client.rbprm.rbprm.getSampleConfig(name,idsample)
Steve Tonneau's avatar
Steve Tonneau committed
144
		
Steve Tonneau's avatar
doc    
Steve Tonneau committed
145
146
147
148
149
	## 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
Steve Tonneau's avatar
Steve Tonneau committed
150
151
    def getSamplePosition(self, name, idsample):
		return self.client.rbprm.rbprm.getSamplePosition(name,idsample)
152

Steve Tonneau's avatar
doc    
Steve Tonneau committed
153
154
155
156
157
158
	## 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
Steve Tonneau's avatar
Steve Tonneau committed
159
160
    def generateContacts(self, configuration, direction):
		return self.client.rbprm.rbprm.generateContacts(configuration, direction)
161
		
Steve Tonneau's avatar
doc    
Steve Tonneau committed
162
163
164
165
166
	## 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
167
168
    def getContactSamplesIds(self, name, configuration, direction):
		return self.client.rbprm.rbprm.getContactSamplesIds(name, configuration, direction)
Steve Tonneau's avatar
Steve Tonneau committed
169
		
170
171
172
173
174
175
176
177
	## 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):
		return self.client.rbprm.rbprm.getSamplesIdsInOctreeNode(limbName, octreeNodeId)
		
Steve Tonneau's avatar
Steve Tonneau committed
178
179
180
181
182
183
	## 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):
		return self.client.rbprm.rbprm.getNumSamples(limbName)
		
184
185
186
187
188
189
	## Get the number of octreeNodes
	#
    # \param limbName name of the limb from which to retrieve a sample
    def getOctreeNodeIds(self, limbName):
		return self.client.rbprm.rbprm.getOctreeNodeIds(limbName)
		
Steve Tonneau's avatar
Steve Tonneau committed
190
191
192
193
194
195
196
197
	## 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):
		return self.client.rbprm.rbprm.getSampleValue(limbName, valueName, sampleId)
		
Steve Tonneau's avatar
doc    
Steve Tonneau committed
198
199
200
201
202
	## Initialize the first configuration of the path discretization 
	# with a balanced configuration for the interpolation problem;
	#
    # \param configuration the desired start configuration
    # \param contacts the array of limbs in contact
Steve Tonneau's avatar
Steve Tonneau committed
203
204
    def setStartState(self, configuration, contacts):
		return self.client.rbprm.rbprm.setStartState(configuration, contacts)
Steve Tonneau's avatar
Steve Tonneau committed
205
			
Steve Tonneau's avatar
doc    
Steve Tonneau committed
206
207
208
209
210
	## 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		
Steve Tonneau's avatar
Steve Tonneau committed
211
212
    def setEndState(self, configuration, contacts):
		return self.client.rbprm.rbprm.setEndState(configuration, contacts)
Steve Tonneau's avatar
doc    
Steve Tonneau committed
213
214
215
216
	
	## Saves a computed contact sequence in a given filename
	#
    # \param The file where the configuration must be saved
Steve Tonneau's avatar
Steve Tonneau committed
217
218
    def saveComputedStates(self, filename):
		return self.client.rbprm.rbprm.saveComputedStates(filename)
Steve Tonneau's avatar
doc    
Steve Tonneau committed
219
	
Steve Tonneau's avatar
Steve Tonneau committed
220
221
222
223
224
225
226
	## 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):
		return self.client.rbprm.rbprm.saveLimbDatabase(limbName, filename)
	
Steve Tonneau's avatar
doc    
Steve Tonneau committed
227
228
229
230
231
	## 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
232
    # \param pathId Id of the path to compute from
233
234
235
    # \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
    def interpolate(self, stepsize, pathId = 1, robustnessTreshold = 0):
		return self.client.rbprm.rbprm.interpolate(stepsize, pathId, robustnessTreshold)
236
237
238
239
240
241
242
243
244
	
	## 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 stateId id of the first state
    # \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).
    def computeContactPoints(self, stateId):
Steve Tonneau's avatar
rebase    
Steve Tonneau committed
245
		rawdata = self.client.rbprm.rbprm.computeContactPoints(stateId)
246
		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]
Steve Tonneau's avatar
rebase    
Steve Tonneau committed
247

Steve Tonneau's avatar
Steve Tonneau committed
248
		
Steve Tonneau's avatar
Steve Tonneau committed
249
250
251
252
253
254
255
256
	## 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
    def interpolateConfigs(self, configs):
		return self.client.rbprm.rbprm.interpolateConfigs(configs)
		
257
258
259
260
261
262
	## 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.
263
264
265
    # \param numOptim Number of iterations of the shortcut algorithm to apply between each states
    def interpolateBetweenStates(self, state1, state2, numOptim = 10):
		return self.client.rbprm.rbprm.interpolateBetweenStates(state1, state2, numOptim)
266
		
267
268
269
270
271
272
273
274
275
276
277
278
	## 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 
    def interpolateBetweenStatesFromPath(self, state1, state2, path, numOptim = 10):
		return self.client.rbprm.rbprm.interpolateBetweenStatesFromPath(state1, state2, path, numOptim)		
279
280
		
		
281
282
283
284
285
286
287
288
289
290
291
	## 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
    def isConfigBalanced(self, config, names, robustness = 0):
		if (self.client.rbprm.rbprm.isConfigBalanced(config, names, robustness) == 1):
			return True
		else:
			return False
		
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
	## 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.
		self.client.rbprm.rbprm.runSampleAnalysis(analysis,isStatic)
		
	## 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.
311
		return self.client.rbprm.rbprm.runLimbSampleAnalysis(limbname, analysis,isStatic)
312
		
313
314
315
316
317
318
319
320
	## 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]):
321
		boxes = self.client.rbprm.rbprm.getOctreeBoxes(limbName, config)
322
323
324
325
326
327
328
329
330
331
332
333
		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()
334
335
336
337
338
339
340
		
	## 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):
		return self.client.rbprm.rbprm.getOctreeBox(limbName, ocId)
341
342
343
344
345
346
347
348
349
350
351
	
	## 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():
		transform = self.client.rbprm.rbprm.getOctreeTransform(limb, configuration)
		viewer.client.gui.applyConfiguration(groupid,transform)
	viewer.client.gui.refresh()

Steve Tonneau's avatar
Steve Tonneau committed
352
353
354
355
356
357
358
359

	## 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):
		em.exportStates(viewer, self.client.basic.robot, configurations, filename)
360
361
362
363
364
365
366
367
368
369
370
371
		
	## 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()
Steve Tonneau's avatar
Steve Tonneau committed
372

Steve Tonneau's avatar
Steve Tonneau committed
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
   ## \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 ()
    ##\}