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.
| #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.
| 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:
| 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.
| 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;
}
|