Adding Force to GameObjects in Unity (using MuJoCo physics)

[mwinnick4]

Hi. I am trying to add a force vector to a cube in Unity, yet to my best knowledge, in order to add a force vector to a GameObject in Unity, the script itself needs a rigidbody. If using MuJoCo physics in Unity, which replaces the need for rigidbodies, how do I add a force vector? The following C# script is how I simply added a force vector to a cube that is not using MuJoCo physics properties and does in fact have a rigidbody.

AddForce

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class AddForce : MonoBehaviour
{

    Rigidbody m_Rigidbody;
    public float m_Thrust = 1000f;

    void Start()
    {
        //Fetch the Rigidbody from the GameObject with this script attached
        m_Rigidbody = GetComponent<Rigidbody>();
        m_Rigidbody.AddForce(transform.forward * m_Thrust);
    }

}

[Balint-H]

Take a look at how MjMouseSpring.cs does it, its inside the Editor components folder of the plugin. Basically it's editing a field in the mujoco scene's data structure, something like this:

scene.Data->xfrc_applied[6*body.MujocoId + 0] = appliedForce.x;
scene.Data->xfrc_applied[6*body.MujocoId + 1] = appliedForce.y;
scene.Data->xfrc_applied[6*body.MujocoId + 2] = appliedForce.z;

Where appliedForce is the Mujoco vector representation of your force. You may need to convert it to that format with MjEngineTool.MjVector3(Vector3 unityVec) first. Also, as far as I can tell, xfrc_applied is persistent, i.e. you need to set it to 0 once you are done with accelerating something.

Alternatively if your cube is just a free body (i.e. has a freejoint) you may also control the appropriate elements of qfrc_applied as well, which may be equivalent in this case.

For more info you can check out the docs adressing this:
https://mujoco.readthedocs.io/en/latest/APIreference.html
https://mujoco.readthedocs.io/en/latest/programming.html?#simulation-loop

[mwinnick4]

Ok. I was looking around MjMouseSpring, MjBody, etc. and was still a little lost (I am very new to MuJoCo so its just a little confusing for me). The goal for what I am doing is to apply a force on the Z axis only to a free body cube. I understand that I would want to do something like

scene.Data->qfrc_applied[body.MujocoId + 2] = appliedForce.z;

but the problem that I kept running into was that I am trying to do this in a MonoBehavior class but then I kept getting stuck on how to add MjBody body = target as MjBody; in order to call .MujocoId on the object itself (basically what I am saying is that I do not know how to implement the qfrc_applied onto a specific game object and therefore use the appliedForce method in a MonoBehavior script)

Thank you in advance. I'm just starting with Mujoco and your help is greatly appreciated.

Would something like this work at all or am I just totally lost?
MjAddForce.txt

[Balint-H]

No problem! In Mujoco there is a slightly different approach to how information about elements in a scene are stored and accessed when compared to how Unity handles it on a surface level. I may go into details that you are already familiar with, but I'd like to mention them just in case.

But first to explain how target as MjBody is used. That is actually only related to the fact that MjMouseSpring is an editor script. When you use the mouse spring, you need to tell the Editor that the component currently selected while clicking is an MjBody specifically. In a regular MonoBehaviour you don't need to cast the component to an MjBody, you can just Get the component once and store it as an MjBody.

Once you have a GameObject with an MjBody attached, an MjScene will be created at the start of play mode. After that, all information about the Mujoco side of the simulation can be accessed through that MjScene. Instead of each MjBody storing all information about their state themselves, the MjScene keeps track of all bodies' current states, velocities and accelerations on separate arrays. Since, for example, all positions are stored in one array, if you want to find the data of a specific body, you need to know where to start within that array. This address will be different for different arrays, depending on how many array elements there are per body/component. There are different ways to access this address for each array, but it is often body.MujocoId * someConstant. In your case that constant is 6 for xfrc_applied (since there are 3 elements for position, 3 for rotation for each body). If you want to use qfrc_applied instead, you need to get the qpos address of the freejoint that is the child of MjBody. I recommend sticking with xfrc for now.

Now that you have the address of where your current MjBody's information starts in the xfrc_applied array, you need the array itself. You access it from the MjScene like this:

MjScene.Instance.Data -> xfrc_applied

Behaviours that often need to access the mujoco Data may store the reference to the MjScene like this:

MjScene scene = MjScene.Instance

Also don't forget that in Mujoco the order of vector components are different (the Z component is up). Perhaps the best way to get the mujoco force vector that accelerates your object towards the local forward is to use MjEngineTool.MjVector3(transform.forward) and apply all 3 components to xfrc_applied.

Let me know if anything else is unclear!

[Balint-H]

If you frequently have to apply xfrc to MjBodies it might be worth writing an extension method for that. Something like this (untested):

