Skip to content

Task-space PD Control

Outlines

In this section, the task-space PD control is implemented in the simulation environment (please note that this example has been verified only in simulations. The controller may not work properly on real robot).

This helps to understand the following items.

  • How to use lowpass filter and filtered derivative.
  • How to use forward kinematics and inverse kinematics.
  • How to get dynamic parameters of Indy.

Example Code

In the Eclipse, generate Task Control Component and fill in the header like follows:

  • DynamicAnalysis6D.h: To get dynamic parameters of Indy.
  • Filter.h: To use lowpass filter and filtered derivative.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
//use Indy7's dynamic parameters
#include <Indy/DynamicAnalysis6D.h>

//use filters such as lowpass filter and filtered derivative
#include <Util/Filter.h>

//type definition for simplicity
typedef NRMKFoundation::IndySDK::DynamicAnalysis6D DynamicAnalysis;
typedef NRMKFoundation::FilterLowPass<double> LowPassFilter;
typedef NRMKFoundation::FilterDerivative<double> FilteredDerivative;
typedef typename ROBOT::JointVec JointVec;
typedef typename ROBOT::JointMat JointMat;

Define the following variables to set control gains and use filters.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
enum
{
    JOINT_DOF = AbstractRobot6D::JOINT_DOF
};
private:
    const double _dim_ratio;
    double _Kp;
    double _Kd;
    TaskVec _e, _edot;
    LowPassFilter _qdot_filtered[ROBOT::JOINT_DOF];
    LowPassFilter _qddot_filtered[ROBOT::JOINT_DOF];
  • dim_ratio: Define the ratio of control gains between displacement and rotation.
  • K_{p}, K_{d}: Control gains of PD controller.

Initialize control gains in the constructor of UserTaskController.cpp like follows:

1
2
3
4
5
6
UserTaskController::UserTaskController()
:_dim_ratio(1.0/30.0)
{
    _Kp = 500;
    _Kd = 80;
}

Set the sampling time of initTaskErr() function. This is used to calculate task position errors.

1
2
3
4
void UserTaskController::initialize(double delt)
{
    initTaskErr(delt);
}

As shown in below, add the PD control algorithm to the computeControlTorq() function. Herein, computeFK() and computeJacobian() functions are respectively used to calculate the forward kinematics and the Jacobian matrix. Please refer to the comments for more details on dynamic parameter calculation and filter usage.

 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
int UserTaskController::computeControlTorq(ROBOT & robot, TaskPosition const & posDesired, TaskVelocity const & velDesired, TaskAcceleration const & accDesired, JointVec & torque)
{
    // calculate task kinematics
    TaskPosition pos;
    TaskVelocity vel;
    TaskAcceleration acc;
    TaskJacobian J;
    TaskJacobian Jdot;
    robot.computeFK(pos,vel);
    robot.computeJacobian(pos,vel,J,Jdot);

    // define matrices for calculating robot dynamics
    JointMat M,C;
    JointVec g;

    //use DynamicAnalysis's library
    DynamicAnalysis::JointDynamics(robot, M, C, g, LieGroup::Vector3D(0,0,-GRAV_ACC));

    //use AbstractRobot6D's library
    //robot.computeDynamicsParams(robot, M, C, g, LieGroup::Vector3D(0,0,-GRAV_ACC));


    // calculate Task-space PD control
    JointMat Kp = JointMat::Zero();
    JointMat Kd = JointMat::Zero();

    // calculate errors in the task domain
    // this function is especially for rotation error calculations
    robot.computeTaskErr(pos, vel, posDesired, velDesired, _e, _edot);

    // set control gains
    for (int i=0; i<ROBOT::JOINT_DOF;i++)
    {
        if(i<3)
        {
            Kp(i,i) = _Kp*_dim_ratio;
            Kd(i,i) = _Kd*_dim_ratio;
        }
        else
        {
            Kp(i,i) = _Kp;
            Kd(i,i) = _Kd;
        }
    }

    // calculate filtered values
    double temp_vel, temp_acc;
    JointVec qdot, qddot;
    temp_vel = 0; temp_acc = 0;
    for (int i=0; i<JOINT_DOF; i++)
    {
        // lowpass filter
        _qdot_filtered[i].filter(robot.qdot()[i],temp_vel);
        // filtered derivative
        _qddot_filtered[i].filter(robot.qdot()[i],temp_acc);

        qdot(i,0) = temp_vel;
        qddot(i,0) = temp_acc;
    }
    // PD control input
    JointVec forcePD = Kp*_e + Kd*_edot;
    torque = J.transpose()*forcePD + M*qddot + C*qdot + g;

    return 0;
}

Task-space PD control and gravity compensation for multi-axis robot manipulators