Skip to content

Commit eeb346e

Browse files
micahpearlmanmicahpearlman
committed
mostly working prismatic joints except limits are still a bit off.
1 parent d5043f7 commit eeb346e

File tree

3 files changed

+21
-65
lines changed

3 files changed

+21
-65
lines changed

‎Runtime/Scripts/Physics/ZOPrismaticJoint.cs‎

Lines changed: 6 additions & 54 deletions
Original file line numberDiff line numberDiff line change
@@ -88,19 +88,6 @@ public PrismaticJointLimits JointLimits {
8888

8989
UnityConfigurableJoint.linearLimit = _prismaticLimits;
9090

91-
if (ConnectedBody != null) {
92-
93-
Vector3 lowerLimit = transform.TransformPoint(Anchor + (JointDirection * JointLimits.LowerLimit));
94-
Vector3 upperLimit = transform.TransformPoint(Anchor + (JointDirection * JointLimits.UpperLimit));
95-
Vector3 midPoint = (lowerLimit + upperLimit) * 0.5f;
96-
Vector3 localMidpoint = ConnectedBody.transform.InverseTransformPoint(midPoint);
97-
98-
UnityConfigurableJoint.connectedAnchor = localMidpoint;
99-
} else {
100-
//TODO:
101-
}
102-
103-
10491
}
10592
}
10693

@@ -202,19 +189,6 @@ public Vector3 Anchor {
202189
set {
203190
UnityConfigurableJoint.anchor = value;
204191

205-
if (ConnectedBody != null) {
206-
207-
Vector3 lowerLimit = transform.TransformPoint(Anchor + (JointDirection * JointLimits.LowerLimit));
208-
Vector3 upperLimit = transform.TransformPoint(Anchor + (JointDirection * JointLimits.UpperLimit));
209-
Vector3 midPoint = (lowerLimit + upperLimit) * 0.5f;
210-
Vector3 localMidpoint = ConnectedBody.transform.InverseTransformPoint(midPoint);
211-
212-
UnityConfigurableJoint.connectedAnchor = localMidpoint;
213-
} else {
214-
//TODO
215-
216-
}
217-
218192
}
219193
}
220194

@@ -223,20 +197,6 @@ public Vector3 ConnectedAnchor {
223197
get {
224198
return UnityConfigurableJoint.connectedAnchor;
225199
}
226-
set {
227-
// Vector3 newConnectedAnchor = value;
228-
if (ConnectedBody != null) {
229-
230-
Vector3 lowerLimit = transform.TransformPoint(Anchor + (JointDirection * JointLimits.LowerLimit));
231-
Vector3 upperLimit = transform.TransformPoint(Anchor + (JointDirection * JointLimits.UpperLimit));
232-
Vector3 midPoint = (lowerLimit + upperLimit) * 0.5f;
233-
Vector3 localMidpoint = ConnectedBody.transform.InverseTransformPoint(midPoint);
234-
235-
UnityConfigurableJoint.connectedAnchor = localMidpoint;
236-
} else {
237-
//TODO:
238-
}
239-
}
240200
}
241201

