Question Reset Pos+Rot of Articulation bodies manually without a cacophony of Derp?

[Started in Unity 2020.1.0b10, transferred over to 2020.1.2f1]
Hey all,

TLDR;
I want to forcefully reposition articulation bodies (and their transforms) to their initial starting position so that the user can start the simulation again without restarting the whole scene and losing all their progress so far.


The Long verion
We are creating a sim where the user has to use a rather expensive piece of machinery to disassemble dangerous items. This uses the new articulation body components for the crane movement, 'telescope' extension for lowering and raising the arm array and then for the rotation of each 'joint' of the robotic arm itself.

In our simulation, when the user "snag's" the £Million arm on anything we pause the simulation and let the user see what they snagged the arm/crane on. Currently they then have to restart the Scene (and lose their progress on the disassembly so far)..

However, because these simulation 'campaigns' are long, we don't want to force the user to restart the scene entirely, thus wasting time. So, how can I 'Snap' the rotation and position of all the articulation bodies to their starting rotations and positions that don't fight against the physics?

Currently when I try disabling the articulation bodies and re-enable them all hell breaks loose. Simply setting the transforms position, rotations and the articulation bodies target is... unpredictable. Is there a trick I'm missing?

As always, many thanks. Stay safe and sane.


Mike

 

You can read/write all jointPosition/jointVelocity for all ArticulationBodies except for the root articulation body. If you save them on scene start (or whatever you see fit), you can restore all of them later.

JointPosition and jointVelocity are properties of type ReducedArticulationSpace and contain as many values as needed to describe the degrees of freedom of the joint you’re dealing with.

 

@tjmaul, Thank you so much. I cant believe I missed that ! Will have a play with that and will update thread with definitive results a little later.

Thank you so much for your assistance

 

For anyone finding this post via google, the above post from tjmaul is absolutely correct. To 'reset' an articulation body (bypassing the needed physics movements) I ran the following code:

Code (CSharp):

1. articulationBody.jointPosition = new ArticulationReducedSpace(0f, 0f, 0f);
2. articulationBody.jointAcceleration = new ArticulationReducedSpace(0f, 0f, 0f);
3. articulationBody.jointForce = new ArticulationReducedSpace(0f, 0f, 0f);
4. articulationBody.jointVelocity = new ArticulationReducedSpace(0f, 0f, 0f);
5.  

This was all that was needed. Happy coding all. Stay safe and sane

 

When I run this code, my arm just quickly flashes, but stays in the same position.

However, If I run this code my joints reset.

Code (CSharp):

1. var drive = articulation.xDrive;
2. drive.target = 0f;
3. articulation.xDrive = drive;

 

To change position and rotation of the Root ArticulationBody use api TeleportRoot()

Code (CSharp):

1. Vector3 Tpos= rootart.transform.position + Vector3.up * 0.1f; ;
2. Quaternion Trot= Quaternion.AngleAxis(30, Vector3.up);
3. rootart.TeleportRoot(Tpos, Trot);

 

For me it was all the things:

1) Reset joint 4 params (like above example);
2) Resetting drive (you can use different strategies here);
3) Resetting A.Body velocities (below):

Code (CSharp):

1. ab.velocity = Vector3.zero;
2. ab.angularVelocity = Vector3.zero;

 

Hello everyone, 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?

Code (CSharp):

