Changeset View
Changeset View
Standalone View
Standalone View
intern/iksolver/intern/IK_QJacobian.cpp
| Show First 20 Lines • Show All 190 Lines • ▼ Show 20 Lines | void IK_QJacobian::Restrict(VectorXd &d_theta, MatrixXd &nullspace) | ||||
| m_jacobian = m_jacobian * nullspace; | m_jacobian = m_jacobian * nullspace; | ||||
| } | } | ||||
| void IK_QJacobian::InvertSDLS() | void IK_QJacobian::InvertSDLS() | ||||
| { | { | ||||
| // Compute the dampeds least squeares pseudo inverse of J. | // Compute the dampeds least squeares pseudo inverse of J. | ||||
| // | // | ||||
| // Since J is usually not invertible (most of the times it's not even | // Since J is usually not invertible (most of the times it's not even | ||||
| // square), the psuedo inverse is used. This gives us a least squares | // square), the pseudo inverse is used. This gives us a least squares | ||||
| // solution. | // solution. | ||||
| // | // | ||||
| // This is fine when the J*Jt is of full rank. When J*Jt is near to | // This is fine when the J*Jt is of full rank. When J*Jt is near to | ||||
| // singular the least squares inverse tries to minimize |J(dtheta) - dX)| | // singular the least squares inverse tries to minimize |J(dtheta) - dX)| | ||||
| // and doesn't try to minimize dTheta. This results in eratic changes in | // and doesn't try to minimize dTheta. This results in erratic changes in | ||||
| // angle. The damped least squares minimizes |dtheta| to try and reduce this | // angle. The damped least squares minimizes |dtheta| to try and reduce this | ||||
| // erratic behavior. | // erratic behavior. | ||||
| // | // | ||||
| // The selectively damped least squares (SDLS) is used here instead of the | // The selectively damped least squares (SDLS) is used here instead of the | ||||
| // DLS. The SDLS damps individual singular values, instead of using a single | // DLS. The SDLS damps individual singular values, instead of using a single | ||||
| // damping term. | // damping term. | ||||
| double max_angle_change = M_PI / 4.0; | double max_angle_change = M_PI / 4.0; | ||||
| ▲ Show 20 Lines • Show All 105 Lines • ▼ Show 20 Lines | |||||
| { | { | ||||
| // Compute damped least squares inverse of pseudo inverse | // Compute damped least squares inverse of pseudo inverse | ||||
| // Compute damping term lambda | // Compute damping term lambda | ||||
| // Note when lambda is zero this is equivalent to the | // Note when lambda is zero this is equivalent to the | ||||
| // least squares solution. This is fine when the m_jjt is | // least squares solution. This is fine when the m_jjt is | ||||
| // of full rank. When m_jjt is near to singular the least squares | // of full rank. When m_jjt is near to singular the least squares | ||||
| // inverse tries to minimize |J(dtheta) - dX)| and doesn't | // inverse tries to minimize |J(dtheta) - dX)| and doesn't | ||||
| // try to minimize dTheta. This results in eratic changes in angle. | // try to minimize dTheta. This results in erratic changes in angle. | ||||
| // Damped least squares minimizes |dtheta| to try and reduce this | // Damped least squares minimizes |dtheta| to try and reduce this | ||||
| // erratic behavior. | // erratic behavior. | ||||
| // We don't want to use the damped solution everywhere so we | // We don't want to use the damped solution everywhere so we | ||||
| // only increase lamda from zero as we approach a singularity. | // only increase lamda from zero as we approach a singularity. | ||||
| // find the smallest non-zero W value, anything below epsilon is | // find the smallest non-zero W value, anything below epsilon is | ||||
| // treated as zero | // treated as zero | ||||
| ▲ Show 20 Lines • Show All 91 Lines • Show Last 20 Lines | |||||