google-deepmind / mujoco_mpc

Real-time behaviour synthesis with MuJoCo, using Predictive Control
https://github.com/deepmind/mujoco_mpc
Apache License 2.0
998 stars 148 forks source link

How to control quadruped flat walk task with python bindings #213

Closed Iz-Idk closed 11 months ago

Iz-Idk commented 11 months ago

Hi I'm a student and I am looking into the repository to control a unitree go1 robot, but first I am looking into this repository for a controller of an unitree a1 robot because of this video, which looks amazing! This is the code so far just to test out the walk of the robot

import time
from absl.testing import absltest
import grpc
import mujoco
import mujoco.viewer as viewer
from mujoco_mpc import agent as agent_lib
import numpy as np
import pathlib

m = mujoco.MjModel.from_xml_path("mjpc/tasks/quadruped/task_flat.xml")
d = mujoco.MjData(m)
path = pathlib.Path(agent_lib.__file__).parent / "mjpc" / "ui_agent_server"

agent = agent_lib.Agent(
   server_binary_path=pathlib.Path(agent_lib.__file__).parent
        / "mjpc"
        / "ui_agent_server",
   task_id="Quadruped Flat",
   model=m

)

agent.set_mode("Walk")

with mujoco.viewer.launch_passive(m, d) as viewer:

    start = time.time()
    while viewer.is_running() and time.time():
        agent.step()
        mujoco.mj_step(m,d)

With the code above I get this Screencast from 03-11-23 14:12:50.webm. Is there something missing here? I am having a little trouble understanding the documentation. I installed the python bindings as stated on the read me page, only changing the install for the develop.

python "${MUJOCO_MPC_ROOT}/python/${API}.py" develop
erez-tom commented 11 months ago

Do you see that vertical orange line in the first two plots on the right? That line indicates the effective lag between planning and acting. If the plan is based on a very stale state, the chances are that it won't yield effective actions. First, try to play with simpler domains (e.g., cart-pole) and verify that in the limit of fast planning everything works as expected. Then, try to slow down the simulation (with the key "-") and see that the orange line comes closer to the middle of the plot. This should result in better planning. If that's where you end up (and there's no other problem at play), the only fix is to use more CPU cores...