Pinocchio의 CLIK 예제는 관절 Jacobian을 구한 뒤 그대로 역기구학에 넣지 않습니다. 목표 자세와 현재 자세의 로그 오차를 정의하고, Jlog6로 그 오차의 변화에 맞는 Jacobian을 만듭니다. 관절 움직임이 말단의 공간 속도로 이어지는 관계와, 그 움직임이 지금 정의한 오차를 얼마나 줄이는지는 서로 다른 계산이기 때문입니다.
공식 예제의 전체 흐름은 현재 자세 계산 → 상대 자세의 로그 오차 → 오차 Jacobian → 감쇠를 넣은 수정량 → 다음 자세입니다. 이 순서를 따라가면 Jlog6가 들어가는 위치뿐 아니라 마지막에 어떤 오차를 보고 멈춰야 하는지도 이해할 수 있습니다.
먼저 오차가 어느 좌표계의 무엇인지 정한다
공식 Python 예제는 샘플 manipulator 모델에서 6번 관절을 대상으로 삼습니다. 목표 oMdes는 위치와 회전을 함께 담는 SE3 객체입니다. 반복문에서는 현재 관절 배치를 계산한 뒤 다음 두 줄로 오차를 만듭니다.
iMd = data.oMi[JOINT_ID].actInv(oMdes)
err = pinocchio.log(iMd).vector
data.oMi[JOINT_ID]는 현재 자세의 순기구학 결과입니다. actInv(oMdes)는 그 현재 자세를 기준으로 목표 자세를 표현하는 상대 변환을 만듭니다. 이를 위치·회전이 함께 있는 변환으로 쓴다면 현재 자세의 역변환 × 목표 자세입니다. 코드의 주석대로 오차는 현재 관절 좌표계에서 다룹니다.
log(iMd)는 이 상대 자세를 6차원 로그 오차로 표현합니다. 단순히 목표 위치에서 현재 위치를 빼는 3차원 벡터와는 구별됩니다. 회전 차이까지 포함하며, 위치와 회전을 함께 다루는 SE3의 로그 연산을 거칩니다.
공식 설명 문서에는 이 부분에서 변수명 iMd·dMi가 섞여 있고, 6차원 오차를 설명하면서 SO(3)를 언급하는 표기가 있습니다. 여기서는 실제 Python 코드의 SE3, iMd, log(iMd)를 기준으로 설명합니다. 회전만 나타내는 SO(3)와 위치·회전을 함께 나타내는 SE3를 같은 것으로 읽으면 다음 Jacobian의 역할도 혼동하기 쉽습니다.
공간 속도 Jacobian에서 오차 Jacobian으로
예제는 현재 관절 좌표계의 Jacobian을 먼저 계산합니다. 이를 J_joint라고 부르면, 다음 줄에서 계산하는 오차 Jacobian은 아래와 같습니다. Jacobian 변수명을 역할에 맞게 나눈 코드입니다.
J_joint = pinocchio.computeJointJacobian(model, data, q, JOINT_ID)
J_error = -np.dot(pinocchio.Jlog6(iMd.inverse()), J_joint)
J_joint는 관절의 작은 움직임과 대상 관절의 공간 운동을 연결합니다. 그런데 줄이려는 값은 공간 속도 자체가 아니라 앞에서 만든 log(iMd)입니다. 상대 자세가 바뀔 때 로그 오차가 어떻게 변하는지까지 연결하려면 로그 연산의 미분이 필요합니다. 이 지점에 Jlog6가 들어갑니다.
이를 연쇄 미분의 관점에서 보면, 관절 변화가 상대 자세를 바꾸고 그 상대 자세가 로그 오차를 바꾸는 두 관계를 연결하는 셈입니다. 따라서 Jlog6를 위치와 회전의 중요도를 조절하는 가중치나 특이점 회피용 damping으로 해석하면 역할이 바뀝니다. 가중치·damping과는 다른, 지금 정의한 잔차를 미분하는 항입니다.
마이너스 부호와 iMd.inverse()도 함께 읽어야 합니다. 이 조합은 현재 코드가 상대 자세와 오차를 정의한 방식에 연결돼 있습니다. 상대 변환의 방향을 뒤집거나 다른 좌표계로 잔차를 표현한 뒤 같은 부호만 붙이는 식으로 옮기면, 미분하려는 오차와 계산식이 맞지 않을 수 있습니다.
수정량을 만든 뒤 새 자세에서 다시 평가한다
공식 코드에서 변환된 J는 다음 계산에 사용됩니다.
v = -J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err))
q = pinocchio.integrate(model, q, v * DT)
첫 줄은 J Jᵀ에 damping 항을 더한 선형계를 풀고, 그 결과를 관절 쪽 수정량으로 옮깁니다. 문서는 특이점 부근의 문제를 줄이기 위해 감쇠 의사역행렬을 사용한다고 설명합니다. 코드가 solve를 사용하는 점도 중요합니다. 행렬의 역행렬을 따로 만들어 곱하는 형태로 작성한 것이 아닙니다.
둘째 줄의 DT는 이 수정량을 이번 반복에 얼마나 적용할지 정합니다. 여기서 계산한 v와 DT는 예제의 반복 수렴에 쓰이는 값입니다. 그대로 실제 구동기의 속도 명령과 제어 주기로 읽기보다, 새 후보 자세를 만드는 갱신량과 스텝으로 보는 편이 맞습니다.
다음 반복에서는 바뀐 q로 순기구학과 오차를 다시 계산합니다. 앞선 Jacobian은 이전 자세에서의 국소 관계이므로, 한 번 계산한 수정량이 전체 경로를 끝까지 설명하는 구조가 아닙니다. 이 예제는 매번 현재 오차에 맞춰 다음 자세를 찾습니다.
예제의 damping과 스텝 값은 이 샘플에 넣은 설정입니다. 설명 문서도 고정 damping이 언제나 가장 좋은 선택은 아니라고 명시합니다. 큰 오차나 특이점 근처에서의 갱신을 이해할 때는 damping과 스텝이 어느 계산에 들어가는지를 먼저 구분해야 합니다. Damping은 선형계에, DT는 계산한 관절 수정량에 적용됩니다.
위치와 회전의 종료 조건을 따로 읽는다
예제의 종료 판정은 norm(err) < eps입니다. 그러나 이 6차원 값의 선형 성분과 각 성분은 물리 단위가 다릅니다. 공식 문서도 두 종류를 하나의 norm으로 합치는 것은 엄밀한 물리 판정이 아니며, 선형부와 각부를 따로 다루는 편이 적절하다고 설명합니다.
실제 작업의 요구가 끝점 위치 정확도와 도구 방향 정확도로 주어졌다면, 이를 각자의 허용치로 표현할 수 있습니다. 다음은 가상의 종료 기준 예시입니다. 위치 거리 오차 2mm 이하와 방향 각도 오차 1도 이하를 모두 요구한다고 가정합니다.
| 후보 자세 | 위치 거리 오차 | 방향 각도 오차 | 가상 기준의 판정 |
|---|---|---|---|
| A | 1mm | 3도 | 위치는 충족, 방향은 계속 줄여야 함 |
| B | 3mm | 0.5도 | 방향은 충족, 위치는 계속 줄여야 함 |
| C | 1mm | 0.5도 | 두 기준을 모두 충족 |
이 표는 로그 벡터의 원소를 그대로 mm·도로 이름만 바꾼 것이 아닙니다. 작업에서 정의한 위치 거리와 방향 각도를 독립적으로 평가하는 예입니다. SE3 로그의 선형부를 사용할지, 말단 위치의 거리 오차를 직접 사용할지까지 포함해 잔차의 정의와 허용치의 의미를 맞춰야 합니다. 최적화 내부의 잔차와 독자가 요구하는 물리 정확도가 어떻게 연결되는지를 명확히 하는 것입니다.
또 이 코드가 반복 한도에 도달해 success=False가 되는 것은, 주어진 초기 자세·갱신법·설정으로 정한 정밀도에 도달하지 못했다는 결과입니다. 코드가 전체 작업공간을 탐색해 목표의 도달 가능성을 판정한 결과와는 다릅니다. 종료 상태를 해석할 때도 실제로 검사한 조건에 맞춰 읽어야 합니다.
관절 각도를 찾는 역기구학과 그 운동에 필요한 힘을 계산하는 역동역학의 큰 차이는 역기구학과 역동역학 설명에서 볼 수 있습니다. 이 CLIK 예제에서 추가로 배우는 부분은, 각도를 반복해서 찾을 때 자세 잔차와 그 미분, 갱신량, 종료 기준을 같은 정의로 연결하는 방법입니다.
이 글은 2026년 10월 16일 확인한 Pinocchio 공식 설명과 Python 예제의 저장된 소스를 해석했습니다. 예제 실행이나 실제 로봇 실험은 수행하지 않았습니다.