로봇 손의 목표 위치와 방향을 지정하는 것만으로는 이동 중의 방향까지 정해지지 않습니다. mplib에서 경로를 따라 특정 자세 관계를 유지하려면, 각 관절 상태가 그 관계를 만족하는지 계산하는 제약 함수와 그 Jacobian을 계획기에 전달합니다. 특정 관절을 움직이지 않게 잠그는 방법도 있지만, 이는 말단의 방향 관계를 제한하는 것과 다른 조건입니다.
아래에서는 mplib 0.2.1 stable의 Constrained Planning 예제를 사용합니다. 예제는 말단의 z축과 아래쪽 방향 사이의 관계를 경로 전체에 적용하며, 제약을 넣지 않은 경로에서는 말단이 옆으로 기울 수 있음을 보여 줍니다.
목표점 사이에도 방향 조건을 넣는다
말단에 긴 막대가 달려 있고, 시작점과 도착점에서 막대의 방향이 같다고 가정해 보겠습니다. 그 두 자세를 연결하는 관절 경로는 여럿일 수 있습니다. 중간에 팔꿈치를 크게 돌려 막대가 기울었다가, 도착 직전에 원래 방향으로 돌아오는 경로도 끝점 조건만 보면 같을 수 있습니다.
이때 제약 함수 f(q)는 관절 상태 q를 받아 원하는 관계에서 얼마나 벗어났는지를 숫자로 돌려줍니다. 공식 예제는 아래와 같은 잔차를 만듭니다.
f(q) = 말단 z축 · 아래쪽 단위벡터 − 0.966
아래쪽 단위벡터 = (0, 0, −1)
목표 관계: f(q) = 0
말단 z축도 방향을 나타내는 단위벡터로 읽으면, 두 벡터의 내적은 사이각의 코사인입니다. 0.966은 약 15도의 코사인에 해당하므로, 잔차가 0인 상태는 두 방향 사이의 각도가 약 15도인 관계를 뜻합니다.
여기서 유지하는 것은 말단의 전체 자세가 아니라 z축과 아래쪽 방향 사이의 관계입니다. 같은 기울기를 가진 방향이 여럿 있을 수 있고, 말단이 자기 z축 둘레로 회전해도 그 축의 방향 자체는 바뀌지 않습니다. 이 잔차 하나가 모든 회전 자유도를 고정한다고 읽으면 필요한 조건을 빠뜨리게 됩니다.
15도 목표와 0.05 허용오차는 서로 다른 숫자다
문서의 코드는 plan_pose에 constraint_tolerance=0.05를 전달합니다. 이는 투영 과정에서 사용하는 제약 잔차의 허용오차이지, 말단 각도를 0.05도 이내로 맞추라는 뜻이 아닙니다. 잔차가 코사인 값의 차이이므로 각도와 숫자가 일대일로 같은 크기로 움직이지도 않습니다.
다음은 예제의 식에 가상의 세 방향을 대입한 계산입니다. 실제 경로 계획의 성공·실패 결과가 아닙니다.
| 아래쪽과의 사이각 | cos(각도) − 0.966 |
잔차 크기를 0.05와 비교하면 |
|---|---|---|
| 0도 | 0.034 | 0.05보다 작음 |
| 15도 | 약 −0.000074 | 0에 매우 가까움 |
| 30도 | 약 −0.099975 | 0.05보다 큼 |
정확히 아래를 보는 0도도 이 잔차 비교에서는 0.05 안에 들어옵니다. 따라서 문서의 “15도 유지”라는 표현만 보고 정확히 15도인 경로가 나올 것으로 기대하기보다, 식과 허용오차를 함께 읽어야 합니다.
또한 f(q)=0이라는 정의는 “아래쪽에서 15도 이내면 모두 허용”이라는 부등식과 다릅니다. 전자는 특정 코사인 값을 맞추는 등식이고, 후자는 각도가 일정 범위 안에 있는 영역을 정합니다. 원하는 것이 특정 기울기 유지인지, 최대 기울기 제한인지 먼저 정해야 알맞은 제약을 표현할 수 있습니다. 위 표의 잔차 하나가 작더라도 다른 관절·충돌·탐색 조건은 별도로 남습니다.
Jacobian은 관절을 어느 방향으로 바꿀지 알려 준다
mplib 문서는 이 기능을 OMPL의 투영 기반 계획 방식으로 설명합니다. 관절 상태를 샘플링한 뒤, 제약 함수를 사용해 조건을 만족하는 상태로 투영하는 흐름입니다. 잔차 값만 있으면 현재 상태가 목표에서 벗어났다는 것은 알 수 있지만, 관절을 조금 바꿀 때 잔차가 어떻게 움직이는지도 필요합니다.
그 변화율이 제약 함수의 Jacobian입니다. 공식 예제는 말단 Jacobian에서 회전에 해당하는 부분을 가져오고, 말단 z축과의 외적을 사용해 각 관절이 방향 잔차를 얼마나 바꾸는지 계산합니다. 말단의 위치·회전 변화량을 표현하는 원래 Jacobian을 그대로 전달하는 것이 아니라, 선택한 스칼라 잔차의 변화율로 가공하는 것입니다.
예를 들어 제약 함수를 높이 오차로 바꿨는데 Jacobian은 여전히 z축 방향의 변화율을 반환한다면, 두 함수가 서로 다른 문제를 설명하게 됩니다. constraint_function과 constraint_jacobian은 이름만 채우는 한 쌍의 인자가 아니라 같은 관계를 계산해야 하는 짝입니다. 문서는 투영에 이 계산이 사용되므로 Jacobian 계산 속도도 계획 성능에 영향을 준다고 설명합니다.
관절을 잠그면 유지하는 대상이 바뀐다
Planning With Fixed Joints 예제는 레일 위의 팔을 두 단계로 움직입니다. 이 예제의 관절 순서에서는 0·1번이 x·y 레일이고, 2번부터 8번까지가 팔 관절입니다.
move_in_two_stage는 먼저 목표 자세에 대한 전체 IK를 구하고, fixed_joint_indices=range(2, 9)를 사용해 팔 관절을 고정한 채 베이스 쪽 경로를 계획합니다. 이어 베이스 관절 0·1번을 고정하고 팔을 움직이는 단계로 넘어갑니다. Python의 range(2, 9)에는 마지막 숫자 9가 포함되지 않으므로, 여기서 잠그는 것은 2~8번입니다. 다른 로봇에 같은 숫자를 붙이기 전에 실제 관절 이름과 순서를 맞춰야 합니다.
이 방법은 특정 관절 값을 유지하는 조건입니다. 반면 앞의 방향 제약은 여러 관절이 함께 움직이더라도 말단 방향의 잔차가 조건에 맞도록 하는 것입니다. 레일과 팔을 순서대로 움직이고 싶다면 관절 고정이 직접적인 표현이고, 팔의 여러 관절을 사용하면서 도구의 기울기 관계를 유지하고 싶다면 말단 방향에 대한 제약이 더 직접적인 표현입니다.
따라서 계획 입력을 정할 때는 “움직이지 말아야 할 관절”과 “움직이는 동안 유지할 말단의 관계”를 구분해 적어 보세요. 같은 정지 화면으로 끝나는 두 계획도 중간에 허용하는 움직임은 달라집니다. 목표 관절 해를 찾는 IK와 그 해까지 연결하는 경로 탐색의 구분은 mplib의 move_group·IK·경로 실패 설명에서 이어집니다.
2026년 10월 16일 확인한 공식 튜토리얼의 설명과 포함된 코드를 해석했습니다. mplib 설치, 경로 계획 또는 실제 로봇 실행은 수행하지 않았습니다.