1. public class PathGeneratorAgent : Agent
2. {
3.     private List<(ArticulationBody, Vector3, Quaternion)> initialStates;
4.     private Vector3 initialRobotPosition;
5.     private Quaternion initialRobotRotation;
6.  
7.       public override void Initialize()
8.     {
9.         GetInitialStates();
10.     }
11.  
12.  
13.     public override void OnEpisodeBegin()
14.     {
15.         ResetToInitialState();
16.     }
17.  
18.     public void GetInitialStates()
19.     {
20.         // Store the initial state of the robot
21.         initialRobotPosition = robot.transform.position;
22.         initialRobotRotation = robot.transform.rotation;
23.  
24.         // Store the initial state of all ArticulationBodies in the robot
25.         initialStates = new List<(ArticulationBody, Vector3, Quaternion)>();
26.         ArticulationBody[] articulationBodies = robot.GetComponentsInChildren<ArticulationBody>();
27.         foreach (ArticulationBody body in articulationBodies)
28.         {
29.             initialStates.Add((body, body.transform.localPosition, body.transform.localRotation));
30.         }
31.     }
32.  
33.     public void ResetToInitialState()
34.     {
35.         // Reset the robot to its initial state
36.         robot.transform.position = initialRobotPosition;
37.         robot.transform.rotation = initialRobotRotation;
38.  
39.         // Reset all ArticulationBodies in the robot to their initial state
40.         foreach (var state in initialStates)
41.         {
42.             // Reset the position and rotation
43.             state.Item1.transform.localPosition = state.Item2;
44.             state.Item1.transform.localRotation = state.Item3;
45.  
46.             // Reset the velocity and angular velocity
47.             state.Item1.velocity = Vector3.zero;
48.             state.Item1.angularVelocity = Vector3.zero;
49.  
50.             // If the ArticulationBody is not the root, reset the xDrive
51.             if (state.Item1 != initialStates[0].Item1)
52.             {
53.                 var drive = state.Item1.xDrive;
54.                 drive.target = 0f;
55.                 state.Item1.xDrive = drive;
56.             }
57.         }
58.     }

 

I have come to bestow a gift that I am currently using as of this post

Code (CSharp):

1. using UnityEngine;
2. using Unity.Mathematics;
3.  
4. using System.Collections.Generic;
5.  
6. public static class JointExtensions
7. {
8.     public static bool3 LockedJoints(this ConfigurableJoint joint)
9.     {
10.         return new bool3(
11.             joint.xMotion == ConfigurableJointMotion.Locked,
12.             joint.yMotion == ConfigurableJointMotion.Locked,
13.             joint.zMotion == ConfigurableJointMotion.Locked
14.         );
15.     }
16.     public static bool[] LockedJointArray(this ConfigurableJoint joint)
17.     {
18.         return new bool[3] {  joint.xMotion == ConfigurableJointMotion.Locked,
19.             joint.yMotion == ConfigurableJointMotion.Locked,
20.             joint.zMotion == ConfigurableJointMotion.Locked};
21.     }
22.     public static bool3 LockedJoints(this ArticulationBody body)
23.     {
24.         switch (body.jointType)
25.         {
26.             case ArticulationJointType.RevoluteJoint: return new bool3(true, false, false);
27.             case ArticulationJointType.SphericalJoint:
28.                 return new bool3(
29.                   body.swingYLock == ArticulationDofLock.LockedMotion,
30.                   body.swingZLock == ArticulationDofLock.LockedMotion,
31.                   body.twistLock == ArticulationDofLock.LockedMotion);
32.             case ArticulationJointType.PrismaticJoint:
33.                 return new bool3(
34.                     body.linearLockX == ArticulationDofLock.LockedMotion,
35.                     body.linearLockY == ArticulationDofLock.LockedMotion,
36.                     body.linearLockZ == ArticulationDofLock.LimitedMotion);
37.         }
38.         return true;
39.     }
40.     public static bool[] LockedJointArray(this ArticulationBody body)
41.     {
42.         switch (body.jointType)
43.         {
44.             case ArticulationJointType.RevoluteJoint: return new bool[3] { true, false, false };
45.             case ArticulationJointType.SphericalJoint:
46.                 return new bool[3] {
47.                   body.swingYLock == ArticulationDofLock.LockedMotion,
48.                   body.swingZLock == ArticulationDofLock.LockedMotion,
49.                   body.twistLock == ArticulationDofLock.LockedMotion};
50.             case ArticulationJointType.PrismaticJoint:
51.                 return new bool[3] { body.linearLockX == ArticulationDofLock.LockedMotion,
52.                     body.linearLockY == ArticulationDofLock.LockedMotion,
53.                     body.linearLockZ == ArticulationDofLock.LimitedMotion };
54.         }
55.         return new bool[3] { true, true, true };
56.     }
57.     /////////////////////////////
58.     // Joint Positions Related //
59.     /////////////////////////////
60.     public static void SetJointPositions(this ArticulationBody body, Vector3 pos)
61.     {
62.         List<float> tmp = new List<float>();
63.         int totalDOF = body.GetJointPositions(tmp);
64.         body.SetJointPositions(tmp,pos);
65.     }
66.     public static void SetJointPositions(this ArticulationBody body,List<float> tmp, Vector3 pos)
67.     {
68.         tmp[body.index] = pos.x;
69.         tmp[body.index + 1] = pos.y;
70.         tmp[body.index + 2] = pos.z;
71.         body.SetJointPositions(tmp);
72.     }
73.     public static void ResetJointPositions(this ArticulationBody body)
74.     {
75.         List<float> tmp = new List<float>();
76.         int totalDOF = body.GetJointPositions(tmp);
77.         for(int i = 0; i < tmp.Count; i++)
78.             tmp[i] = 0;
79.         body.SetJointPositions(tmp);
80.     }
81.     public static List<float> GetJointPositions(this ArticulationBody body)
82.     {
83.         var positions = new List<float>();
84.         body.GetJointPositions(positions);
85.         return positions;
86.     }
87.     ///////////////////////////
88.     // Joint Forces Related  //
89.     ///////////////////////////
90.     public static ArticulationReducedSpace DesiredAccelerationToReducedSpace(this ArticulationBody body, Vector3 desiredAcceleration)
91.     {
92.         if (body.dofCount == 3)
93.             return new ArticulationReducedSpace(desiredAcceleration.x, desiredAcceleration.y, desiredAcceleration.z);
94.         else if (body.dofCount == 2)
95.             return new ArticulationReducedSpace(desiredAcceleration.x, desiredAcceleration.y);
96.         else if (body.dofCount == 1)
97.             return new ArticulationReducedSpace(desiredAcceleration.x);
98.         else if (body.dofCount == 0)
99.             Debug.LogError("This function doesn't support bodies with 0 DOF");
100.         else
101.             Debug.LogError($"Failed to create ArticulationReducedSpace with {body.dofCount} DOFs");
102.         return new ArticulationReducedSpace();
103.     }
104.     public static void SetJointForces(this ArticulationBody body, ArticulationReducedSpace newForce)
105.     {
106.         body.SetJointForces(body.GetJointForces(), newForce);
107.     }
108.     public static void SetJointForces(this ArticulationBody body, Vector3 newForce)
109.     {
110.         body.SetJointForces(body.GetJointForces(), newForce);
111.     }
112.     public static void SetJointForces(this ArticulationBody body, List<float> allForces, ArticulationReducedSpace newForce)
113.     {
114.         if (body.dofCount == 0)
115.             return;
116.         if (body.dofCount >= 1)
117.             allForces[body.index] = newForce[0];
118.         if (body.dofCount >= 2)
119.             allForces[body.index + 1] = newForce[1];
120.         if (body.dofCount >= 3)
121.             allForces[body.index + 2] = newForce[2];
122.         body.SetJointForces(allForces);
123.     }
124.     public static void SetJointForces(this ArticulationBody body,List<float> allForces, Vector3 newForce)
125.     {
126.         if (body.dofCount == 0)
127.             return;
128.         if (body.dofCount >= 1)
129.             allForces[body.index] = newForce.x;
130.         if (body.dofCount >= 2)
131.             allForces[body.index + 1] = newForce.y;
132.         if (body.dofCount >= 3)
133.             allForces[body.index + 2] = newForce.z;
134.         body.SetJointForces(allForces);
135.     }
136.     public static List<float> GetJointForces(this ArticulationBody body)
137.     {
138.         List<float> tmp = new List<float>();
139.         int length = body.GetJointForces(tmp);
140.         return tmp;
141.     }
142.     ////////////////////////////////
143.     // Joint Acceleration Related //
144.     ////////////////////////////////
145.     public static void SetJointAcceleration(this ArticulationBody body,Vector3 acc)
146.     {
147.         if (body.dofCount == 0)
148.             return;
149.  
150.         body.SetJointForces(body.GetJointForcesForAcceleration(body.DesiredAccelerationToReducedSpace(acc)));
151.     }
152.     public static void SetJointAcceleration(this ArticulationBody body,ArticulationReducedSpace acc)
153.     {
154.         if (body.dofCount == 0)
155.             return;
156.  
157.         body.SetJointForces(body.GetJointForcesForAcceleration(acc));
158.     }
159.     /// <summary>
160.     /// Resets the Acceleration of the Articulation Body to 0 using the GetJointForcesForAcceleration function
161.     /// </summary>
162.     /// <param name="body"></param>
163.     public static void ResetJointAccelerations(this ArticulationBody body)
164.     {
165.         if (body.dofCount == 0)
166.             return;
167.  
168.         var newForce = body.DesiredAccelerationToReducedSpace(Vector3.zero);
169.  
170.         body.SetJointForces(newForce);
171.     }
172.     /////////////////////////////
173.     // Joint Velocity Related  //
174.     /////////////////////////////
175.     public static List<float> GetJointVelocities(this ArticulationBody body)
176.     {
177.         List<float> tmp = new List<float>();
178.         int totalDOF = body.GetJointVelocities(tmp);
179.         return tmp;
180.     }
181.     public static void ResetJointVelocities(this ArticulationBody body)
182.     {
183.         List<float> tmp = new List<float>();
184.         int totalDOF = body.GetJointVelocities(tmp);
185.         body.ResetJointVelocities(tmp);
186.     }
187.     public static void ResetJointVelocities(this ArticulationBody body,List<float> allVelocities)
188.     {
189.         for (int i = 0; i < allVelocities.Count; i++)
190.             allVelocities[i] = 0;
191.         body.SetJointVelocities(allVelocities);
192.     }
193.     public static void SetJointVelocities(this ArticulationBody body, Vector3 pos)
194.     {
195.         var tmp = body.GetJointVelocities();
196.         body.SetJointVelocities(tmp,pos);
197.     }
198.     public static void SetJointVelocities(this ArticulationBody body, ArticulationReducedSpace vel)
199.     {
200.         var tmp = body.GetJointVelocities();
201.         body.SetJointVelocities(tmp, vel);
202.     }
203.     public static void SetJointVelocities(this ArticulationBody body,in List<float> allVelocities,in Vector3 vel)
204.     {
205.         if (body.dofCount >= 1)
206.             allVelocities[body.index] = vel.x;
207.         if (body.dofCount >= 2)
208.             allVelocities[body.index + 1] = vel.y;
209.         if (body.dofCount >= 3)
210.             allVelocities[body.index + 2] = vel.z;
211.         body.SetJointVelocities(allVelocities);
212.     }
213.     public static void SetJointVelocities(this ArticulationBody body, in List<float> allVelocities, in ArticulationReducedSpace vel)
214.     {
215.         if (body.dofCount >= 1)
216.             allVelocities[body.index] = vel[0];
217.         if (body.dofCount >= 2)
218.             allVelocities[body.index + 1] = vel[1];
219.         if (body.dofCount >= 3)
220.             allVelocities[body.index + 2] = vel[2];
221.         body.SetJointVelocities(allVelocities);
222.     }
223.     ////////////////////////
224.     //
225.     ////////////////////////
226.    
227.     public static List<float> GetDriveForces(this ArticulationBody body){
228.         var tmp = new List<float>();
229.         int totalDof = body.GetDriveForces(tmp);
230.         return tmp;
231.     }
232.     public static void ResetDrives(this ArticulationBody body)
233.     {
234.         body.xDrive.ResetDriveTargetAndTargetVelocity();
235.         body.yDrive.ResetDriveTargetAndTargetVelocity();
236.         body.zDrive.ResetDriveTargetAndTargetVelocity();
237.     }
238.     public static void ResetDriveTargetAndTargetVelocity(this ArticulationDrive drive)
239.     {
240.         drive = new ArticulationDrive
241.         {
242.             damping =drive.damping,
243.             stiffness = drive.stiffness,
244.             driveType = drive.driveType,
245.             forceLimit = drive.forceLimit,
246.             lowerLimit = drive.lowerLimit,
247.             upperLimit = drive.upperLimit,
248.             target = 0,
249.             targetVelocity = 0,
250.         };
251.     }
252.     public static List<float> GetDriveTargets(this ArticulationBody body)
253.     {
254.         var tmp = new List<float>();
255.         int totalDof = body.GetDriveTargets(tmp);
256.         return tmp;
257.     }
258.     public static List<float> GetDriveTargetVelocities(this ArticulationBody body)
259.     {
260.         var tmp = new List<float>();
261.         int totalDof = body.GetDriveTargetVelocities(tmp);
262.         return tmp;
263.     }
264. }
265.  
266. public static class ListExtensions
267. {
268.     public static string ToString(this List<float> list,bool placeHolder)
269.     {
270.         string tmp = "{";
271.         if (list.Count > 0)
272.         {
273.             tmp += list[0];
274.             for (int i = 1; i < list.Count; i++)
275.                 tmp += "," + list[i];
276.         }
277.         return tmp + "}";
278.     }
279. }

It isn't the most efficent but I'm just trying to get things to work.

Reset code assumes theres a list of BodyParts (bp) with an articulation body in them.

Code (CSharp):

1.  if (bp.articulationBody.isRoot)
2. {
3.      // I was testing array lengths
4.      Debug.Log($"Accel: {bp.articulationBody.GetJointPositions().Count},{bp.articulationBody.GetJointForces().Count},{bp.articulationBody.GetJointVelocities().Count},{bp.articulationBody.GetDriveTargets().Count}");
5.      bp.articulationBody.TeleportRoot(startingPos, startingRot);
6.      bp.articulationBody.ResetJointVelocities();
7.      bp.articulationBody.ResetJointPositions();
8.   }
9. bp.articulationBody.ResetInertiaTensor();
10. bp.articulationBody.ResetDrives();
11. bp.articulationBody.velocity = Vector3.zero;
12. bp.articulationBody.angularVelocity = Vector3.zero;