Skip to content
Snippets Groups Projects
Commit 164cf1aa authored by Sebastian Ebert's avatar Sebastian Ebert
Browse files

integrated force adaption by parameter

parent 59a6a9a8
No related branches found
No related tags found
No related merge requests found
......@@ -33,6 +33,7 @@ void gripperCommandExecuteCallback(
double default_speed,
actionlib::SimpleActionServer<control_msgs::GripperCommandAction>* action_server,
const control_msgs::GripperCommandGoalConstPtr& goal) {
auto gripper_command_handler = [goal, grasp_epsilon, default_speed, &gripper]() {
// HACK: As one gripper finger is <mimic>, MoveIt!'s trajectory execution manager
// only sends us the width of one finger. Multiply by 2 to get the intended width.
......@@ -51,7 +52,19 @@ void gripperCommandExecuteCallback(
if (target_width >= state.width) {
return gripper.move(target_width, default_speed);
}
return gripper.grasp(target_width, default_speed, goal->command.max_effort, grasp_epsilon.inner,
double force = 0.0;
if(!ros::param::has("tud_grasp_force")){
ros::param::set("tud_grasp_force", 0.0);
force = 0.0;
}else{
ros::param::get("tud_grasp_force", force);
}
std::cout << "force: " << force << std::endl;
return gripper.grasp(target_width, default_speed, force /*goal->command.max_effort*/, grasp_epsilon.inner,
grasp_epsilon.outer);
};
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment