Motion travels from the leader to the follower. The remote wrench returns through the leader’s Jacobian as a torque command. The following equations describe the force-feedback formulation used in the C++ implementation.움직임은 리더에서 팔로워로 전달되고, 원격 힘·토크는 리더의 Jacobian을 통해 관절 토크 명령으로 돌아옵니다. 아래 수식은 C++ 구현에 사용된 힘반사 방식을 설명합니다.
Notation: L = leader, F = follower. Spatial vectors use angular/moment components first: \(\boldsymbol{w}=[\boldsymbol{m}^{T},\boldsymbol{f}^{T}]^{T}\). Wrench and Jacobian must use matching axes, reference origins, and row order.기호: L은 리더, F는 팔로워입니다. 공간 벡터는 모멘트·각성분을 먼저 놓습니다: \(\boldsymbol{w}=[\boldsymbol{m}^{T},\boldsymbol{f}^{T}]^{T}\). 힘·토크와 Jacobian의 축, 기준 원점, 행 순서는 서로 일치해야 합니다.
01Leader motion → follower pose리더 움직임 → 팔로워 목표 자세
\[\boldsymbol{T}_{F}^{\mathrm{des}}=\operatorname{FK}_{F}(\boldsymbol{q}_{F}^{\mathrm{des}})\]
Forward kinematics converts the follower target joint angles corresponding to leader motion into a target pose.리더 움직임에 대응하는 팔로워 목표 관절각을 순기구학으로 변환하여 목표 pose를 계산합니다.
02Wrench frame transformation힘·토크 좌표변환
\[\begin{aligned}\boldsymbol{f}_{\mathrm{base}}&=\boldsymbol{R}_{\mathrm{base,sensor}}\boldsymbol{f}_{\mathrm{sensor}},\\ \boldsymbol{m}_{\mathrm{base}}&=\boldsymbol{R}_{\mathrm{base,sensor}}\boldsymbol{m}_{\mathrm{sensor}}+\boldsymbol{p}_{\mathrm{base},A}\times\boldsymbol{f}_{\mathrm{base}}.\end{aligned}\]
sensor denotes the input sensor frame, base the robot base frame, and A the force application point. The rotation matrix expresses force and moment in the base frame; the cross-product term transfers the moment to the base origin.sensor는 입력 힘·토크의 센서 좌표계, base는 로봇 기준 좌표계, A는 힘 작용점입니다. 회전 행렬로 힘과 모멘트를 기준 좌표계에 표현하고, 외적항으로 모멘트를 기준 원점으로 이전합니다.
03Jacobian-transpose force reflectionJacobian 전치 기반 힘반사
\[\boldsymbol{\tau}_{\mathrm{JT}}=\boldsymbol{G}_{\tau}\,\boldsymbol{J}_{L}^{T}(\boldsymbol{q}_{L})\,\boldsymbol{w}_{\mathrm{ref}}\]
The leader’s spatial Jacobian converts remote wrench into joint torques.리더의 공간 Jacobian으로 원격 힘·토크를 관절 토크로 변환합니다.
04Cartesian virtual impedanceCartesian 가상 임피던스
\[\begin{aligned}\boldsymbol{v}_{L}&=\boldsymbol{J}_{L}\dot{\boldsymbol{q}}_{L},\qquad \boldsymbol{a}_{L}=\dot{\boldsymbol{v}}_{L},\\ \boldsymbol{W}_{L}&=\boldsymbol{G}_{w}\boldsymbol{w}_{\mathrm{ref}}-\boldsymbol{D}_{c}\boldsymbol{v}_{L}-\boldsymbol{M}_{c}\boldsymbol{a}_{L},\\ \boldsymbol{\tau}_{\mathrm{cart}}&=\boldsymbol{J}_{L}^{T}\boldsymbol{W}_{L}.\end{aligned}\]
An alternative to per-joint reflection: scale the wrench in Cartesian space, then add velocity damping and optional virtual inertia. \(\boldsymbol{v}_{L}\) is the spatial velocity associated with the same Jacobian. This formulation renders torque while preserving the leader’s backdrivability.관절별 힘반사의 대안으로, Cartesian 공간에서 원격 힘·토크의 세기를 조절하고 속도 감쇠와 선택적인 가상 관성항을 더합니다. \(\boldsymbol{v}_{L}\)은 동일한 Jacobian에 대응하는 공간 속도입니다. 리더의 자유로운 움직임을 유지하면서 토크로 힘을 전달합니다.
05Gravity compensation & motor current중력보상과 모터 전류 출력
\[\boldsymbol{\tau}_{\mathrm{cmd}}=\boldsymbol{\tau}_{g}+\operatorname{clip}(\widetilde{\boldsymbol{\tau}}_{\mathrm{fb}})+\boldsymbol{\tau}_{\mathrm{fric}}\]
\[u_{j}=\operatorname{sat}\!\left(\operatorname{round}\!\left(\frac{\tau_{\mathrm{cmd},j}}{K_{t,j}\,c_{I,j}}\right)\right)\]
Filtered feedback, including optional joint damping, is limited per joint and combined with gravity compensation and optional friction compensation. Torque constants \(K_{t,j}\) and current-unit scales \(c_{I,j}\) convert torque to bounded Dynamixel goal-current units \(u_j\).선택적인 관절 감쇠를 포함한 피드백 토크를 필터링·제한한 뒤 중력보상과 선택적인 마찰보상을 합산합니다. 토크 상수 \(K_{t,j}\)와 전류 단위 \(c_{I,j}\)로 토크를 Dynamixel 목표 전류값 \(u_j\)으로 변환하고 최대 전류를 제한합니다.
Optional friction feedforward선택적인 마찰보상
\[\tau_{\mathrm{fric},j}=\operatorname{clip}\!\left(s_j\left[F_{c,j}\tanh\!\left(\frac{\dot q_j}{v_{\epsilon,j}}\right)+F_{v,j}\dot q_j\right]\right)\]
A smoothed Coulomb-plus-viscous model assists motion to compensate for leader joint friction. Its parameters need to be identified for the hardware; the term is optional.Coulomb 마찰과 점성 마찰 모델을 이용해 리더 관절의 마찰을 보상합니다. 매끄러운 tanh 함수로 움직임을 보조하며, 하드웨어에 맞게 계수를 식별하여 선택적으로 사용합니다.