Creating a custom MuJoCo plugin #2045
-
IntroHi! I am a undergrad at Caltech. My setupWindows latest version, latest MJX/Mujoco python API My questionI am currently trying to simulate a robot that has motors on its joints. The motors have built-in PD controllers with a feed-forward term. How can I achieve this functionality with a single custom MuJoCo plugin? I tried using the built-in PID extension as a template to write the PDFF controller, but even though my code compiles, I could not get MuJoCo to find and use it when I call it in the XML file. Right now, I compiled my code to a Please review the following stack overflow question: Minimal model and/or code that explain my questionHere's the python script that tries to load the XML: and here is the XML itself: The script gives the below error: Confirmations
|
Beta Was this translation helpful? Give feedback.
Replies: 1 comment 1 reply
-
|
I assume you're using the feedforward term for gravcomp? In that case, I think you can achieve the functionality you want without a plugin, using a combination of def gravcomp(
model: mujoco.MjModel,
data: mujoco.MjData,
body_ids: np.ndarray,
) -> np.ndarray:
"""Compute force to counteract gravity for the given bodies."""
jac = np.empty((3, model.nv))
qfrc_gravcomp = np.zeros((model.nv))
for i in body_ids:
body_weight = model.opt.gravity * model.body(i).mass
mujoco.mj_jac(model, data, jac, None, data.body(i).xipos, i)
q_weight = jac.T @ body_weight
qfrc_gravcomp -= q_weight
return qfrc_gravcomp
def pd_controller(
model: mujoco.MjModel,
data: mujoco.MjData,
qpos_desired,
Kp: np.ndarray,
Kd: np.ndarray,
) -> None:
"""u = Kp * (q_d - q) - Kd * qdot"""
tau = Kp * (qpos_desired - data.qpos) - Kd * data.qvel
np.clip(tau, *model.actuator_ctrlrange.T, out=data.ctrl)
def joint_stiffness_controller(
model: mujoco.MjModel,
data: mujoco.MjData,
qpos_desired,
Kp: np.ndarray,
Kd: np.ndarray,
body_ids: Sequence[int],
) -> None:
"""PD controller + gravity compensation.
u = -tau_gravity + Kp * (q_d - q) -Kd * qdot
"""
tau_gravcomp = gravcomp(model, data, body_ids)
data.qfrc_applied = tau_gravcomp
tau = Kp * (qpos_desired - data.qpos) - Kd * data.qvel
np.clip(tau, *model.actuator_ctrlrange.T, out=data.ctrl)You can use the above controller helper functions in the following control loop: # MuJoCo model and data pair.
model = ...
data = ...
# List of bodies to apply gravity compensation to.
body_ids: list[int]
# PD controller gains.
Kp: np.ndarray
Kd: np.ndarray
while True:
qpos_target: np.ndarray # Assume some higher level logic is providing this.
joint_stiffness_controller(model, data, qpos_target, Kp, Kd, body_ids)
mujoco.mj_step(model, data) |
Beta Was this translation helpful? Give feedback.
-
|
Thanks! That was exactly what I was looking for. |
Beta Was this translation helpful? Give feedback.
I assume you're using the feedforward term for gravcomp? In that case, I think you can achieve the functionality you want without a plugin, using a combination of
motoractuators andqfrc_applied.