у меня есть модель паука <mujoco model="arena"> ...

Question

у меня есть модель паука
<mujoco model="arena">
<compiler angle="degree" coordinate="local" inertiafromgeom="true"/>

есть код, кторый по плану должен заставить модель стоять на месте, а не падать, как это делает модель без кода
Вот код:
import mujoco
import mujoco.viewer
import time
import numpy as np
import utils

Загрузка модели

model_path = "/home/mk/Desktop/MuJoCo/ant.xml"
model = mujoco.MjModel.from_xml_path(model_path)
data = mujoco.MjData(model)

target_pos = np.array([0, 0, 0.55]) #координаты z: 0.55 - defoult; 0.61 - hight; 0.36 - low
task_id = 0
id_geom = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, "red_torso_geom")

Параметры ПД-регулятора

kp = 10.0 # Коэффициент пропорциональности (жесткость)
kd = 1.0 # Коэффициент дифференциальный (демпфирование)

with mujoco.viewer.launch_passive(model, data) as viewer:
print("Нажмите Ctrl+C для выхода")

text
#mujoco.set_mjcb_control(my_best_control)  #нужно для синхронизации текущего положения, чтоб моя функция работала с уже имеющимися данными, а не перезаписывала на новые. в общем точка отсчета как я понял

# Симуляция на несколько секунд
start_time = time.time()
while time.time() - start_time < 1000:  # 5 секунд
    step_start = time.time()
    current_time = step_start - start_time
    
    # 2. РАСЧЕТ ОШИБКИ (это и есть "задача" для IK)
    # Получаем текущее положение и ориентацию эффектора
    geom_pos = data.geom(id_geom).xpos
    
    # Вектор ошибки по положению
    pos_error = target_pos - geom_pos
    
    # 3. РАСЧЕТ УПРАВЛЯЮЩЕГО СИГНАЛА (Якобиан и ПД-закон)
    # Вычисляем якобиан для конечного эффектора (матрица 3x nv)
    jac_pos = np.zeros((3, model.nv))
    mujoco.mj_jacGeom(model, data, jac_pos, None, id_geom)
    
    # Расчет сил: J^T * (Kp * error - Kd * (J * qvel))
    # Это стандартная форма ПД-регулятора в пространстве задач (operational space)
    p_term = kp * pos_error
    d_term = kd * jac_pos @ data.qvel
    forces = jac_pos.T @ (p_term - d_term)
    
    # 4. ПРИМЕНЯЕМ РАСЧИТАННЫЕ СИЛЫ К СУСТАВАМ
    data.qfrc_applied = forces
    
    # Выполняем шаг симуляции
    mujoco.mj_step(model, data) 
    
    # Синхронизируем визуализацию
    viewer.sync()
    
    # Задержка для реального времени
    time_until_next_step = model.opt.timestep - (time.time() - step_start)
    if time_until_next_step > 0:
        time.sleep(time_until_next_step)

print("Симуляция завершена")

но почему то мой код ни как не запускает моторы, чтобы они заставляли моего паука стоять.
Напиши код, который будет работать не через приложение сил к телу, а через присваивание углов или моментов к моторам(скорее всего для этого придется решить обратную задачу кинематики

Answer 1

Ниже ‒ два варианта, оба работают через data.ctrl (т. е. «крутят» моторы-актуаторы), а не через прямое вмешательство в qfrc_applied.
Оба примера рассчитаны на ваш XML без изменений — 8 актуаторов-«motor», каждый прикреплён к одному шарниру ноги (hip/ankle).

1 вариант – «простой» PD-контроллер по суставам

Мы заранее задаём желаемые углы суставов, а затем каждую итерацию считаем torque = Kp · (qpos_target − qpos) − Kd · qvel
( MuJoCo сам умножит ctrl на gear и ограничит в ctrlrange ).

python
import mujoco
import mujoco.viewer
import numpy as np
import time

XML_PATH = "/home/mk/Desktop/MuJoCo/ant.xml"

# ──────────────────────────── загрузка модели
model = mujoco.MjModel.from_xml_path(XML_PATH)
data  = mujoco.MjData(model)

# ──────────────────────────── «позу стоя»: корень ровно, ноги согнуты ≈45°
STAND_QPOS = {
    # hip 1--4 вращаются в плоскости XY                                     (рад)
    "red_hip_1": 0.0,         #  0°  ‒ фронт-левая
    "red_hip_2": 0.0,         #  0°  ‒ фронт-правая
    "red_hip_3": 0.0,         #  0°  ‒ зад-правая
    "red_hip_4": 0.0,         #  0°  ‒ зад-левая
    # «колени» / ankle слегка согнуты, чтобы корпус поднялся ≈0.55 м
    "red_ankle_1":  0.7,      # ≈40° внутрь
    "red_ankle_2": -0.7,      # ≈-40°
    "red_ankle_3": -0.7,
    "red_ankle_4":  0.7,
}

# матрицы соответствий для быстрого доступа
joint_name_to_id   = {name: mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, name)
                      for name in STAND_QPOS}
