Joint-space Impedance Control
System and Controller Configurations
This example introduces the robust joint-level impedance control method which consists of two parts. First part is a robust control which enables to overcome the friction effect acting on robot joints and model uncertainties. Then, the impedance control is applied to the nominal robot that does not include the friction effect and model uncertainties. This control strategy can cope with the external force while ensuring the position control performance. As for the system configuration, the F/T sensor is attached to the end-effector and the interaction occurs only through the F/T sensor (for more details on the installation of F/T sensor, refer to FT Sensor-based Friction Compensation Example).
This example discusses about the following items.
- How to transform the coordinate of F/T sensor
- How to use robot dynamic models
Note
For safety, an emergency stop occurs when a certain amount of position error occurs in the actual robot. For this reason, the control algorithm introduced below may not work in certain control gain settings even if it works well in simulation.
Example Code
To begin with, generate the joint control component. Then, add the following code in the header so as to use dynamic simulation libraries and shared memory:
1 2 3 4 5 6 7 8 9 10 |
|
Moreover, define the computeImpedanceControlTorq function and several relevant variables in the header as follows:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 |
|
In the constructor, allocate the memory to the pointer variable, which allows to acquire the F/T sensor data from the shared memory (for more details, see IndyFramework or F/T Sensor and Gripper Example).
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 |
|
In the destructor, release the memory allocated for the nominal robot and shared memory.
1 2 3 4 5 6 7 8 9 |
|
In the top of this example, the computeImpedanceControlTorq function was newly defined to implement the joint-space impedance controller. Here, let us implement the PD (Proportional-Derivative)-type impedance control as shown below (i.e., stiffness-damping control).
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 |
|
The PD controller introduced above may not work well due to the effect of friction and gravity. To solve this problem, let construct the following robust controller into the computeControlTorque() function. This will attenuate the effect of friction and reduce the effect of inertia in the range of the stability margin.
-
Get F/T sensor data from the shared memeory to detect the external force. As mentioned in F/T Sensor and Gripper Example, users can get F/T sensor data using either ExtraIOData or ControlData structures. This is defined in readFTSensor function to be introduced below.
-
Change force configuration because the measured F/T sensor value is constructed in the order of (force, moment), while the Wrench (used in IndySDK) is constructed in the order of (moment, force). This is defined in readFTSensor function to be introduced below.
-
Initialize the kinematic information of nominal robot with those of real robot when the new motion is started. Then, update the state variables and velocity kinematic information.
-
Calculate the forward kinematics for both real and nominal robots. More specifically, calculate the acceleration of nominal robot using external forces, gravity forces, and impedance forces as inputs.
-
Finally, calculate the auxiliary input based on the errors between nominal robot and real robot, which compensates the friction effect and model uncertainty.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 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 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 |
|
readFTSensor function is used to get and transform F/T sensor data.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 |
|
Note that it is important to reset the controller. To this end, define the flag in the reset() function.
1 2 3 4 5 6 7 8 9 10 11 12 13 |
|
In addition to the joint position control mode, the direct teaching mode is supposed to be defined in the joint control component. In this example, apply the gravity compensation control.
1 2 3 4 5 6 7 8 9 10 11 12 |
|