Computed Torque 제어
본 예제에서는 아래 그림에 설명 된 Computed Torque 제어를 IndySDK를 이용하여 구현합니다.
그림 1. Computed Torque Method (CTM)
1) 첫 번째로 로봇의 동역학 모델을 사용하기 위해서 DynamicAnalysis6D.h 를 선언하고 타입 DynamicAnalysis 를 정의합니다.
1 2 | #include <Indy/DynamicAnalysis6D.h> typedef NRMKFoundation::IndySDK::DynamicAnalysis6D DynamicAnalysis; |
2) 다음으로 computeControlTorq() 함수에 아래 코드와 같이 작성합니다. 여기서 JointDynamics 함수는 M, C, g에 각각 로봇의 관성행렬, 코리올리 행렬 및 중력벡터를 반환합니다.
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 | JointMat M, C; JointVec g; DynamicAnalysis::JointDynamics(robot, M, C, g, LieGroup::Vector3D(0,0,-GRAV_ACC)); JointMat Kp, Kv; JointVec tauCTM; JointVec qddot_ref; for (int i=0; i<JOINT_DOF; ++i) { switch(i) { case 0: case 1: Kp(i,i) = 70; Kv(i,i) = 55; break; case 2: Kp(i,i) = 40; Kv(i,i) = 30; break; case 3: case 4: Kp(i,i) = 25; Kv(i,i) = 15; break; case 5: Kp(i,i) = 18; Kv(i,i) = 3; break; } } qddot_ref = qddotDesired + Kv*(qdotDesired - robot.qdot()) + Kp*(qDesired - robot.q()); tauCTM = M*qddot_ref + C*robot.qdot() + g; torque = tauCTM; |