Skip to content

Task-space PD Control

This section introduces Task-space PD controller as an example. The objectives of the section are as follows:

  • Learn how to use Lowpass Filter and Filtered Derivative.
  • Learn how to use Forward Kinematics and Inverse Kinematics.
  • Learn how to use Dynamic Parameters of Indy.

1) Create a Task Control Component in Eclipse.

2) Include some header files and define types as shown below.

1
2
3
4
5
6
7
8
9
#include <Indy/DynamicAnalysis6D.h>
#include <Util/Filter.h>

typedef NRMKFoundation::IndySDK::DynamicAnalysis6D DynamicAnalysis;
typedef NRMKFoundation::CompositeHTransformTaskController TaskController;
typedef NRMKFoundation::FilterLowPass<double> LowPassFilter;
typedef NRMKFoundation::FilterDerivative<double> FilteredDerivative;
typedef typename ROBOT::JointVec JointVec;
typedef typename ROBOT::JointMat JointMat;

3) Let us define several variables to design the controller and use the filters.

1
2
3
4
5
6
7
8
private:
    const double _dim_ratio;
    double _Kp;
    double _Kd;
    TaskVec _e, _edot;
    LowPassFilter _qdot_filtered[ROBOT::JOINT_DOF];
    LowPassFilter _qddot_filtered[ROBOT::JOINT_DOF];
    TaskController _taskController;

4) Initialize the variables in the constructor of UserTaskController.cpp as follows:

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

5) Set the period of the Task Controller(_taskController). Task Controller is used to define errors in the Cartesian space.

1
2
3
4
void UserTaskController::setPeriod(double delt)
{
    _taskController.setPeriod(delt);
}

6) As shown in Code 2, let us design Task-space PD controller inside computeControlTorq() function. computeFK is for calculation of the forward kinematics and computeJacobian is for calculation of the Jacobian matrix. Please refer to the commented part for dynamics and filters.

 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
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);

    // calculate dynamics
    JointMat M,C;
    JointVec g;
    DynamicAnalysis::JointDynamics(robot, M, C, g, LieGroup::Vector3D(0,0,-GRAV_ACC));

    // calculate Task-space PD control
    JointMat Kp = JointMat::Zero();
    JointMat Kd = JointMat::Zero();
    _taskController.error(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;
}