50 #ifndef _GRIPPER_CONTROLLER 51 #define _GRIPPER_CONTROLLER 65 bool grabObject(
double close_speed,
int contactsDesired);
68 bool place(
int placeConditions,
double acc_trigger,
double slip_trigger);
184 bool returnValue =
false;
186 if(placeConditions == 0 )
187 returnValue = accContact || (sideImpact != -1);
188 else if(placeConditions == 1 )
189 returnValue = accContact && slipContact;
190 else if(placeConditions == 2 )
191 returnValue = accContact || slipContact || (sideImpact != -1);
192 else if(placeConditions == 3)
193 returnValue = slipContact;
194 else if(placeConditions == 4)
195 returnValue = accContact;
206 double slipIncrementPercent = 0.002;
260 double kP_f = 0.0008;
262 if( desired_Force-minFingerForce < 0)
266 double v_force = -kP_f * (minFingerForce - desired_Force);
269 double v_bound = 0.50;
270 if(v_force < -v_bound)
272 else if(v_force > v_bound)
277 double p_bound = 0.03;
357 if (desiredVel > 0.0) vel_force +=
coulomb;
358 else if (desiredVel < 0.0) vel_force -=
coulomb;
360 double fbk_force = pos_force + vel_force;
406 float b_vfilt[] = { 0.0155, 0.0155};
407 float a_vfilt[] = {1.0, -0.9691};
427 #endif // _GRIPPER_CONTROLLER double fingertip_force_limit
double force_servo_force_tolerance
bool checkSlip(double slip_motion_limit=0.007, double slip_force_limit=0.18)
double gripper_state_now_velocity
bool checkPlaceContact(double dF)
bool deformation_limit_flag
digitalFilter * velocityLPFilt
double padForce_left_cur_nonbiased
bool graspContact(int contactsDesired)
bool positionServo(double desiredPos, double desiredVel)
bool checkPlaceContact(double dAcc)
bool grabObject(double close_speed, int contactsDesired)
pr2_mechanism_model::JointState * jointState
double gripper_state_now_measured_effort
float getNextFilteredValue(float u_current)
bool place(int placeConditions, double acc_trigger, double slip_trigger)
bool forceServo2(double desired_Force)
double padForce_cur_nonbiased
double gripper_state_prev_measured_effort
gripperController(pr2_mechanism_model::JointState *joint_state_, pressureObserver *pressure_observer_, accelerationObserver *acceleration_observer_)
double positionMarker_limit
double gripper_state_prev_velocity
double gripper_state_prev_position
accelerationObserver * myAccelerationObserver
bool forceRampTo(double force, double duration)
double gripper_state_now_position
double vel_integral_vcontrol
int checkSideImpact(double dF)
double padForce_right_cur_nonbiased
bool velocityServo(double desiredVel)
pressureObserver * myPressureObserver