shadow-robot / sr-ros-interface-ethercat

Compare 992416d ... +0 ... 0e6db3c


@@ -94,6 +94,10 @@
Loading
94 94
95 95
namespace shadow_robot
96 96
{
97 +
#define NB_CALIBRATION_POINTS  (25)
98 +
#define NB_SURROUNDING_POINTS  (10)
99 +
#define NB_TOTAL_POINTS (NB_CALIBRATION_POINTS + NB_SURROUNDING_POINTS)
100 +
97 101
template<class StatusType, class CommandType>
98 102
class SrRobotLib
99 103
{
@@ -425,7 +429,7 @@
Loading
425 429
    -0.34906,
426 430
    -0.6981
427 431
  };
428 -
  double *zi;
432 +
  double zi[1];
429 433
};  // end class
430 434
}  // namespace shadow_robot
431 435

@@ -973,22 +973,18 @@
Loading
973 973
          int raw_pos_2nd_joint = status_data->sensors[this->joints_vector[this->find_joint_by_name("THJ2")].joint_to_sensor.joint_to_sensor_vector[0].sensor_id];
974 974
          this->xyi[0] = static_cast<double> (raw_pos);
975 975
          this->xyi[1] = static_cast<double> (raw_pos_2nd_joint);
976 -
          this->zi = pwl_interp_2d_scattered_value (this->node_num, this->node_xy, this->zd_thj1, this->element_num,
977 -
                                                    this->triangle, this->element_neighbor, this->ni, this->xyi);
976 +
          pwl_interp_2d_scattered_value (this->node_num, this->node_xy, this->zd_thj1, this->element_num,
977 +
                                         this->triangle, this->element_neighbor, this->ni, this->xyi, this->zi);
978 978
          tmp_cal_value = this->zi[0];
979 -
          // ROS_INFO("THJ1: %f THJ2: %f Interpolated THJ1: %f", this->xyi[0], this->xyi[1], tmp_cal_value);
980 -
          delete [] this->zi;
981 979
        }
982 980
        else if (joint_tmp->joint_name.find("THJ2")!= std::string::npos)
983 981
        {
984 982
          int raw_pos_2nd_joint = status_data->sensors[this->joints_vector[this->find_joint_by_name("THJ1")].joint_to_sensor.joint_to_sensor_vector[0].sensor_id];
985 983
          this->xyi[0] = static_cast<double> (raw_pos_2nd_joint);
986 984
          this->xyi[1] = static_cast<double> (raw_pos);
987 -
          this->zi = pwl_interp_2d_scattered_value (this->node_num, this->node_xy, this->zd_thj2, this->element_num,
988 -
                                                    this->triangle, this->element_neighbor, this->ni, this->xyi);
985 +
          pwl_interp_2d_scattered_value (this->node_num, this->node_xy, this->zd_thj2, this->element_num,
986 +
                                         this->triangle, this->element_neighbor, this->ni, this->xyi, this->zi);
989 987
          tmp_cal_value = this->zi[0];
990 -
          // ROS_INFO("THJ1: %f THJ2: %f Interpolated THJ2: %f", this->xyi[0], this->xyi[1], tmp_cal_value);
991 -
          delete [] this->zi;
992 988
        }
993 989
        else
994 990
        {

@@ -227,23 +227,26 @@
Loading
227 227
            // initialize the calibration map
228 228
            calibration_map(read_joint_calibration())
229 229
  {
230 +
    // Generate additional points around the actual calibration points
231 +
    add_surrounding_points(NB_CALIBRATION_POINTS, node_xy, zd_thj1, NB_SURROUNDING_POINTS);
232 +
    add_surrounding_points(NB_CALIBRATION_POINTS, node_xy, zd_thj2, NB_SURROUNDING_POINTS);
230 233
    //
231 234
    //  Set up the Delaunay triangulation.
232 235
    //
233 -
      r8tris2 ( node_num, node_xy, element_num, triangle, element_neighbor );
236 +
    r8tris2 ( node_num, node_xy, element_num, triangle, element_neighbor );
234 237
235 -
      for ( int j = 0; j < element_num; j++ )
238 +
    for ( int j = 0; j < element_num; j++ )
239 +
    {
240 +
      for ( int i = 0; i < 3; i++ )
236 241
      {
237 -
        for ( int i = 0; i < 3; i++ )
242 +
        if ( 0 < element_neighbor[i+j*3] )
238 243
        {
239 -
          if ( 0 < element_neighbor[i+j*3] )
240 -
          {
241 -
            element_neighbor[i+j*3] = element_neighbor[i+j*3] - 1;
242 -
          }
244 +
          element_neighbor[i+j*3] = element_neighbor[i+j*3] - 1;
243 245
        }
244 246
      }
245 -
      filter_edge_triangles_by_min_angle(node_num, node_xy, element_num, triangle, element_neighbor, 0.17);
246 -
      triangulation_order3_print ( node_num, element_num, node_xy, triangle, element_neighbor );
247 +
    }
248 +
    // filter_edge_triangles_by_min_angle(node_num, node_xy, element_num, triangle, element_neighbor, 0.17);
249 +
    // triangulation_order3_print ( node_num, element_num, node_xy, triangle, element_neighbor );
247 250
  }
248 251
249 252
  template<class StatusType, class CommandType>

@@ -879,8 +879,8 @@
Loading
879 879
}
880 880
//****************************************************************************80
881 881
882 -
double *pwl_interp_2d_scattered_value ( int nd, double xyd[], double zd[], 
883 -
  int t_num, int t[], int t_neighbor[], int ni, double xyi[] )
882 +
void pwl_interp_2d_scattered_value ( int nd, double xyd[], double zd[],
883 +
  int t_num, int t[], int t_neighbor[], int ni, double xyi[], double zi[] )
884 884
885 885
//****************************************************************************80
886 886
//
@@ -928,9 +928,6 @@
Loading
928 928
  int i;
929 929
  int j;
930 930
  int step_num;
931 -
  double *zi;
932 -
933 -
  zi = new double[ni];
934 931
935 932
  for ( i = 0; i < ni; i++ )
936 933
  {
@@ -946,7 +943,7 @@
Loading
946 943
          + beta  * zd[t[1+j*3]] 
947 944
          + gamma * zd[t[2+j*3]];
948 945
  }
949 -
  return zi;
946 +
  return;
950 947
}
951 948
//****************************************************************************80
952 949
@@ -2352,4 +2349,71 @@
Loading
2352 2349
      done_filtering = true;
2353 2350
    }
2354 2351
  }
2355 -
}

Learn more Showing 2 files with coverage changes found.

Changes in sr_robot_lib/src/pwl_interp_2d_scattered.cpp
-160
+192
Loading file...
Changes in sr_robot_lib/src/r8lib.cpp
-29
-7
+36
Loading file...
Files Coverage
sr_robot_lib -1.83% 6.36%
Project Totals (33 files) 6.36%
Loading