Search Unity

Question ArticulationBody.AddForce vs SetJointForces

Discussion in 'Physics' started by jnl77, Nov 19, 2023.

  1. jnl77

    jnl77

    Joined:
    Nov 19, 2023
    Posts:
    7
    I'm still confused about the usage of AddForce and SetJointForces. What does the input Vector3 represent in AddForce? Does it represent X Drive, Y Drive, etc.. or does it represent the force needed to rotate along each axis? The way I use AddForce for now is to loop through every moveable joints of the robot arm, and call AddForce for each articulationbody. However, I realize that the ArticulationBody is a chain, so does calling AddForce applying a cartesian force on the center of mass of this ArticulationBody? In this case, is SetJointForces a better choice? Thanks!
     
  2. arkano22

    arkano22

    Joined:
    Sep 20, 2012
    Posts:
    1,924
    As per the docs:
    So it's just a force vector, that is, direction and magnitude of the force applied. It's not an angular drive, or a torque (rotational force).

    It isn't. An ArticulationBody is a "link" in the chain. The chain is the entire physics articulation, composed of multiple ArticulationBodies.

    Yes, just like Rigidbody.AddForce the force is applied at the center of mass. If you want to add a force at any other point, use AddForceAtPosition.

    Depends on your use case since they do completely different things. AddForce adds an external force to a body. SetJointForces sets internal articulation forces, that is, forces that the joints exert on the bodies attached to them.
     
    Last edited: Nov 20, 2023
    jnl77 likes this.
  3. jnl77

    jnl77

    Joined:
    Nov 19, 2023
    Posts:
    7
    @arkano22 Thank you! I just implemented a version, but I am not sure why it is not behaving in the way I expected. No matter what values I sent, the arm behaves as if the JointForces are 0 anyways and shows free fall. I am passing a float list from Python to C# side. The way I call SetJointForces is as follows:

    First, I parse the float32 from python and store them into a List<float>. I verify this part by setting log to see the values.

    Code (CSharp):
    1.         private void SetJointForces(IncomingMessage msg){
    2.          
    3.             int jointCount = msg.ReadInt32();
    4.             //For debug
    5.             if (moveableJoints.Count != jointCount)
    6.             {
    7.                 Debug.LogError(string.Format("The number of target joint positions is {0}, but the valid number of joints in robot arm is {1}", jointCount, moveableJoints.Count));
    8.                 return;
    9.             }
    10.  
    11.             List<float> forces = new List<float>();
    12.             for (int i = 0; i < jointCount; i++)
    13.             {
    14.                 float f = msg.ReadFloat32();
    15.                 Debug.Log($"Forces count before SetJointForces: {f}");
    16.                 forces.Add(f);
    17.             }
    18.             // Debug.Log($"Forces count before SetJointForces: {forces.Count}");
    19.             // foreach (var force in forces)
    20.             // {
    21.             //     Debug.Log($"Force: {force}");
    22.             // }
    23.             SetJointForces(forces);
    24.         }
    Then, it calls another function that calls the setJointForces in another class. This is a part where I am unsure about, because I'm not sure which articulationBody I should use as the object instance. I set it to the first moveable joints.

    Code (CSharp):
    1.         private void SetJointForces(List<float> forces){
    2.             moveableJoints[0].GetUnit().SetJointForces(forces);
    3.         }
    After this, the SetJointForces() in the other file sets the forcemode and call ArticulationBody.SetJointForces as follows:

    Code (CSharp):
    1.  
    2. public void SetJointForces(List<float> forces){
    3. forceMode = ForceMode.JointForce;
    4. forces = forces;
    5. if (forces == null)
    6. {
    7. Debug.LogError("Received null forces list from the message.");
    8. return;
    9. }
    10. }
    Code (CSharp):
    1. enum ForceMode
    2.     {
    3.         None,
    4.         Force,
    5.         JointForce,
    6.         ForceAtPosition,
    7.         Torque
    8.     }
    9.     private ForceMode forceMode = ForceMode.None;
    10.     private Vector3 force = Vector3.zero;
    11.     private Vector3 position = Vector3.zero;
    12.     private List<float> forces = new List<float>(new float[13]); //CHANGED
    13.     void FixedUpdate()
    14.     {
    15.         // Debug.Log($"Forces count before SetJointForces: {forces.Count}");
    16.         // foreach (var force in forces)
    17.         // {
    18.         //     Debug.Log($"Force: {force}");
    19.         // }
    20.         switch (forceMode)
    21.         {
    22.             case ForceMode.Force:
    23.                 articulationBody.AddForce(force);
    24.                 break;
    25.             case ForceMode.JointForce:
    26.                 articulationBody.SetJointForces(forces); // CHANGED
    27.                 break;
    28.             case ForceMode.ForceAtPosition:
    29.                 articulationBody.AddForceAtPosition(force, position);
    30.                 break;
    31.             case ForceMode.Torque:
    32.                 articulationBody.AddTorque(force);
    33.                 break;
    34.         }
    35.         forceMode = ForceMode.None;
    36.     }
     
  4. jnl77

    jnl77

    Joined:
    Nov 19, 2023
    Posts:
    7
    Another problem is that I think we should set isKinematic() to false to enable our SetJointForces to work. However, if I unticked them in Unity Editor, the meshes disappear as soon as the arm starts to move. I'm not sure what could go wrong.
     
  5. arkano22

    arkano22

    Joined:
    Sep 20, 2012
    Posts:
    1,924
    You won't prevent free fall by setting internal joint forces, it's like trying to stay airborne by clenching your buttocks. If you want to make the articulated body levitate or something like that, you need to apply an external force (that is, use AddForce).

    Assuming SetJointForces is the correct choice for your use case, you're not using ArticulationBody.GetDofStartIndices or ArticulationBody.index to find where in the forces array you should write your values, your code assumes exactly one DOF per joint (hinge-like or prismatic joints). Not sure if it is a correct assumption in your case, but the code sure looks broken to me.

    Doesn't matter, since it affects the entire body hierarchy:
    https://docs.unity3d.com/ScriptReference/ArticulationBody.SetJointForces.html
    Closest thing to kinematic in an articulation is "Immovable", as far as i know. Anyway immovable or kinematic objects have no forces/accelerations of any kind applied to them.
     
    Last edited: Nov 21, 2023
  6. jnl77

    jnl77

    Joined:
    Nov 19, 2023
    Posts:
    7
    @arkano22 Thank you so much! You really help me get a better sense of the entire thing. I guess my last question will be if it is better to use AddTorque for torque control, such as gravity compensation, than to use AddForce? When using AddForce on a link, will its movement be affected by the joint attached to the link? If so, AddTorque is also set on the center of mass by default right?
     
  7. arkano22

    arkano22

    Joined:
    Sep 20, 2012
    Posts:
    1,924
    Entirely depends on your use case. AddForce will not impart any rotational motion in a body, since the force is added exactly at their center of mass (it would nee to be applied off-center to cause any rotation). So if you want to have some additional control over the rotation of a body, you either need to use AddForceAtPosition on a position away from the center of mass (which will result in both linear and angular movement) or use AddTorque.

    For instance, when controlling a quadcopter drone using a PID controller you typically have 4 off-center forces (one per rotor). These are enough to control both linear and angular movement, no need to tinker with torques directly.

    Yep. The body is still part of an articulation hierarchy and constrained by joints, no matter the forces you apply on it.

    Right, bodies always rotate around their center of mass so it doesn't make sense to apply torque anywhere else. Any compound motion that makes it look like the object is rotating around some other point is in fact just a combination of center of mass rotation + center of mass translation.
     
  8. jnl77

    jnl77

    Joined:
    Nov 19, 2023
    Posts:
    7

    Thank you! The use case for us is to implement gravity compensation by torque control. Each joint only has 1 DOF, and they appeared to be rotating along y-axis, so for each joint when I sent the force vector, it was <0, a, 0>, where a is the torque. To ensure that the links are rotating with their local y-axis, I used AddRelativeTorque instead of AddTorque. Unfortunately, the behavior is again not as what we expected. Even though each joint only has 1 DOF, setting values on either x or z entry can also rotate the link. Hence, we are unsure what's happening. Do you mind giving me some insight?

    The code workflow is as follows:

    On Python end, we send the torque in this way:

    def AddJointTorque(kwargs: dict) -> OutgoingMessage:
    compulsory_params = ["id", "joint_torques"]
    optional_params = []
    utility.CheckKwargs(kwargs, compulsory_params)

    msg = OutgoingMessage()
    joint_torque = kwargs["joint_torques"]
    num_joints = len(joint_torque)

    msg.write_int32(kwargs["id"])
    msg.write_string("AddRelativeTorque")
    msg.write_int32(num_joints)
    for i in range(num_joints):
    msg.write_float32(joint_torque[i][0])
    msg.write_float32(joint_torque[i][1])
    msg.write_float32(joint_torque[i][2])

    return msg


    Then, the information passed from this function will be sent to C# side in this sequence:
    In a case switch statement, it identifies the written string by:

    case "AddRelativeTorque":
    AddRelativeTorque(msg);
    return;

    Then, it calls:

    private void AddRelativeTorque(IncomingMessage msg){

    int jointCount = msg.ReadInt32();
    if (moveableJoints.Count != jointCount)
    {
    Debug.LogError(string.Format("The number of target joint positions is {0}, but the valid number of joints in robot arm is {1}", jointCount, moveableJoints.Count));
    return;
    }
    List<Vector3> jointTorques = new List<Vector3>();
    for (int i = 0; i < jointCount; i++)
    {
    Vector3 force = new Vector3();
    force.x = msg.ReadFloat32();
    force.y = msg.ReadFloat32();
    force.z = msg.ReadFloat32();
    jointTorques.Add(force);
    }
    AddRelativeTorque(jointTorques);
    }


    Then it calls:

    private void AddRelativeTorque(List<Vector3> jointTorques){
    for (int i = 0; i < moveableJoints.Count; i++)
    {
    moveableJoints[i].GetUnit().AddRelativeTorque(jointTorques[i]);
    }
    }


    Which calls in another run-time file:

    public void AddRelativeTorque(Vector3 forces){
    forceMode = ForceMode.RelativeTorque;
    force = forces;
    }


    The AddRelativeTorque is called in:

    void FixedUpdate()
    {
    // Debug.Log($"Forces count before SetJointForces: {forces.Count}");
    // foreach (var force in forces)
    // {
    // Debug.Log($"Force: {force}");
    // }
    switch (forceMode)
    {
    case ForceMode.Force:
    articulationBody.AddForce(force);
    break;
    case ForceMode.RelativeTorque:
    articulationBody.AddRelativeTorque(force); // CHANGED
    break;
    case ForceMode.ForceAtPosition:
    articulationBody.AddForceAtPosition(force, position);
    break;
    case ForceMode.Torque:
    articulationBody.AddTorque(force);
    break;
    }
    forceMode = ForceMode.None;
    }