Featherstone's solver for articulations

Another question about teleportroot. I couldn't find much information about what exactly it is doing. I want to do the following thing on a default game object:

Code (CSharp):

1.   Quaternion q = new Quaternion();
2.   // turn the object in a certain direction
3.   q.SetFromToRotation(transform.right, -Vector3.up);
4.   // apply the rotation
5.   articulationBody.TeleportRoot(transform.position, q);
6.   // add a velocity
7.   articulationBody.velocity = transform.right * 22;

I would expect the velocity vector to go downwards. Instead it uses the initial rotation and goes right as if it hasn't updated the rotation yet. Now, if I wait a frame with applying the velocity and apply it after an update cycle, it actually goes down as expected.
So it seems, TeleportRoot or the transform needs an update to actually get the rotation applied? Is this a bug or just undocumented/misunderstood behaviour? I can make a test project, if necessary.

This is on 2022.1.0b3

 

Oh, that makes sense, thank you! I guess, it's just a bit unexpected, because when you apply a rotation normally it doesn't go through the physics engine. Maybe one could put a hint in the documentation that stuff like this can happen.
On that note, I also had this issue with spherical joints: https://issuetracker.unity3d.com/is...rting-articulation-body-with-spherical-joints
Why not put info like this in the documentation? It took me quite a bit to figure out that there actually isn't a problem with my setup, but an underlying bugginess. I guess, I should check the issue tracker more carefully in the future.

 

Hi!
I ran into this error when trying to do SetJointPositions (or force, acceleration, velocity...)
"Articulation cache size(total degrees of freedom of all joints + 6 if root body is not immovable) does not match supplied list size!
UnityEngine.StackTraceUtility:ExtractStackTrace ()"

it was fixed in a later version: https://issuetracker.unity3d.com/is...ied-list-size-after-removing-child-gameobject

Is there any plan to port this fix back to the LTS? I'm working with 2020.3.26f and I don't really move away from the LTS

fyi, I checked my project on the beta, and it fixes my error.


edit: btw, it still seems to work. At least from what I can tell it still sets the positions, just just a spams the console like crazy (I have to do this quite often, multiple times per frame)

 

I did some more digging and found a repro for my case. It might be only tangentially related to the bug I posted.

setup is a simple 2 body setup. 1 immovable root, 1 prismatic limited body as a child.
this is the script on the root:

Code (CSharp):

1. using System.Collections;
2. using System.Collections.Generic;
3. using UnityEngine;
4.  
5. public class ArticulationTest : MonoBehaviour
6. {
7.     ArticulationBody m_Body;
8.     int m_DofCount;
9.  
10.     List<float> m_FloatList = new List<float>();
11.     List<int> m_DofStartIndices = new List<int>();
12.  
13.     public bool RecreateList;
14.  
15.     private void Awake()
16.     {
17.         m_Body = GetComponent<ArticulationBody>();
18.         m_DofCount = m_Body.GetDofStartIndices( m_DofStartIndices );
19.  
20.         for( int i = 0; i < m_DofCount; ++i )
21.         {
22.             m_FloatList.Add( 0f );
23.         }
24.  
25.         Debug.Log( "inital list " + m_FloatList.Count );
26.     }
27.  
28.     private void FixedUpdate()
29.     {
30.         m_FloatList.Clear();
31.  
32.         if( RecreateList )
33.         {
34.             m_FloatList = new List<float>();
35.         }
36.         m_Body.GetJointPositions( m_FloatList );
37.         Debug.Log( "list count " + m_FloatList.Count + " inital dof " + m_DofCount );
38.  
39.         m_Body.SetJointPositions( m_FloatList );
40.  
41.     }
42. }
43.  

this will initially throw the error. If the list is re-created (by enabling "RecreateList" in the inspector) the error disappears. It also will not reappear even when disabling it again.

It seems if the m_FloatList has data previously when first getting the joint data it breaks. And once data is got with a new List the issue is fixed for that body.
I don't know whats going on, but I think thats enough info for me to avoid the issue in my main project
Attached the files anyway incase this is interesting.

 

