# sim/model/kinematics/ik.py
def compute_fk_all_links_recursive(
    node: BodyNode,
    state: RobotState,
    home_pose,
    cum_exp,  # 누적 exponential product
    all_link_poses,
    include_node_joints=True,
):
    if include_node_joints:  # 현재 노드에 달린 joint 변환을 FK에 반영할지 정하는 flag
        for joint in node.joints:
            joint_transform = home_pose[node.name]
            q = joint_transform[:3, :3] @ joint["pos"] + joint_transform[:3, 3]
            w = joint_transform[:3, :3] @ joint["axis"]
            S = unit_screw_axis(q, w, 0, joint["type"])
            theta = state.get(
                joint["name"]
            )  # PoE에서 세타는 기준 자세로부터 joint displacement

            # M(home configuration)이 0이 아니면 목표 관절값에서 M만큼 빼줌
            if "_qpos" in home_pose:
                theta -= home_pose["_qpos"][joint["qpos_addr"]]

            S_exp = screw_hat(S) * theta
            cum_exp = cum_exp @ expm(S_exp)

    all_link_poses[node.name] = cum_exp @ home_pose[node.name]

    for child in node.children:
        compute_fk_all_links_recursive(child, state, home_pose, cum_exp, all_link_poses)

    return

# sim/model/kinematics/ik.py
def solve_newton_raphson_geometric(
    robot: RobotModel,
    state: RobotState,
    target_pos,
    rot,
    home_pose,
    target_body="arm_r_link7",
):
    T_sb = robot.body_node_for(target_body).world_transform
    T_sd = T_sb.copy()
    T_sd[:3, 3] = target_pos
    T_sd[:3, :3] = rot  # 기준 회전에 RPY offset을 적용한 절대 목표 회전

    theta_prev = state.qpos.copy()
    theta = state.qpos.copy()
    iter = 0

    while True:
        state.qpos = theta.copy()
        link_poses = compute_fk(robot, state, home_pose)
        T_sb = link_poses[target_body]
        twist_error_mat, twist_error = calculate_twist_error(T_sb, T_sd)

        # space frame 기준 자코비안 행렬 J
        J, joints = compute_geometric_jacobian(robot, link_poses, target_body)

        J_b = (
            Adjoint(np.linalg.inv(T_sb)) @ J
        )  # body frame로의 좌표계 변환을 위해 big adjoint 연산 수행
        rows, cols = J_b.shape

        if rows == cols:
            dq = np.linalg.pinv(J_b) @ twist_error
        elif rows > cols:
            dq = np.linalg.pinv(J_b.T @ J_b) @ J_b.T @ twist_error
        else:
            dq = J_b.T @ np.linalg.pinv(J_b @ J_b.T) @ twist_error

        for joint, dq_i in zip(joints, dq):
            theta[joint["qpos_addr"]] += dq_i

        theta = clamp_joint_ranges(theta, joints)

        if np.linalg.norm(twist_error) <= 1e-4:
            break
        if iter > 20:
            break

        iter += 1

    return state

for index, i in enumerate(range(1, 5)):  # 엄지 순회
    finger_node = robot.body_node_for(f"finger_r_link{i}")
    joint = finger_node.joints[0]
    qpos_index = joint["qpos_addr"]
    state.qpos[qpos_index] = (1 - alpha) * q_open_list[index] + alpha * q_closed_list[
        index
    ]

# sim/model/motion/trajectory.py
# 제약 조건 4개: s(0) = 0, s(Tf) = 1, s_dot(0) = 0, s_dot(T) = 0
def cubic_time_scaling(T, t):
    scaled_t = (
        t / T
    )  # 서로 다른 입력 T에 대해 동일한 time-scaling 계수를 사용하기 위해 정규화된 시간을 사용함. 다시 말해, t로 표현된 s(t)가 0과 1사이 값을 반환하게 하기 위해 t를 정규화함
    s_t = 3 * scaled_t**2 - 2 * scaled_t**3
    s_dot_t = (6 * scaled_t - 6 * scaled_t**2) / T
    s_ddot_t = (6 - 12 * scaled_t) / T**2

    return s_t, s_dot_t, s_ddot_t

# sim/model/solver/contact.py
def __post_init__(self):
    self.V_a = np.asarray(self.V_a, dtype=float).reshape(6)
    self.V_b = np.asarray(self.V_b, dtype=float).reshape(6)
    self.V_rel = self.V_a - self.V_b
    self.depth = max(0, -self.distance)
    # ...
    self.contact_type = contact_type(self)

# sim/model/solver/contact.py
def contact_type(contactpoint: ContactPoint):
    # ...
    if np.allclose( # 접촉점 p에서 두 강체의 속도가 같으면 붙어있는 rolling 모드로 봄
        v_a + create_skew(w_a) @ contactpoint.p_a,
        v_b + create_skew(w_b) @ contactpoint.p_b,
    ):

