// Update is called once per frame void Update() { Matrix <float> T = ForwardKin2(q); Matrix <float> J = AGILUSjacobian(q); Matrix <float> end_effector_matrix = ForwardKin(q); Matrix <float> ball_matrix = MathOperations.CalcMatrixT(0, 600, 1000, Mathf.PI / 2, 0, 0); float[] error = MathOperations.CalcErorr(ball_matrix, end_effector_matrix); float diff_r = error[0] * error[0] + error[1] * error[1] + error[2] * error[2]; float diif_o = error[3] * error[3] + error[4] * error[4] + error[5] * error[5]; if (diff_r > 1 || diif_o > 0.02F) { Vector <float> errorVec = Vector <float> .Build.DenseOfArray(error); Vector <float> del_q = J.PseudoInverse() * errorVec; for (int i = 0; i < 6; i++) { q[i] = q[i] - del_q[i] / 100; } SendState(q); } }