Unable to reset position and rotation of robot with ArticulationBody joints?

[wyd0817]

Hello everyone, I've encountered a similar problem with #143 . I'm trying to run my environment within the ML-Agent setting. However, I've noticed that my robot is unable to promptly return to the position I've assigned. My robot is a c30r tracked robot, named 'robot'. There's no ArticulationBody or RigidBody present on 'robot'. However, there is an ArticulationBody (excluding xDrive) on /c30r/base_link and also ArticulationBody (including xDrive) on the child objects of /c30r/base_link.

Here, robot/base_link serves as the root node for the ArticulationBody, which doesn't possess an xDrive due to the absence of a parent ArticulationBody for it to rotate or move in relation to. As for the child objects of robot/base_link, they do have the ArticulationBody component, including the xDrive. This is because they may need to rotate or move relative to their parent object, which is the robot/base_link.

In the Initialize() function, I keep track of the robot's state, and attempt to restore the robot's orientation and position in the OnEpisodeBegin(). Despite following the advice given above, I'm failing. Below is the core of my code, am I missing something?

public class PathGeneratorAgent : Agent
{
    private List<(ArticulationBody, Vector3, Quaternion)> initialStates;
    private Vector3 initialRobotPosition;
    private Quaternion initialRobotRotation;

      public override void Initialize() 
    {   
        GetInitialStates();  
    }

    public override void OnEpisodeBegin()
    {
        ResetToInitialState();
    }

    public void GetInitialStates()
    {
        // Store the initial state of the robot
        initialRobotPosition = robot.transform.position;
        initialRobotRotation = robot.transform.rotation;

        // Store the initial state of all ArticulationBodies in the robot
        initialStates = new List<(ArticulationBody, Vector3, Quaternion)>();
        ArticulationBody[] articulationBodies = robot.GetComponentsInChildren<ArticulationBody>();
        foreach (ArticulationBody body in articulationBodies)
        {
            initialStates.Add((body, body.transform.localPosition, body.transform.localRotation));
        }
    }

    public void ResetToInitialState()
    {
        // Reset the robot to its initial state
        robot.transform.position = initialRobotPosition;
        robot.transform.rotation = initialRobotRotation;

        // Reset all ArticulationBodies in the robot to their initial state
        foreach (var state in initialStates)
        {
            // Reset the position and rotation
            state.Item1.transform.localPosition = state.Item2;
            state.Item1.transform.localRotation = state.Item3;

            // Reset the velocity and angular velocity
            state.Item1.velocity = Vector3.zero;
            state.Item1.angularVelocity = Vector3.zero;

            // If the ArticulationBody is not the root, reset the xDrive
            if (state.Item1 != initialStates[0].Item1)
            {
                var drive = state.Item1.xDrive;
                drive.target = 0f;
                state.Item1.xDrive = drive;
            }
        }
    }

### Tasks

[Syjgin]

You can't directly change position & rotation when using articulation body. Try this instead