Warning

Warning: Unity Simulation is deprecated as of December 2023, and is no longer available.

Create a custom controller

You may find yourself in a position with a vehicle model that operates differently and that needs a custom controller of your own. Let's go over the basic structure of these controllers and see if we can create a new one!

You can build your controller to send position, velocity, or torque commands to the Articulation body drives. You can read more on these drives in the articulation body reference documentation, but let's go over a few of the options for each Articulation body drive.

Control method 1: ArticulationBody velocity or position control

The articulation bodies themselves have a PhysX natively implemented PD controller. This is the best option because the PD controller is implemented in PhysX and is usually fairly stable. Start with this control method as it will usually require less tuning and details.

Each drive operates a simple PD controller, which can operate with a target position or velocity as well as configurable control parameters like stiffness (P gain), dampening (D gain), force limit(s), and joint limit(s).

When deciding how to control the ArticulationDrive properties in the ArticulationBody component, you can configure the properties in the inspector to get either velocity or position control or both by setting the control gains.

  • Set the drive Stiffness to 0 if you wish to control the robot using target velocities and damping only.
  • Set the drive Damping to 0 if you wish to control the robot using target positions and stiffness only.
  • Set a combination of Stiffness + target and Damping + targetVelocity if you wish to control the robot with a combination of position and velocity control.

You can then set the setpoints for the articulation drive in your code like this (note you need to set the drive to copy the drive before changing its velocity because of how the PhysX integration is interfaced).

//ab is assumed to be a reference to an ArticulationBody component.
var drive = ab.xDrive;
drive.targetVelocity = targetVelocity; 
ab.xDrive = drive;

Control method 2: ArticulationBody torque control.

Each drive has the ability to add torque directly. Be careful with this as the simulator will set the torques however you set them - this can result in instabilities and bizarre behaviors. You should only use this control method if you absolutely need to set the torque yourselves, otherwise use the higher-level interface exposed by the ArticulationBody drive.

//ab is assumed to be a reference to an ArticulationBody component.
ab.addTorque(1.0f); // this adds 1 Nm of torque to the articulationBody joint

Main components

The controller stack consists of these basic elements:

  • Controller script. This script takes a message and converts it to specific inputs for the ArticulationBody components. Think of the differential-drive controller taking in a linear speed input and converting that to target angular velocities for the driving wheels.
  • Message struct. This is the main package of information that will be sent to the controller. It will most likely match the ROS-equivalent schema for a given controller, but at the end of the day it can hold any information that you want to be sending. If you are not using ROS, then you are free to make whatever struct fits your needs without worrying about it.
  • Adapters. Adapters capture some form of input and produce a message that will be sent to the controller. This can be a Keyboard Adapter producing keystroke inputs, or a ROS message coming from the TCP connection, or any other form of input.
  • Authoring script (optional). These scripts are optional and only serve to make the setup and editing of certain elements easier. For example, instead of going to each ArticulationBody wheel and setting its stiffness or damping in the project hierarchy, you can create a script that will set all the wheels from one spot.

Controller

