From a706b12647a97cf74095be2d90f038596a7798fa Mon Sep 17 00:00:00 2001 From: rickstaa Date: Mon, 27 Sep 2021 13:04:09 +0200 Subject: [PATCH 1/3] Fixes franka_gripper_sim GripperCommand action This commit makes sure that users can also send gripper widths between 0.0 and 0.08. In the old version the gripper would only open and close based on whether the set gripper width was smaller or bigger than the current width. --- franka_gazebo/config/franka_hw_sim.yaml | 6 +++--- franka_gazebo/src/franka_gripper_sim.cpp | 27 +++++++++++++++++------- 2 files changed, 22 insertions(+), 11 deletions(-) diff --git a/franka_gazebo/config/franka_hw_sim.yaml b/franka_gazebo/config/franka_hw_sim.yaml index 08a2f767a..41eedd264 100644 --- a/franka_gazebo/config/franka_hw_sim.yaml +++ b/franka_gazebo/config/franka_hw_sim.yaml @@ -1,9 +1,9 @@ arm_id: $(arg arm_id) -m_ee: 0.76 +m_ee: 0.76 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/src/franka_gripper_sim.cpp b/franka_gazebo/src/franka_gripper_sim.cpp index 117bd0327..96aca3de1 100644 --- 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 , 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(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,7 +459,6 @@ void FrankaGripperSim::onGripperActionGoal(const control_msgs::GripperCommandGoa return; } - control_msgs::GripperCommandResult result; if (this->state_ != State::HOLDING) { result.reached_goal = static_cast(false); std::string error = "Unexpected state transistion: The gripper not in HOLDING as expected"; @@ -454,7 +466,6 @@ void FrankaGripperSim::onGripperActionGoal(const control_msgs::GripperCommandGoa 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_; From 743569c1afdd34632fbb51738d19c39bee916071 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 7 Nov 2021 15:32:35 +0100 Subject: [PATCH 2/3] Define meaningful tool frame MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit When a hand is mounted, the link8 frame is 45° rotated wrt. the hand. Better define a tool frame that is nicely aligned with the hand, i.e. x=normal, y=slide, z=approach, and nicely centered between the fingers. --- franka_description/robots/panda_arm.urdf.xacro | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/franka_description/robots/panda_arm.urdf.xacro b/franka_description/robots/panda_arm.urdf.xacro index d5b8d0061..d567f7ea8 100644 --- a/franka_description/robots/panda_arm.urdf.xacro +++ b/franka_description/robots/panda_arm.urdf.xacro @@ -18,6 +18,21 @@ + + + + + + + + + + + + + + + From 5f09db0d78ce855040ed805156128c1f29b9c6df Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 8 Nov 2021 08:25:15 +0100 Subject: [PATCH 3/3] Define trajectory and goal constraints Fixes `GOAL_TOLERANCE_VIOLATED` error of `joint_trajectory_controller`. --- franka_gazebo/config/sim_controllers.yaml | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/franka_gazebo/config/sim_controllers.yaml b/franka_gazebo/config/sim_controllers.yaml index c5de5b9b8..6cdbe1730 100644 --- 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