## 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 //use filters such as lowpass filter and filtered derivative #include //type definition for simplicity typedef NRMKFoundation::IndySDK::DynamicAnalysis6D DynamicAnalysis; typedef NRMKFoundation::FilterLowPass LowPassFilter; typedef NRMKFoundation::FilterDerivative 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(ROBOT & robot, double delt) { robot.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

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