Merge PRs frankaemika#194, frankaemika#186, frankaemika#173

rhaschke · Nov 8, 2021 · 6bc77d7 · 6bc77d7
4 parents 7cc7c26 + 743569c + 5f09db0 + a706b12
commit 6bc77d7
Show file tree

Hide file tree

Showing 4 changed files with 46 additions and 10 deletions.
diff --git a/franka_description/robots/panda_arm.urdf.xacro b/franka_description/robots/panda_arm.urdf.xacro
@@ -21,6 +21,21 @@
  </xacro:if>
  </xacro:unless>
 
+ <!-- Define a tool frame -->
+ <link name="$(arg arm_id)_tool" />
+ <joint name="$(arg arm_id)_tool_joint" type="fixed">
+ <xacro:if value="$(arg hand)">
+ <!-- Place between the fingers, orientation: x=normal, y=slide, z=approach -->
+ <origin xyz="0 0 0.1034" rpy="0 0 ${-pi/4}" />
+ </xacro:if>
+ <!-- Identical to link8 -->
+ <xacro:unless value="$(arg hand)">
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ </xacro:unless>
+ <parent link="$(arg arm_id)_link8" />
+ <child link="$(arg arm_id)_tool" />
+ </joint>
+
  <xacro:if value="$(arg gazebo)">
 
  <xacro:arg name="xyz" default="0 0 0" />

diff --git a/franka_gazebo/config/franka_hw_sim.yaml b/franka_gazebo/config/franka_hw_sim.yaml
@@ -2,8 +2,8 @@ arm_id: $(arg arm_id)
 singularity_warning_threshold: 0.0001 # print a warning if the smallest singular value of J x J^T drops below this value (use -1 to disable)
 
 franka_gripper:
- type:  franka_gazebo/FrankaGripperSim
- arm_id:  $(arg arm_id)
+ type: franka_gazebo/FrankaGripperSim
+ arm_id: $(arg arm_id)
 
  finger1:
  gains: { p: 100, i: 25, d: 20 }

diff --git a/franka_gazebo/config/sim_controllers.yaml b/franka_gazebo/config/sim_controllers.yaml
@@ -12,6 +12,7 @@ franka_state_controller:
  - $(arg arm_id)_joint7
 
 effort_joint_trajectory_controller: &default_trajectory_controller
+ # http://wiki.ros.org/joint_trajectory_controller#Parameters
  type: effort_controllers/JointTrajectoryController
  joints:
  - $(arg arm_id)_joint1
@@ -29,6 +30,15 @@ effort_joint_trajectory_controller: &default_trajectory_controller
  $(arg arm_id)_joint5: { p: 250, d: 10, i: 0 }
  $(arg arm_id)_joint6: { p: 150, d: 10, i: 0 }
  $(arg arm_id)_joint7: { p: 50, d: 5, i: 0 }
+ constraints:
+ goal_time: 0.1
+ $(arg arm_id)_joint1: { trajectory: 0.01, goal: 0.001 }
+ $(arg arm_id)_joint2: { trajectory: 0.01, goal: 0.001 }
+ $(arg arm_id)_joint3: { trajectory: 0.01, goal: 0.001 }
+ $(arg arm_id)_joint4: { trajectory: 0.01, goal: 0.001 }
+ $(arg arm_id)_joint5: { trajectory: 0.01, goal: 0.001 }
+ $(arg arm_id)_joint6: { trajectory: 0.01, goal: 0.001 }
+ $(arg arm_id)_joint7: { trajectory: 0.01, goal: 0.001 }
 
 # define default controller for panda_moveit_config:
 joint_trajectory_controller: *default_trajectory_controller

diff --git a/franka_gazebo/src/franka_gripper_sim.cpp b/franka_gazebo/src/franka_gripper_sim.cpp
@@ -421,22 +421,35 @@ void FrankaGripperSim::onGraspGoal(const franka_gripper::GraspGoalConstPtr& goal
 }
 
 void FrankaGripperSim::onGripperActionGoal(const control_msgs::GripperCommandGoalConstPtr& goal) {
+ control_msgs::GripperCommandResult result;
+ double width_d = goal->command.position * 2.0;
+
  ROS_INFO_STREAM_NAMED("FrankaGripperSim", "New Gripper Command Action Goal received: "
- << goal->command.max_effort << "N");
+ << width_d << "m, " << goal->command.max_effort
+ << "N");
 
  // 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.
  double width = this->finger1_.getPosition() + this->finger2_.getPosition();
 
+ if (width_d > kMaxFingerWidth || width_d < 0.0) {
+ std::string error =
+ "Commanding out of range width! max_width = " + std::to_string(kMaxFingerWidth) +
+ " command = " + std::to_string(width_d);
+ ROS_ERROR_STREAM_NAMED("FrankaGripperSim", error);
+ result.reached_goal = static_cast<decltype(result.reached_goal)>(false);
+ action_gc_->setAborted(result, error);
+ return;
+ }
+
  franka_gripper::GraspEpsilon eps;
  eps.inner = this->tolerance_gripper_action_;
  eps.outer = this->tolerance_gripper_action_;
 
- transition(State::GRASPING,
- Config{.width_desired = goal->command.position * 2.0 < width ? 0 : kMaxFingerWidth,
- .speed_desired = this->speed_default_,
- .force_desired = goal->command.max_effort,
- .tolerance = eps});
+ transition(State::GRASPING, Config{.width_desired = width_d,
+ .speed_desired = this->speed_default_,
+ .force_desired = goal->command.max_effort,
+ .tolerance = eps});
 
  waitUntilStateChange();
 
@@ -446,15 +459,13 @@ void FrankaGripperSim::onGripperActionGoal(const control_msgs::GripperCommandGoa
  return;
  }
 
- control_msgs::GripperCommandResult result;
  if (this->state_ != State::HOLDING) {
  result.reached_goal = static_cast<decltype(result.reached_goal)>(false);
  std::string error = "Unexpected state transistion: The gripper not in HOLDING as expected";
  action_gc_->setAborted(result, error);
  return;
  }
 
- double width_d = goal->command.position * 2.0;
  width = this->finger1_.getPosition() + this->finger2_.getPosition(); // recalculate
  bool inside_tolerance = width_d - this->tolerance_gripper_action_ < width and
  width < width_d + this->tolerance_gripper_action_;