242202
public bool _debug = false;
@@ -284,11 +244,6 @@ public Rigidbody ConnectedBody {
284244
set {
285245
if (UnityConfigurableJoint.connectedBody != value) {
286246
UnityConfigurableJoint.connectedBody = value;
287-
if (UnityConfigurableJoint.connectedBody != null) {
288-
ConnectedAnchor = UnityConfigurableJoint.connectedBody.transform.InverseTransformPoint(Anchor);
289-
} else {
290-
ConnectedAnchor = transform.TransformPoint(Anchor);
291-
}
292247
}
293248

294249
// update the name
@@ -369,6 +324,11 @@ private void Reset() {
369324
private void OnValidate() {
370325
SetupPrismaticJointFromConfigurableJoint();
371326
}
327+
private void OnDestroy() {
328+
if ((Application.isEditor == true) && (Application.isPlaying == false) && (UnityConfigurableJoint != null) && (Application.isLoadingLevel == false)) {
329+
DestroyImmediate(UnityConfigurableJoint);
330+
}
331+
}
372332

373333

374334

@@ -403,7 +363,7 @@ protected void CreateRequirements() {
403363
}
404364

405365
protected void SetupPrismaticJointFromConfigurableJoint() {
406-
UnityConfigurableJoint.autoConfigureConnectedAnchor = false;
366+
UnityConfigurableJoint.autoConfigureConnectedAnchor = true;
407367
UnityConfigurableJoint.yMotion = ConfigurableJointMotion.Locked;
408368
UnityConfigurableJoint.zMotion = ConfigurableJointMotion.Locked;
409369
UnityConfigurableJoint.angularXMotion = ConfigurableJointMotion.Locked;
@@ -418,14 +378,6 @@ protected void SetupPrismaticJointFromConfigurableJoint() {
418378

419379
}
420380

421-
// private void OnDestroy() {
422-
// if ((Application.isEditor ==true) && (Application.isPlaying == false) && (_hingeJoint != null) && (Application.isLoadingLevel == false)) {
423-
// DestroyImmediate(_hingeJoint);
424-
// }
425-
// }
426-
427-
428-
429381
}
430382

431383
}

‎Runtime/Scripts/Util/ImportExport/ZOExportURDF.cs‎

Lines changed: 7 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -237,7 +237,8 @@ protected void BuildURDFJoints(XElement robot, ZOSimOccurrence parent, ZOSimOccu
237237
Matrix4x4 newWorldJointMatrix = jointMatrix;
238238

239239
// subtract out the parent root
240-
jointMatrix = jointMatrix * worldJointMatrix.inverse;
240+
//jointMatrix = jointMatrix * worldJointMatrix.inverse;
241+
jointMatrix = worldJointMatrix.inverse * jointMatrix;
241242
worldJointMatrix = newWorldJointMatrix;
242243

243244
Vector3 xyz = jointMatrix.Position().Unity2Ros();
@@ -278,10 +279,12 @@ protected void BuildURDFJoints(XElement robot, ZOSimOccurrence parent, ZOSimOccu
278279
// Vector3 xyz = prismaticJoint.Anchor.Unity2Ros();//localAnchor.Unity2Ros();
279280
// Vector3 rpy = Vector3.zero; // BUGBUG
280281
jointMatrix = prismaticJoint.transform.WorldTranslationRotationMatrix();
281-
// jointMatrix = jointMatrix.AddTranslation(prismaticJoint.Anchor);
282+
jointMatrix = jointMatrix.AddTranslation(prismaticJoint.Anchor);
283+
284+
// TODO: the links should be offset by the connected anchor. Not the way done below.
282285
Vector3 worldConnectedAnchor = prismaticJoint.ConnectedBody.transform.TransformPoint(prismaticJoint.ConnectedAnchor);
283286
Vector3 relativeToParentConnectedAnchor = prismaticJoint.transform.InverseTransformPoint(worldConnectedAnchor);
284-
jointMatrix = jointMatrix.AddTranslation(relativeToParentConnectedAnchor);
287+
// jointMatrix = jointMatrix.AddTranslation(relativeToParentConnectedAnchor);
285288

286289
// save this off as the new world joint matrix
287290
Matrix4x4 newWorldJointMatrix = jointMatrix;
@@ -301,6 +304,7 @@ protected void BuildURDFJoints(XElement robot, ZOSimOccurrence parent, ZOSimOccu
301304
jointX.Add(origin);
302305

303306
URDFJoint j = new URDFJoint(child, parent, prismaticJoint.Anchor, prismaticJoint.ConnectedAnchor);
307+
// URDFJoint j = new URDFJoint(child, parent, prismaticJoint.Anchor, Vector3.zero);
304308
joints.Add(j);
305309

306310
} else { // children of the parent even without an explicit joint are "fixed" joints

‎Samples~/ZeroSimSamples/Scenes/PrismaticJoint_test.unity‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -1695,8 +1695,8 @@ ConfigurableJoint:
16951695
m_ConnectedArticulationBody: {fileID: 0}
16961696
m_Anchor: {x: 0, y: 0.6, z: 0}
16971697
m_Axis: {x: -0, y: -0, z: -1}
1698-
m_AutoConfigureConnectedAnchor: 0
1699-
m_ConnectedAnchor: {x: 0, y: 0, z: 0.25}
1698+
m_AutoConfigureConnectedAnchor: 1
1699+
m_ConnectedAnchor: {x: 0, y: 0, z: 0}
17001700
serializedVersion: 2
17011701
m_SecondaryAxis: {x: 0, y: 1, z: 0}
17021702
m_XMotion: 1
@@ -1817,8 +1817,8 @@ ConfigurableJoint:
18171817
m_ConnectedArticulationBody: {fileID: 0}
18181818
m_Anchor: {x: 0, y: 0.4, z: 0}
18191819
m_Axis: {x: -0, y: -0, z: -1}
1820-
m_AutoConfigureConnectedAnchor: 0
1821-
m_ConnectedAnchor: {x: 0, y: 0, z: 0.25}
1820+
m_AutoConfigureConnectedAnchor: 1
1821+
m_ConnectedAnchor: {x: 0, y: 0, z: 0}
18221822
serializedVersion: 2
18231823
m_SecondaryAxis: {x: 0, y: 1, z: 0}
18241824
m_XMotion: 1
@@ -1937,9 +1937,9 @@ ConfigurableJoint:
19371937
m_GameObject: {fileID: 1299972866}
19381938
m_ConnectedBody: {fileID: 1389040765}
19391939
m_ConnectedArticulationBody: {fileID: 0}
1940-
m_Anchor: {x: 0, y: 0.2, z: 0}
1940+
m_Anchor: {x: 0, y: 0.2, z: 0.25}
19411941
m_Axis: {x: -0, y: -0, z: -1}
1942-
m_AutoConfigureConnectedAnchor: 0
1942+
m_AutoConfigureConnectedAnchor: 1
19431943
m_ConnectedAnchor: {x: 0, y: 0.000000014901161, z: 0.25}
19441944
serializedVersion: 2
19451945
m_SecondaryAxis: {x: 0, y: 1, z: 0}
@@ -2038,7 +2038,7 @@ MonoBehaviour:
20382038
m_EditorClassIdentifier:
20392039
_name: joint.prismatic_from_base_to_LowDrawer
20402040
_connectedBody: {fileID: 1389040765}
2041-
_anchor: {x: 0, y: 0.2, z: 0}
2041+
_anchor: {x: 0, y: 0.2, z: 0.25}
20422042
_axis: {x: 0, y: 0, z: 1}
20432043
_useSpring: 0
20442044
_useMotor: 0
@@ -2265,7 +2265,7 @@ ConfigurableJoint:
22652265
m_ConnectedArticulationBody: {fileID: 0}
22662266
m_Anchor: {x: 0, y: 0, z: 0}
22672267
m_Axis: {x: -0, y: -1, z: -0}
2268-
m_AutoConfigureConnectedAnchor: 0
2268+
m_AutoConfigureConnectedAnchor: 1
22692269
m_ConnectedAnchor: {x: 0, y: -2, z: 0}
22702270
serializedVersion: 2
22712271
m_SecondaryAxis: {x: 0, y: 1, z: 0}

0 commit comments

Comments
 (0)