21 m_task_size = task_size;
23 m_jacobian.resize(task_size, dof);
29 m_nullspace.resize(dof, dof);
31 m_d_theta.resize(dof);
32 m_d_theta_tmp.resize(dof);
33 m_d_norm_weight.resize(dof);
38 m_beta.resize(task_size);
41 m_weight_sqrt.resize(dof);
43 m_weight_sqrt.setOnes();
45 if (task_size >= dof) {
48 m_jacobian_tmp.resize(task_size, dof);
50 m_svd_u.resize(task_size, dof);
51 m_svd_v.resize(dof, dof);
54 m_svd_u_beta.resize(dof);
61 m_jacobian_tmp.resize(dof, task_size);
63 m_svd_u.resize(task_size, task_size);
64 m_svd_v.resize(dof, task_size);
65 m_svd_w.resize(task_size);
67 m_svd_u_beta.resize(task_size);
73 m_beta[
id + 0] =
v.x();
74 m_beta[
id + 1] =
v.y();
75 m_beta[
id + 2] =
v.z();
80 m_jacobian(
id + 0, dof_id) =
v.x() * m_weight_sqrt[dof_id];
81 m_jacobian(
id + 1, dof_id) =
v.y() * m_weight_sqrt[dof_id];
82 m_jacobian(
id + 2, dof_id) =
v.z() * m_weight_sqrt[dof_id];
84 m_d_norm_weight[dof_id] = norm_weight;
92 Eigen::JacobiSVD<MatrixXd> svd(m_jacobian.transpose(),
93 Eigen::ComputeThinU | Eigen::ComputeThinV);
94 m_svd_u = svd.matrixV();
95 m_svd_w = svd.singularValues();
96 m_svd_v = svd.matrixU();
101 Eigen::JacobiSVD<MatrixXd> svd(m_jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV);
102 m_svd_u = svd.matrixU();
103 m_svd_w = svd.singularValues();
104 m_svd_v = svd.matrixV();
119 for (i = 0; i < m_svd_w.size(); i++)
123 if (rank < m_task_size)
126 MatrixXd basis(m_svd_v.rows(), rank);
129 for (i = 0; i < m_svd_w.size(); i++)
131 for (j = 0; j < m_svd_v.rows(); j++)
132 basis(j,
b) = m_svd_v(j, i);
136 m_nullspace = basis * basis.transpose();
138 for (i = 0; i < m_nullspace.rows(); i++)
139 for (j = 0; j < m_nullspace.cols(); j++)
141 m_nullspace(i, j) = 1.0 - m_nullspace(i, j);
143 m_nullspace(i, j) = -m_nullspace(i, j);
154 jacobian.
Restrict(m_d_theta, m_nullspace);
163 for (i = 0; i < m_d_theta.size(); i++)
164 m_d_theta[i] = m_d_theta[i] + jacobian.
AngleUpdate(i);
170 m_beta = m_beta - m_jacobian * d_theta;
175 m_jacobian = m_jacobian * nullspace;
178 void IK_QJacobian::InvertSDLS()
196 double max_angle_change =
M_PI_4;
203 for (i = 0; i < m_dof; i++) {
205 for (j = 0; j < m_task_size; j += 3) {
207 n += m_jacobian(j, i) * m_jacobian(j, i);
208 n += m_jacobian(j + 1, i) * m_jacobian(j + 1, i);
209 n += m_jacobian(j + 2, i) * m_jacobian(j + 2, i);
210 m_norm[i] +=
sqrt(n);
214 for (i = 0; i < m_svd_w.size(); i++) {
218 double wInv = 1.0 / m_svd_w[i];
223 for (j = 0; j < m_svd_u.rows(); j += 3) {
224 alpha += m_svd_u(j, i) * m_beta[j];
225 alpha += m_svd_u(j + 1, i) * m_beta[j + 1];
226 alpha += m_svd_u(j + 2, i) * m_beta[j + 2];
231 tmp = m_svd_u(j, i) * m_svd_u(j, i);
232 tmp += m_svd_u(j + 1, i) * m_svd_u(j + 1, i);
233 tmp += m_svd_u(j + 2, i) * m_svd_u(j + 2, i);
240 double max_dtheta = 0.0, abs_dtheta;
242 for (j = 0; j < m_d_theta.size(); j++) {
243 double v = m_svd_v(j, i);
247 m_d_theta_tmp[j] =
v * alpha;
251 abs_dtheta =
fabs(m_d_theta_tmp[j]) * m_weight_sqrt[j];
252 if (abs_dtheta > max_dtheta)
253 max_dtheta = abs_dtheta;
259 double gamma = max_angle_change;
263 double damp = (gamma < max_dtheta) ? gamma / max_dtheta : 1.0;
265 for (j = 0; j < m_d_theta.size(); j++) {
270 double dofdamp = damp / m_weight[j];
274 m_d_theta[j] += 0.80 * dofdamp * m_d_theta_tmp[j];
277 if (damp < m_min_damp)
282 double max_angle = 0.0, abs_angle;
284 for (j = 0; j < m_dof; j++) {
285 m_d_theta[j] *= m_weight[j];
287 abs_angle =
fabs(m_d_theta[j]);
289 if (abs_angle > max_angle)
290 max_angle = abs_angle;
293 if (max_angle > max_angle_change) {
294 double damp = (max_angle_change) / (max_angle_change + max_angle);
296 for (j = 0; j < m_dof; j++)
297 m_d_theta[j] *= damp;
301 void IK_QJacobian::InvertDLS()
321 double max_angle_change = 0.1;
322 double x_length =
sqrt(m_beta.dot(m_beta));
327 for (i = 0; i < m_svd_w.size(); i++) {
328 if (m_svd_w[i] >
epsilon && m_svd_w[i] < w_min)
334 double d = x_length / max_angle_change;
340 lambda =
sqrt(w_min * (d - w_min));
353 m_svd_u_beta = m_svd_u.transpose() * m_beta;
357 for (i = 0; i < m_svd_w.size(); i++) {
359 double wInv = m_svd_w[i] / (m_svd_w[i] * m_svd_w[i] + lambda);
362 m_svd_u_beta[i] *= wInv;
364 for (j = 0; j < m_d_theta.size(); j++)
365 m_d_theta[j] += m_svd_v(j, i) * m_svd_u_beta[i];
369 for (j = 0; j < m_d_theta.size(); j++)
370 m_d_theta[j] *= m_weight[j];
377 for (i = 0; i < m_task_size; i++) {
378 m_beta[i] -= m_jacobian(i, dof_id) * delta;
379 m_jacobian(i, dof_id) = 0.0;
382 m_norm[dof_id] = 0.0;
383 m_d_theta[dof_id] = 0.0;
388 return m_d_theta[dof_id];
394 double mx = 0.0, dtheta_abs;
396 for (i = 0; i < m_d_theta.size(); i++) {
397 dtheta_abs =
fabs(m_d_theta[i] * m_d_norm_weight[i]);
407 m_weight[dof] = weight;
408 m_weight_sqrt[dof] =
sqrt(weight);
ATTR_WARN_UNUSED_RESULT const BMVert const BMEdge * e
ATTR_WARN_UNUSED_RESULT const BMVert * v
void Lock(int dof_id, double delta)
void SetDerivatives(int id, int dof_id, const Vector3d &v, double norm_weight)
void SetDoFWeight(int dof, double weight)
void Restrict(VectorXd &d_theta, MatrixXd &nullspace)
void SubTask(IK_QJacobian &jacobian)
double AngleUpdateNorm() const
void SetBetas(int id, int size, const Vector3d &v)
double AngleUpdate(int dof_id) const
void ArmMatrices(int dof, int task_size)
bool ComputeNullProjection()
ccl_device_inline float2 fabs(const float2 &a)
static const pxr::TfToken b("b", pxr::TfToken::Immortal)