Hi, Articulation bodies have so many possibilities, so glad they have been added to Unity

I've implemented A Wrist joint with 3 Revolute joints YXZ, and rotations are fed to the Joint Drives from a VR Controller.
they work well and copy the controller rotations exactly. I also have code that keeps the angles from looping back and causing any jumps then the angles go past -180 or over -180.
However, since the joints are using Euler angles there is a fast YAW FLIP when pitch passes near 0; this is normal for Euler angles.
if the Pitch moves farther away from 0 degrees there is no noticeable problem. But if it's near 0 then the flip of YAW will be 180 degrees in a single physics time step. for nonphysical objects this is fine but these High-speed angle changes on a physics-based object make the entire rig jerk in different directions.

is there another way of setting up x3 revolute joints so this no longer occurs.

the Sphere joint (Currently not working) uses Euler angles as well, I'm hoping that this problem doesn't occur in them as well

 

Im trying to match an reference rig localrotations. And it works
mostly fine (ARMS only , trunk of torso and legs are not handled.) but - as I move the IK target down ( on the reference rig ) along the trunk it looses accuracy.


to make sure its not due to constraints I opened upperarm to -80,80 on all three
spherical drives both on upper and lower arm. I also disabled self collisions both
in physics layer and just disabling the colliders themselves


Im using the code below to translate the localrotations to reduced space from
marathon envs example.

Code (CSharp):

1.  
2.     public static Vector3 GetSwingTwist(Quaternion localRotation)
3.     {
4.  
5.  
6.         Quaternion a = new Quaternion();
7.         Quaternion b = new Quaternion();
8.  
9.  
10.         return GetSwingTwist(localRotation, out a, out b);
11.  
12.     }
13.  
14.     public static Vector3 GetSwingTwist(Quaternion localRotation, out Quaternion swing, out Quaternion twist)
15.     {
16.  
17.         //the decomposition in swing-twist, typically works like this:
18.  
19.         swing = new Quaternion(0.0f, localRotation.y, localRotation.z, localRotation.w);
20.         swing = swing.normalized;
21.  
22.         //Twist: assuming   q_localRotation = q_swing * q_twist
23.  
24.         twist = Quaternion.Inverse(swing) * localRotation;
25.  
26.  
27.         //double check:
28.         Quaternion temp = swing * twist;
29.  
30.         bool isTheSame = (Mathf.Abs(Quaternion.Angle(temp, localRotation)) < 0.001f);
31.  
32.  
33.         if (!isTheSame)
34.         {
35.             //  Debug.LogError("I have: " + temp + "which does not match: " + localRotation + "because their angle is: " + Quaternion.Angle(temp, localRotation));
36.             return Vector3.zero;
37.         }
38.        
39.  
40.  
41.         Vector3 InReducedCoord = new Vector3(twist.eulerAngles.x, swing.eulerAngles.y, swing.eulerAngles.z);            //this is consistent with how the values are stored in ArticulationBody:
42.  
43.  
44.         //we make sure we keep the values nearest to 0 (with a modulus)
45.         if (Mathf.Abs(InReducedCoord.x - 360) < Mathf.Abs(InReducedCoord.x))
46.             InReducedCoord.x = (InReducedCoord.x - 360);
47.         if (Mathf.Abs(InReducedCoord.y - 360) < Mathf.Abs(InReducedCoord.y))
48.             InReducedCoord.y = (InReducedCoord.y - 360);
49.         if (Mathf.Abs(InReducedCoord.z - 360) < Mathf.Abs(InReducedCoord.z))
50.             InReducedCoord.z = (InReducedCoord.z - 360);
51.  
52.         return InReducedCoord;
53.  
54.  
55.  
56.     }
57.  

Is there a proper way of doing this ? Like it mostly works but Im trying to make the character almost touch self to simulate bullet hits/ grab wound physical effect.

 

So I gave this a shot - editor is crashing when Im trying to assign a value to drive.target -
Stackoverflow. I can manipulate the revolute joints by their own in the editor / set targets / stiffness etc.

The lockup sorta makes it hard to debug cause it freezes.

Here is how I set it up.

 

