// 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);
}
}