Merge pull request open-rdc#8 from open-rdc/pseudo-inverse_matrix

擬似逆行列計算部分を追加
jsupratman13 · Apr 12, 2016 · a53710c · a53710c
2 parents 3358068 + 3af364f
commit a53710c
Showing 1 changed file with 6 additions and 1 deletion.
diff --git a/Kinematics/Kinematics.cpp b/Kinematics/Kinematics.cpp
@@ -94,7 +94,12 @@ bool Kinematics::calcInverseKinematics(int to, Link target)
 		J = calcJacobian(idx);
 		err = calcVWerr(target, ulink[to]);
 		if(err.norm() < eps) return true;
-		dq = lambda * (J.inverse() * err);
+
+		if(idx == 6)
+			dq = lambda * (J.inverse() * err);
+		else
+			dq = lambda * ((J.transpose()*J).inverse()*J.transpose()*err);
+
 		for(size_t nn=0;nn<idx.size();nn++)
 		{
 			int j = idx[nn];