just a fyi - I removed all by one joint(first r_XAxis) revolute and it still crashes when a assign to it :S what could even cause this ?

 

I have created a small repo for the error.
https://easyupload.io/wd3qz4

Im unsure what "case number" means.

Edit : sorry I was scatter brained - bug submitted.
CASE 1407315

 

Hi, I'm currently trying to network articulation bodies for a vr game. I now run into an issue with the root. I get the values (position, velocity, etc) with GetJointPositions(), but for the root they are all 0. It has 6 dof (which seems correct for position and rotation) but all these values are always 0.
I can even push it around and give it forces etc and it moves. Do I have to get these values with the transform position and _body.velocity ?
why does it have 6 dof if all the values are 0? I would expect the to hold the position / rotation data? maybe I'm mistaken. The same method works fine with an immovable root and child articulations btw.

I'm using 2020.3.26f1 if thats relevant.

 

well this is embarrassing. I hade the method called UpdateJoints() and for whatever reason I I typed out FixedUpdate instead. Not only that I replicated the error.

I have no excuses. sorry for wasting everyones time.

Edit:

Trying the axis thing now - this does not seem to give me any more precision then before
sadly.

 

Anyone experiance problems with hiting articulation bodies with raycast if set to immovable and using animator ?
Using Unity 2021.1.16f1 (64-bit)

Btw Animator settings
Always animate , Animate physics.

Edit: Looking closer at the problem - it seems colliders are not simulated with the animator. Perhaps this is a quirk of articulate bodies - if so , Im up *** creek.

 

Sounds like I built a giant paper weight I dont really know what todo to be able to interact with it when its static then.

Edit
I guess I could create a separate character within that I could raycast to ?

Edit #2
Yup that did it - I had to duplicate the character and match animations. Is there any plans to update transforms in the future ? Of course that might not even be desirable.

 

I have humanoid character that mostly animated via animator - ie what I call static is just "tradtionally" animated.
Like normally you have a ragdoll be "kinematic" until its state is triggered and it flops over.

Its just that my ragdoll is built with articulate bodies and driven with MLAgent for stumbling. But most of the time the bodies are immovable.

 

Hey

Thought I should update you on this. I fixed the detecting of the character by creating a reference animator with no articulate bodies. How ever there is like this weird "ghost" t-pose of the physical articulate bodies at the initial position of the rig. Have you ever experienced this ? I does not appear in the editor view either / no selectable - hence the "ghost" term.

Its a shame cause I only found it due to raycasting the shooting and the physical ragdoll collides with it ( the other characters )

A quick fix for this would just have a separate layer for "fixed joints" animated by meccaim character while not physical on separate layer and swap it to "ABragdoll" layer or something.

 

Been having problems sampling a direction(world space) and turning it into a rotation I can feed an articulate body.

(stepPoint - joint.transform.position).normalized - is the the direction I want to convert to something
articulate body can rotate towards.

spherical joint is xyz constraints.
low : -90
high: 90

Code (CSharp):

1.  
2.  
3.         Quaternion rotation = Quaternion.LookRotation((stepPoint - joint.transform.position).normalized, Vector3.up);
4.         Quaternion normalizedRotation = Quaternion.Normalize(Quaternion.Inverse(joint.parentAnchorRotation) *      rotation);
5.  
6.  
7.         if (joint.twistLock == ArticulationDofLock.LimitedMotion)
8.         {
9.             var drive = joint.xDrive;
10.             var scale = (drive.upperLimit - drive.lowerLimit) / 2f;
11.             var midpoint = drive.lowerLimit + scale;
12.             var target = midpoint + (normalizedRotation.x * scale);
13.  
14.             drive.stiffness = 1000f;
15.             drive.damping = 10f;
16.             drive.forceLimit = float.MaxValue;
17.             drive.target = target;
18.             joint.xDrive = drive;
19.         }
20.         if (joint.swingYLock == ArticulationDofLock.LimitedMotion)
21.         {
22.             var drive = joint.yDrive;
23.             var scale = (drive.upperLimit - drive.lowerLimit) / 2f;
24.             var midpoint = drive.lowerLimit + scale;
25.             var target = midpoint + (normalizedRotation.y * scale);
26.  
27.             drive.stiffness = 1000f;
28.             drive.damping = 10f;
29.             drive.forceLimit = float.MaxValue;
30.             drive.target = target;
31.             joint.yDrive = drive;
32.         }
33.         if (joint.swingZLock == ArticulationDofLock.LimitedMotion)
34.         {
35.             var drive = joint.zDrive;
36.             var scale = (drive.upperLimit - drive.lowerLimit) / 2f;
37.             var midpoint = drive.lowerLimit + scale;
38.             var target = midpoint + (normalizedRotation.z * scale);
39.  
40.             drive.stiffness = 1000f;
41.             drive.damping = 10f;
42.             drive.forceLimit = float.MaxValue;
43.             drive.target = target;
44.             joint.zDrive = drive;
45.         }

 

