TSID의 TsidBiped 예제는 지지발을 스윙발로 바꿀 때 발 운동 기준과 접촉 모델을 함께 다룹니다. 발 운동 task의 기준과 지지 접촉 모델이 따로 존재하기 때문입니다. 접촉을 제거할 때는 현재 발 자세를 운동 task의 기준으로 먼저 복사하고, 다시 접촉을 추가할 때는 현재 발 자세를 접촉 모델의 기준으로 잡습니다.
이 순서를 보면 스윙 궤적을 시작할 위치와 새 지지를 만들 위치가 어떻게 연결되는지 알 수 있습니다. 동시에 transition_time이라는 같은 인자가 두 helper에 있어도, 실제 아래 함수로 전달되는 방식은 다르다는 점을 확인할 수 있습니다.
발 운동 목표와 지지 접촉은 함께 존재한다
공식 tsid_biped.py는 직사각형 발로 서 있는 이족 로봇의 formulation을 구성합니다. 양발에는 Contact6d를 만들고, 각 발의 운동에는 별도로 TaskSE3Equality를 만듭니다.
접촉 모델 생성에는 발 프레임, 네 접촉점의 위치, 접촉 법선, 마찰계수와 힘 한계가 들어갑니다. 발 운동 task에는 위치·방향을 다루는 기준 궤적이 연결됩니다. 두 객체가 모두 발 자세를 참조하더라도 역할은 같지 않습니다. 하나는 formulation의 지지 접촉 모델이고, 다른 하나는 발의 원하는 운동을 표현합니다.
또 발 운동 task는 스윙을 시작할 때 처음 생성되는 것이 아닙니다. 초기화에서 leftFootTask와 rightFootTask를 만들고, 각각 addMotionTask로 등록합니다. 이후 접촉 제거 helper가 하는 일은 이미 있는 운동 task의 기준을 갱신하는 것입니다.
이 구조를 놓치고 발 목표만 위쪽으로 바꾸면, formulation에는 이전 지지 접촉 모델이 여전히 남아 있습니다. 발을 움직이라는 목표와 그 발을 지지 접촉으로 다루는 설정을 함께 바꿔야 하는 이유입니다.
지지를 뺄 때는 현재 발 자세를 운동 기준으로 복사한다
오른발을 대상으로 하는 제거 helper의 순서는 다음과 같습니다. 공식 코드의 해당 함수 본문입니다.
H_rf_ref = self.robot.framePosition(self.formulation.data(), self.RF)
self.trajRF.setReference(H_rf_ref)
self.rightFootTask.setReference(self.trajRF.computeNext())
self.formulation.removeRigidContact(self.contactRF.name, transition_time)
self.contact_RF_active = False
첫 줄은 formulation의 data에서 오른발 프레임의 자세를 읽습니다. 이어 그 자세를 trajRF의 기준으로 설정하고, 이 궤적에서 만든 샘플을 오른발 운동 task에 넣습니다. 접촉 제거 요청보다 운동 기준 갱신이 먼저입니다.
가상의 전환 상황으로, 오른발이 이전 스윙 목표와 조금 다른 자세에 서 있다고 가정해 보겠습니다. 오래된 스윙 목표를 그대로 남긴 채 지지를 제거하면, 운동 task는 그 목표와 현재 자세의 차이를 줄이려 합니다. 이 helper는 접촉을 빼는 순간의 발 자세를 기준으로 삼아, 다음 운동을 어디에서 시작할지 연결합니다.
그 다음 removeRigidContact에 접촉 이름과 transition_time을 넘기고, helper의 활성 상태 플래그를 False로 바꿉니다. 플래그 대입은 호출 직후에 일어납니다. 이 플래그는 helper의 접촉 제거 요청 상태를 기록합니다.
왼발 helper도 같은 순서를 사용합니다. 어느 발인지보다 중요한 조건은 framePosition이 읽는 data입니다. 이 함수는 새 센서 관측이나 새 q를 인자로 받지 않고, formulation에 저장돼 있는 data를 사용합니다. 호출 시점의 모델 상태가 이 data에 반영돼 있어야 의도한 현재 발 자세가 기준으로 들어갑니다.
다시 접촉을 추가할 때는 접촉 기준을 새로 잡는다
오른발 추가 helper는 먼저 현재 발 자세를 읽어 contactRF.setReference로 넘깁니다. 제거 helper가 운동 궤적과 운동 task의 기준을 바꿨다면, 여기서는 접촉 모델의 기준을 바꿉니다.
그 다음 w_contact 값에 따라 등록 호출을 고릅니다. w_contact >= 0.0이면 w_forceRef, w_contact, 1을 함께 넘기는 경로를 사용하고, 음수이면 w_forceRef까지만 넘기는 경로를 사용합니다. 마지막으로 활성 플래그를 True로 바꿉니다.
| 전환 | 현재 발 자세가 들어가는 곳 | 이어지는 formulation 호출 |
|---|---|---|
| 지지에서 스윙으로 | 발 궤적 → 발 운동 task 기준 | removeRigidContact(name, transition_time) |
| 스윙에서 지지로 | Contact6d의 접촉 기준 |
가중치 분기에 따른 addRigidContact(...) |
추가 helper는 선언부에 transition_time=0.0을 갖고 있습니다. 그러나 현재 파일의 두 addRigidContact 호출에는 그 인자가 전달되지 않습니다. 예컨대 이 helper에 0이 아닌 전환 시간을 넣어 호출해도, 그 값은 이 함수 본문의 접촉 등록 호출로 이어지지 않습니다. 제거 helper와 추가 helper의 함수 인자 이름만 보고 같은 시간 전환 동작을 한다고 판단하면 이 차이를 놓치게 됩니다.
추가 helper의 reference 갱신 대상은 contactRF입니다. 발 운동 task의 기준을 갱신하는 호출은 이 함수에 없으며, 지지 자세를 위한 접촉 기준이 현재 발 자세로 설정됩니다.
초기 발 높이와 균형 task는 별도의 역할을 맡는다
초기화에서는 SRDF의 half_sitting 자세를 가져오고, 오른발 프레임의 높이를 읽어 보정합니다. 중간 data 변수 대입을 생략해 핵심 순서만 추리면 다음과 같습니다.
q[2] -= H_rf_ref.translation[2] - conf.lz
formulation.computeProblemData(0.0, q, v)
H_rf_ref = robot.framePosition(formulation.data(), self.RF)
contactRF.setReference(H_rf_ref)
이 코드는 발 프레임의 높이를 conf.lz에 맞추려는 초기 설정입니다. 앞에서 정의한 발바닥 접촉점들의 발 좌표계 z값은 -conf.lz입니다. 오른발 높이를 기준으로 로봇의 초기 위치를 조정한 뒤, 문제 data를 다시 계산하고 그 결과로 접촉 기준 자세를 설정하는 순서입니다. 왼발 기준도 이 보정 이후의 data에서 읽습니다.
여기서도 자세를 바꾼 뒤 data를 다시 계산하고 reference를 잡는 순서가 드러납니다. 이 초기화가 사용하는 입력은 설정 파일의 발 치수와 모델에서 계산한 오른발 높이입니다.
한편 무게중심(COM)과 압력중심(COP) task는 같은 접촉 객체를 대신하는 이름이 아닙니다. 이 파일은 TaskComEquality를 addMotionTask로, TaskCopEquality를 addForceTask로 등록합니다. 몸의 운동 목표와 접촉력에 관련된 목표를 다른 종류의 task로 다루는 것입니다. 발을 어느 곳에 접촉시키는가, 몸을 어떻게 움직이는가, 접촉력을 어떻게 다루는가를 분리해 읽을 수 있습니다.
이 목표들의 균형 제어 배경은 휴머노이드의 ZMP·캡처 포인트·MPC 설명과 연결됩니다. TsidBiped에서 볼 수 있는 구현상의 핵심은 각 목표의 이름을 나열하는 것보다, 현재 발 자세가 어느 reference에 복사되고 접촉 모델이 언제 제거·등록되는지입니다. 그 순서가 지지와 스윙 사이의 계산 연결을 만듭니다.
이 글은 2026년 10월 16일 확인한 TSID 공식 TsidBiped 소스를 해석했습니다. Solver 실행이나 실제 보행 실험은 수행하지 않았습니다.