We've created a recipe that goes over each component of the Controller script in detail. The source code can be found below. Have a look!

  1. Setup the controller boilerplate. We use the Unity.Simulation.VehicleControllers namespace for all of our controllers. For ease of development you may want to use it as well.
    The controller should inherit from MonoBehaviour and the IRobotController interface.
    The IRobotController interface should have your specific vehicle message type defined.

  2. Specify which GameObjects you will control. The ArticulationBody object is the core Unity component that participates in the simulation. Here we specify two properties which will be able to be filled in the Inspector.
    Since this one is a differential-drive controller, we are going to need to have an ArticulationBody for the left and right wheels.

  3. Define our command interface. In the ConsumeMessage method here we define the way in which our vehicle can be commanded and a struct that it can accept.
    It should accept the same message type that the controller inherited in the IRobotController interface.
    We recommend even if you aren't using ROS to try and adhere to a ROS message schema.

  4. Define your core control logic. Here we define our core control logic. For our differential-drive example, we use simple differential-drive kinematics to compute the desired wheel velocity.
    We call ApplyInput only when a new command is received. Vehicle controllers usually receive a command and transform it using some kinematics to become individual joint commands. Since there is no feedback, this means it's usually not a good practice to update continuously, but rather only to update when new data is sent.
    Check out the ArticulationBody documentation for the detailed description of all the properties.
    One thing to note: methods like SetXDriveTargetVelocity are not part of Unity Core and have been added from the ArticulationBodyExtensions.cs found in the Shared folder for the Vehicle Controllers package.

  5. (Optional) Initialize parameters based on geometry. In your control logic, you can grab details of the pieces you are controlling to grab dimensions and ensure things are ready for your controller to control them.
    In our differential-drive controller, we grabbed the distance between the wheels or m_TrackWidth as well as the radius of the left wheel or m_WheelRadius.
    We make an assumption that both wheels will have the same radius and get the radius from the collider's bounds. Since the collider should be a sphere, the y extent is the same as a radius from the center of the sphere.

using UnityEngine;
using UnityEngine.Assertions;

namespace Unity.Simulation.VehicleControllers
{
    public class DifferentialDriveController : MonoBehaviour, IRobotController<DifferentialDriveControlMessage>
    {
        [SerializeField] private ArticulationBody m_LeftWheel;
        [SerializeField] private ArticulationBody m_RightWheel;

        private float m_WheelRadius;
        private float m_TrackWidth;

        private float m_ForwardSpeed;
        private float m_AngularSpeed;

        public ArticulationBody LeftWheel => m_LeftWheel;
        public ArticulationBody RighWheel => m_RightWheel;

        public void ConsumeMessage(DifferentialDriveControlMessage message)
        {
            m_ForwardSpeed = message.linear.x;      // (m/s)
            m_AngularSpeed = message.angular.z;     // (rad/s)

            ApplyInput();
        }

        private void ApplyInput()
        {
            var leftWheelRotationSpeed = m_ForwardSpeed / m_WheelRadius;
            var rightWheelRotationSpeed = leftWheelRotationSpeed;

            var addedAngularRotationalSpeed = m_AngularSpeed * m_TrackWidth / m_WheelRadius;

            leftWheelRotationSpeed = (leftWheelRotationSpeed + addedAngularRotationalSpeed / 2f) * Mathf.Rad2Deg;
            rightWheelRotationSpeed = (rightWheelRotationSpeed - addedAngularRotationalSpeed / 2f) * Mathf.Rad2Deg;

            m_LeftWheel.SetXDriveTargetVelocity(leftWheelRotationSpeed);
            m_RightWheel.SetXDriveTargetVelocity(rightWheelRotationSpeed);
        }

        // Start is called before the first frame update
        private void Start()
        {
            InitWheels();
        }

        private void InitWheels()
        {
            Debug.Assert(m_LeftWheel, "Left wheel is not set!");
            Debug.Assert(m_RightWheel, "Right wheel is not set!");

            m_TrackWidth = Vector3.Distance(m_LeftWheel.transform.position, m_RightWheel.transform.position);

            DrivingWheelSetup(m_LeftWheel, m_RightWheel);
        }

        private void DrivingWheelSetup(ArticulationBody leftWheel, ArticulationBody rightWheel)
        {
            var leftCollider = leftWheel.GetComponentInChildren<Collider>();
            var rightCollider = rightWheel.GetComponentInChildren<Collider>();

            if (leftCollider == null)
            {
                Debug.LogError($"Left wheel {leftWheel.name} ArticulationBody has no collider!");
                return;
            }

            if (rightCollider == null)
            {
                Debug.LogError($"Right wheel {rightWheel.name} ArticulationBody has no collider!");
                return;
            }

            //We're using bounds.extents.y instead of radius, because radius is not affected by scale
            if (rightCollider.bounds.extents.y != leftCollider.bounds.extents.y)
                Debug.LogError($"Left wheel radius and Right wheel radius are not identical! L: {leftCollider.bounds.extents.y}, R: {rightCollider.bounds.extents.y}");

            m_WheelRadius = leftCollider.bounds.extents.y;

            Assert.AreNotEqual(0f, leftCollider.bounds.extents.y, $"Left wheel {leftWheel.name} radius was 0f!");
            Assert.AreNotEqual(0f, rightCollider.bounds.extents.y, $"Right wheel {rightWheel.name} radius was 0f!");
        }
    }
}