Hi, I’m working on an AR project on HoloLens 2, it’s about using hands to control robot.

I add an “object manipulator” script on every component so I can grab the robot and rotate it with my hand.

First, I tried to link every component of the robot with Hinge Joint. It works, but there are some problems with the self collision.(the upper arm of the robot can’t collide with the lower arm or the base).



Then I found the Articulation Body component, I used Revolute type and added same scripts to all the joints like the first time. However, I can’t really rotate a joint anymore. When I try to rotate a joint, it will shake at the target position of X Drive.(like the vedio below)

Does anyone have any ideas about how to solve the problem? How can I rotate a joint and update it’s target position in real time, so the joint will stop rotating immediately and stay in the new location. (just like I did with Hinge Joint)

Please forgive my poor English.

 

Hey, yant. Thanks for your reply, that solved some of my doubts. Maybe I'm not making myself clear, let me explain it again.


I use the “object manipulator” script to control every joint of the robot. You can see the introduction of it in https://docs.microsoft.com/en-us/windows/mixed-reality/mrtk-unity/

Features/ux-building-blocks/object-manipulator?view=mrtkunity-2021-05.




When I use Hinge Joint with Rigidbody(every rigidbody is kinematic), it’s working well. I can’t see the code of the “object manipulator” script because it’s not open source, but I think the script drives the joint just by changing Transform values because rigidbody is kinematic and it’s not working anymore with the ArticulationBody.

As you said, I tried to change the drive target value and it works. When changing the value of the drive target, the joint moving immediately and stay in target position when I stop it.

However, I don’t actually know how to write a code to implement the idea because I’m not familiar with C# and unity. Should I use ArticulationBody.SetDriveTargets or other methods?

Also I have no experience about how to link the new script with the object manipulator script.
I will try to learn about it these days, and if you have any ideas about that I would be grateful.

Anyway, your advice had already helped me a lot. Looking for your reply.
Thanks and best regards!

 

Hello, this is still an issue. The computeParentAnchor property isn't exposed in version 2020.3.32f1 that I am using, and judging from the Unity Documentation for some reason it has only been exposed in Unity version 2021.1, not the others. Is it possible to fix this? Doesn't sound like a difficult fix that would take that long, and currently it can be very frustrating to go through weird workarounds just because there is no access to a single flag. Please let me know if I missing something, thank you!

Edit: I forgot to mention I have managed to get access to it when I am in the editor through the serialized property, but when I am trying to build for android it gives me an error.

 

@yant @JuozasK Has there been a regression with ArticulationBody.index? It is giving me indices that don't seem to correlate with ArticulationBody.GetDofStartIndices(). I'm on Unity 2022.1.0b11 macOS. Using ArticulationBody.driveForce[] also seems to be giving me forces from other bodies in the tree.

Steps to repro:
1. Make ArticulationBody robot/ragdoll with branching linkages.
2. Query ArticulationBody.index in Editor (via Inspector GUI or something).
3. Hit Play, Stop
4. Query ArticulationBody.index in Editor again. Different indices are returned. Persists even when hitting Play again and querying in Play mode.

