Jacobian.cpp 20 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735
  1. /*
  2. *
  3. * Inverse Kinematics software, with several solvers including
  4. * Selectively Damped Least Squares Method
  5. * Damped Least Squares Method
  6. * Pure Pseudoinverse Method
  7. * Jacobian Transpose Method
  8. *
  9. *
  10. * Author: Samuel R. Buss, [email protected].
  11. * Web page: http://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html
  12. *
  13. *
  14. This software is provided 'as-is', without any express or implied warranty.
  15. In no event will the authors be held liable for any damages arising from the use of this software.
  16. Permission is granted to anyone to use this software for any purpose,
  17. including commercial applications, and to alter it and redistribute it freely,
  18. subject to the following restrictions:
  19. 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  20. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  21. 3. This notice may not be removed or altered from any source distribution.
  22. *
  23. *
  24. */
  25. #include <stdlib.h>
  26. #include <math.h>
  27. #include <assert.h>
  28. #include <iostream>
  29. using namespace std;
  30. #include "Jacobian.h"
  31. void Arrow(const VectorR3& tail, const VectorR3& head);
  32. //extern RestPositionOn;
  33. //extern VectorR3 target1[];
  34. // Optimal damping values have to be determined in an ad hoc manner (Yuck!)
  35. const double Jacobian::DefaultDampingLambda = 0.6; // Optimal for the "Y" shape (any lower gives jitter)
  36. //const double Jacobian::DefaultDampingLambda = 1.1; // Optimal for the DLS "double Y" shape (any lower gives jitter)
  37. // const double Jacobian::DefaultDampingLambda = 0.7; // Optimal for the DLS "double Y" shape with distance clamping (lower gives jitter)
  38. const double Jacobian::PseudoInverseThresholdFactor = 0.01;
  39. const double Jacobian::MaxAngleJtranspose = 30.0*DegreesToRadians;
  40. const double Jacobian::MaxAnglePseudoinverse = 5.0*DegreesToRadians;
  41. const double Jacobian::MaxAngleDLS = 45.0*DegreesToRadians;
  42. const double Jacobian::MaxAngleSDLS = 45.0*DegreesToRadians;
  43. const double Jacobian::BaseMaxTargetDist = 0.4;
  44. Jacobian::Jacobian(Tree* tree)
  45. {
  46. m_tree = tree;
  47. m_nEffector = tree->GetNumEffector();
  48. nJoint = tree->GetNumJoint();
  49. nRow = 3 * m_nEffector; // Include only the linear part
  50. nCol = nJoint;
  51. Jend.SetSize(nRow, nCol); // The Jocobian matrix
  52. Jend.SetZero();
  53. Jtarget.SetSize(nRow, nCol); // The Jacobian matrix based on target positions
  54. Jtarget.SetZero();
  55. SetJendActive();
  56. U.SetSize(nRow, nRow); // The U matrix for SVD calculations
  57. w .SetLength(Min(nRow, nCol));
  58. V.SetSize(nCol, nCol); // The V matrix for SVD calculations
  59. dS.SetLength(nRow); // (Target positions) - (End effector positions)
  60. dTheta.SetLength(nCol); // Changes in joint angles
  61. dPreTheta.SetLength(nCol);
  62. // Used by Jacobian transpose method & DLS & SDLS
  63. dT1.SetLength(nRow); // Linearized change in end effector positions based on dTheta
  64. // Used by the Selectively Damped Least Squares Method
  65. //dT.SetLength(nRow);
  66. dSclamp.SetLength(m_nEffector);
  67. errorArray.SetLength(m_nEffector);
  68. Jnorms.SetSize(m_nEffector, nCol); // Holds the norms of the active J matrix
  69. Reset();
  70. }
  71. Jacobian::Jacobian(bool useAngularJacobian,int nDof)
  72. {
  73. m_tree = 0;
  74. m_nEffector = 1;
  75. if (useAngularJacobian)
  76. {
  77. nRow = 2 * 3 * m_nEffector; // Include both linear and angular part
  78. } else
  79. {
  80. nRow = 3 * m_nEffector; // Include only the linear part
  81. }
  82. nCol = nDof;
  83. Jend.SetSize(nRow, nCol); // The Jocobian matrix
  84. Jend.SetZero();
  85. Jtarget.SetSize(nRow, nCol); // The Jacobian matrix based on target positions
  86. Jtarget.SetZero();
  87. SetJendActive();
  88. U.SetSize(nRow, nRow); // The U matrix for SVD calculations
  89. w .SetLength(Min(nRow, nCol));
  90. V.SetSize(nCol, nCol); // The V matrix for SVD calculations
  91. dS.SetLength(nRow); // (Target positions) - (End effector positions)
  92. dTheta.SetLength(nCol); // Changes in joint angles
  93. dPreTheta.SetLength(nCol);
  94. // Used by Jacobian transpose method & DLS & SDLS
  95. dT1.SetLength(nRow); // Linearized change in end effector positions based on dTheta
  96. // Used by the Selectively Damped Least Squares Method
  97. //dT.SetLength(nRow);
  98. dSclamp.SetLength(m_nEffector);
  99. errorArray.SetLength(m_nEffector);
  100. Jnorms.SetSize(m_nEffector, nCol); // Holds the norms of the active J matrix
  101. Reset();
  102. }
  103. void Jacobian::Reset()
  104. {
  105. // Used by Damped Least Squares Method
  106. DampingLambda = DefaultDampingLambda;
  107. DampingLambdaSq = Square(DampingLambda);
  108. // DampingLambdaSDLS = 1.5*DefaultDampingLambda;
  109. dSclamp.Fill(HUGE_VAL);
  110. }
  111. // Compute the deltaS vector, dS, (the error in end effector positions
  112. // Compute the J and K matrices (the Jacobians)
  113. void Jacobian::ComputeJacobian(VectorR3* targets)
  114. {
  115. if (m_tree==0)
  116. return;
  117. // Traverse tree to find all end effectors
  118. VectorR3 temp;
  119. Node* n = m_tree->GetRoot();
  120. while ( n ) {
  121. if ( n->IsEffector() ) {
  122. int i = n->GetEffectorNum();
  123. const VectorR3& targetPos = targets[i];
  124. // Compute the delta S value (differences from end effectors to target positions.
  125. temp = targetPos;
  126. temp -= n->GetS();
  127. dS.SetTriple(i, temp);
  128. // Find all ancestors (they will usually all be joints)
  129. // Set the corresponding entries in the Jacobians J, K.
  130. Node* m = m_tree->GetParent(n);
  131. while ( m ) {
  132. int j = m->GetJointNum();
  133. assert ( 0 <=i && i<m_nEffector && 0<=j && j<nJoint );
  134. if ( m->IsFrozen() ) {
  135. Jend.SetTriple(i, j, VectorR3::Zero);
  136. Jtarget.SetTriple(i, j, VectorR3::Zero);
  137. }
  138. else {
  139. temp = m->GetS(); // joint pos.
  140. temp -= n->GetS(); // -(end effector pos. - joint pos.)
  141. temp *= m->GetW(); // cross product with joint rotation axis
  142. Jend.SetTriple(i, j, temp);
  143. temp = m->GetS(); // joint pos.
  144. temp -= targetPos; // -(target pos. - joint pos.)
  145. temp *= m->GetW(); // cross product with joint rotation axis
  146. Jtarget.SetTriple(i, j, temp);
  147. }
  148. m = m_tree->GetParent( m );
  149. }
  150. }
  151. n = m_tree->GetSuccessor( n );
  152. }
  153. }
  154. void Jacobian::SetJendTrans(MatrixRmn& J)
  155. {
  156. Jend.SetSize(J.GetNumRows(), J.GetNumColumns());
  157. Jend.LoadAsSubmatrix(J);
  158. }
  159. void Jacobian::SetDeltaS(VectorRn& S)
  160. {
  161. dS.Set(S);
  162. }
  163. // The delta theta values have been computed in dTheta array
  164. // Apply the delta theta values to the joints
  165. // Nothing is done about joint limits for now.
  166. void Jacobian::UpdateThetas()
  167. {
  168. // Traverse the tree to find all joints
  169. // Update the joint angles
  170. Node* n = m_tree->GetRoot();
  171. while ( n ) {
  172. if ( n->IsJoint() ) {
  173. int i = n->GetJointNum();
  174. n->AddToTheta( dTheta[i] );
  175. }
  176. n = m_tree->GetSuccessor( n );
  177. }
  178. // Update the positions and rotation axes of all joints/effectors
  179. m_tree->Compute();
  180. }
  181. void Jacobian::UpdateThetaDot()
  182. {
  183. if (m_tree==0)
  184. return;
  185. // Traverse the tree to find all joints
  186. // Update the joint angles
  187. Node* n = m_tree->GetRoot();
  188. while ( n ) {
  189. if ( n->IsJoint() ) {
  190. int i = n->GetJointNum();
  191. n->UpdateTheta( dTheta[i] );
  192. }
  193. n = m_tree->GetSuccessor( n );
  194. }
  195. // Update the positions and rotation axes of all joints/effectors
  196. m_tree->Compute();
  197. }
  198. void Jacobian::CalcDeltaThetas()
  199. {
  200. switch (CurrentUpdateMode) {
  201. case JACOB_Undefined:
  202. ZeroDeltaThetas();
  203. break;
  204. case JACOB_JacobianTranspose:
  205. CalcDeltaThetasTranspose();
  206. break;
  207. case JACOB_PseudoInverse:
  208. CalcDeltaThetasPseudoinverse();
  209. break;
  210. case JACOB_DLS:
  211. CalcDeltaThetasDLS();
  212. break;
  213. case JACOB_SDLS:
  214. CalcDeltaThetasSDLS();
  215. break;
  216. }
  217. }
  218. void Jacobian::ZeroDeltaThetas()
  219. {
  220. dTheta.SetZero();
  221. }
  222. // Find the delta theta values using inverse Jacobian method
  223. // Uses a greedy method to decide scaling factor
  224. void Jacobian::CalcDeltaThetasTranspose()
  225. {
  226. const MatrixRmn& J = ActiveJacobian();
  227. J.MultiplyTranspose( dS, dTheta );
  228. // Scale back the dTheta values greedily
  229. J.Multiply ( dTheta, dT1 ); // dT = J * dTheta
  230. double alpha = Dot(dS,dT1) / dT1.NormSq();
  231. assert ( alpha>0.0 );
  232. // Also scale back to be have max angle change less than MaxAngleJtranspose
  233. double maxChange = dTheta.MaxAbs();
  234. double beta = MaxAngleJtranspose/maxChange;
  235. dTheta *= Min(alpha, beta);
  236. }
  237. void Jacobian::CalcDeltaThetasPseudoinverse()
  238. {
  239. MatrixRmn& J = const_cast<MatrixRmn&>(ActiveJacobian());
  240. // Compute Singular Value Decomposition
  241. // This an inefficient way to do Pseudoinverse, but it is convenient since we need SVD anyway
  242. J.ComputeSVD( U, w, V );
  243. // Next line for debugging only
  244. assert(J.DebugCheckSVD(U, w , V));
  245. // Calculate response vector dTheta that is the DLS solution.
  246. // Delta target values are the dS values
  247. // We multiply by Moore-Penrose pseudo-inverse of the J matrix
  248. double pseudoInverseThreshold = PseudoInverseThresholdFactor*w.MaxAbs();
  249. long diagLength = w.GetLength();
  250. double* wPtr = w.GetPtr();
  251. dTheta.SetZero();
  252. for ( long i=0; i<diagLength; i++ ) {
  253. double dotProdCol = U.DotProductColumn( dS, i ); // Dot product with i-th column of U
  254. double alpha = *(wPtr++);
  255. if ( fabs(alpha)>pseudoInverseThreshold ) {
  256. alpha = 1.0/alpha;
  257. MatrixRmn::AddArrayScale(V.GetNumRows(), V.GetColumnPtr(i), 1, dTheta.GetPtr(), 1, dotProdCol*alpha );
  258. }
  259. }
  260. // Scale back to not exceed maximum angle changes
  261. double maxChange = dTheta.MaxAbs();
  262. if ( maxChange>MaxAnglePseudoinverse ) {
  263. dTheta *= MaxAnglePseudoinverse/maxChange;
  264. }
  265. }
  266. void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV)
  267. {
  268. const MatrixRmn& J = ActiveJacobian();
  269. MatrixRmn::MultiplyTranspose(J, J, U); // U = J * (J^T)
  270. U.AddToDiagonal( DampingLambdaSq );
  271. // Use the next four lines instead of the succeeding two lines for the DLS method with clamped error vector e.
  272. // CalcdTClampedFromdS();
  273. // VectorRn dTextra(3*m_nEffector);
  274. // U.Solve( dT, &dTextra );
  275. // J.MultiplyTranspose( dTextra, dTheta );
  276. // Use these two lines for the traditional DLS method
  277. U.Solve( dS, &dT1 );
  278. J.MultiplyTranspose( dT1, dTheta );
  279. // Compute JInv in damped least square form
  280. MatrixRmn UInv(U.GetNumRows(),U.GetNumColumns());
  281. U.ComputeInverse(UInv);
  282. assert(U.DebugCheckInverse(UInv));
  283. MatrixRmn JInv(J.GetNumColumns(), J.GetNumRows());
  284. MatrixRmn::TransposeMultiply(J, UInv, JInv);
  285. // Compute null space projection
  286. MatrixRmn JInvJ(J.GetNumColumns(), J.GetNumColumns());
  287. MatrixRmn::Multiply(JInv, J, JInvJ);
  288. MatrixRmn P(J.GetNumColumns(), J.GetNumColumns());
  289. P.SetIdentity();
  290. P -= JInvJ;
  291. // Compute null space velocity
  292. VectorRn nullV(J.GetNumColumns());
  293. P.Multiply(desiredV, nullV);
  294. // Add null space velocity
  295. dTheta += nullV;
  296. // Scale back to not exceed maximum angle changes
  297. double maxChange = dTheta.MaxAbs();
  298. if ( maxChange>MaxAngleDLS ) {
  299. dTheta *= MaxAngleDLS/maxChange;
  300. }
  301. }
  302. void Jacobian::CalcDeltaThetasDLS()
  303. {
  304. const MatrixRmn& J = ActiveJacobian();
  305. MatrixRmn::MultiplyTranspose(J, J, U); // U = J * (J^T)
  306. U.AddToDiagonal( DampingLambdaSq );
  307. // Use the next four lines instead of the succeeding two lines for the DLS method with clamped error vector e.
  308. // CalcdTClampedFromdS();
  309. // VectorRn dTextra(3*m_nEffector);
  310. // U.Solve( dT, &dTextra );
  311. // J.MultiplyTranspose( dTextra, dTheta );
  312. // Use these two lines for the traditional DLS method
  313. U.Solve( dS, &dT1 );
  314. J.MultiplyTranspose( dT1, dTheta );
  315. // Scale back to not exceed maximum angle changes
  316. double maxChange = dTheta.MaxAbs();
  317. if ( maxChange>MaxAngleDLS ) {
  318. dTheta *= MaxAngleDLS/maxChange;
  319. }
  320. }
  321. void Jacobian::CalcDeltaThetasDLSwithSVD()
  322. {
  323. const MatrixRmn& J = ActiveJacobian();
  324. // Compute Singular Value Decomposition
  325. // This an inefficient way to do DLS, but it is convenient since we need SVD anyway
  326. J.ComputeSVD( U, w, V );
  327. // Next line for debugging only
  328. assert(J.DebugCheckSVD(U, w , V));
  329. // Calculate response vector dTheta that is the DLS solution.
  330. // Delta target values are the dS values
  331. // We multiply by DLS inverse of the J matrix
  332. long diagLength = w.GetLength();
  333. double* wPtr = w.GetPtr();
  334. dTheta.SetZero();
  335. for ( long i=0; i<diagLength; i++ ) {
  336. double dotProdCol = U.DotProductColumn( dS, i ); // Dot product with i-th column of U
  337. double alpha = *(wPtr++);
  338. alpha = alpha/(Square(alpha)+DampingLambdaSq);
  339. MatrixRmn::AddArrayScale(V.GetNumRows(), V.GetColumnPtr(i), 1, dTheta.GetPtr(), 1, dotProdCol*alpha );
  340. }
  341. // Scale back to not exceed maximum angle changes
  342. double maxChange = dTheta.MaxAbs();
  343. if ( maxChange>MaxAngleDLS ) {
  344. dTheta *= MaxAngleDLS/maxChange;
  345. }
  346. }
  347. void Jacobian::CalcDeltaThetasSDLS()
  348. {
  349. const MatrixRmn& J = ActiveJacobian();
  350. // Compute Singular Value Decomposition
  351. J.ComputeSVD( U, w, V );
  352. // Next line for debugging only
  353. assert(J.DebugCheckSVD(U, w , V));
  354. // Calculate response vector dTheta that is the SDLS solution.
  355. // Delta target values are the dS values
  356. int nRows = J.GetNumRows();
  357. int numEndEffectors = m_tree->GetNumEffector(); // Equals the number of rows of J divided by three
  358. int nCols = J.GetNumColumns();
  359. dTheta.SetZero();
  360. // Calculate the norms of the 3-vectors in the Jacobian
  361. long i;
  362. const double *jx = J.GetPtr();
  363. double *jnx = Jnorms.GetPtr();
  364. for ( i=nCols*numEndEffectors; i>0; i-- ) {
  365. double accumSq = Square(*(jx++));
  366. accumSq += Square(*(jx++));
  367. accumSq += Square(*(jx++));
  368. *(jnx++) = sqrt(accumSq);
  369. }
  370. // Clamp the dS values
  371. CalcdTClampedFromdS();
  372. // Loop over each singular vector
  373. for ( i=0; i<nRows; i++ ) {
  374. double wiInv = w[i];
  375. if ( NearZero(wiInv,1.0e-10) ) {
  376. continue;
  377. }
  378. wiInv = 1.0/wiInv;
  379. double N = 0.0; // N is the quasi-1-norm of the i-th column of U
  380. double alpha = 0.0; // alpha is the dot product of dT and the i-th column of U
  381. const double *dTx = dT1.GetPtr();
  382. const double *ux = U.GetColumnPtr(i);
  383. long j;
  384. for ( j=numEndEffectors; j>0; j-- ) {
  385. double tmp;
  386. alpha += (*ux)*(*(dTx++));
  387. tmp = Square( *(ux++) );
  388. alpha += (*ux)*(*(dTx++));
  389. tmp += Square(*(ux++));
  390. alpha += (*ux)*(*(dTx++));
  391. tmp += Square(*(ux++));
  392. N += sqrt(tmp);
  393. }
  394. // M is the quasi-1-norm of the response to angles changing according to the i-th column of V
  395. // Then is multiplied by the wiInv value.
  396. double M = 0.0;
  397. double *vx = V.GetColumnPtr(i);
  398. jnx = Jnorms.GetPtr();
  399. for ( j=nCols; j>0; j-- ) {
  400. double accum=0.0;
  401. for ( long k=numEndEffectors; k>0; k-- ) {
  402. accum += *(jnx++);
  403. }
  404. M += fabs((*(vx++)))*accum;
  405. }
  406. M *= fabs(wiInv);
  407. double gamma = MaxAngleSDLS;
  408. if ( N<M ) {
  409. gamma *= N/M; // Scale back maximum permissable joint angle
  410. }
  411. // Calculate the dTheta from pure pseudoinverse considerations
  412. double scale = alpha*wiInv; // This times i-th column of V is the psuedoinverse response
  413. dPreTheta.LoadScaled( V.GetColumnPtr(i), scale );
  414. // Now rescale the dTheta values.
  415. double max = dPreTheta.MaxAbs();
  416. double rescale = (gamma)/(gamma+max);
  417. dTheta.AddScaled(dPreTheta,rescale);
  418. /*if ( gamma<max) {
  419. dTheta.AddScaled( dPreTheta, gamma/max );
  420. }
  421. else {
  422. dTheta += dPreTheta;
  423. }*/
  424. }
  425. // Scale back to not exceed maximum angle changes
  426. double maxChange = dTheta.MaxAbs();
  427. if ( maxChange>MaxAngleSDLS ) {
  428. dTheta *= MaxAngleSDLS/(MaxAngleSDLS+maxChange);
  429. //dTheta *= MaxAngleSDLS/maxChange;
  430. }
  431. }
  432. void Jacobian::CalcdTClampedFromdS()
  433. {
  434. long len = dS.GetLength();
  435. long j = 0;
  436. for ( long i=0; i<len; i+=3, j++ ) {
  437. double normSq = Square(dS[i])+Square(dS[i+1])+Square(dS[i+2]);
  438. if ( normSq>Square(dSclamp[j]) ) {
  439. double factor = dSclamp[j]/sqrt(normSq);
  440. dT1[i] = dS[i]*factor;
  441. dT1[i+1] = dS[i+1]*factor;
  442. dT1[i+2] = dS[i+2]*factor;
  443. }
  444. else {
  445. dT1[i] = dS[i];
  446. dT1[i+1] = dS[i+1];
  447. dT1[i+2] = dS[i+2];
  448. }
  449. }
  450. }
  451. double Jacobian::UpdateErrorArray(VectorR3* targets)
  452. {
  453. double totalError = 0.0;
  454. // Traverse tree to find all end effectors
  455. VectorR3 temp;
  456. Node* n = m_tree->GetRoot();
  457. while ( n ) {
  458. if ( n->IsEffector() ) {
  459. int i = n->GetEffectorNum();
  460. const VectorR3& targetPos = targets[i];
  461. temp = targetPos;
  462. temp -= n->GetS();
  463. double err = temp.Norm();
  464. errorArray[i] = err;
  465. totalError += err;
  466. }
  467. n = m_tree->GetSuccessor( n );
  468. }
  469. return totalError;
  470. }
  471. void Jacobian::UpdatedSClampValue(VectorR3* targets)
  472. {
  473. // Traverse tree to find all end effectors
  474. VectorR3 temp;
  475. Node* n = m_tree->GetRoot();
  476. while ( n ) {
  477. if ( n->IsEffector() ) {
  478. int i = n->GetEffectorNum();
  479. const VectorR3& targetPos = targets[i];
  480. // Compute the delta S value (differences from end effectors to target positions.
  481. // While we are at it, also update the clamping values in dSclamp;
  482. temp = targetPos;
  483. temp -= n->GetS();
  484. double normSi = sqrt(Square(dS[i])+Square(dS[i+1])+Square(dS[i+2]));
  485. double changedDist = temp.Norm()-normSi;
  486. if ( changedDist>0.0 ) {
  487. dSclamp[i] = BaseMaxTargetDist + changedDist;
  488. }
  489. else {
  490. dSclamp[i] = BaseMaxTargetDist;
  491. }
  492. }
  493. n = m_tree->GetSuccessor( n );
  494. }
  495. }
  496. void Jacobian::CompareErrors( const Jacobian& j1, const Jacobian& j2, double* weightedDist1, double* weightedDist2 )
  497. {
  498. const VectorRn& e1 = j1.errorArray;
  499. const VectorRn& e2 = j2.errorArray;
  500. double ret1 = 0.0;
  501. double ret2 = 0.0;
  502. int len = e1.GetLength();
  503. for ( long i=0; i<len; i++ ) {
  504. double v1 = e1[i];
  505. double v2 = e2[i];
  506. if ( v1<v2 ) {
  507. ret1 += v1/v2;
  508. ret2 += 1.0;
  509. }
  510. else if ( v1 != 0.0 ) {
  511. ret1 += 1.0;
  512. ret2 += v2/v1;
  513. }
  514. else {
  515. ret1 += 0.0;
  516. ret2 += 0.0;
  517. }
  518. }
  519. *weightedDist1 = ret1;
  520. *weightedDist2 = ret2;
  521. }
  522. void Jacobian::CountErrors( const Jacobian& j1, const Jacobian& j2, int* numBetter1, int* numBetter2, int* numTies )
  523. {
  524. const VectorRn& e1 = j1.errorArray;
  525. const VectorRn& e2 = j2.errorArray;
  526. int b1=0, b2=0, tie=0;
  527. int len = e1.GetLength();
  528. for ( long i=0; i<len; i++ ) {
  529. double v1 = e1[i];
  530. double v2 = e2[i];
  531. if ( v1<v2 ) {
  532. b1++;
  533. }
  534. else if ( v2<v1 ) {
  535. b2++;
  536. }
  537. else {
  538. tie++;
  539. }
  540. }
  541. *numBetter1 = b1;
  542. *numBetter2 = b2;
  543. *numTies = tie;
  544. }
  545. /* THIS VERSION IS NOT AS GOOD. DO NOT USE!
  546. void Jacobian::CalcDeltaThetasSDLSrev2()
  547. {
  548. const MatrixRmn& J = ActiveJacobian();
  549. // Compute Singular Value Decomposition
  550. J.ComputeSVD( U, w, V );
  551. // Next line for debugging only
  552. assert(J.DebugCheckSVD(U, w , V));
  553. // Calculate response vector dTheta that is the SDLS solution.
  554. // Delta target values are the dS values
  555. int nRows = J.GetNumRows();
  556. int numEndEffectors = tree->GetNumEffector(); // Equals the number of rows of J divided by three
  557. int nCols = J.GetNumColumns();
  558. dTheta.SetZero();
  559. // Calculate the norms of the 3-vectors in the Jacobian
  560. long i;
  561. const double *jx = J.GetPtr();
  562. double *jnx = Jnorms.GetPtr();
  563. for ( i=nCols*numEndEffectors; i>0; i-- ) {
  564. double accumSq = Square(*(jx++));
  565. accumSq += Square(*(jx++));
  566. accumSq += Square(*(jx++));
  567. *(jnx++) = sqrt(accumSq);
  568. }
  569. // Loop over each singular vector
  570. for ( i=0; i<nRows; i++ ) {
  571. double wiInv = w[i];
  572. if ( NearZero(wiInv,1.0e-10) ) {
  573. continue;
  574. }
  575. double N = 0.0; // N is the quasi-1-norm of the i-th column of U
  576. double alpha = 0.0; // alpha is the dot product of dS and the i-th column of U
  577. const double *dSx = dS.GetPtr();
  578. const double *ux = U.GetColumnPtr(i);
  579. long j;
  580. for ( j=numEndEffectors; j>0; j-- ) {
  581. double tmp;
  582. alpha += (*ux)*(*(dSx++));
  583. tmp = Square( *(ux++) );
  584. alpha += (*ux)*(*(dSx++));
  585. tmp += Square(*(ux++));
  586. alpha += (*ux)*(*(dSx++));
  587. tmp += Square(*(ux++));
  588. N += sqrt(tmp);
  589. }
  590. // P is the quasi-1-norm of the response to angles changing according to the i-th column of V
  591. double P = 0.0;
  592. double *vx = V.GetColumnPtr(i);
  593. jnx = Jnorms.GetPtr();
  594. for ( j=nCols; j>0; j-- ) {
  595. double accum=0.0;
  596. for ( long k=numEndEffectors; k>0; k-- ) {
  597. accum += *(jnx++);
  598. }
  599. P += fabs((*(vx++)))*accum;
  600. }
  601. double lambda = 1.0;
  602. if ( N<P ) {
  603. lambda -= N/P; // Scale back maximum permissable joint angle
  604. }
  605. lambda *= lambda;
  606. lambda *= DampingLambdaSDLS;
  607. // Calculate the dTheta from pure pseudoinverse considerations
  608. double scale = alpha*wiInv/(Square(wiInv)+Square(lambda)); // This times i-th column of V is the SDLS response
  609. MatrixRmn::AddArrayScale(nCols, V.GetColumnPtr(i), 1, dTheta.GetPtr(), 1, scale );
  610. }
  611. // Scale back to not exceed maximum angle changes
  612. double maxChange = dTheta.MaxAbs();
  613. if ( maxChange>MaxAngleSDLS ) {
  614. dTheta *= MaxAngleSDLS/maxChange;
  615. }
  616. } */