diff options
Diffstat (limited to 'intern/iksolver/intern/IK_QJacobian.cpp')
| -rw-r--r-- | intern/iksolver/intern/IK_QJacobian.cpp | 617 |
1 files changed, 308 insertions, 309 deletions
diff --git a/intern/iksolver/intern/IK_QJacobian.cpp b/intern/iksolver/intern/IK_QJacobian.cpp index 678486e36f4..7f77968a5d4 100644 --- a/intern/iksolver/intern/IK_QJacobian.cpp +++ b/intern/iksolver/intern/IK_QJacobian.cpp @@ -21,11 +21,9 @@ * \ingroup iksolver */ - #include "IK_QJacobian.h" -IK_QJacobian::IK_QJacobian() - : m_sdls(true), m_min_damp(1.0) +IK_QJacobian::IK_QJacobian() : m_sdls(true), m_min_damp(1.0) { } @@ -35,392 +33,393 @@ IK_QJacobian::~IK_QJacobian() void IK_QJacobian::ArmMatrices(int dof, int task_size) { - m_dof = dof; - m_task_size = task_size; + m_dof = dof; + m_task_size = task_size; - m_jacobian.resize(task_size, dof); - m_jacobian.setZero(); + m_jacobian.resize(task_size, dof); + m_jacobian.setZero(); - m_alpha.resize(dof); - m_alpha.setZero(); + m_alpha.resize(dof); + m_alpha.setZero(); - m_nullspace.resize(dof, dof); + m_nullspace.resize(dof, dof); - m_d_theta.resize(dof); - m_d_theta_tmp.resize(dof); - m_d_norm_weight.resize(dof); + m_d_theta.resize(dof); + m_d_theta_tmp.resize(dof); + m_d_norm_weight.resize(dof); - m_norm.resize(dof); - m_norm.setZero(); + m_norm.resize(dof); + m_norm.setZero(); - m_beta.resize(task_size); + m_beta.resize(task_size); - m_weight.resize(dof); - m_weight_sqrt.resize(dof); - m_weight.setOnes(); - m_weight_sqrt.setOnes(); + m_weight.resize(dof); + m_weight_sqrt.resize(dof); + m_weight.setOnes(); + m_weight_sqrt.setOnes(); - if (task_size >= dof) { - m_transpose = false; + if (task_size >= dof) { + m_transpose = false; - m_jacobian_tmp.resize(task_size, dof); + m_jacobian_tmp.resize(task_size, dof); - m_svd_u.resize(task_size, dof); - m_svd_v.resize(dof, dof); - m_svd_w.resize(dof); + m_svd_u.resize(task_size, dof); + m_svd_v.resize(dof, dof); + m_svd_w.resize(dof); - m_svd_u_beta.resize(dof); - } - else { - // use the SVD of the transpose jacobian, it works just as well - // as the original, and often allows using smaller matrices. - m_transpose = true; + m_svd_u_beta.resize(dof); + } + else { + // use the SVD of the transpose jacobian, it works just as well + // as the original, and often allows using smaller matrices. + m_transpose = true; - m_jacobian_tmp.resize(dof, task_size); + m_jacobian_tmp.resize(dof, task_size); - m_svd_u.resize(task_size, task_size); - m_svd_v.resize(dof, task_size); - m_svd_w.resize(task_size); + m_svd_u.resize(task_size, task_size); + m_svd_v.resize(dof, task_size); + m_svd_w.resize(task_size); - m_svd_u_beta.resize(task_size); - } + m_svd_u_beta.resize(task_size); + } } -void IK_QJacobian::SetBetas(int id, int, const Vector3d& v) +void IK_QJacobian::SetBetas(int id, int, const Vector3d &v) { - m_beta[id + 0] = v.x(); - m_beta[id + 1] = v.y(); - m_beta[id + 2] = v.z(); + m_beta[id + 0] = v.x(); + m_beta[id + 1] = v.y(); + m_beta[id + 2] = v.z(); } -void IK_QJacobian::SetDerivatives(int id, int dof_id, const Vector3d& v, double norm_weight) +void IK_QJacobian::SetDerivatives(int id, int dof_id, const Vector3d &v, double norm_weight) { - m_jacobian(id + 0, dof_id) = v.x() * m_weight_sqrt[dof_id]; - m_jacobian(id + 1, dof_id) = v.y() * m_weight_sqrt[dof_id]; - m_jacobian(id + 2, dof_id) = v.z() * m_weight_sqrt[dof_id]; + m_jacobian(id + 0, dof_id) = v.x() * m_weight_sqrt[dof_id]; + m_jacobian(id + 1, dof_id) = v.y() * m_weight_sqrt[dof_id]; + m_jacobian(id + 2, dof_id) = v.z() * m_weight_sqrt[dof_id]; - m_d_norm_weight[dof_id] = norm_weight; + m_d_norm_weight[dof_id] = norm_weight; } void IK_QJacobian::Invert() { - if (m_transpose) { - // SVD will decompose Jt into V*W*Ut with U,V orthogonal and W diagonal, - // so J = U*W*Vt and Jinv = V*Winv*Ut - Eigen::JacobiSVD<MatrixXd> svd(m_jacobian.transpose(), Eigen::ComputeThinU | Eigen::ComputeThinV); - m_svd_u = svd.matrixV(); - m_svd_w = svd.singularValues(); - m_svd_v = svd.matrixU(); - } - else { - // SVD will decompose J into U*W*Vt with U,V orthogonal and W diagonal, - // so Jinv = V*Winv*Ut - Eigen::JacobiSVD<MatrixXd> svd(m_jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV); - m_svd_u = svd.matrixU(); - m_svd_w = svd.singularValues(); - m_svd_v = svd.matrixV(); - } - - if (m_sdls) - InvertSDLS(); - else - InvertDLS(); + if (m_transpose) { + // SVD will decompose Jt into V*W*Ut with U,V orthogonal and W diagonal, + // so J = U*W*Vt and Jinv = V*Winv*Ut + Eigen::JacobiSVD<MatrixXd> svd(m_jacobian.transpose(), + Eigen::ComputeThinU | Eigen::ComputeThinV); + m_svd_u = svd.matrixV(); + m_svd_w = svd.singularValues(); + m_svd_v = svd.matrixU(); + } + else { + // SVD will decompose J into U*W*Vt with U,V orthogonal and W diagonal, + // so Jinv = V*Winv*Ut + Eigen::JacobiSVD<MatrixXd> svd(m_jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV); + m_svd_u = svd.matrixU(); + m_svd_w = svd.singularValues(); + m_svd_v = svd.matrixV(); + } + + if (m_sdls) + InvertSDLS(); + else + InvertDLS(); } bool IK_QJacobian::ComputeNullProjection() { - double epsilon = 1e-10; - - // compute null space projection based on V - int i, j, rank = 0; - for (i = 0; i < m_svd_w.size(); i++) - if (m_svd_w[i] > epsilon) - rank++; - - if (rank < m_task_size) - return false; - - MatrixXd basis(m_svd_v.rows(), rank); - int b = 0; - - for (i = 0; i < m_svd_w.size(); i++) - if (m_svd_w[i] > epsilon) { - for (j = 0; j < m_svd_v.rows(); j++) - basis(j, b) = m_svd_v(j, i); - b++; - } - - m_nullspace = basis * basis.transpose(); - - for (i = 0; i < m_nullspace.rows(); i++) - for (j = 0; j < m_nullspace.cols(); j++) - if (i == j) - m_nullspace(i, j) = 1.0 - m_nullspace(i, j); - else - m_nullspace(i, j) = -m_nullspace(i, j); - - return true; + double epsilon = 1e-10; + + // compute null space projection based on V + int i, j, rank = 0; + for (i = 0; i < m_svd_w.size(); i++) + if (m_svd_w[i] > epsilon) + rank++; + + if (rank < m_task_size) + return false; + + MatrixXd basis(m_svd_v.rows(), rank); + int b = 0; + + for (i = 0; i < m_svd_w.size(); i++) + if (m_svd_w[i] > epsilon) { + for (j = 0; j < m_svd_v.rows(); j++) + basis(j, b) = m_svd_v(j, i); + b++; + } + + m_nullspace = basis * basis.transpose(); + + for (i = 0; i < m_nullspace.rows(); i++) + for (j = 0; j < m_nullspace.cols(); j++) + if (i == j) + m_nullspace(i, j) = 1.0 - m_nullspace(i, j); + else + m_nullspace(i, j) = -m_nullspace(i, j); + + return true; } -void IK_QJacobian::SubTask(IK_QJacobian& jacobian) +void IK_QJacobian::SubTask(IK_QJacobian &jacobian) { - if (!ComputeNullProjection()) - return; + if (!ComputeNullProjection()) + return; - // restrict lower priority jacobian - jacobian.Restrict(m_d_theta, m_nullspace); + // restrict lower priority jacobian + jacobian.Restrict(m_d_theta, m_nullspace); - // add angle update from lower priority - jacobian.Invert(); + // add angle update from lower priority + jacobian.Invert(); - // note: now damps secondary angles with minimum damping value from - // SDLS, to avoid shaking when the primary task is near singularities, - // doesn't work well at all - int i; - for (i = 0; i < m_d_theta.size(); i++) - m_d_theta[i] = m_d_theta[i] + /*m_min_damp * */ jacobian.AngleUpdate(i); + // note: now damps secondary angles with minimum damping value from + // SDLS, to avoid shaking when the primary task is near singularities, + // doesn't work well at all + int i; + for (i = 0; i < m_d_theta.size(); i++) + m_d_theta[i] = m_d_theta[i] + /*m_min_damp * */ jacobian.AngleUpdate(i); } -void IK_QJacobian::Restrict(VectorXd& d_theta, MatrixXd& nullspace) +void IK_QJacobian::Restrict(VectorXd &d_theta, MatrixXd &nullspace) { - // subtract part already moved by higher task from beta - m_beta = m_beta - m_jacobian * d_theta; + // subtract part already moved by higher task from beta + m_beta = m_beta - m_jacobian * d_theta; + + // note: should we be using the norm of the unrestricted jacobian for SDLS? - // note: should we be using the norm of the unrestricted jacobian for SDLS? - - // project jacobian on to null space of higher priority task - m_jacobian = m_jacobian * nullspace; + // project jacobian on to null space of higher priority task + m_jacobian = m_jacobian * nullspace; } void IK_QJacobian::InvertSDLS() { - // Compute the dampeds least squeares pseudo inverse of J. - // - // 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 - // solution. - // - // 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)| - // and doesn't try to minimize dTheta. This results in eratic changes in - // angle. The damped least squares minimizes |dtheta| to try and reduce this - // erratic behaviour. - // - // The selectively damped least squares (SDLS) is used here instead of the - // DLS. The SDLS damps individual singular values, instead of using a single - // damping term. - - double max_angle_change = M_PI / 4.0; - double epsilon = 1e-10; - int i, j; - - m_d_theta.setZero(); - m_min_damp = 1.0; - - for (i = 0; i < m_dof; i++) { - m_norm[i] = 0.0; - for (j = 0; j < m_task_size; j += 3) { - double n = 0.0; - n += m_jacobian(j, i) * m_jacobian(j, i); - n += m_jacobian(j + 1, i) * m_jacobian(j + 1, i); - n += m_jacobian(j + 2, i) * m_jacobian(j + 2, i); - m_norm[i] += sqrt(n); - } - } - - for (i = 0; i < m_svd_w.size(); i++) { - if (m_svd_w[i] <= epsilon) - continue; - - double wInv = 1.0 / m_svd_w[i]; - double alpha = 0.0; - double N = 0.0; - - // compute alpha and N - for (j = 0; j < m_svd_u.rows(); j += 3) { - alpha += m_svd_u(j, i) * m_beta[j]; - alpha += m_svd_u(j + 1, i) * m_beta[j + 1]; - alpha += m_svd_u(j + 2, i) * m_beta[j + 2]; - - // note: for 1 end effector, N will always be 1, since U is - // orthogonal, .. so could be optimized - double tmp; - tmp = m_svd_u(j, i) * m_svd_u(j, i); - tmp += m_svd_u(j + 1, i) * m_svd_u(j + 1, i); - tmp += m_svd_u(j + 2, i) * m_svd_u(j + 2, i); - N += sqrt(tmp); - } - alpha *= wInv; - - // compute M, dTheta and max_dtheta - double M = 0.0; - double max_dtheta = 0.0, abs_dtheta; - - for (j = 0; j < m_d_theta.size(); j++) { - double v = m_svd_v(j, i); - M += fabs(v) * m_norm[j]; - - // compute tmporary dTheta's - m_d_theta_tmp[j] = v * alpha; - - // find largest absolute dTheta - // multiply with weight to prevent unnecessary damping - abs_dtheta = fabs(m_d_theta_tmp[j]) * m_weight_sqrt[j]; - if (abs_dtheta > max_dtheta) - max_dtheta = abs_dtheta; - } - - M *= wInv; - - // compute damping term and damp the dTheta's - double gamma = max_angle_change; - if (N < M) - gamma *= N / M; - - double damp = (gamma < max_dtheta) ? gamma / max_dtheta : 1.0; - - for (j = 0; j < m_d_theta.size(); j++) { - // slight hack: we do 0.80*, so that if there is some oscillation, - // the system can still converge (for joint limits). also, it's - // better to go a little to slow than to far - - double dofdamp = damp / m_weight[j]; - if (dofdamp > 1.0) dofdamp = 1.0; - - m_d_theta[j] += 0.80 * dofdamp * m_d_theta_tmp[j]; - } - - if (damp < m_min_damp) - m_min_damp = damp; - } - - // weight + prevent from doing angle updates with angles > max_angle_change - double max_angle = 0.0, abs_angle; - - for (j = 0; j < m_dof; j++) { - m_d_theta[j] *= m_weight[j]; - - abs_angle = fabs(m_d_theta[j]); - - if (abs_angle > max_angle) - max_angle = abs_angle; - } - - if (max_angle > max_angle_change) { - double damp = (max_angle_change) / (max_angle_change + max_angle); - - for (j = 0; j < m_dof; j++) - m_d_theta[j] *= damp; - } + // Compute the dampeds least squeares pseudo inverse of J. + // + // 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 + // solution. + // + // 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)| + // and doesn't try to minimize dTheta. This results in eratic changes in + // angle. The damped least squares minimizes |dtheta| to try and reduce this + // erratic behaviour. + // + // The selectively damped least squares (SDLS) is used here instead of the + // DLS. The SDLS damps individual singular values, instead of using a single + // damping term. + + double max_angle_change = M_PI / 4.0; + double epsilon = 1e-10; + int i, j; + + m_d_theta.setZero(); + m_min_damp = 1.0; + + for (i = 0; i < m_dof; i++) { + m_norm[i] = 0.0; + for (j = 0; j < m_task_size; j += 3) { + double n = 0.0; + n += m_jacobian(j, i) * m_jacobian(j, i); + n += m_jacobian(j + 1, i) * m_jacobian(j + 1, i); + n += m_jacobian(j + 2, i) * m_jacobian(j + 2, i); + m_norm[i] += sqrt(n); + } + } + + for (i = 0; i < m_svd_w.size(); i++) { + if (m_svd_w[i] <= epsilon) + continue; + + double wInv = 1.0 / m_svd_w[i]; + double alpha = 0.0; + double N = 0.0; + + // compute alpha and N + for (j = 0; j < m_svd_u.rows(); j += 3) { + alpha += m_svd_u(j, i) * m_beta[j]; + alpha += m_svd_u(j + 1, i) * m_beta[j + 1]; + alpha += m_svd_u(j + 2, i) * m_beta[j + 2]; + + // note: for 1 end effector, N will always be 1, since U is + // orthogonal, .. so could be optimized + double tmp; + tmp = m_svd_u(j, i) * m_svd_u(j, i); + tmp += m_svd_u(j + 1, i) * m_svd_u(j + 1, i); + tmp += m_svd_u(j + 2, i) * m_svd_u(j + 2, i); + N += sqrt(tmp); + } + alpha *= wInv; + + // compute M, dTheta and max_dtheta + double M = 0.0; + double max_dtheta = 0.0, abs_dtheta; + + for (j = 0; j < m_d_theta.size(); j++) { + double v = m_svd_v(j, i); + M += fabs(v) * m_norm[j]; + + // compute tmporary dTheta's + m_d_theta_tmp[j] = v * alpha; + + // find largest absolute dTheta + // multiply with weight to prevent unnecessary damping + abs_dtheta = fabs(m_d_theta_tmp[j]) * m_weight_sqrt[j]; + if (abs_dtheta > max_dtheta) + max_dtheta = abs_dtheta; + } + + M *= wInv; + + // compute damping term and damp the dTheta's + double gamma = max_angle_change; + if (N < M) + gamma *= N / M; + + double damp = (gamma < max_dtheta) ? gamma / max_dtheta : 1.0; + + for (j = 0; j < m_d_theta.size(); j++) { + // slight hack: we do 0.80*, so that if there is some oscillation, + // the system can still converge (for joint limits). also, it's + // better to go a little to slow than to far + + double dofdamp = damp / m_weight[j]; + if (dofdamp > 1.0) + dofdamp = 1.0; + + m_d_theta[j] += 0.80 * dofdamp * m_d_theta_tmp[j]; + } + + if (damp < m_min_damp) + m_min_damp = damp; + } + + // weight + prevent from doing angle updates with angles > max_angle_change + double max_angle = 0.0, abs_angle; + + for (j = 0; j < m_dof; j++) { + m_d_theta[j] *= m_weight[j]; + + abs_angle = fabs(m_d_theta[j]); + + if (abs_angle > max_angle) + max_angle = abs_angle; + } + + if (max_angle > max_angle_change) { + double damp = (max_angle_change) / (max_angle_change + max_angle); + + for (j = 0; j < m_dof; j++) + m_d_theta[j] *= damp; + } } void IK_QJacobian::InvertDLS() { - // Compute damped least squares inverse of pseudo inverse - // Compute damping term lambda + // Compute damped least squares inverse of pseudo inverse + // Compute damping term lambda + + // Note when lambda is zero this is equivalent to the + // least squares solution. This is fine when the m_jjt is + // of full rank. When m_jjt is near to singular the least squares + // inverse tries to minimize |J(dtheta) - dX)| and doesn't + // try to minimize dTheta. This results in eratic changes in angle. + // Damped least squares minimizes |dtheta| to try and reduce this + // erratic behaviour. - // Note when lambda is zero this is equivalent to the - // least squares solution. This is fine when the m_jjt is - // of full rank. When m_jjt is near to singular the least squares - // inverse tries to minimize |J(dtheta) - dX)| and doesn't - // try to minimize dTheta. This results in eratic changes in angle. - // Damped least squares minimizes |dtheta| to try and reduce this - // erratic behaviour. + // We don't want to use the damped solution everywhere so we + // only increase lamda from zero as we approach a singularity. - // We don't want to use the damped solution everywhere so we - // only increase lamda from zero as we approach a singularity. + // find the smallest non-zero W value, anything below epsilon is + // treated as zero - // find the smallest non-zero W value, anything below epsilon is - // treated as zero + double epsilon = 1e-10; + double max_angle_change = 0.1; + double x_length = sqrt(m_beta.dot(m_beta)); - double epsilon = 1e-10; - double max_angle_change = 0.1; - double x_length = sqrt(m_beta.dot(m_beta)); + int i, j; + double w_min = std::numeric_limits<double>::max(); - int i, j; - double w_min = std::numeric_limits<double>::max(); + for (i = 0; i < m_svd_w.size(); i++) { + if (m_svd_w[i] > epsilon && m_svd_w[i] < w_min) + w_min = m_svd_w[i]; + } - for (i = 0; i < m_svd_w.size(); i++) { - if (m_svd_w[i] > epsilon && m_svd_w[i] < w_min) - w_min = m_svd_w[i]; - } - - // compute lambda damping term + // compute lambda damping term - double d = x_length / max_angle_change; - double lambda; + double d = x_length / max_angle_change; + double lambda; - if (w_min <= d / 2) - lambda = d / 2; - else if (w_min < d) - lambda = sqrt(w_min * (d - w_min)); - else - lambda = 0.0; + if (w_min <= d / 2) + lambda = d / 2; + else if (w_min < d) + lambda = sqrt(w_min * (d - w_min)); + else + lambda = 0.0; - lambda *= lambda; + lambda *= lambda; - if (lambda > 10) - lambda = 10; + if (lambda > 10) + lambda = 10; - // immediately multiply with Beta, so we can do matrix*vector products - // rather than matrix*matrix products + // immediately multiply with Beta, so we can do matrix*vector products + // rather than matrix*matrix products - // compute Ut*Beta - m_svd_u_beta = m_svd_u.transpose() * m_beta; + // compute Ut*Beta + m_svd_u_beta = m_svd_u.transpose() * m_beta; - m_d_theta.setZero(); + m_d_theta.setZero(); - for (i = 0; i < m_svd_w.size(); i++) { - if (m_svd_w[i] > epsilon) { - double wInv = m_svd_w[i] / (m_svd_w[i] * m_svd_w[i] + lambda); + for (i = 0; i < m_svd_w.size(); i++) { + if (m_svd_w[i] > epsilon) { + double wInv = m_svd_w[i] / (m_svd_w[i] * m_svd_w[i] + lambda); - // compute V*Winv*Ut*Beta - m_svd_u_beta[i] *= wInv; + // compute V*Winv*Ut*Beta + m_svd_u_beta[i] *= wInv; - for (j = 0; j < m_d_theta.size(); j++) - m_d_theta[j] += m_svd_v(j, i) * m_svd_u_beta[i]; - } - } + for (j = 0; j < m_d_theta.size(); j++) + m_d_theta[j] += m_svd_v(j, i) * m_svd_u_beta[i]; + } + } - for (j = 0; j < m_d_theta.size(); j++) - m_d_theta[j] *= m_weight[j]; + for (j = 0; j < m_d_theta.size(); j++) + m_d_theta[j] *= m_weight[j]; } void IK_QJacobian::Lock(int dof_id, double delta) { - int i; + int i; - for (i = 0; i < m_task_size; i++) { - m_beta[i] -= m_jacobian(i, dof_id) * delta; - m_jacobian(i, dof_id) = 0.0; - } + for (i = 0; i < m_task_size; i++) { + m_beta[i] -= m_jacobian(i, dof_id) * delta; + m_jacobian(i, dof_id) = 0.0; + } - m_norm[dof_id] = 0.0; // unneeded - m_d_theta[dof_id] = 0.0; + m_norm[dof_id] = 0.0; // unneeded + m_d_theta[dof_id] = 0.0; } double IK_QJacobian::AngleUpdate(int dof_id) const { - return m_d_theta[dof_id]; + return m_d_theta[dof_id]; } double IK_QJacobian::AngleUpdateNorm() const { - int i; - double mx = 0.0, dtheta_abs; - - for (i = 0; i < m_d_theta.size(); i++) { - dtheta_abs = fabs(m_d_theta[i] * m_d_norm_weight[i]); - if (dtheta_abs > mx) - mx = dtheta_abs; - } - - return mx; + int i; + double mx = 0.0, dtheta_abs; + + for (i = 0; i < m_d_theta.size(); i++) { + dtheta_abs = fabs(m_d_theta[i] * m_d_norm_weight[i]); + if (dtheta_abs > mx) + mx = dtheta_abs; + } + + return mx; } void IK_QJacobian::SetDoFWeight(int dof, double weight) { - m_weight[dof] = weight; - m_weight_sqrt[dof] = sqrt(weight); + m_weight[dof] = weight; + m_weight_sqrt[dof] = sqrt(weight); } - |