Adjust the IK setup to address the inverse kinematics issues mentioned in #1249.

This commit is contained in:
yunfeibai
2017-09-27 14:14:57 -07:00
parent 4d0d9f9870
commit adcece7927
3 changed files with 13 additions and 4 deletions

View File

@@ -354,6 +354,12 @@ void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV)
// Compute null space velocity
VectorRn nullV(J.GetNumColumns());
P.Multiply(desiredV, nullV);
// Compute residual
VectorRn residual(J.GetNumRows());
J.Multiply(nullV, residual);
// TODO: Use residual to set the null space term coefficient adaptively.
//printf("residual: %f\n", residual.Norm());
// Add null space velocity
dTheta += nullV;