public DenseMatrix getInversedJacobiFromSVD(DenseVector angles)
{
DenseMatrix jacobi = this.getJacobi(angles);
var svd = jacobi.Svd(true);
var s = new DiagonalMatrix(jacobi.RowCount, jacobi.ColumnCount, (1 / svd.S).ToArray());
// 疑似逆行列を計算する
var m = svd.VT.Transpose() * s.Transpose() * svd.U.Transpose();
return((DenseMatrix)m);
}
public void Compute_Vector()
{
Counting++;
DH_kinematics();
//Debug.Log(Key_Input.purpos_x);
/*
* if (Mathf.Abs(Key_Input.purpos_x - Key_Input.coordinate_x) == 0.0f)
* {
* move_x = 0.0f;
* }else
* {
* move_x = ((Key_Input.purpos_x - Key_Input.coordinate_x) / (Mathf.Abs(Key_Input.purpos_x - Key_Input.coordinate_x))) * step;
* }
*
* if (Mathf.Abs(Key_Input.purpos_y - Key_Input.coordinate_y) == 0.0f)
* {
* move_y = 0.0f;
* }
* else
* {
* move_y =((Key_Input.purpos_y - Key_Input.coordinate_y) / (Mathf.Abs(Key_Input.purpos_y - Key_Input.coordinate_y))) * step;
* }
*
*
* if (Mathf.Abs(Key_Input.purpos_z - Key_Input.coordinate_z) == 0.0f)
* {
* move_z = 0.0f;
* }
* else
* {
* move_z = ((Key_Input.purpos_z - Key_Input.coordinate_z) / (Mathf.Abs(Key_Input.purpos_z - Key_Input.coordinate_z))) * step;
* }
*/
/****目標位置まで移動するベクトルを算出****/
var move_vector = DenseMatrix.OfArray(new double[, ] {
{ Key_Input.purpos_x *step }, { Key_Input.purpos_y *step }, { Key_Input.purpos_z *step }
}); //DenseMatrix.OfArray(new double[,] { { move_x }, { move_y }, { move_z } });
//Debug.Log(move_vector);
/****ヤコビアン****/
var jacobian = DenseMatrix.OfArray(new double[, ] {
{ Mathf.Round(L4 * Mathf.Cos(t4) * (Mathf.Cos(t1) * Mathf.Sin(t3) - Mathf.Cos(t2) * Mathf.Cos(t3) * Mathf.Sin(t1)) - L2 * Mathf.Sin(t1) - L3 * Mathf.Sin(t1) * Mathf.Sin(t2) + L4 * Mathf.Sin(t1) * Mathf.Sin(t2) * Mathf.Sin(t4)), Mathf.Round(L3 * Mathf.Cos(t1) * Mathf.Cos(t2) - L4 * Mathf.Cos(t1) * Mathf.Cos(t2) * Mathf.Sin(t4) - L4 * Mathf.Cos(t1) * Mathf.Cos(t3) * Mathf.Cos(t4) * Mathf.Sin(t2)), Mathf.Round(L4 * Mathf.Cos(t4) * (Mathf.Cos(t3) * Mathf.Sin(t1) - Mathf.Cos(t1) * Mathf.Cos(t2) * Mathf.Sin(t3))), Mathf.Round(-L4 * Mathf.Sin(t4) * (Mathf.Sin(t1) * Mathf.Sin(t3) + Mathf.Cos(t1) * Mathf.Cos(t2) * Mathf.Cos(t3)) - L4 * Mathf.Cos(t1) * Mathf.Cos(t4) * Mathf.Sin(t2)) },
{ Mathf.Round(L2 * Mathf.Cos(t1) + L4 * Mathf.Cos(t4) * (Mathf.Sin(t1) * Mathf.Sin(t3) + Mathf.Cos(t1) * Mathf.Cos(t2) * Mathf.Cos(t3)) + L3 * Mathf.Cos(t1) * Mathf.Sin(t2) - L4 * Mathf.Cos(t1) * Mathf.Sin(t2) * Mathf.Sin(t4)), Mathf.Round(L3 * Mathf.Cos(t2) * Mathf.Sin(t1) - L4 * Mathf.Cos(t2) * Mathf.Sin(t1) * Mathf.Sin(t4) - L4 * Mathf.Cos(t3) * Mathf.Cos(t4) * Mathf.Sin(t1) * Mathf.Sin(t2)), Mathf.Round(-L4 * Mathf.Cos(t4) * (Mathf.Cos(t1) * Mathf.Cos(t3) + Mathf.Cos(t2) * Mathf.Sin(t1) * Mathf.Sin(t3))), Mathf.Round(L4 * Mathf.Sin(t4) * (Mathf.Cos(t1) * Mathf.Sin(t3) - Mathf.Cos(t2) * Mathf.Cos(t3) * Mathf.Sin(t1)) - L4 * Mathf.Cos(t4) * Mathf.Sin(t1) * Mathf.Sin(t2)) },
{ 0, Mathf.Round(L3 * Mathf.Sin(t2) - L4 * Mathf.Sin(t2) * Mathf.Sin(t4) + L4 * Mathf.Cos(t2) * Mathf.Cos(t3) * Mathf.Cos(t4)), Mathf.Round(-L4 * Mathf.Cos(t4) * Mathf.Sin(t2) * Mathf.Sin(t3)), Mathf.Round(L4 * Mathf.Cos(t2) * Mathf.Cos(t4) - L4 * Mathf.Cos(t3) * Mathf.Sin(t2) * Mathf.Sin(t4)) }
});
//Debug.Log(jacobian);
/****特異値分解****/
var svd = jacobian.Svd(true);
/****ベクトルを行列に逆数を取って代入****/
var S = new DiagonalMatrix(jacobian.RowCount, jacobian.ColumnCount, (1 / svd.S).ToArray());
/**0除算の例外処理**/
for (int i = 0; i < S.RowCount; i++)
{
for (int j = 0; j < S.ColumnCount; j++)
{
if (double.IsInfinity(S[i, j]))
{
S[i, j] = 0.0f;
}
}
}
/**擬似逆行列**/
var pseudoInv = svd.VT.Transpose() * S.Transpose() * svd.U.Transpose();
//var pseudoInv = jacobian.Transpose() * (jacobian * jacobian.Transpose()).Inverse();
Debug.Log("psudoInv = " + pseudoInv);
Debug.Log("move_vector = " + move_vector);
/********Δθの算出*********/
var delta_theta = pseudoInv * move_vector;
Debug.Log("delta_theta = " + delta_theta);
t1 += (float)delta_theta[0, 0];
t2 += (float)delta_theta[1, 0];
t3 += (float)delta_theta[2, 0];
t4 += (float)delta_theta[3, 0];
//Debug.Log(t1 +", "+ t2 + ", " + t3 + ", " + t4);
DH_kinematics();
/*if(Mathf.Abs(Key_Input.purpos_x - Key_Input.coordinate_x) > error || Mathf.Abs(Key_Input.purpos_y - Key_Input.coordinate_y) > error || Mathf.Abs(Key_Input.purpos_z - Key_Input.coordinate_z) > error)
* {
* if (Counting != 30)
* {
* Compute_Vector();
* }
* else
* {
* Debug.Log("It could not Compute values of theta");
* Counting = 0;
* }
* }*/
}
