diff --git a/2018-unity-sim/Assets/Foot.cs b/2018-unity-sim/Assets/Foot.cs index 19607e5..db17b44 100644 --- a/2018-unity-sim/Assets/Foot.cs +++ b/2018-unity-sim/Assets/Foot.cs @@ -25,6 +25,10 @@ public class Foot : Component public double rotationX=0, rotationY=0, rotationZ=0; public double O1, O2, O3; + public Vector3 transformation = new Vector3(0, 0, 0); + + static float temp = 2; + void RL3() { if ((X == 0) && (Z == 0)) @@ -34,6 +38,9 @@ public class Foot : Component if (X < 0) l3 = l3 * -1; + + //todo + l3 -= temp; } double FO1() @@ -55,7 +62,12 @@ public class Foot : Component double y2 = Y * Y; double z2 = Z * Z; - s = Math.Sqrt(x2 + y2 + z2); + double t = Math.Sqrt(x2 + z2); + t -= temp; + t = t * t; + //todo: X,Z remove distance temp. + s = Math.Sqrt(y2 + t); + //s = Math.Sqrt(x2 + y2 + z2); double t1 = 0; double s2 = s * s; @@ -127,10 +139,14 @@ public class Foot : Component Y += translateY; Z += translateZ; + X -= transformation.x; + Y -= transformation.y; + Z -= transformation.z; + /*RotateX(); RotateY(); RotateZ();*/ - + // the order of these rotation transformations is important!!! Vector3 v = new Vector3((float)X, (float)Y, (float)Z); v = Quaternion.Euler((float)0, (float)rotationY, (float)0) * v; @@ -139,7 +155,7 @@ public class Foot : Component X = v.x; Y = v.y; Z = v.z; - + X -= translateX; Y -= translateY; Z -= translateZ; @@ -153,7 +169,11 @@ public class Foot : Component Z = tx * -1; // run IK + // add adjustment from knie + O1 = (90 - FO1()); + // remove adjustment from knie + O2 = T1(); O3 = 180 - T3(); diff --git a/2018-unity-sim/Assets/Leg.cs b/2018-unity-sim/Assets/Leg.cs index d30967f..50cb6be 100644 --- a/2018-unity-sim/Assets/Leg.cs +++ b/2018-unity-sim/Assets/Leg.cs @@ -14,6 +14,8 @@ public class Leg : MonoBehaviour { public Vector3 point = new Vector3(10, 10, 0); public Quaternion rotation = new Quaternion(0, 0, 0, 0); + public Vector3 transformation = new Vector3(0, 0, 0); + public bool invert = false; public double translateX = 0; @@ -45,6 +47,8 @@ public class Leg : MonoBehaviour { foot.translateY = translateY; foot.translateZ = translateZ; + foot.transformation = transformation; + foot.invert = invert; foot.setleg(); diff --git a/2018-unity-sim/Assets/Leg.prefab b/2018-unity-sim/Assets/Leg.prefab index 19039e7..e597d20 100644 --- a/2018-unity-sim/Assets/Leg.prefab +++ b/2018-unity-sim/Assets/Leg.prefab @@ -1834,7 +1834,7 @@ MeshRenderer: m_LightProbeUsage: 1 m_ReflectionProbeUsage: 1 m_Materials: - - {fileID: 10756, guid: 0000000000000000e000000000000000, type: 0} + - {fileID: 10303, guid: 0000000000000000f000000000000000, type: 0} m_StaticBatchInfo: firstSubMesh: 0 subMeshCount: 0 @@ -3050,6 +3050,7 @@ MonoBehaviour: a3: 0 point: {x: 10, y: 10, z: 0} rotation: {x: 0, y: 0, z: 0, w: 0} + transformation: {x: 0, y: 0, z: 0} invert: 0 translateX: 5 translateY: 0 diff --git a/2018-unity-sim/Assets/Main.unity b/2018-unity-sim/Assets/Main.unity index 981e277..e09c1fa 100644 --- a/2018-unity-sim/Assets/Main.unity +++ b/2018-unity-sim/Assets/Main.unity @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:0b7396bf84046e0454ff9b50e8a912c5a16957bc69b409f12860d5f4f9254c73 -size 9964272 +oid sha256:e246558765b9012632b9fd65fba8186ffdfe19b1e96581c9f671176030e8e2d6 +size 9964531