8
8
/* *
9
9
* @authors: Giulia Vezzani <giulia.vezzani@iit.it>
10
10
*/
11
-
11
+
12
12
#include < cmath>
13
13
#include < limits>
14
14
#include < iomanip>
15
- #include < boost/range/irange.hpp>
16
15
17
16
#include " graspComputation.h"
18
17
19
18
using namespace std ;
20
19
using namespace Eigen ;
21
- using namespace boost ;
22
20
using namespace SuperqModel ;
23
21
using namespace SuperqGrasp ;
24
22
@@ -63,7 +61,7 @@ void graspComputation::init(GraspParams &g_params)
63
61
H_h2w.col (3 ).segment (0 ,3 )=g_params.hand_superq .getSuperqCenter ();
64
62
65
63
// Sampled points on the half of the hand ellipsoid closest to the robot palm
66
- for (auto i: irange ( 0 , (int )sqrt (n_hands), 1 ) )
64
+ for (int i= 0 ; i< (int )sqrt (n_hands); i++ )
67
65
{
68
66
for (double theta=0 ; theta <= 2 *M_PI; theta+= M_PI/((int )sqrt (n_hands)))
69
67
{
@@ -263,7 +261,7 @@ bool graspComputation::get_bounds_info(Ipopt::Index n, Ipopt::Number *x_l, Ipopt
263
261
point_tmp (3 )=1 ;
264
262
point_tmp.segment (0 ,3 )=point;
265
263
266
- for (auto i: irange ( 0 , 6 , 1 ) )
264
+ for (int i= 0 ; i< 6 ; i++ )
267
265
x_tmp (i)=x[i];
268
266
269
267
Matrix4d H_x;
@@ -345,7 +343,7 @@ bool graspComputation::get_bounds_info(Ipopt::Index n, Ipopt::Number *x_l, Ipopt
345
343
{
346
344
// Compute constraints
347
345
Vector6d x_tmp;
348
- for (auto i: irange ( 0 , 6 , 1 ) )
346
+ for (int i= 0 ; i< 6 ; i++ )
349
347
x_tmp (i)=x[i];
350
348
351
349
Matrix4d H_x;
@@ -408,7 +406,7 @@ bool graspComputation::get_bounds_info(Ipopt::Index n, Ipopt::Number *x_l, Ipopt
408
406
// Constraints on obstacle superquadric avoidance
409
407
if (num_superq>0 )
410
408
{
411
- for (auto j: irange ( 0 , num_superq, 1 ) )
409
+ for (int j= 0 ; j< num_superq; j++ )
412
410
{
413
411
g[5 +j]=0 ;
414
412
@@ -480,7 +478,7 @@ bool graspComputation::get_bounds_info(Ipopt::Index n, Ipopt::Number *x_l, Ipopt
480
478
481
479
if (num_superq>0 )
482
480
{
483
- for (auto j: irange ( 0 , num_superq, 1 ) )
481
+ for (int j= 0 ; j< num_superq; j++ )
484
482
{
485
483
g[5 +j]=0 ;
486
484
@@ -525,9 +523,9 @@ bool graspComputation::get_bounds_info(Ipopt::Index n, Ipopt::Number *x_l, Ipopt
525
523
{
526
524
if (num_superq==0 )
527
525
{
528
- for (auto j: irange ( 0 , m, 1 ) )
526
+ for (int j= 0 ; j<m; j++ )
529
527
{
530
- for (auto i: irange ( 0 , n, 1 ) )
528
+ for (int i= 0 ; i<n; i++ )
531
529
{
532
530
jCol[j*(n) + i]= i;
533
531
iRow[j*(n)+ i] = j;
@@ -536,9 +534,9 @@ bool graspComputation::get_bounds_info(Ipopt::Index n, Ipopt::Number *x_l, Ipopt
536
534
}
537
535
else
538
536
{
539
- for (auto j: irange ( 0 , m, 1 ) )
537
+ for (int j= 0 ; j<m; j++ )
540
538
{
541
- for (auto i: irange ( 0 , n, 1 ) )
539
+ for (int i= 0 ; i<n; i++ )
542
540
{
543
541
jCol[j*(n) + i]= i;
544
542
iRow[j*(n)+ i] = j;
@@ -590,7 +588,7 @@ void graspComputation::finalize_solution(Ipopt::SolverReturn status, Ipopt::Inde
590
588
Ipopt::Number obj_value, const Ipopt::IpoptData *ip_data,
591
589
Ipopt::IpoptCalculatedQuantities *ip_cq)
592
590
{
593
- for (auto i: irange ( 0 , 6 , 1 ) )
591
+ for (int i= 0 ; i< 6 ; i++ )
594
592
solution_vector (i)=x[i];
595
593
596
594
Matrix4d H_x;
@@ -686,7 +684,7 @@ deque<double> graspComputation::get_final_constr_values() const
686
684
double graspComputation::computeObstacleValues (const Ipopt::Number *x, int k)
687
685
{
688
686
Vector6d pose_hand;
689
- for (auto i : irange ( 0 , 6 , 1 ) )
687
+ for (int i= 0 ; i< 6 ; i++ )
690
688
pose_hand (i)=x[i];
691
689
692
690
Matrix4d H_robot;
0 commit comments