Message

The message struct is just the information that is being passed on. You should create your own message type with the data you will want to be transferring to the controller.

namespace Unity.Simulation.VehicleControllers
{
    [Serializable]
    public struct DifferentialDriveControlMessage
    {
        public Vector3 linear;   
        public Vector3 angular;
    }
}

Adapter

The Adapter is a script that captures input from a source, constructs a message and sends that to the controller. We've created a recipe that explains how to create your own Adapter. The source code can be found below the steps. Let's go over it right now!

  1. Setup the adapter boilerplate. We use the Unity.Simulation.VehicleControllers namespace for all of our controllers. For ease of development you may want to use it as well.
    The Adapter should inherit from MonoBehaviour and an Interface made for your controller type. For example all adapters for the differential drive controller will be inheriting from the IDifferentialDriveAdapter.
    If you have a controller type that you created yourself, you should create an adapter interface for it as well. Take one of the other adapter interface classes as an example.
    There may not be that much content in our pre-made interfaces right now, but this is done with the future in mind, to give us a way to adjust all adapters if we so wish.

  2. Reference to controller. The adapter needs a reference to the controller. For this we have created a specific method that will fetch a controller with a specific message type from the Game Object that this script is currently attached to.
    If you have created a custom controller, you will probably also have to create a custom message script that carries data. Input your CustomControlMessage in place of the DifferentialDriveControlMessage

  3. Capture input. This is an example of the Keyboard Adapter, so it captures input by using the InputActionAsset from the new Input system package.

If you're creating an adapter from some other form of input (ROS for example), you should gather that information here.

  1. Turn the input into a message. Here we take the input and format it into a message type object so that we can send it to the controller.
    If you have a custom control message, this is where you should construct it.

  2. Send message to controller. We instruct the controller to consume our message, converting the input into actions for ArticulationBodies.

using UnityEngine.InputSystem;

namespace Unity.Simulation.VehicleControllers
{
    public class DifferentialDriveKeyboardAdapter : MonoBehaviour, IDifferentialDriveAdapter
    {
        [SerializeField] private float m_MaxLinearSpeed = 0.2f;         // (m/s)
        [SerializeField] private float m_MaxAngularSpeed = 45f;           // (degrees/s)

        [SerializeField] private InputActionAsset m_DifferentialInput;

        private IRobotController<DifferentialDriveControlMessage> m_Controller;
        private InputActionMap m_InputActionMap;
        private InputAction m_DriveAction;

        private void Start()
        {
            m_Controller = this.FetchControllerWithMessageType<DifferentialDriveControlMessage>();
            m_InputActionMap = m_DifferentialInput.FindActionMap("MainControl");

            m_DriveAction = m_InputActionMap.FindAction("Drive");

            m_DriveAction.Enable();
        }

        private void Update()
        {
            var drive = m_DriveAction.ReadValue<Vector2>(); // X: A/D  Y: W/S

            var message = new DifferentialDriveControlMessage()
            {
                linear = new Vector3(0, 0, drive.y * m_MaxLinearSpeed),
                angular = new Vector3(0, drive.x * m_MaxAngularSpeed * Mathf.Deg2Rad, 0)
            };
            m_Controller.ConsumeMessage(message);
        }
    }
}

You should now be able to create your own custom Controller and Adapter based on whatever vehicle model you have!