fix problem in returning to home

Assets/Walk2.cs

@@ -15,6 +15,7 @@ public class Walk2 : MonoBehaviour
             legsInitial.Add(legs[index].GetComponent<Leg>().point);
             step2.Add(0);
             step3.Add(0);
+            step4.Add(new Vector3(0, 0, 0));
         }
     }
 
@@ -66,6 +67,7 @@ public class Walk2 : MonoBehaviour
     //public float walkspeed = 1;
     private List<float> step2 = new List<float>();
     private List<float> step3 = new List<float>();
+    private List<Vector3> step4 = new List<Vector3>();
 
     // Update is called once per frame
     void FixedUpdate()
@@ -89,11 +91,12 @@ public class Walk2 : MonoBehaviour
                 {
                     step2[index] = (rotation * s[t].rotation) / speed2;
                     step3[index] = s[t].height / speed2;
+                    step4[index] = new Vector3(0, 0, 0);
                 }
                 else
                 {
-                    step2[index] = (rotation * s[t].rotation) / speed2;
-                    step3[index] = s[t].height / speed2;
+                    Vector3 t2 = legsInitial[index] - legs[index].GetComponent<Leg>().point;
+                    step4[index] = t2 / speed2;
                 }
             }
 
@@ -110,11 +113,19 @@ public class Walk2 : MonoBehaviour
     {
         for (int index = 0; index < legs.Count; ++index)
         {
-            Vector3 n = legs[index].GetComponent<Leg>().getWorldPos();
-            n = Quaternion.Euler(0, step2[index], 0) * n;
-            n.y += step3[index];
-            // rotate x,y around rotation.
-            legs[index].GetComponent<Leg>().setWorldPos(n);
+            if (step4[index] == new Vector3(0, 0, 0))
+            {
+                Vector3 n = legs[index].GetComponent<Leg>().getWorldPos();
+                n = Quaternion.Euler(0, step2[index], 0) * n;
+                n.y += step3[index];
+                // rotate x,y around rotation.
+                legs[index].GetComponent<Leg>().setWorldPos(n);
+            }
+            else
+            {
+                // return to home
+                legs[index].GetComponent<Leg>().point += step4[index];
+            }
         }
     }
 }