81 double poshp_fc = 5.0;
82 double poshp_lambda = 2*3.14159265*poshp_fc;
86 for(
int i = 0; (i < 22); i ++)
115 for(
int i = 0; i < 7; i++)
192 double tempLoading = 0.0;
195 for(
int i = 7; i < 22; i ++)
197 tempLoading += pressureArray[i];
210 for(
int i = 0; i < 22; i ++)
222 for(
int i = 0; i < 22; i ++)
238 bool newData =
false;
239 bool updated =
false;
244 for(
int i = 0; i < 22; i ++)
246 pressure_now.pressure_left[i] = (double)pressureVector_left[i];
247 pressure_now.pressure_right[i] = (double)pressureVector_right[i];
261 for(
int i = 0; i < 22; i ++)
287 double y_weights[15] = {-1, 0, 1, -1, 0, 1, -1, 0, 1, -1, 0, 1, -1, 0, 1};
288 double x_weights[15] = {-1, -1, -1, -0.5, -0.5, -0.5, 0, 0, 0, 0.5, 0.5, 0.5, 1, 1, 1};
293 for(
int i = 7; i < 22; i++)
295 *y += pressureArray[i]*y_weights[i-7];
296 *x += pressureArray[i]*x_weights[i-7];
307 double x_centroid_left, y_centroid_left, x_centroid_right, y_centroid_right;
363 for(
int i = 0; i < 22; i ++)
372 float b_vfilt[] = { 0.6323, 0, -0.6323};
373 float a_vfilt[] = {1.0, -0.6294, -0.2647};
374 for(
int i=0; i < 22; i++)
digitalFilter * pressureLFFilt_right[22]
pr2_hardware_interface::PressureSensor * right_finger
bool checkSlip(double slip_motion_limit=0.007, double slip_force_limit=0.18)
bool checkPlaceContact(double dF)
pr2_hardware_interface::PressureSensor * left_finger
bool updatePressureState()
double padForce_left_cur_nonbiased
bool graspContact(int contactsDesired)
void getPadCentroid(boost::array< double, 22 > pressureArray, double *x, double *y)
pr2_gripper_sensor_msgs::PR2GripperPressureData pressure_cur_bias
pr2_gripper_sensor_msgs::PR2GripperPressureData pressure_bp
pressureObserver(pr2_hardware_interface::PressureSensor *left, pr2_hardware_interface::PressureSensor *right)
double padForce_right_cur
pr2_gripper_sensor_msgs::PR2GripperPressureData pressure_current
float getNextFilteredValue(float u_current)
pr2_gripper_sensor_msgs::PR2GripperPressureData pressure_now
double getPadForce(boost::array< double, 22 > pressureArray)
double padForce_cur_nonbiased
pr2_gripper_sensor_msgs::PR2GripperPressureData zero_offset
pr2_gripper_sensor_msgs::PR2GripperPressureData pressure_current_zerod
pr2_gripper_sensor_msgs::PR2GripperPressureData pressure_prev_bias
PressureSensorState state_
std::vector< uint16_t > data_
pr2_gripper_sensor_msgs::PR2GripperPressureData pressure_prev_zerod
double totalLoading_right
double padForce_right_prev
void updateBrokenSensorStatus()
double padForce_left_prev
void updateTotalLoading()
int checkSideImpact(double dF)
double padForce_right_cur_nonbiased
pr2_gripper_sensor_msgs::PR2GripperPressureData pressure_prev
digitalFilter * pressureLFFilt_left[22]
void updateContactState()