# sim/model/grasping/contact_constraint.py
v_rel = contact.v_rel  # 물체 기준에서 본 손 속도

normal = np.asarray(contact.normal, dtype=float)
norm = np.linalg.norm(normal)
if norm < 1e-8:
    continue

normal = normal / norm
normal_amount = np.dot(v_rel, normal)

if normal_amount < 0:
    contact.force = np.zeros(3)
    continue

friction_force = np.zeros(3)
if contact.tangent is not None:
    tangent_direction = contact.tangent
    friction_amount = np.linalg.norm(friction_coefficient * normal_force)
    friction_force = friction_amount * tangent_direction  # 접선 방향의 마찰력 계산

# 접촉힘 = 법선힘 + 접선힘
contact.force = normal_force * normal + friction_force

# sim/model/collision/collision_check.py
# collision pair 필터링
if not should_check_collision_pair(r_a, r_b):
    continue

# 계산 효율을 위해 각 노드를 proxy로 근사하고 proxy 간 거리를 계산
distance_result = proxy_distance(r_a, r_b, proxy_cache)  # python fcl 기반 거리 측정
if len(distance_result) == 4:
    p_a, p_b, d, normal = distance_result
else:
    p_a, p_b, d = distance_result
    normal = contact_normal(p_a, p_b)
body_names = {r_a.body_name, r_b.body_name}

# 손이 물체를 관통하는 상황은 충돌보다 접촉에 가까운 것으로 봄
if "pr_cokeCan" in body_names and d <= 0.002:
    is_contact = True

    if return_contacts:  # 접촉점 배열 형성
        contact_candidates.append(
            ContactPoint(
                record_a=r_a,
                record_b=r_b,
                point=0.5 * (p_a + p_b),
                p_a=p_a,
                p_b=p_b,
                normal=normal,
                distance=d,
                depth=max(0.0, -d),  # penetration depth
                V_a=state.body_twists.get(r_a.body_name, np.zeros(6)),
                V_b=state.body_twists.get(r_b.body_name, np.zeros(6)),
            )
        )

# sim/model/solver/form_closure.py
def check_form_closure(contact_points):
    # 조건0: positive span이 공간 전체를 덮어야 해 최소 7개의 접촉점을 가져야함 (공간 기준)
    if len(contact_points) < 7:
        return False

    wrenches = [
        force_to_wrench(contact.point, contact.normal)
        for contact in contact_points
        if contact.normal is not None
    ]

    if not wrenches:
        return False

    G = np.column_stack(wrenches)

    # 조건1. G의 rank가 공간 차원 전체
    rank = np.linalg.matrix_rank(G)

    if rank != 6:  # 평면인 경우 공간인 경우 분기처리하기
        return False

    # 조건2. Gk = 0, k > 0를 만족하는 k가 존재
    if not has_positive_k(G):
        return False

    return True

# sim/model/solver/force_closure.py
def solve_contact_forces(contact_points, external_wrench, friction_coefficient):
    # external_wrench: 중력항
    external_wrench = np.asarray(external_wrench, dtype=float).reshape(6)
    wrenches = []

    # 각 접촉점마다 friction cone edge force 생성
    for contact in contact_points:
        if contact.normal is None or contact.point is None:
            continue

        normal = np.asarray(contact.normal, dtype=float)
        normal_norm = np.linalg.norm(normal)
        if normal_norm < 1e-8:
            continue

        normal = normal / normal_norm

        if abs(normal[0]) < 0.9:
            a = np.array([1.0, 0.0, 0.0])
        else:
            a = np.array([0.0, 1.0, 0.0])

        basis1 = np.cross(normal, a)
        basis1 = basis1 / np.linalg.norm(basis1)

        basis2 = np.cross(normal, basis1)
        basis2 = basis2 / np.linalg.norm(basis2)

        edge_forces = [  # 각 edge force를 wrench로 변환
            normal + friction_coefficient * basis1,
            normal - friction_coefficient * basis1,
            normal + friction_coefficient * basis2,
            normal - friction_coefficient * basis2,
        ]

        for edge_force in edge_forces:
            wrenches.append(force_to_wrench(contact.point, edge_force))

    if not wrenches:
        return False, None

    G = np.column_stack(wrenches)  # wrench edge를 합쳐 G 생성
    m = G.shape[1]  # wrench edge 계수

    # 정적 평형(Static Equilibrium) 상태를 위한 접촉력 가중치 계산...
    result = linprog(
        c=np.ones(m),  # minimize sum(f)
        A_eq=G,
        b_eq=-external_wrench,
        bounds=[(0.0, None)] * m,  # f >= 0 제약
        method="highs",
    )