A port of pink for MuJoCo.
pip install -e ".[examples]"from mink import FrameTask, PostureTask, ComTask
tasks = [
pelvis_orientation_task := FrameTask(
frame_name="pelvis",
frame_type="body",
position_cost=0.0,
orientation_cost=10.0,
),
posture_task := PostureTask(cost=1e-1),
com_task := ComTask(cost=200.0),
]
hand_tasks = []
for hand in hands:
task = FrameTask(
frame_name=hand,
frame_type="site",
position_cost=4.0,
orientation_cost=0.0,
lm_damping=1.0,
)
hand_tasks.append(task)
tasks.extend(hand_tasks)model = mujoco.MjModel.from_xml_path(_XML.as_posix())
configuration = mink.Configuration(model)
# Initialize from the "stand" keyframe configuration.
configuration.update_from_keyframe("stand")
pelvis_orientation_task.set_target_from_configuration(configuration)
posture_task.set_target_from_configuration(configuration)
# Set target from mocap (for interactive setting).
for i, (hand_task, foot_task) in enumerate(zip(hand_tasks, feet_tasks)):
foot_task.set_target_from_mocap(data, feet_mocap_ids[i])
hand_task.set_target_from_mocap(data, hands_mocap_ids[i])# Joint position limits.
limits = [
mink.ConfigurationLimit(model=model),
]import time
from mink import solve_ik
solver = "quadprog"
dt = 0.02 # [s]
for t in np.arange(0.0, 5.0, dt):
velocity = solve_ik(configuration, tasks, limits, dt, solver)
configuration.integrate_inplace(velocity, dt)
time.sleep(dt)mjpython examples/arm_ur5e.py # On macOS
python examples/arm_ur5e.py # On Linux
