Skip to content
Snippets Groups Projects
Commit f6ba076a authored by Simon Gabl's avatar Simon Gabl
Browse files

Merge pull request #41 in SWDEV/franka_ros from...

Merge pull request #41 in SWDEV/franka_ros from SWDEV-402-finalise-gripper-urdf-meshes-and to master

* commit '7c0ce84f47883e7bc971f0bbe6a18a39419c2b50':
  Rename grasp parameter 'max_force' to 'force'.
  Removed newton_to_m_ampere_factor, changed gripper grasp parameter from mA to N.
parents bf5bc2aa dfbf3078
No related branches found
No related tags found
No related merge requests found
float64 width # [m]
float64 speed # [m/s]
float64 max_current # [mA]
float64 force # [N]
---
bool success
string error
......
......@@ -4,4 +4,3 @@ joint_names:
- franka_emika_finger_joint2
width_tolerance: 0.01 # [m]
default_speed: 0.1 # [m/s]
\ No newline at end of file
newton_to_m_ampere_factor: 14.9 # [mA/N]
......@@ -31,14 +31,12 @@ bool updateGripperState(const franka::Gripper& gripper, franka::GripperState* st
*
* @param[in] gripper A pointer to a franka gripper
* @param[in] default_speed The default speed for a gripper action
* @param[in] newton_to_m_ampere_factor The mapping factor from Newton to milliampere
* @param[in] action_server A pointer to a gripper action server
* @param[in] goal A gripper action goal
*/
void gripperCommandExecuteCallback(
const franka::Gripper& gripper,
double default_speed,
double newton_to_m_ampere_factor,
actionlib::SimpleActionServer<control_msgs::GripperCommandAction>* action_server,
const control_msgs::GripperCommandGoalConstPtr& goal);
......
......@@ -28,7 +28,6 @@ bool updateGripperState(const franka::Gripper& gripper, franka::GripperState* st
void gripperCommandExecuteCallback(
const franka::Gripper& gripper,
double default_speed,
double newton_to_m_ampere_factor,
actionlib::SimpleActionServer<control_msgs::GripperCommandAction>* action_server,
const control_msgs::GripperCommandGoalConstPtr& goal) {
std::function<bool()> gripper_command_handler = [=, &gripper]() {
......@@ -41,8 +40,7 @@ void gripperCommandExecuteCallback(
if (goal->command.position >= state.width) {
return gripper.move(goal->command.position, default_speed);
}
return gripper.grasp(goal->command.position, default_speed,
goal->command.max_effort * newton_to_m_ampere_factor);
return gripper.grasp(goal->command.position, default_speed, goal->command.max_effort);
};
try {
......@@ -77,7 +75,7 @@ bool stop(const franka::Gripper& gripper, const StopGoalConstPtr& /*goal*/) {
}
bool grasp(const franka::Gripper& gripper, const GraspGoalConstPtr& goal) {
return gripper.grasp(goal->width, goal->speed, goal->max_current);
return gripper.grasp(goal->width, goal->speed, goal->force);
}
} // namespace franka_gripper
......@@ -73,12 +73,6 @@ int main(int argc, char** argv) {
ROS_INFO_STREAM("franka_gripper_node: Found default_speed" << default_speed);
}
double newton_to_m_ampere_factor(14.9);
if (node_handle.getParam("newton_to_m_ampere_factor", newton_to_m_ampere_factor)) {
ROS_INFO_STREAM("franka_gripper_node: Found newton_to_m_ampere_factor"
<< newton_to_m_ampere_factor);
}
franka::Gripper gripper(robot_ip);
std::function<bool(const HomingGoalConstPtr&)> homing_handler =
......@@ -114,7 +108,7 @@ int main(int argc, char** argv) {
SimpleActionServer<GripperCommandAction> gripper_command_action_server(
node_handle, "gripper_action",
std::bind(&gripperCommandExecuteCallback, std::cref(gripper), default_speed,
newton_to_m_ampere_factor, &gripper_command_action_server, std::placeholders::_1),
&gripper_command_action_server, std::placeholders::_1),
false);
homing_action_server_.start();
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment