Question regarding simultaneous agent server access

[Iz-Idk]

Hi, regarding this issue #217. We have been meaning to understand it more, however, we have run into a problem that involves the mocap_pos. From what I have understood, when the agent server is called it tries to find a free port to access it (which in both ui_agent_servers does connect to different ports at the same time). Eventhough I want to give different goals to each robot, each take the last goal that it was connected to or was recently updated to. Is it because the client from the grpc is using the same server? or is there anything you could recommend us?
Here is an example code

Working example

import time as time_

import mujoco
import mujoco.viewer as viewer
from mujoco_mpc import agent as agent_lib
import numpy as np

import pathlib

# Creating model and data
m = mujoco.MjModel.from_xml_path("../../../../../mujoco_mpc/mjpc/tasks/quadruped/task_flat2.xml")
d = mujoco.MjData(m)

# Initializing our agent (agent server/executable)
agent = agent_lib.Agent(
        # This is to enable the ui
        server_binary_path=pathlib.Path(agent_lib.__file__).parent
        / "mjpc"
        / "ui_agent_server",
        task_id="Quadruped Flat", 
        model=m)

agent2 = agent_lib.Agent(
        # Enable the ui and finds a freeport
        server_binary_path=pathlib.Path(agent_lib.__file__).parent
        / "mjpc"
        / "ui_agent_server",
        task_id="Quadruped Flat", 
        model=m)



## Data for agents

# weights
agent.set_cost_weights({"Position": 0.15})
print("Cost weights:", agent.get_cost_weights())

# parameters
agent.set_task_parameter("Walk speed", 1.0)
print("Parameters:", agent.get_task_parameters())

# weights for second agent
agent2.set_cost_weights({"Position": 0.15})
print("Cost weights:", agent.get_cost_weights())

# parameters for second agent
agent2.set_task_parameter("Walk speed", 1.0)
print("Parameters:", agent.get_task_parameters())


# run planner for num_steps
num_steps = 8
for _ in range(num_steps):
    agent.planner_step()
    agent2.planner_step()

# Multiple goals
goals = [
    [7, 0, 0.26],
    [0, -2, 0.26],
    [-2, 0, 0.26],
    [0, -5, 0.26]
]
i = 0
goal_agent = []
goal_agent2 = []

# Settings different goals for the different robots
goal_agent = d.mocap_pos
goal_agent[0] = goals[0]
print(goal_agent)

goal_agent2 = d.mocap_pos
goal_agent2[0] = goals[1]
print(goal_agent)

with mujoco.viewer.launch_passive(m, d) as viewer:
    # Close the viewer automatically after 30 wall-seconds.

    while viewer.is_running():
        
        # set planner state
        agent.set_state(
            time=d.time,
            qpos=d.qpos[:19],
            qvel=d.qvel[:18],
            act=d.act,
            mocap_pos=goal_agent,
            mocap_quat=d.mocap_quat,
        )

        # set planner state for agent 2
        agent2.set_state(
            time=d.time,
            qpos=d.qpos[19:],
            qvel=d.qvel[18:],
            act=d.act,
            mocap_pos=goal_agent2,
            mocap_quat=d.mocap_quat,
        )

        print(d.mocap_pos) # Seems to take only in consideration the second agents goal
        # Obtain their joints from each agent and display them on the robots
        agent_ctrl = agent.get_action()
        agent_ctrl2 = agent2.get_action()
        agent_ctrl = np.append(agent_ctrl, agent_ctrl2, axis=0)
        d.ctrl = agent_ctrl
        
        # To detect when the robot is close to the goal
        if (np.linalg.norm(d.mocap_pos[0] - d.body('trunk').xpos) < 1) or (np.linalg.norm(d.mocap_pos[0] - d.body('trunk2').xpos) < 1) :
            goal_agent2[0] = goals[i] # Only updates 1 agent's goal
            print("\nARRIVED!")
            i = (i + 1) % len(goals)

        mujoco.mj_step(m,d)

        # Example modification of a viewer option: toggle contact points every two seconds.
        with viewer.lock():
            viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_CONTACTPOINT] = int(d.time % 2)

        # Pick up changes to the physics state, apply perturbations, update options from GUI.
        viewer.sync()

A few notes are that the task_flat2 contains 2 robots with different spawning points and they both go to a same goal. And we have also tried putting both goals to no avail. Here is a video example Video
Thank you for your time and sorry for the trouble.

[thowell]

Cool project!

I took a look at the code you provided and didn't immediately see any problems. Can you provide a link to: task_flat2.xml? This way I can try and replicate the issue on my end.

[Iz-Idk]

Hi thank you for your answer!
The code for the task_flat2.xml is

task_flat2.xml

<mujoco model="Quadruped">
  <include file="../common.xml"/>

  <size memory="1M"/>

  <custom>
    <!-- agent -->
    <text name="task_transition" data="Quadruped|Biped|Walk|Scramble|Flip" />
    <numeric name="agent_planner" data="2 " />
    <numeric name="agent_horizon" data="0.35" />
    <numeric name="agent_timestep" data="0.01" />
    <numeric name="sampling_spline_points" data="3" />
    <numeric name="sampling_trajectories" data="60"/>
    <numeric name="sampling_exploration" data="0.04" />
    <numeric name="gradient_spline_points" data="5" />

    <!-- residual -->
    <numeric name="residual_select_Gait" data="0"/>
    <text name="residual_list_Gait" data="Stand|Walk|Trot|Canter|Gallop"/>
    <numeric name="residual_select_Gait switch" data="1"/>
    <text name="residual_list_Gait switch" data="Manual|Automatic"/>
    <numeric name="residual_Cadence" data="2 0 4" />
    <numeric name="residual_Amplitude" data=".06 0 0.2" />
    <numeric name="residual_Duty ratio" data="0 0 1"/>
    <numeric name="residual_Walk speed" data="0 0 4"/>
    <numeric name="residual_Walk turn" data="0 -2 2"/>
    <numeric name="residual_select_Flip dir" data="0"/>
    <text name="residual_list_Flip dir" data="Back Flip|Front Flip"/>
    <numeric name="residual_select_Biped type" data="0"/>
    <text name="residual_list_Biped type" data="Foot Stand|Hand Stand"/>
    <numeric name="residual_Heading" data="0 -3.14 3.14" />

    <!-- estimator -->
    <numeric name="estimator" data="0" />
    <numeric name="estimator_sensor_start" data="9" />
    <numeric name="estimator_number_sensor" data="21" />
    <numeric name="estimator_timestep" data="0.005" />
    <numeric name="batch_configuration_length" data="3" />
    <numeric name="batch_scale_prior" data="1.0e-3" />
  </custom>

  <visual>
    <headlight diffuse="0.6 0.6 0.6"  ambient="0.3 0.3 0.3" specular="0 0 0"/>
    <global azimuth="-90"/>
  </visual>

  <statistic extent="1" center="0 0 .3"/>

  <worldbody>
    <light pos="0 0 1.5" dir="0 0 -1" diffuse="0.5 0.5 0.5" specular="0.3 0.3 0.3"
      directional="true" castshadow="false"/>
    <geom name="floor" size="0 0 0.1" pos="0 0 -0.01" type="plane" material="blue_grid"/>

     <body name="goal" mocap="true" pos=".4 0.4 0.3">
        <geom size="0.12" contype="0" conaffinity="0" rgba="0 1 0 .5" group="2" />
      </body>
      
      <body name="box" mocap="true" pos="-2.5 0 0">
        <geom name="box" class="prop" size="1 1 0.3"/>
      </body>
      <geom name="ramp" class="prop" pos="3.13 2.5 -.18" size="1.6 1 .5" euler="0 -0.2 0"/>
      <geom name="hill" class="prop" pos="6 6 -5.5" size="6" type="sphere"/>
  </worldbody>

  <default>
    <default class="torque">
      <general gainprm="40" ctrllimited="true" ctrlrange="-1 1"/>
    </default>
    <default class="task">
      <site rgba="1 0 0 1" size=".02" group="5"/>
      <default class="flight">
        <site rgba="1 .3 .3 1" size="0.019" group="5"/>
      </default>
      <default class="stance">
        <site rgba=".3 1 .3 1" size=".018" group="4"/>
      </default>
    </default>
    <default class="prop">
      <geom type="box" rgba="0 0.4 1 1"/>
    </default>
  </default>

  <include file="a1.xml" />

  <actuator>
    <general class="torque" name="FR_hip" joint="FR_hip_joint"/>
    <general class="torque" name="FR_thigh" joint="FR_thigh_joint"/>
    <general class="torque" name="FR_calf" joint="FR_calf_joint"/>
    <general class="torque" name="FL_hip" joint="FL_hip_joint"/>
    <general class="torque" name="FL_thigh" joint="FL_thigh_joint"/>
    <general class="torque" name="FL_calf" joint="FL_calf_joint"/>
    <general class="torque" name="RR_hip" joint="RR_hip_joint"/>
    <general class="torque" name="RR_thigh" joint="RR_thigh_joint"/>
    <general class="torque" name="RR_calf" joint="RR_calf_joint"/>
    <general class="torque" name="RL_hip" joint="RL_hip_joint"/>
    <general class="torque" name="RL_thigh" joint="RL_thigh_joint"/>
    <general class="torque" name="RL_calf" joint="RL_calf_joint"/>
    
    <general class="torque" name="FR_hip2" joint="FR_hip_joint2"/>
    <general class="torque" name="FR_thigh2" joint="FR_thigh_joint2"/>
    <general class="torque" name="FR_calf2" joint="FR_calf_joint2"/>
    <general class="torque" name="FL_hip2" joint="FL_hip_joint2"/>
    <general class="torque" name="FL_thigh2" joint="FL_thigh_joint2"/>
    <general class="torque" name="FL_calf2" joint="FL_calf_joint2"/>
    <general class="torque" name="RR_hip2" joint="RR_hip_joint2"/>
    <general class="torque" name="RR_thigh2" joint="RR_thigh_joint2"/>
    <general class="torque" name="RR_calf2" joint="RR_calf_joint2"/>
    <general class="torque" name="RL_hip2" joint="RL_hip_joint2"/>
    <general class="torque" name="RL_thigh2" joint="RL_thigh_joint2"/>
    <general class="torque" name="RL_calf2" joint="RL_calf_joint2"/>
    
  </actuator>

  <sensor>
    <!-- cost -->
    <user name="Upright" dim="3" user="6 1 0 3 0.05" />
    <user name="Height" dim="1" user="6 1 0 3 0.04" />
    <user name="Position" dim="3" user="2 0.2 0 0.5 0.1" />
    <user name="Gait" dim="4" user="6 2 0 10 0.03" />
    <user name="Balance" dim="2" user="2 0.2 0 0.3 0.1" />
    <user name="Effort" dim="12" user="0 0.03 0.0 0.1" />
    <user name="Posture" dim="12" user="0 0.02 0.0 0.1" />
    <user name="Orientation" dim="2" user="0 0 0 .03" />
    <user name="Angmom" dim="3" user="0 0 0 .03" />

    <!-- estimator measurements -->
    <framepos name="torso_pos" objtype="site" objname="torso" />
    <framepos name="FR_pos" objtype="site" objname="FR" />
    <framepos name="FL_pos" objtype="site" objname="FL" />
    <framepos name="RR_pos" objtype="site" objname="RR" />
    <framepos name="RL_pos" objtype="site" objname="RL" />
   
    <framepos name="torso_pos2" objtype="site" objname="torso2" />
    <framepos name="FR_pos2" objtype="site" objname="FR2" />
    <framepos name="FL_pos2" objtype="site" objname="FL2" />
    <framepos name="RR_pos2" objtype="site" objname="RR2" />
    <framepos name="RL_pos2" objtype="site" objname="RL2" />
 
    <jointpos name="pos_FR_hip_joint" joint="FR_hip_joint" />
    <jointpos name="pos_FR_thigh_joint" joint="FR_thigh_joint" />
    <jointpos name="pos_FR_calf_joint" joint="FR_calf_joint" />
    <jointpos name="pos_FL_hip_joint" joint="FL_hip_joint" />
    <jointpos name="pos_FL_thigh_joint" joint="FL_thigh_joint" />
    <jointpos name="pos_FL_calf_joint" joint="FL_calf_joint" />
    <jointpos name="pos_RR_hip_joint" joint="RR_hip_joint" />
    <jointpos name="pos_RR_thigh_joint" joint="RR_thigh_joint" />
    <jointpos name="pos_RR_calf_joint" joint="RR_calf_joint" />
    <jointpos name="pos_RL_hip_joint" joint="RL_hip_joint" />
    <jointpos name="pos_RL_thigh_joint" joint="RL_thigh_joint" />
    <jointpos name="pos_RL_calf_joint" joint="RL_calf_joint" />

    <jointpos name="pos_FR_hip_joint2" joint="FR_hip_joint2" />
    <jointpos name="pos_FR_thigh_joint2" joint="FR_thigh_joint2" />
    <jointpos name="pos_FR_calf_joint2" joint="FR_calf_joint2" />
    <jointpos name="pos_FL_hip_joint2" joint="FL_hip_joint2" />
    <jointpos name="pos_FL_thigh_joint2" joint="FL_thigh_joint2" />
    <jointpos name="pos_FL_calf_joint2" joint="FL_calf_joint2" />
    <jointpos name="pos_RR_hip_joint2" joint="RR_hip_joint2" />
    <jointpos name="pos_RR_thigh_joint2" joint="RR_thigh_joint2" />
    <jointpos name="pos_RR_calf_joint2" joint="RR_calf_joint2" />
    <jointpos name="pos_RL_hip_joint2" joint="RL_hip_joint2" />
    <jointpos name="pos_RL_thigh_joint2" joint="RL_thigh_joint2" />
    <jointpos name="pos_RL_calf_joint2" joint="RL_calf_joint2" />
    
    <touch name="FR_touch" site="FR"/>
    <touch name="FL_touch" site="FL"/>
    <touch name="RR_touch" site="RR"/>
    <touch name="RL_touch" site="RL"/>

    <touch name="FR_touch2" site="FR2"/>
    <touch name="FL_touch2" site="FL2"/>
    <touch name="RR_touch2" site="RR2"/>
    <touch name="RL_touch2" site="RL2"/>
    
    <!-- trace -->
    <framepos      name="trace0" objtype="site" objname="head"/>

    <!-- residual -->
    <subtreecom    name="torso_subtreecom" body="trunk"/>
    <subtreelinvel name="torso_subtreelinvel" body="trunk"/>
    <subtreelinvel name="torso_angmom" body="trunk"/>
    
    <subtreecom    name="torso_subtreecom2" body="trunk2"/>
    <subtreelinvel name="torso_subtreelinvel2" body="trunk2"/>
    <subtreelinvel name="torso_angmom2" body="trunk2"/>
    
    
  </sensor>

  <keyframe>
    <key name="home"
         qpos="0 0 0.26
               1 0 0 
               -0.000341931 0.0181576 -0.0268335
               0.00160968 0.0247957 -0.0270045
               0.00191398 -0.033048 -0.0675298
               -0.00199489 -0.0374747 -0.0681862 

               0 0 0 0.26
                1 0 0 
                -0.000341931 0.0181576 -0.0268335
                0.00160968 0.0247957 -0.0270045
                0.00191398 -0.033048 -0.0675298
                -0.00199489 -0.0374747 -0.0681862 

               
               "/>
    <key name="crouch"
         qpos="-0.0501827 0.00107117 0.143925
               1 0 0 
               0 0 -0.5
               0 0 -0.5
               0 0 -0.5
               0 0 -0.5

               -0.0501827 0.00107117 0.143925
               1 0 0 0
               0 0 -0.5
               0 0 -0.5
               0 0 -0.5
               0 0 -0.5
               "/>

  </keyframe>