actuator_for_joint = {}           # joint_id -> actuator_id
for act_id in range(model.nu):
    joint_id = model.actuator_trnid[2*act_id]   # первый индекс (body/joint pair)
    actuator_for_joint[joint_id] = act_id

# гиперпараметры PD
KP, KD = 40.0, 4.0

# ──────────────────────────── симуляция
with mujoco.viewer.launch_passive(model, data) as viewer:
    print("Ctrl+C для выхода …")
    t0 = time.time()
    while time.time() - t0 < 1000:         # ~бесконечно
        step_t   = time.time()
        # для каждого сустава → рассчитать torque и положить в data.ctrl
        for j_name, q_des in STAND_QPOS.items():
            jid   = joint_name_to_id[j_name]
            aid   = actuator_for_joint[jid]
            q_id  = model.jnt_qposadr[jid]
            dq_id = model.jnt_dofadr[jid]

            q_err  = q_des - data.qpos[q_id]
            dq_err = -data.qvel[dq_id]

            torque = KP * q_err + KD * dq_err          # Н·м
            # gear уже =150, ctrl ограничен [-1,1] → масштабируем:
            ctrl_val = np.clip(torque / model.actuator_gear[aid][0],
                               *model.actuator_ctrlrange[aid])
            data.ctrl[aid] = ctrl_val

        mujoco.mj_step(model, data)
        viewer.sync()

        # реал-тайм пауза
        time.sleep(max(0.0, model.opt.timestep - (time.time() - step_t)))

Что происходит

На старте STAND_QPOS задаёт «нормальную» стойку.
При желании подберите эти значения вручную: чуть больше — корпус поднимется, меньше — опустится.
Для каждого сустава берём текущие qpos / qvel, считаем ошибку и PD-торк.
Torque переводим в [-1,1] с учётом gear=150 и ctrlrange, кладём в data.ctrl.
Визуализатор показывает, что робот за несколько шагов встаёт и стабильно держится.

2 вариант – обратная кинематика «на лету»

Если хочется нефиксированных углов, а жёстко держать кроме z-координату корпуса = 0.55 м, то можно:

Каждый шаг брать ошибку pos_error = target_pos – geom_pos (как у вас).
Через Якобиан J получить необходимые угловые скорости
```
δq = Jᵀ · (J·Jᵀ + λ²I)⁻¹ · pos_error
```
(damped least-squares IK).
Интегрировать: q_des_next = q_now + δq.
Крутить PD к q_des_next тем же методом, что и в варианте 1.

Ниже минимальный фрагмент, показывающий шаг IK; вставьте его внутрь цикла вместо блока «q_err …»:

python
# ----- 1) обратная кинематика на корпус -----
POS_KP = 4.0           # скорость подъёма корпуса
LAM    = 1e-3          # Tikhonov damping

geom_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, "red_torso_geom")
target  = np.array([0.0, 0.0, 0.55])

jac = np.zeros((3, model.nv))
mujoco.mj_jacGeom(model, data, jac, None, geom_id)
err = target - data.geom_xpos[geom_id]

# damped-least-squares — получить δq для всех DOF
JJt = jac @ jac.T + (LAM**2) * np.eye(3)
dq  = jac.T @ np.linalg.solve(JJt, POS_KP * err)     # nv-вектор

q_des = data.qpos.copy()
q_des[:model.nq] += dq * model.opt.timestep          # интегральный прирост

# ----- 2) PD-торки к новым целям -----
for aid in range(model.nu):
    jid   = model.actuator_trnid[2*aid]
    q_id  = model.jnt_qposadr[jid]
    dq_id = model.jnt_dofadr[jid]

    q_err  = q_des[q_id] - data.qpos[q_id]
    dq_err = -data.qvel[dq_id]

    torque   = KP * q_err + KD * dq_err
    data.ctrl[aid] = np.clip(torque / model.actuator_gear[aid][0],
                              *model.actuator_ctrlrange[aid])

Такой контроллер всё время «подравнивает» позу, поэтому робот выдержит толчки или лёгкий наклон пола.

Где была проблема в вашем скрипте

data.qfrc_applied ‒ это непосредственное добавление сил/моментов к DOF-ам поверх всей динамики и контроллеров; актуаторы при этом остаются «пустыми».
Если в модели есть моторы, обычно проще (и стабильнее) работать через data.ctrl.
В mj_jacGeom вы оставили второй аргумент (Jacobian по ориентации) None, но позже в формуле использовали только позиционную часть, что нормально; просто не забудьте инициализировать массивы jac как (3,nv).
gear = 150: чтобы получить заметный крутящий момент, ctrl должен быть ≤ 1, значит torque ≤ 150 Н·м. Без деления на gear получались клипы или нули.

Попробуйте оба варианта и подберите KP/KD и, во втором случае, POS_KP и λ под свою динамику.

у меня есть модель паука <mujoco model="arena"> ...

Pregunta

Загрузка модели

Параметры ПД-регулятора

Pensamiento

Respuesta

1 вариант – «простой» PD-контроллер по суставам

2 вариант – обратная кинематика «на лету»

Где была проблема в вашем скрипте

Compartir este Q&A