public void Rotate(bool direction) { float[] timers = { 0.01f, 1 / velocity, 1 / velocity, 1 / velocity, 1 / velocity }; for (int i = 0; i < 6; i++) { float[][] cartesian; if (i % 2 == 0) { cartesian = robotMove.GetRotateCycle(robotMove.l1 + robotMove.l2 + walkAmplitude, 0, -robotMove.l3 - walkHigh, stepHigh, direction, rotateStep, true); } else { cartesian = robotMove.GetRotateCycle(robotMove.l1 + robotMove.l2 + walkAmplitude, 0, -robotMove.l3 - walkHigh, stepHigh, direction, rotateStep, false); } float[][] jointsEven = robotMove.GetJointsWalkingCycle(cartesian[0], cartesian[1], cartesian[2]); bool notNan = true; for (int j = 0; j < jointsEven.Length; j++) { for (int k = 0; k < jointsEven[j].Length; k++) { if (float.IsNaN(jointsEven[j][k])) { notNan = false; } } } if (notNan) { coxa.SetTrajectoryCoxa(jointsEven[0], timers, i); femur.SetTrajectoryFemur(jointsEven[1], timers, i); tibia.SetTrajectoryTibia(jointsEven[2], timers, i); } else { print("Commands impossible to reach"); } } }