0
# Joint and Motor Control
1
2
Comprehensive joint control capabilities including position, velocity, and torque control with PD controllers, multi-DOF joint support, and force/torque sensing. Essential for robotic manipulation, locomotion, and precise motion control.
3
4
## Capabilities
5
6
### Joint Information
7
8
Query joint properties, constraints, and current state for control and analysis.
9
10
```python { .api }
11
def getNumJoints(bodyUniqueId, physicsClientId=0):
12
"""
13
Get number of joints for a multibody.
14
15
Args:
16
bodyUniqueId (int): Body unique ID
17
physicsClientId (int, optional): Physics client ID. Defaults to 0.
18
19
Returns:
20
int: Number of joints in the body
21
"""
22
23
def getJointInfo(bodyUniqueId, jointIndex, physicsClientId=0):
24
"""
25
Get comprehensive joint information.
26
27
Args:
28
bodyUniqueId (int): Body unique ID
29
jointIndex (int): Joint index (0 to getNumJoints-1)
30
physicsClientId (int, optional): Physics client ID. Defaults to 0.
31
32
Returns:
33
tuple: Joint information containing:
34
- jointIndex (int): Joint index
35
- jointName (str): Joint name from URDF
36
- jointType (int): Joint type constant (p.JOINT_REVOLUTE, etc.)
37
- qIndex (int): Position index in generalized coordinates
38
- uIndex (int): Velocity index in generalized coordinates
39
- flags (int): Joint flags
40
- jointDamping (float): Joint damping coefficient
41
- jointFriction (float): Joint friction coefficient
42
- jointLowerLimit (float): Lower position limit
43
- jointUpperLimit (float): Upper position limit
44
- jointMaxForce (float): Maximum force/torque
45
- jointMaxVelocity (float): Maximum velocity
46
- linkName (str): Link name from URDF
47
- jointAxis (list): Joint axis in local frame [x, y, z]
48
- parentFramePos (list): Position in parent frame [x, y, z]
49
- parentFrameOrn (list): Orientation in parent frame [x, y, z, w]
50
- parentIndex (int): Parent link index
51
"""
52
53
def getJointState(bodyUniqueId, jointIndex, physicsClientId=0):
54
"""
55
Get current joint state including position, velocity, and forces.
56
57
Args:
58
bodyUniqueId (int): Body unique ID
59
jointIndex (int): Joint index
60
physicsClientId (int, optional): Physics client ID. Defaults to 0.
61
62
Returns:
63
tuple: Joint state containing:
64
- jointPosition (float): Current joint position
65
- jointVelocity (float): Current joint velocity
66
- jointReactionForces (list): Reaction forces/torques [Fx, Fy, Fz, Mx, My, Mz]
67
- appliedJointMotorTorque (float): Applied motor torque
68
"""
69
70
def getJointStates(bodyUniqueId, jointIndices, physicsClientId=0):
71
"""
72
Get multiple joint states efficiently.
73
74
Args:
75
bodyUniqueId (int): Body unique ID
76
jointIndices (list): List of joint indices
77
physicsClientId (int, optional): Physics client ID. Defaults to 0.
78
79
Returns:
80
list: List of joint state tuples (same format as getJointState)
81
"""
82
```
83
84
### Multi-DOF Joint Support
85
86
Support for joints with multiple degrees of freedom (spherical joints, planar joints).
87
88
```python { .api }
89
def getJointStateMultiDof(bodyUniqueId, jointIndex, physicsClientId=0):
90
"""
91
Get multi-DOF joint state (for spherical and planar joints).
92
93
Args:
94
bodyUniqueId (int): Body unique ID
95
jointIndex (int): Joint index
96
physicsClientId (int, optional): Physics client ID. Defaults to 0.
97
98
Returns:
99
tuple: Multi-DOF joint state containing:
100
- jointPosition (list): Position values (length depends on DOF)
101
- jointVelocity (list): Velocity values
102
- jointReactionForces (list): Reaction forces/torques
103
- appliedJointMotorTorque (list): Applied motor torques
104
"""
105
106
def getJointStatesMultiDof(bodyUniqueId, jointIndices, physicsClientId=0):
107
"""
108
Get multiple multi-DOF joint states efficiently.
109
110
Returns:
111
list: List of multi-DOF joint state tuples
112
"""
113
```
114
115
### Joint State Modification
116
117
Reset joint positions and velocities directly for initialization and testing.
118
119
```python { .api }
120
def resetJointState(bodyUniqueId, jointIndex, targetValue, targetVelocity=0, physicsClientId=0):
121
"""
122
Reset joint state immediately (teleport).
123
124
Args:
125
bodyUniqueId (int): Body unique ID
126
jointIndex (int): Joint index
127
targetValue (float): Target joint position
128
targetVelocity (float, optional): Target joint velocity. Defaults to 0.
129
physicsClientId (int, optional): Physics client ID. Defaults to 0.
130
131
Note:
132
This function immediately sets joint state without physics simulation.
133
Use for initialization or testing scenarios.
134
"""
135
136
def resetJointStateMultiDof(bodyUniqueId, jointIndex, targetValue, targetVelocity=None, physicsClientId=0):
137
"""
138
Reset multi-DOF joint state immediately.
139
140
Args:
141
bodyUniqueId (int): Body unique ID
142
jointIndex (int): Joint index
143
targetValue (list): Target joint positions
144
targetVelocity (list, optional): Target joint velocities
145
physicsClientId (int, optional): Physics client ID. Defaults to 0.
146
"""
147
148
def resetJointStatesMultiDof(bodyUniqueId, jointIndices, targetValues, targetVelocities=None, physicsClientId=0):
149
"""
150
Reset multiple multi-DOF joints efficiently.
151
"""
152
```
153
154
### Motor Control
155
156
Advanced motor control with multiple control modes and precise parameter tuning.
157
158
```python { .api }
159
def setJointMotorControl2(bodyUniqueId, jointIndex, controlMode, targetPosition=None, targetVelocity=None, force=None, positionGain=None, velocityGain=None, maxVelocity=None, physicsClientId=0):
160
"""
161
Set joint motor control with advanced parameters.
162
163
Args:
164
bodyUniqueId (int): Body unique ID
165
jointIndex (int): Joint index
166
controlMode (int): Control mode constant:
167
- p.POSITION_CONTROL: Position control with PD controller
168
- p.VELOCITY_CONTROL: Velocity control
169
- p.TORQUE_CONTROL: Direct torque control
170
- p.PD_CONTROL: Pure PD control
171
- p.STABLE_PD_CONTROL: Stable PD control
172
targetPosition (float, optional): Target position (position control modes)
173
targetVelocity (float, optional): Target velocity (velocity/position control)
174
force (float, optional): Maximum force/torque to apply
175
positionGain (float, optional): Position gain (Kp) for PD control
176
velocityGain (float, optional): Velocity gain (Kd) for PD control
177
maxVelocity (float, optional): Maximum velocity limit
178
physicsClientId (int, optional): Physics client ID. Defaults to 0.
179
180
Note:
181
Different control modes require different parameter combinations:
182
- POSITION_CONTROL: targetPosition, force, positionGain, velocityGain
183
- VELOCITY_CONTROL: targetVelocity, force
184
- TORQUE_CONTROL: force (direct torque)
185
- PD_CONTROL: targetPosition, targetVelocity, force, positionGain, velocityGain
186
"""
187
188
def setJointMotorControlArray(bodyUniqueId, jointIndices, controlMode, targetPositions=None, targetVelocities=None, forces=None, positionGains=None, velocityGains=None, physicsClientId=0):
189
"""
190
Set motor control for multiple joints efficiently.
191
192
Args:
193
bodyUniqueId (int): Body unique ID
194
jointIndices (list): List of joint indices
195
controlMode (int): Control mode for all joints
196
targetPositions (list, optional): List of target positions
197
targetVelocities (list, optional): List of target velocities
198
forces (list, optional): List of maximum forces
199
positionGains (list, optional): List of position gains
200
velocityGains (list, optional): List of velocity gains
201
physicsClientId (int, optional): Physics client ID. Defaults to 0.
202
203
Note:
204
More efficient than individual setJointMotorControl2 calls.
205
All parameter lists must match length of jointIndices.
206
"""
207
208
def setJointMotorControlMultiDof(bodyUniqueId, jointIndex, controlMode, targetPosition=None, targetVelocity=None, force=None, positionGain=None, velocityGain=None, physicsClientId=0):
209
"""
210
Set motor control for multi-DOF joints.
211
212
Args:
213
bodyUniqueId (int): Body unique ID
214
jointIndex (int): Joint index
215
controlMode (int): Control mode
216
targetPosition (list, optional): Target positions for each DOF
217
targetVelocity (list, optional): Target velocities for each DOF
218
force (list, optional): Maximum forces for each DOF
219
positionGain (list, optional): Position gains for each DOF
220
velocityGain (list, optional): Velocity gains for each DOF
221
physicsClientId (int, optional): Physics client ID. Defaults to 0.
222
"""
223
224
def setJointMotorControlMultiDofArray(bodyUniqueId, jointIndices, controlMode, targetPositions=None, targetVelocities=None, forces=None, positionGains=None, velocityGains=None, physicsClientId=0):
225
"""
226
Set motor control for multiple multi-DOF joints efficiently.
227
"""
228
```
229
230
### Force and Torque Sensing
231
232
Enable and configure joint force/torque sensors for feedback control and analysis.
233
234
```python { .api }
235
def enableJointForceTorqueSensor(bodyUniqueId, jointIndex, enableSensor, physicsClientId=0):
236
"""
237
Enable or disable joint force/torque sensor.
238
239
Args:
240
bodyUniqueId (int): Body unique ID
241
jointIndex (int): Joint index
242
enableSensor (int): 1 to enable sensor, 0 to disable
243
physicsClientId (int, optional): Physics client ID. Defaults to 0.
244
245
Note:
246
When enabled, getJointState returns accurate reaction forces.
247
Adds computational overhead, so only enable when needed.
248
"""
249
```
250
251
## Control Modes
252
253
### Position Control (p.POSITION_CONTROL)
254
255
PID position controller with optional velocity feedforward.
256
257
- **Use case**: Precise positioning, trajectory following
258
- **Parameters**: targetPosition, force, positionGain, velocityGain
259
- **Behavior**: Drives joint to target position with PD control
260
- **Best for**: Pick-and-place, precise manipulation tasks
261
262
### Velocity Control (p.VELOCITY_CONTROL)
263
264
Direct velocity control with maximum force limiting.
265
266
- **Use case**: Continuous motion, velocity profiles
267
- **Parameters**: targetVelocity, force
268
- **Behavior**: Maintains constant velocity until force limit reached
269
- **Best for**: Locomotion, conveyor systems, scanning motions
270
271
### Torque Control (p.TORQUE_CONTROL)
272
273
Direct force/torque application without position or velocity control.
274
275
- **Use case**: Force control, impedance control, gravity compensation
276
- **Parameters**: force (desired torque)
277
- **Behavior**: Applies constant torque regardless of position/velocity
278
- **Best for**: Force-sensitive tasks, compliant control, dynamic motions
279
280
### PD Control (p.PD_CONTROL)
281
282
Pure proportional-derivative control without built-in integrator.
283
284
- **Use case**: Custom control loops, research applications
285
- **Parameters**: targetPosition, targetVelocity, force, positionGain, velocityGain
286
- **Behavior**: PD control with user-specified gains and target velocity
287
- **Best for**: Custom control algorithms, academic research
288
289
### Stable PD Control (p.STABLE_PD_CONTROL)
290
291
Mathematically stable PD controller for improved performance.
292
293
- **Use case**: High-gain control, fast precise movements
294
- **Parameters**: Same as PD_CONTROL
295
- **Behavior**: Stable PD implementation resistant to high gains
296
- **Best for**: High-performance robotic applications
297
298
## Usage Examples
299
300
### Basic Position Control
301
302
```python
303
import pybullet as p
304
import time
305
306
p.connect(p.GUI)
307
robotId = p.loadURDF("pr2_gripper.urdf")
308
309
# Get joint information
310
numJoints = p.getNumJoints(robotId)
311
for i in range(numJoints):
312
info = p.getJointInfo(robotId, i)
313
print(f"Joint {i}: {info[1]} (Type: {info[2]})")
314
315
# Position control for multiple joints
316
jointIndices = [0, 1, 2]
317
targetPositions = [0.5, -0.3, 0.8]
318
319
for step in range(1000):
320
p.setJointMotorControlArray(robotId,
321
jointIndices,
322
p.POSITION_CONTROL,
323
targetPositions=targetPositions,
324
forces=[100, 100, 100])
325
p.stepSimulation()
326
time.sleep(1./240.)
327
```
328
329
### Advanced PD Control Tuning
330
331
```python
332
import pybullet as p
333
334
p.connect(p.GUI)
335
robotId = p.loadURDF("kuka_iiwa/model.urdf")
336
337
# High-performance PD control with custom gains
338
jointIndex = 1
339
targetPosition = 1.57 # 90 degrees
340
341
# Tuned PD gains for precise control
342
positionGain = 0.3 # Kp
343
velocityGain = 1.0 # Kd
344
345
for step in range(1000):
346
# Get current joint state for monitoring
347
jointState = p.getJointState(robotId, jointIndex)
348
currentPos = jointState[0]
349
currentVel = jointState[1]
350
351
# Apply PD control
352
p.setJointMotorControl2(robotId,
353
jointIndex,
354
p.PD_CONTROL,
355
targetPosition=targetPosition,
356
targetVelocity=0,
357
force=100,
358
positionGain=positionGain,
359
velocityGain=velocityGain)
360
361
p.stepSimulation()
362
363
# Print error for monitoring
364
if step % 100 == 0:
365
error = targetPosition - currentPos
366
print(f"Step {step}: Position error = {error:.4f}")
367
```
368
369
### Velocity Control for Scanning Motion
370
371
```python
372
import pybullet as p
373
import math
374
375
p.connect(p.GUI)
376
robotId = p.loadURDF("kuka_iiwa/model.urdf")
377
378
jointIndex = 2
379
scanSpeed = 0.5 # rad/s
380
381
for step in range(2000):
382
# Sinusoidal velocity profile for scanning
383
targetVelocity = scanSpeed * math.sin(step * 0.01)
384
385
p.setJointMotorControl2(robotId,
386
jointIndex,
387
p.VELOCITY_CONTROL,
388
targetVelocity=targetVelocity,
389
force=50)
390
391
p.stepSimulation()
392
```
393
394
### Torque Control for Gravity Compensation
395
396
```python
397
import pybullet as p
398
399
p.connect(p.GUI)
400
robotId = p.loadURDF("pr2_gripper.urdf")
401
402
# Enable force sensors for feedback
403
for i in range(p.getNumJoints(robotId)):
404
p.enableJointForceTorqueSensor(robotId, i, 1)
405
406
# Gravity compensation loop
407
for step in range(1000):
408
# Get current joint positions and velocities
409
numJoints = p.getNumJoints(robotId)
410
jointPositions = []
411
jointVelocities = []
412
413
for i in range(numJoints):
414
state = p.getJointState(robotId, i)
415
jointPositions.append(state[0])
416
jointVelocities.append(state[1])
417
418
# Calculate gravity compensation torques
419
gravityTorques = p.calculateInverseDynamics(robotId,
420
jointPositions,
421
[0] * numJoints, # Zero velocity
422
[0] * numJoints) # Zero acceleration
423
424
# Apply gravity compensation
425
for i in range(numJoints):
426
p.setJointMotorControl2(robotId, i, p.TORQUE_CONTROL, force=gravityTorques[i])
427
428
p.stepSimulation()
429
```
430
431
### Multi-DOF Spherical Joint Control
432
433
```python
434
import pybullet as p
435
436
p.connect(p.GUI)
437
# Create object with spherical joint
438
sphericalJointId = 0 # Assuming spherical joint at index 0
439
440
# Multi-DOF position control
441
targetPositions = [0.5, 0.3, 0.1] # 3 DOF for spherical joint
442
targetVelocities = [0, 0, 0]
443
forces = [10, 10, 10]
444
positionGains = [0.3, 0.3, 0.3]
445
velocityGains = [1, 1, 1]
446
447
for step in range(1000):
448
p.setJointMotorControlMultiDof(robotId,
449
sphericalJointId,
450
p.POSITION_CONTROL,
451
targetPosition=targetPositions,
452
targetVelocity=targetVelocities,
453
force=forces,
454
positionGain=positionGains,
455
velocityGain=velocityGains)
456
457
# Monitor multi-DOF joint state
458
if step % 100 == 0:
459
state = p.getJointStateMultiDof(robotId, sphericalJointId)
460
print(f"Spherical joint positions: {state[0]}")
461
462
p.stepSimulation()
463
```
464
465
### Trajectory Following with Interpolation
466
467
```python
468
import pybullet as p
469
import numpy as np
470
471
p.connect(p.GUI)
472
robotId = p.loadURDF("kuka_iiwa/model.urdf")
473
474
# Define trajectory waypoints
475
waypoints = [
476
[0, 0, 0, 0, 0, 0, 0], # Home position
477
[0.5, -0.5, 0.8, -1.2, 0, 0.5, 0], # Waypoint 1
478
[1.0, 0, 1.5, -0.8, 0.3, -0.2, 0], # Waypoint 2
479
[0, 0, 0, 0, 0, 0, 0] # Return home
480
]
481
482
# Trajectory parameters
483
totalSteps = 2000
484
stepsPerSegment = totalSteps // (len(waypoints) - 1)
485
jointIndices = list(range(7)) # 7-DOF arm
486
487
for step in range(totalSteps):
488
# Calculate current segment and interpolation factor
489
segment = min(step // stepsPerSegment, len(waypoints) - 2)
490
t = (step % stepsPerSegment) / stepsPerSegment
491
492
# Linear interpolation between waypoints
493
currentWaypoint = waypoints[segment]
494
nextWaypoint = waypoints[segment + 1]
495
496
targetPositions = []
497
for i in range(len(jointIndices)):
498
pos = currentWaypoint[i] + t * (nextWaypoint[i] - currentWaypoint[i])
499
targetPositions.append(pos)
500
501
# Apply position control
502
p.setJointMotorControlArray(robotId,
503
jointIndices,
504
p.POSITION_CONTROL,
505
targetPositions=targetPositions,
506
forces=[100] * len(jointIndices))
507
508
p.stepSimulation()
509
510
# Monitor progress
511
if step % 200 == 0:
512
print(f"Step {step}, Segment {segment}, t={t:.2f}")
513
```
514
515
## Best Practices
516
517
1. **Choose appropriate control modes** - Use position control for precision, velocity for continuous motion, torque for force control
518
2. **Tune PD gains carefully** - Start with low gains and increase gradually to avoid oscillation
519
3. **Use force limits** - Always specify reasonable force limits to prevent damage
520
4. **Batch operations** - Use array functions for multiple joints to improve performance
521
5. **Monitor joint states** - Check joint positions and forces for safety and debugging
522
6. **Enable sensors selectively** - Only enable force sensors when needed to minimize computational overhead
523
7. **Reset joint states carefully** - Use resetJointState for initialization, not during normal control
524
8. **Consider joint limits** - Respect joint position and velocity limits from URDF specifications
525
526
### External Force and Torque Application
527
528
```python { .api }
529
def applyExternalForce(objectUniqueId, linkIndex, forceObj, posObj, flags, physicsClientId=0):
530
"""
531
Apply external force to an object or link.
532
533
Args:
534
objectUniqueId (int): Object unique ID
535
linkIndex (int): Link index (-1 for base)
536
forceObj (list): Force vector [Fx, Fy, Fz] in Newtons
537
posObj (list): Position where force is applied [x, y, z]
538
flags (int): Reference frame flag (WORLD_FRAME or LINK_FRAME)
539
physicsClientId (int, optional): Physics client ID
540
"""
541
542
def applyExternalTorque(objectUniqueId, linkIndex, torqueObj, flags, physicsClientId=0):
543
"""
544
Apply external torque to an object or link.
545
546
Args:
547
objectUniqueId (int): Object unique ID
548
linkIndex (int): Link index (-1 for base)
549
torqueObj (list): Torque vector [Tx, Ty, Tz] in Newton-meters
550
flags (int): Reference frame flag (WORLD_FRAME or LINK_FRAME)
551
physicsClientId (int, optional): Physics client ID
552
"""
553
554
def resetBaseVelocity(objectUniqueId, linearVelocity, angularVelocity, physicsClientId=0):
555
"""
556
Reset base linear and angular velocity.
557
558
Args:
559
objectUniqueId (int): Object unique ID
560
linearVelocity (list): Linear velocity [vx, vy, vz]
561
angularVelocity (list): Angular velocity [wx, wy, wz]
562
physicsClientId (int, optional): Physics client ID
563
"""
564
565
def resetJointState(bodyUniqueId, jointIndex, targetValue, targetVelocity=0, physicsClientId=0):
566
"""
567
Reset joint position and velocity.
568
569
Args:
570
bodyUniqueId (int): Body unique ID
571
jointIndex (int): Joint index
572
targetValue (float): Target joint position
573
targetVelocity (float, optional): Target joint velocity
574
physicsClientId (int, optional): Physics client ID
575
"""
576
577
def resetJointStateMultiDof(bodyUniqueId, jointIndex, targetValue, targetVelocity=None, physicsClientId=0):
578
"""
579
Reset multi-DOF joint state.
580
581
Args:
582
bodyUniqueId (int): Body unique ID
583
jointIndex (int): Joint index
584
targetValue (list): Target joint values
585
targetVelocity (list, optional): Target joint velocities
586
physicsClientId (int, optional): Physics client ID
587
"""
588
```
589
590
### Force/Torque Application Examples
591
592
```python
593
import pybullet as p
594
import time
595
596
p.connect(p.GUI)
597
p.setGravity(0, 0, -9.81)
598
p.loadURDF("plane.urdf")
599
600
# Load objects
601
cube_id = p.loadURDF("cube_small.urdf", [0, 0, 1])
602
robot_id = p.loadURDF("r2d2.urdf", [1, 0, 1])
603
604
# Apply force to cube in world coordinates
605
for i in range(1000):
606
# Apply upward force to counteract gravity plus extra lift
607
p.applyExternalForce(
608
objectUniqueId=cube_id,
609
linkIndex=-1, # Base link
610
forceObj=[0, 0, 15], # Upward force in Newtons
611
posObj=[0, 0, 0], # At center of mass
612
flags=p.WORLD_FRAME
613
)
614
615
# Apply spinning torque to robot
616
p.applyExternalTorque(
617
objectUniqueId=robot_id,
618
linkIndex=-1,
619
torqueObj=[0, 0, 0.5], # Z-axis torque
620
flags=p.WORLD_FRAME
621
)
622
623
p.stepSimulation()
624
time.sleep(1./240.)
625
626
# Reset object velocities
627
p.resetBaseVelocity(cube_id, [0, 0, 0], [0, 0, 0])
628
p.resetBaseVelocity(robot_id, [0, 0, 0], [0, 0, 0])
629
630
# Reset joint to specific state
631
if p.getNumJoints(robot_id) > 0:
632
p.resetJointState(robot_id, 0, 0.5, 0) # Joint 0 to 0.5 rad, zero velocity
633
```