</mujoco>

There were a bit of modifications to the a1.xml too, so I will also put it here.

for the a1.xml

<mujoco model="Unitree A1">
  <compiler angle="radian" meshdir="assets" texturedir="assets"/>

  <option cone="elliptic" impratio="10"/>

    <default class="a1">
      <geom friction="0.6" margin="0.001" condim="1"/>
      <joint axis="0 1 0" damping="2" armature="0.01" frictionloss="0.2" limited="true"/>
      <position kp="50" ctrllimited="true" forcelimited="true" forcerange="-33.5 33.5"/>
      <default class="abduction">
        <joint axis="1 0 0" damping="1" range="-0.802851 0.802851"/>
        <position ctrlrange="-0.802851 0.802851"/>
      </default>
      <default class="hip">
        <joint range="-1.9472 3.28879" ref="-0.9"/>
        <position ctrlrange="-1.9472 3.28879"/>
      </default>
      <default class="knee">
        <joint range="-0.89653 0.883702" ref="1.8"/>
        <position ctrlrange="-0.89653 0.883702"/>
      </default>
      <default class="visual">
        <geom type="mesh" contype="0" conaffinity="0" group="2" material="dark"/>
      </default>
      <default class="collision">
        <geom group="3" type="capsule"/>
        <default class="hip_left">
          <geom size="0.04 0.04" quat="1 1 0 0" type="cylinder" pos="0 0.055 0"/>
        </default>
        <default class="hip_right">
          <geom size="0.04 0.04" quat="1 1 0 0" type="cylinder" pos="0 -0.055 0"/>
        </default>
        <default class="thigh1">
          <geom size="0.015" fromto="-0.02 0 0 -0.02 0 -0.16"/>
        </default>
        <default class="thigh2">
          <geom size="0.015" fromto="0 0 0 -0.02 0 -0.1"/>
        </default>
        <default class="thigh3">
          <geom size="0.015" fromto="-0.02 0 -0.16 0 0 -0.2"/>
        </default>
        <default class="calf1">
          <geom size="0.01" fromto="0 0 0 0.02 0 -0.13"/>
        </default>
        <default class="calf2">
          <geom size="0.01" fromto="0.02 0 -0.13 0 0 -0.2"/>
        </default>
        <default class="foot">
          <geom type="sphere" size="0.02" pos="0 0 -0.2" priority="1" solimp="0.015 1 0.031"
              condim="6" friction="0.8 0.02 0.01"/>
        </default>
      </default>
    </default>

  <asset>
    <material name="dark" specular="0" shininess="0.25" rgba="0.2 0.2 0.2 1"/>
    <texture type="2d" name="trunk_A1" file="trunk_A1.png"/>
    <material name="carbonfibre" texture="trunk_A1" specular="0" shininess="0.25"/>

    <mesh class="a1" file="calf.obj"/>
    <mesh class="a1" file="hip.obj"/>
    <mesh class="a1" file="thigh.obj"/>
    <mesh class="a1" file="thigh_mirror.obj"/>
    <mesh class="a1" file="trunk.obj"/>
  </asset>

  <worldbody>
    <body name="trunk" pos="0.0 0.5 0.5" quat="1 0 0 0" childclass="a1">
      <light pos="0 0 5" mode="trackcom" diffuse="0.8 0.8 0.8"/>
      <site name="torso"/>
      <site name="head" class="task" pos=".3 0 0"/>
      <freejoint/>
      <inertial mass="4.713" pos="0 0.0041 -0.0005"
          fullinertia="0.0158533 0.0377999 0.0456542 -3.66e-05 -6.11e-05 -2.75e-05"/>
      <geom class="visual" mesh="trunk" material="carbonfibre"/>
      <geom class="collision" size="0.125 0.04 0.057" type="box"/>
      <geom class="collision" quat="1 0 1 0" pos="0 -0.04 0" size="0.058 0.125" type="cylinder"/>
      <geom class="collision" quat="1 0 1 0" pos="0 +0.04 0" size="0.058 0.125" type="cylinder"/>
      <geom class="collision" pos="0.25 0 0" size="0.005 0.06 0.05" type="box"/>
      <geom class="collision" pos="0.25 0.06 -0.01" size="0.009 0.035"/>
      <geom class="collision" pos="0.25 -0.06 -0.01" size="0.009 0.035"/>
      <geom class="collision" pos="0.25 0 -0.05" size="0.005 0.06" quat="1 1 0 0"/>
      <geom class="collision" pos="0.255 0 0.0355" size="0.021 0.052" quat="1 1 0 0"/>
      <body name="FR_hip" pos="0.183 -0.047 0">
        <inertial mass="0.696" pos="-0.003311 -0.000635 3.1e-05"
            quat="0.507528 0.506268 0.491507 0.494499"
            diaginertia="0.000807752 0.00055293 0.000468983"/>
        <joint class="abduction" name="FR_hip_joint"/>
        <geom class="visual" mesh="hip" quat="0 1 0 0"/>
        <geom class="hip_right"/>
        <body name="FR_thigh1" pos="0 -0.08505 0">
          <inertial mass="1.013" pos="-0.003237 0.022327 -0.027326"
              quat="0.999125 -0.00256393 -0.0409531 -0.00806091"
              diaginertia="0.00555739 0.00513936 0.00133944"/>
          <joint class="hip" name="FR_thigh_joint"/>
          <geom class="visual" mesh="thigh_mirror"/>
          <geom class="thigh1"/>
          <geom class="thigh2"/>
          <geom class="thigh3"/>
          <body name="FR_calf" pos="0 0 -0.2">
            <inertial mass="0.226" pos="0.00472659 0 -0.131975"
                quat="0.706886 0.017653 0.017653 0.706886"
                diaginertia="0.00340344 0.00339393 3.54834e-05"/>
            <joint class="knee" name="FR_calf_joint"/>
            <geom class="visual" mesh="calf"/>
            <geom class="calf1"/>
            <geom class="calf2"/>
            <geom name="FR" class="foot"/>
            <site name="FR" pos="0 0 -0.2" type="sphere" size=".015"/>
          </body>
        </body>
      </body>
      <body name="FL_hip" pos="0.183 0.047 0">
        <inertial mass="0.696" pos="-0.003311 0.000635 3.1e-05"
            quat="0.494499 0.491507 0.506268 0.507528"
            diaginertia="0.000807752 0.00055293 0.000468983"/>
        <joint class="abduction" name="FL_hip_joint"/>
        <geom class="visual" mesh="hip"/>
        <geom class="hip_left"/>
        <geom class="collision" size="0.04 0.04" pos="0 0.055 0" quat="1 1 0 0" type="cylinder"/>
        <body name="FL_thigh" pos="0 0.08505 0">
          <inertial mass="1.013" pos="-0.003237 -0.022327 -0.027326"
              quat="0.999125 0.00256393 -0.0409531 0.00806091"
              diaginertia="0.00555739 0.00513936 0.00133944"/>
          <joint class="hip" name="FL_thigh_joint"/>
          <geom class="visual" mesh="thigh"/>
          <geom class="thigh1"/>
          <geom class="thigh2"/>
          <geom class="thigh3"/>
          <body name="FL_calf" pos="0 0 -0.2">
            <inertial mass="0.226" pos="0.00472659 0 -0.131975"
                quat="0.706886 0.017653 0.017653 0.706886"
                diaginertia="0.00340344 0.00339393 3.54834e-05"/>
            <joint class="knee" name="FL_calf_joint"/>
            <geom class="visual" mesh="calf"/>
            <geom class="calf1"/>
            <geom class="calf2"/>
            <geom name="FL" class="foot"/>
            <site name="FL" pos="0 0 -0.2" type="sphere" size=".015"/>
          </body>
        </body>
      </body>
      <body name="HR_hip" pos="-0.183 -0.047 0">
        <inertial mass="0.696" pos="0.003311 -0.000635 3.1e-05"
            quat="0.491507 0.494499 0.507528 0.506268"
            diaginertia="0.000807752 0.00055293 0.000468983"/>
        <joint class="abduction" name="RR_hip_joint"/>
        <geom class="visual" quat="0 0 0 -1" mesh="hip"/>
        <geom class="hip_right"/>
        <body name="RR_thigh" pos="0 -0.08505 0">
          <inertial mass="1.013" pos="-0.003237 0.022327 -0.027326"
              quat="0.999125 -0.00256393 -0.0409531 -0.00806091"
              diaginertia="0.00555739 0.00513936 0.00133944"/>
          <joint class="hip" name="RR_thigh_joint"/>
          <geom class="visual" mesh="thigh_mirror"/>
          <geom class="thigh1"/>
          <geom class="thigh2"/>
          <geom class="thigh3"/>
          <body name="RR_calf" pos="0 0 -0.2">
            <inertial mass="0.226" pos="0.00472659 0 -0.131975"
                quat="0.706886 0.017653 0.017653 0.706886"
                diaginertia="0.00340344 0.00339393 3.54834e-05"/>
            <joint class="knee" name="RR_calf_joint"/>
            <geom class="visual" mesh="calf"/>
            <geom class="calf1"/>
            <geom class="calf2"/>
            <geom name="HR" class="foot"/>
            <site name="RR" pos="0 0 -0.2" type="sphere" size=".015"/>
          </body>
        </body>
      </body>
      <body name="HL_hip" pos="-0.183 0.047 0">
        <inertial mass="0.696" pos="0.003311 0.000635 3.1e-05"
            quat="0.506268 0.507528 0.494499 0.491507"
            diaginertia="0.000807752 0.00055293 0.000468983"/>
        <joint class="abduction" name="RL_hip_joint"/>
        <geom class="visual" quat="0 0 1 0" mesh="hip"/>
        <geom class="hip_left"/>
        <body name="RL_thigh" pos="0 0.08505 0">
          <inertial mass="1.013" pos="-0.003237 -0.022327 -0.027326"
              quat="0.999125 0.00256393 -0.0409531 0.00806091"
              diaginertia="0.00555739 0.00513936 0.00133944"/>
          <joint class="hip" name="RL_thigh_joint"/>
          <geom class="visual" mesh="thigh"/>
          <geom class="thigh1"/>
          <geom class="thigh2"/>
          <geom class="thigh3"/>
          <body name="RL_calf" pos="0 0 -0.2">
            <inertial mass="0.226" pos="0.00472659 0 -0.131975"
                quat="0.706886 0.017653 0.017653 0.706886"
                diaginertia="0.00340344 0.00339393 3.54834e-05"/>
            <joint class="knee" name="RL_calf_joint"/>
            <geom class="visual" mesh="calf"/>
            <geom class="calf1"/>
            <geom class="calf2"/>
            <geom name="HL" class="foot"/>
            <site name="RL" pos="0 0 -0.2" type="sphere" size=".015"/>
          </body>
        </body>
      </body>
    </body>


    <body name="trunk2" pos="0.0 0.0 0.5" quat="1 0 0 0" childclass="a1">
      <light pos="0 0 5" mode="trackcom" diffuse="0.8 0.8 0.8"/>
      <site name="torso2"/>
      <site name="head2" class="task" pos=".3 0 0"/>
      <freejoint/>
      <inertial mass="4.713" pos="0 0.0041 -0.0005"
          fullinertia="0.0158533 0.0377999 0.0456542 -3.66e-05 -6.11e-05 -2.75e-05"/>
      <geom class="visual" mesh="trunk" material="carbonfibre"/>
      <geom class="collision" size="0.125 0.04 0.057" type="box"/>
      <geom class="collision" quat="1 0 1 0" pos="0 -0.04 0" size="0.058 0.125" type="cylinder"/>
      <geom class="collision" quat="1 0 1 0" pos="0 +0.04 0" size="0.058 0.125" type="cylinder"/>
      <geom class="collision" pos="0.25 0 0" size="0.005 0.06 0.05" type="box"/>
      <geom class="collision" pos="0.25 0.06 -0.01" size="0.009 0.035"/>
      <geom class="collision" pos="0.25 -0.06 -0.01" size="0.009 0.035"/>
      <geom class="collision" pos="0.25 0 -0.05" size="0.005 0.06" quat="1 1 0 0"/>
      <geom class="collision" pos="0.255 0 0.0355" size="0.021 0.052" quat="1 1 0 0"/>
      <body name="FR_hip2" pos="0.183 -0.047 0">
        <inertial mass="0.696" pos="-0.003311 -0.000635 3.1e-05"
            quat="0.507528 0.506268 0.491507 0.494499"
            diaginertia="0.000807752 0.00055293 0.000468983"/>
        <joint class="abduction" name="FR_hip_joint2"/>
        <geom class="visual" mesh="hip" quat="0 1 0 0"/>
        <geom class="hip_right"/>
        <body name="FR_thigh2" pos="0 -0.08505 0">
          <inertial mass="1.013" pos="-0.003237 0.022327 -0.027326"
              quat="0.999125 -0.00256393 -0.0409531 -0.00806091"
              diaginertia="0.00555739 0.00513936 0.00133944"/>
          <joint class="hip" name="FR_thigh_joint2"/>
          <geom class="visual" mesh="thigh_mirror"/>
          <geom class="thigh1"/>
          <geom class="thigh2"/>
          <geom class="thigh3"/>
          <body name="FR_calf2" pos="0 0 -0.2">
            <inertial mass="0.226" pos="0.00472659 0 -0.131975"
                quat="0.706886 0.017653 0.017653 0.706886"
                diaginertia="0.00340344 0.00339393 3.54834e-05"/>
            <joint class="knee" name="FR_calf_joint2"/>
            <geom class="visual" mesh="calf"/>
            <geom class="calf1"/>
            <geom class="calf2"/>
            <geom name="FR2" class="foot"/>
            <site name="FR2" pos="0 0 -0.2" type="sphere" size=".015"/>
          </body>
        </body>
      </body>
      <body name="FL_hip2" pos="0.183 0.047 0">
        <inertial mass="0.696" pos="-0.003311 0.000635 3.1e-05"
            quat="0.494499 0.491507 0.506268 0.507528"
            diaginertia="0.000807752 0.00055293 0.000468983"/>
        <joint class="abduction" name="FL_hip_joint2"/>
        <geom class="visual" mesh="hip"/>
        <geom class="hip_left"/>
        <geom class="collision" size="0.04 0.04" pos="0 0.055 0" quat="1 1 0 0" type="cylinder"/>
        <body name="FL_thigh2" pos="0 0.08505 0">
          <inertial mass="1.013" pos="-0.003237 -0.022327 -0.027326"
              quat="0.999125 0.00256393 -0.0409531 0.00806091"
              diaginertia="0.00555739 0.00513936 0.00133944"/>
          <joint class="hip" name="FL_thigh_joint2"/>
          <geom class="visual" mesh="thigh"/>
          <geom class="thigh1"/>
          <geom class="thigh2"/>
          <geom class="thigh3"/>
          <body name="FL_calf2" pos="0 0 -0.2">
            <inertial mass="0.226" pos="0.00472659 0 -0.131975"
                quat="0.706886 0.017653 0.017653 0.706886"
                diaginertia="0.00340344 0.00339393 3.54834e-05"/>
            <joint class="knee" name="FL_calf_joint2"/>
            <geom class="visual" mesh="calf"/>
            <geom class="calf1"/>
            <geom class="calf2"/>
            <geom name="FL2" class="foot"/>
            <site name="FL2" pos="0 0 -0.2" type="sphere" size=".015"/>
          </body>
        </body>
      </body>
      <body name="HR_hip2" pos="-0.183 -0.047 0">
        <inertial mass="0.696" pos="0.003311 -0.000635 3.1e-05"
            quat="0.491507 0.494499 0.507528 0.506268"
            diaginertia="0.000807752 0.00055293 0.000468983"/>
        <joint class="abduction" name="RR_hip_joint2"/>
        <geom class="visual" quat="0 0 0 -1" mesh="hip"/>
        <geom class="hip_right"/>
        <body name="RR_thigh2" pos="0 -0.08505 0">
          <inertial mass="1.013" pos="-0.003237 0.022327 -0.027326"
              quat="0.999125 -0.00256393 -0.0409531 -0.00806091"
              diaginertia="0.00555739 0.00513936 0.00133944"/>
          <joint class="hip" name="RR_thigh_joint2"/>
          <geom class="visual" mesh="thigh_mirror"/>
          <geom class="thigh1"/>
          <geom class="thigh2"/>
          <geom class="thigh3"/>
          <body name="RR_calf2" pos="0 0 -0.2">
            <inertial mass="0.226" pos="0.00472659 0 -0.131975"
                quat="0.706886 0.017653 0.017653 0.706886"
                diaginertia="0.00340344 0.00339393 3.54834e-05"/>
            <joint class="knee" name="RR_calf_joint2"/>
            <geom class="visual" mesh="calf"/>
            <geom class="calf1"/>
            <geom class="calf2"/>
            <geom name="HR2" class="foot"/>
            <site name="RR2" pos="0 0 -0.2" type="sphere" size=".015"/>
          </body>
        </body>
      </body>
      <body name="HL_hip2" pos="-0.183 0.047 0">
        <inertial mass="0.696" pos="0.003311 0.000635 3.1e-05"
            quat="0.506268 0.507528 0.494499 0.491507"
            diaginertia="0.000807752 0.00055293 0.000468983"/>
        <joint class="abduction" name="RL_hip_joint2"/>
        <geom class="visual" quat="0 0 1 0" mesh="hip"/>
        <geom class="hip_left"/>
        <body name="RL_thigh2" pos="0 0.08505 0">
          <inertial mass="1.013" pos="-0.003237 -0.022327 -0.027326"
              quat="0.999125 0.00256393 -0.0409531 0.00806091"
              diaginertia="0.00555739 0.00513936 0.00133944"/>
          <joint class="hip" name="RL_thigh_joint2"/>
          <geom class="visual" mesh="thigh"/>
          <geom class="thigh1"/>
          <geom class="thigh2"/>
          <geom class="thigh3"/>
          <body name="RL_calf2" pos="0 0 -0.2">
            <inertial mass="0.226" pos="0.00472659 0 -0.131975"
                quat="0.706886 0.017653 0.017653 0.706886"
                diaginertia="0.00340344 0.00339393 3.54834e-05"/>
            <joint class="knee" name="RL_calf_joint2"/>
            <geom class="visual" mesh="calf"/>
            <geom class="calf1"/>
            <geom class="calf2"/>
            <geom name="HL2" class="foot"/>
            <site name="RL2" pos="0 0 -0.2" type="sphere" size=".015"/>
          </body>
        </body>
      </body>
    </body>
  </worldbody>
</mujoco>

Thank you again for your help :)

[thowell]

A couple things to look into:

• the keyframe is incorrect. the nominal quaternion should be 1 0 0 0 for each quadruped.
• the main script doesn't call planner_step in the main loop, this routine should to be called at least once for each quadruped before a simulation step.

If you think the mocap goal position is the issue, I recommend calling get_state after set_state to ensure that the mocap input to the agent was valid.