fix rand_q method

jhavl · jhavl · commit 15b4ebeefea9 · 2022-07-06T11:46:35.000+10:00
diff --git a/roboticstoolbox/core/ik.cpp b/roboticstoolbox/core/ik.cpp
@@ -69,7 +69,7 @@ extern "C"
         }
         else
         {
-            q = _rand_q(ets);
+            _rand_q(ets, q);
         }
 
         // Global search up to slimit
@@ -138,7 +138,7 @@ extern "C"
             *it += iter;
             iter = 0;
             *search += 1;
-            q = _rand_q(ets);
+            _rand_q(ets, q);
         }
 
         free(np_e);
@@ -203,7 +203,7 @@ extern "C"
         }
         else
         {
-            q = _rand_q(ets);
+            _rand_q(ets, q);
         }
 
         // Global search up to slimit
@@ -272,7 +272,7 @@ extern "C"
             *it += iter;
             iter = 0;
             *search += 1;
-            q = _rand_q(ets);
+            _rand_q(ets, q);
         }
 
         free(np_e);
@@ -326,7 +326,7 @@ extern "C"
         }
         else
         {
-            q = _rand_q(ets);
+            _rand_q(ets, q);
         }
 
         // Global search up to slimit
@@ -392,7 +392,7 @@ extern "C"
             *it += iter;
             iter = 0;
             *search += 1;
-            q = _rand_q(ets);
+            _rand_q(ets, q);
         }
 
         free(np_e);
@@ -440,7 +440,7 @@ extern "C"
         }
         else
         {
-            q = _rand_q(ets);
+            _rand_q(ets, q);
         }
 
         // Global search up to slimit
@@ -503,7 +503,7 @@ extern "C"
             *it += iter;
             iter = 0;
             *search += 1;
-            q = _rand_q(ets);
+            _rand_q(ets, q);
         }
 
         free(np_e);
@@ -552,7 +552,7 @@ extern "C"
         }
         else
         {
-            q = _rand_q(ets);
+            _rand_q(ets, q);
         }
 
         // Global search up to slimit
@@ -618,7 +618,7 @@ extern "C"
             *it += iter;
             iter = 0;
             *search += 1;
-            q = _rand_q(ets);
+            _rand_q(ets, q);
         }
 
         free(np_e);
@@ -703,17 +703,17 @@ extern "C"
         }
     }
 
-    VectorX _rand_q(ETS *ets)
+    void _rand_q(ETS *ets, MapVectorX q)
     {
         Eigen::Map<Eigen::ArrayXd> qlim_l(ets->qlim_l, ets->n);
         Eigen::Map<Eigen::ArrayXd> q_range2(ets->q_range2, ets->n);
 
-        VectorX q = VectorX::Random(ets->n);
+        q = VectorX::Random(ets->n);
 
         q = (q.array() + 1) * q_range2;
         q = q.array() + qlim_l;
 
-        return q;
+        // return q;
     }
 
 } /* extern "C" */
diff --git a/roboticstoolbox/core/ik.h b/roboticstoolbox/core/ik.h
@@ -48,7 +48,7 @@ extern "C"
         double lambda, MapVectorX we);
 
     void _pseudo_inverse(Eigen::Map<Eigen::MatrixXd> J, Eigen::Map<Eigen::MatrixXd> J_pinv, double damping);
-    VectorX _rand_q(ETS *ets);
+    void _rand_q(ETS *ets, MapVectorX q);
     int _check_lim(ETS *ets, MapVectorX q);
     void _angle_axis(MapMatrix4dc Te, Matrix4dc Tep, MapVectorX e);
 
