Adjust the IK setup to address the inverse kinematics issues mentioned in #1249.
This commit is contained in:
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user