public static unsafe void AddForce(this MjBody body, Vector3 unityFrc)
{
    Vector3 appliedForce = MjEngineTool.MjVector3(unityFrc);
    MjScene scene = MjScene.Instance;
    scene.Data->xfrc_applied[6*body.MujocoId + 0] = appliedForce.x;
    scene.Data->xfrc_applied[6*body.MujocoId + 1] = appliedForce.y;
    scene.Data->xfrc_applied[6*body.MujocoId + 2] = appliedForce.z;

}

I think quality of life functions like this will eventually be included in the plugin.

[mwinnick4]

That is very helpful! The only problem that I am running into right now is during runtime, a NullPointerException is thrown, stating that an object reference is not set to an instance of the object. I presume this issue is coming from the body.MujocoId. I have been playing around with how to properly initiate and declare MjBody body. Do you know how to properly solve this issue? I added the txt file to the code I have as of this moment.
MjAddForce (4).txt

[Balint-H]

Thats likely related to the fact that the MjScene initializes on Start() as well. So currently you want to acces the mujoco data before its Start() has been called by Unity. There are a couple ways to adress this.

Inside the project settings in Unity, you can define the execution order of different scripts, and can place MjScene at the top.

Instead you may also call MjScene.Instance.CreateScene() in your behaviour to make sure the scene is ready to be accessed.

You could also set the force in Update(), and skip it once its initialised

You may also call a couroutine in start that skips one frame and initializes the force on the next one.

These are I think the main ways to guarantee a MjScene being ready. In the future it might be good to maybe have an event after scene creation that scripts could subscribe to on Awake(), or another convenient way to simplify this issue.

[mwinnick4]

Ok. Is the way I initiated and declared correct (as in setting Mujoco.MjBody body = GetComponent<Mujoco.MjBody>())? I know that I can't use MjBody body = new MjBody() or use MjBody body= MjBody.Instance. At this moment, after making sure that the scene is initialized before start by redefining the execution order, I still obtain the same error of null pointer exception due to an object reference is not set to an instance of the object.

[Balint-H]

As long as the component is attached to the same gameobject as the behaviour is, then that should give you the reference to it. If its on a different GameObject then you may need to FindComponent or use [SerializeField] and assign the reference manually.

If you double click the error message it should take you to the line it occurs. Alternatively you could also call if(!body) Debug.Log("Body not found") to check if you have the reference. You could do something similar with MjScene.Instance to double check.

[mwinnick4]

Hi. I'm sorry for the repeated questions. I got it to a point where there are no compiler or runtime issues. I have safeguards to make sure that the scene and the body references are present and attached to the gameObject. When ran, the block that the force should be enacted on is not moving at all, which is strange because I even have 3 lines that are printed to the console that prints the forces enacted on each the x, y, and z axis. Not really sure where my issue is coming from now. LMK if you have any ideas but I greatly appreciate your help.

        public float m_Thrust = 1000f;
        Mujoco.MjBody body;
        Mujoco.MjScene scene;
        Vector3 unityVec;
        unsafe void Awake()
        {
            scene = MjScene.Instance;
            scene.CreateScene();
            body = GetComponent<Mujoco.MjBody>() as MjBody;
        }



        unsafe void Start()
        {
            //The unity vector uses the Unity strange coordinates (where z is the blue line)
            unityVec = new Vector3(0.0f, 0.0f, 50.0f);
            Vector3 appliedForce = MjEngineTool.MjVector3(m_Thrust * unityVec);

            if(!scene)
            {
                Debug.Log("scene not found");
            }

            if (!body)
            {
                Debug.Log("body not found");
            }

            if (scene != null)
            {
                scene.Data->xfrc_applied[6 * body.MujocoId] = appliedForce.x;
                scene.Data->xfrc_applied[6 * body.MujocoId + 1] = appliedForce.y;
                scene.Data->xfrc_applied[6 * body.MujocoId + 2] = appliedForce.z;
                Debug.Log("x-axis force: "+ appliedForce.x);
                Debug.Log("y-axis force: "+ appliedForce.y);
                Debug.Log("z-axis force: "+ appliedForce.z);
            }



            else
            {
                MjScene.Instance.CreateScene();
            }

        }


    }

[Balint-H]

Are you sure that the gameobject has a child gameobject that has a mjfreejoint component? In Mujoco by default bodies are not allowed to move. You need to allow a body degrees of freedom (number of ways it can move in relation to its parent body) by associating a joint with it. Freejoint allows unrestricted movement, which is what you need.

If you didnt have a joint, then there is also a chance you dont have an MjGeom either. MjGeom components are the equivalent of colliders in mujoco (+ some extras). That should be attached to another child gameobject (sibling of the joint). That gives you a collider but no rendering. You can right click the geom component and you should see the option to automatically add the corresponding mesh renderer.

So it should be:

• MjBody

• MjFreeJoint

• MjGeom + MjMeshFilter

I recommend importing a complete scene from an xml file and looking how its set up.

[mwinnick4]

Finally got it to work. I had the MjFreeJoint, MjGeom, and MjMeshFilter. The problem eventually was that it should not have been in Start() but rather in Update(). Once again thank you so much.