diff --git a/roboticstoolbox/core/methods.h b/roboticstoolbox/core/methods.h
@@ -17,7 +17,6 @@ extern "C"
 {
 #endif /* __cplusplus */
 
-    VectorX _rand_q(ETS *ets);
     int _check_lim(ETS *ets, MapVectorX q);
     void _angle_axis(MapMatrix4dc Te, Matrix4dc Tep, MapVectorX e);
     void _ETS_hessian(int n, MapMatrixJc &J, MapMatrixHr &H);

Original file line number	Diff line number	Diff line change
`@@ -69,7 +69,7 @@ extern "C"`
`69`	`69`	`}`
`70`	`70`	`else`
`71`	`71`	`{`
`72`		`- q = _rand_q(ets);`
	`72`	`+ _rand_q(ets, q);`
`73`	`73`	`}`
`74`	`74`
`75`	`75`	`// Global search up to slimit`
`@@ -138,7 +138,7 @@ extern "C"`
`138`	`138`	`*it += iter;`
`139`	`139`	`iter = 0;`
`140`	`140`	`*search += 1;`
`141`		`- q = _rand_q(ets);`
	`141`	`+ _rand_q(ets, q);`
`142`	`142`	`}`
`143`	`143`
`144`	`144`	`free(np_e);`
`@@ -203,7 +203,7 @@ extern "C"`
`203`	`203`	`}`
`204`	`204`	`else`
`205`	`205`	`{`
`206`		`- q = _rand_q(ets);`
	`206`	`+ _rand_q(ets, q);`
`207`	`207`	`}`
`208`	`208`
`209`	`209`	`// Global search up to slimit`
`@@ -272,7 +272,7 @@ extern "C"`
`272`	`272`	`*it += iter;`
`273`	`273`	`iter = 0;`
`274`	`274`	`*search += 1;`
`275`		`- q = _rand_q(ets);`
	`275`	`+ _rand_q(ets, q);`
`276`	`276`	`}`
`277`	`277`
`278`	`278`	`free(np_e);`
`@@ -326,7 +326,7 @@ extern "C"`
`326`	`326`	`}`
`327`	`327`	`else`
`328`	`328`	`{`
`329`		`- q = _rand_q(ets);`
	`329`	`+ _rand_q(ets, q);`
`330`	`330`	`}`
`331`	`331`
`332`	`332`	`// Global search up to slimit`
`@@ -392,7 +392,7 @@ extern "C"`
`392`	`392`	`*it += iter;`
`393`	`393`	`iter = 0;`
`394`	`394`	`*search += 1;`
`395`		`- q = _rand_q(ets);`
	`395`	`+ _rand_q(ets, q);`
`396`	`396`	`}`
`397`	`397`
`398`	`398`	`free(np_e);`
`@@ -440,7 +440,7 @@ extern "C"`
`440`	`440`	`}`
`441`	`441`	`else`
`442`	`442`	`{`
`443`		`- q = _rand_q(ets);`
	`443`	`+ _rand_q(ets, q);`
`444`	`444`	`}`
`445`	`445`
`446`	`446`	`// Global search up to slimit`
`@@ -503,7 +503,7 @@ extern "C"`
`503`	`503`	`*it += iter;`
`504`	`504`	`iter = 0;`
`505`	`505`	`*search += 1;`
`506`		`- q = _rand_q(ets);`
	`506`	`+ _rand_q(ets, q);`
`507`	`507`	`}`
`508`	`508`
`509`	`509`	`free(np_e);`
`@@ -552,7 +552,7 @@ extern "C"`
`552`	`552`	`}`
`553`	`553`	`else`
`554`	`554`	`{`
`555`		`- q = _rand_q(ets);`
	`555`	`+ _rand_q(ets, q);`
`556`	`556`	`}`
`557`	`557`
`558`	`558`	`// Global search up to slimit`
`@@ -618,7 +618,7 @@ extern "C"`
`618`	`618`	`*it += iter;`
`619`	`619`	`iter = 0;`
`620`	`620`	`*search += 1;`
`621`		`- q = _rand_q(ets);`
	`621`	`+ _rand_q(ets, q);`
`622`	`622`	`}`
`623`	`623`
`624`	`624`	`free(np_e);`
`@@ -703,17 +703,17 @@ extern "C"`
`703`	`703`	`}`
`704`	`704`	`}`
`705`	`705`
`706`		`- VectorX _rand_q(ETS *ets)`
	`706`	`+ void _rand_q(ETS *ets, MapVectorX q)`
`707`	`707`	`{`
`708`	`708`	`Eigen::Map<Eigen::ArrayXd> qlim_l(ets->qlim_l, ets->n);`
`709`	`709`	`Eigen::Map<Eigen::ArrayXd> q_range2(ets->q_range2, ets->n);`
`710`	`710`
`711`		`- VectorX q = VectorX::Random(ets->n);`
	`711`	`+ q = VectorX::Random(ets->n);`
`712`	`712`
`713`	`713`	`q = (q.array() + 1) * q_range2;`
`714`	`714`	`q = q.array() + qlim_l;`
`715`	`715`
`716`		`- return q;`
	`716`	`+ // return q;`
`717`	`717`	`}`
`718`	`718`
`719`	`719`	`} /* extern "C" */`
Original file line number	Diff line number	Diff line change
`@@ -17,7 +17,6 @@ extern "C"`
`17`	`17`	`{`
`18`	`18`	`#endif /* __cplusplus */`
`19`	`19`
`20`		`- VectorX _rand_q(ETS *ets);`
`21`	`20`	`int _check_lim(ETS *ets, MapVectorX q);`
`22`	`21`	`void _angle_axis(MapMatrix4dc Te, Matrix4dc Tep, MapVectorX e);`
`23`	`22`	`void _ETS_hessian(int n, MapMatrixJc &J, MapMatrixHr &H);`