It also seems like GetDriveForces() is assuming the same indexing as GetDriveTargets(), but in fact that would be wrong. I have some fixed joints in between revolute joints and GetDriveForces() skips those joints. (The List<float> size is the same, but the last few indices are constantly zeroed) Or maybe I'm understanding something completely wrong here? I'm starting to doubt my sanity at this point.

Case 1416501

 

Okay I see, but why though only 2021.1 has the property exposed and not 2021.2?

 

Noted! Thanks for the quick reply!

 

Is the simulated force torque sensor already available? I checked the documentation and could not find any hint.

 

Did a solution to this turn up? I'm doing a networked game with ArticulationBody vehicles and I need to bleed off errors between the server and client simulations; whilst there's almost nothing I can do about the children's locations, I *should* be able to teleport the parent. But when I do this, the simulation explodes.; even teleporting the parent to the same exact identical location and rotation causes the children to explode. If I stop calling TeleportRoot, the instability goes away. This is a short clip of my running this code:

Code (CSharp):

1.  
2. void FixedUpdate()
3. {
4.     mAB.TeleportRoot(mAB.transform.position, mAB.transform.rotation);
5. }

https://i.gyazo.com/ffc0cdc0f385b2d8551a6b8388263809.mp4

I *assumed* that if I teleported an objects with a new rotation, it should move the entire chain of simulated bodies without any change in velocity or rotation or relative position of the children? The documentation on this is laughable, I'm afraid, so I'm stuck with asking here.

 

"In this case, we should assume all joint//velocity/pose is to be zero"

Does that mean we can't move an object once any/all of the children have moved?

 

Because of that line, in my case, I've had success with basically resetting the entire chain manually before ever calling TeleportRoot

So it goes something like this (in order, in a single timestep/frame)
1. Set velocity and angularVelocity to Vector3.zero for the root and all bodies in the tree
2. Disable all bodies in the tree (ArticulationBody.enabled = false)
3. Set all transforms in the tree to their initial pose (transform.localRotation/localPosition/scale)
4. Enable all bodies in the tree
5. Set velocity and angularVelocity to zero for the root and all bodies again
6. Finally call TeleportRoot(position, rotation)

In your case, you might have to set the pose and their targets to the previous pose? Not sure if it will work though.

 

So from what I'm reading, there is *no* way to safely move an Articulated Body? I believe there's no way to set the pose of a child AB at all - if that's the case, how are you supposed to write a networked game using them?

 

I completely missed the networking part of your post. Networking with Articulations (and networked physics in general) sounds like a nightmare! Good luck!

 

I'm hoping I get a technical solution to this rather than hoping on 'luck', especially as the developer is in this forum. It saddens me to even think that Unity would shove this system in *and* continue to develop it, but not offer any method whatsoever to use it properly. So - *does* the documentation need a massive improvement to point out that this CANNOT be used to move anything with children unless they're in their default pose? And if that's the case *how* does one write a game where you need to move an ArticulatedBody?

 

Thanks for adding featherstone articulations to Unity. I noticed from the docs that there are two types of articulation.

Maximal coordinate: game use-cases, specifically powered ragdoll simulations.
Reduced coordinate / featherstone: robotics use-cases.

I made an experiment with Maximal coordinate articulations using the UnityPhysXPlugin. It just copies an animation and applies torque on the pelvis for balance.

Are Maximal coordinate articulations:

• Currently supported but I just can’t find them
• Planned for a future release
• Won’t be supported

 

When trying to sync the position/velocities arrays over the network I came across an issue with the indices. The indices order seems not be deterministic or in any order that I can predict. And, on top of that, it differs between android and windows. I think this mostly happens when its a flat hierarchy ( multiple bodies on the same parent) but I didn't test this exhaustively.
My hackfix for this was to disable all the bodies and reenable them in the order I want to reinitialize the indices. It feels like the indices should be in some way deterministic or at least consistent across platforms?

The fix works for me so I'm good, but I thought I'd mention so it can be fixed and in case someone else has the same issue.
I use 2020.3.31f1 btw.

 

Any chance I could get an official response to this? Without the ability to bleed off delta errors between server and client, I don't see how the ArticulationBody systems can be used *at all* in network games!