From e584a1048bfed701f17bf541b2fba81e8bea3d90 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 27 Oct 2021 19:43:01 +0200 Subject: [PATCH 01/20] Drop .travis.yml --- .travis.yml | 36 ------------------------------------ 1 file changed, 36 deletions(-) delete mode 100644 .travis.yml diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index 3a56a7105..000000000 --- a/.travis.yml +++ /dev/null @@ -1,36 +0,0 @@ -sudo: required -language: none - -services: - - docker - -matrix: - include: - - env: CI_ROS=kinetic - - env: CI_ROS=melodic - - env: CI_ROS=noetic - -before_script: - - mkdir -p ~/workspace/catkin_ws/src - - | - cd ~/workspace - git clone https://github.com/frankaemika/libfranka.git - cd libfranka - git checkout ${TRAVIS_PULL_REQUEST_BRANCH:-${TRAVIS_BRANCH}} || git checkout develop - git submodule update --init --recursive - - | - cd ~/workspace - docker build -t worker -f $TRAVIS_BUILD_DIR/.ci/Dockerfile.$CI_ROS $TRAVIS_BUILD_DIR/.ci - ln -sf $TRAVIS_BUILD_DIR catkin_ws/src/franka_ros - -script: - - docker run - -v $TRAVIS_BUILD_DIR:$TRAVIS_BUILD_DIR - -v ~/workspace:/workspace - worker /bin/sh -c ' - cd /workspace/libfranka && - .ci/libonly.sh && - export CMAKE_PREFIX_PATH="/workspace/libfranka/build-libonly:$CMAKE_PREFIX_PATH" && - cd /workspace/catkin_ws && - src/franka_ros/.ci/debug.sh - ' From f9c134b70196182dca9e832e5d8b7d8754f70416 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Thu, 4 Nov 2021 14:12:59 +0100 Subject: [PATCH 02/20] fix while loop in interactive marker server --- CHANGELOG.md | 1 + .../scripts/dual_arm_interactive_marker.py | 2 +- franka_example_controllers/scripts/interactive_marker.py | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index c83cbbea5..c91311a82 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,7 @@ ## 0.x - UNRELEASED +* Prevent interactive marker server from not shutting down when not initialized * Add realistic hand/finger collision geometries to the Gazebo robot description * Add `joint_state_desired` publisher to `franka_gazebo` * No further ROS Kinetic support, since [End-of-Life was in April 2021](http://wiki.ros.org/Distributions) diff --git a/franka_example_controllers/scripts/dual_arm_interactive_marker.py b/franka_example_controllers/scripts/dual_arm_interactive_marker.py index 3119ce57e..2e57f681f 100755 --- a/franka_example_controllers/scripts/dual_arm_interactive_marker.py +++ b/franka_example_controllers/scripts/dual_arm_interactive_marker.py @@ -109,7 +109,7 @@ def reset_marker_pose_blocking(): PoseStamped, centering_pose_callback) # Get initial pose for the interactive marker - while not centering_frame_ready: + while not centering_frame_ready and not rospy.is_shutdown(): rospy.sleep(0.1) centering_frame_pose_sub.unregister() diff --git a/franka_example_controllers/scripts/interactive_marker.py b/franka_example_controllers/scripts/interactive_marker.py index 7755e4a63..7e01bcff3 100755 --- a/franka_example_controllers/scripts/interactive_marker.py +++ b/franka_example_controllers/scripts/interactive_marker.py @@ -65,7 +65,7 @@ def process_feedback(feedback): link_name = rospy.get_param("~link_name") # Get initial pose for the interactive marker - while not initial_pose_found and rospy.is_shutdown(): + while not initial_pose_found and not rospy.is_shutdown(): rospy.sleep(1) state_sub.unregister() From 4ead4054bcb7f1e7c4bf9b0b55d575d98497060b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 28 Oct 2021 01:18:56 +0200 Subject: [PATCH 03/20] Define finger_joint2 as mimic joint of finger_joint1 Although, Gazebo doesn't support mimic joints natively, FrankaHWSim does and MoveIt - for planning - should know that the fingers will move in sync. --- franka_description/robots/panda_gazebo.xacro | 1 + 1 file changed, 1 insertion(+) diff --git a/franka_description/robots/panda_gazebo.xacro b/franka_description/robots/panda_gazebo.xacro index 984597546..ecd34fa66 100644 --- a/franka_description/robots/panda_gazebo.xacro +++ b/franka_description/robots/panda_gazebo.xacro @@ -351,6 +351,7 @@ + From 2f10a5379159cfeb9a5b9c2a0e4e90a4f1c65839 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 25 Oct 2021 14:58:31 +0200 Subject: [PATCH 04/20] Simplify use of arguments No need to declare an intermediate property --- .../robots/panda_arm.urdf.xacro | 40 +++++++++---------- 1 file changed, 19 insertions(+), 21 deletions(-) diff --git a/franka_description/robots/panda_arm.urdf.xacro b/franka_description/robots/panda_arm.urdf.xacro index d5b8d0061..8fa6a317c 100644 --- a/franka_description/robots/panda_arm.urdf.xacro +++ b/franka_description/robots/panda_arm.urdf.xacro @@ -5,16 +5,14 @@ - - - + - + @@ -27,12 +25,12 @@ - + - - - + + + @@ -40,27 +38,27 @@ - + - - - - - - - + + + + + + + - - + - ${arm_id} + $(arg arm_id) 0.001 franka_gazebo/FrankaHWSim From da8a709fd69595fd68df7b5979eafdb837e78295 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 25 Oct 2021 18:37:02 +0200 Subject: [PATCH 05/20] Place XML comments in front of xacro commands ... to hide them in the final URDF --- franka_description/robots/panda_arm.urdf.xacro | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/franka_description/robots/panda_arm.urdf.xacro b/franka_description/robots/panda_arm.urdf.xacro index 8fa6a317c..8dc6cd9bb 100644 --- a/franka_description/robots/panda_arm.urdf.xacro +++ b/franka_description/robots/panda_arm.urdf.xacro @@ -1,9 +1,12 @@ - - - + + + + + + From 5bc246f22cc229f0a8f067ddffe128f55c9439bd Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 25 Oct 2021 18:38:11 +0200 Subject: [PATCH 06/20] Fix typo --- franka_description/robots/panda_gazebo.xacro | 2 +- franka_gazebo/test/launch/panda-gazebo.urdf | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/franka_description/robots/panda_gazebo.xacro b/franka_description/robots/panda_gazebo.xacro index 984597546..0eb1df4f5 100644 --- a/franka_description/robots/panda_gazebo.xacro +++ b/franka_description/robots/panda_gazebo.xacro @@ -8,7 +8,7 @@ - + diff --git a/franka_gazebo/test/launch/panda-gazebo.urdf b/franka_gazebo/test/launch/panda-gazebo.urdf index cd262f36e..3ef04c6c5 100644 --- a/franka_gazebo/test/launch/panda-gazebo.urdf +++ b/franka_gazebo/test/launch/panda-gazebo.urdf @@ -7,7 +7,7 @@ - + From 689e9469ea7ab253125f8ffbe03972010f4dc0af Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 27 Oct 2021 16:27:26 +0200 Subject: [PATCH 07/20] Simplify use of argument `paused` ... avoiding code duplication --- franka_gazebo/launch/panda.launch | 17 ++++------------- 1 file changed, 4 insertions(+), 13 deletions(-) diff --git a/franka_gazebo/launch/panda.launch b/franka_gazebo/launch/panda.launch index 527802021..9226b13f5 100644 --- a/franka_gazebo/launch/panda.launch +++ b/franka_gazebo/launch/panda.launch @@ -31,6 +31,7 @@ + @@ -51,23 +52,13 @@ + - - - - + "/> Date: Mon, 25 Oct 2021 22:33:00 +0200 Subject: [PATCH 08/20] Move `remap from=equilibrium_pose` to the `interactive_marker.py` node ... to clarify that this node requires the remapping. --- franka_gazebo/launch/panda.launch | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/franka_gazebo/launch/panda.launch b/franka_gazebo/launch/panda.launch index 9226b13f5..39b287439 100644 --- a/franka_gazebo/launch/panda.launch +++ b/franka_gazebo/launch/panda.launch @@ -76,8 +76,6 @@ args="franka_state_controller $(arg controller)" /> - - [franka_state_controller/joint_states, franka_gripper/joint_states] @@ -95,6 +93,7 @@ type="interactive_marker.py" if="$(eval arg('controller') == 'cartesian_impedance_example_controller')"> + From ce03ab7bdc9ded4d247c4e44588fdac223c711f5 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 29 Oct 2021 17:16:52 +0200 Subject: [PATCH 09/20] Add detailed collision models for fingers --- franka_description/robots/hand.xacro | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/franka_description/robots/hand.xacro b/franka_description/robots/hand.xacro index 22e6a0876..86505452f 100644 --- a/franka_description/robots/hand.xacro +++ b/franka_description/robots/hand.xacro @@ -58,6 +58,11 @@ + + + + + @@ -66,6 +71,12 @@ + + + + + + From 6bc09775c749658aa2cf8a3fda12dd8d0ebc3a4b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 29 Oct 2021 17:44:37 +0200 Subject: [PATCH 10/20] Distinguish links hand_coarse and hand using coarse vs. detailed collision models --- franka_description/robots/hand.xacro | 13 ++++++++++++- franka_description/robots/panda_gazebo.xacro | 5 +++++ 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/franka_description/robots/hand.xacro b/franka_description/robots/hand.xacro index 86505452f..3fa25873f 100644 --- a/franka_description/robots/hand.xacro +++ b/franka_description/robots/hand.xacro @@ -12,9 +12,16 @@ - + + + + + + + + @@ -52,6 +59,10 @@ + + + + diff --git a/franka_description/robots/panda_gazebo.xacro b/franka_description/robots/panda_gazebo.xacro index 984597546..f5587934e 100644 --- a/franka_description/robots/panda_gazebo.xacro +++ b/franka_description/robots/panda_gazebo.xacro @@ -300,6 +300,11 @@ izz="0.0017" /> + + + + + From b558403891de6cc78f3c7f6642a08b3a67cffaeb Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Mon, 8 Nov 2021 09:29:56 +0100 Subject: [PATCH 11/20] use wait_for_message to get initial pose in interactive_marker.py --- .../scripts/interactive_marker.py | 45 +++++++++---------- 1 file changed, 20 insertions(+), 25 deletions(-) diff --git a/franka_example_controllers/scripts/interactive_marker.py b/franka_example_controllers/scripts/interactive_marker.py index 7e01bcff3..4142c04d2 100755 --- a/franka_example_controllers/scripts/interactive_marker.py +++ b/franka_example_controllers/scripts/interactive_marker.py @@ -12,7 +12,6 @@ from franka_msgs.msg import FrankaState marker_pose = PoseStamped() -initial_pose_found = False pose_pub = None # [[min_x, max_x], [min_y, max_y], [min_z, max_z]] position_limits = [[-0.6, 0.6], [-0.6, 0.6], [0.05, 0.9]] @@ -24,24 +23,6 @@ def publisher_callback(msg, link_name): pose_pub.publish(marker_pose) -def franka_state_callback(msg): - initial_quaternion = \ - tf.transformations.quaternion_from_matrix( - np.transpose(np.reshape(msg.O_T_EE, - (4, 4)))) - initial_quaternion = initial_quaternion / \ - np.linalg.norm(initial_quaternion) - marker_pose.pose.orientation.x = initial_quaternion[0] - marker_pose.pose.orientation.y = initial_quaternion[1] - marker_pose.pose.orientation.z = initial_quaternion[2] - marker_pose.pose.orientation.w = initial_quaternion[3] - marker_pose.pose.position.x = msg.O_T_EE[12] - marker_pose.pose.position.y = msg.O_T_EE[13] - marker_pose.pose.position.z = msg.O_T_EE[14] - global initial_pose_found - initial_pose_found = True - - def process_feedback(feedback): if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: marker_pose.pose.position.x = max([min([feedback.pose.position.x, @@ -57,17 +38,31 @@ def process_feedback(feedback): server.applyChanges() +def wait_for_initial_pose(): + msg = rospy.wait_for_message("franka_state_controller/franka_states", + FrankaState) # type: FrankaState + + initial_quaternion = \ + tf.transformations.quaternion_from_matrix( + np.transpose(np.reshape(msg.O_T_EE, + (4, 4)))) + initial_quaternion = initial_quaternion / \ + np.linalg.norm(initial_quaternion) + marker_pose.pose.orientation.x = initial_quaternion[0] + marker_pose.pose.orientation.y = initial_quaternion[1] + marker_pose.pose.orientation.z = initial_quaternion[2] + marker_pose.pose.orientation.w = initial_quaternion[3] + marker_pose.pose.position.x = msg.O_T_EE[12] + marker_pose.pose.position.y = msg.O_T_EE[13] + marker_pose.pose.position.z = msg.O_T_EE[14] + + if __name__ == "__main__": rospy.init_node("equilibrium_pose_node") - state_sub = rospy.Subscriber("franka_state_controller/franka_states", - FrankaState, franka_state_callback) listener = tf.TransformListener() link_name = rospy.get_param("~link_name") - # Get initial pose for the interactive marker - while not initial_pose_found and not rospy.is_shutdown(): - rospy.sleep(1) - state_sub.unregister() + wait_for_initial_pose() pose_pub = rospy.Publisher( "equilibrium_pose", PoseStamped, queue_size=10) From e28696dee3442ecf50c9970e1998668380cdea5c Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Mon, 8 Nov 2021 10:30:54 +0100 Subject: [PATCH 12/20] use wait_for_message in dual_arm_interactive_marker.py --- .../scripts/dual_arm_interactive_marker.py | 30 ++----------------- 1 file changed, 2 insertions(+), 28 deletions(-) diff --git a/franka_example_controllers/scripts/dual_arm_interactive_marker.py b/franka_example_controllers/scripts/dual_arm_interactive_marker.py index 2e57f681f..a26c2d909 100755 --- a/franka_example_controllers/scripts/dual_arm_interactive_marker.py +++ b/franka_example_controllers/scripts/dual_arm_interactive_marker.py @@ -16,8 +16,6 @@ marker_pose = PoseStamped() -centering_frame_ready = False - has_error = False left_has_error = False right_has_error = False @@ -80,39 +78,15 @@ def right_franka_state_callback(msg): has_error = left_has_error or right_has_error -def centering_pose_callback(msg): - """ - This callback function sets the marker pose to the current centering pose from a subscribed - topic. - :return: None - """ - global centering_frame_ready - global marker_pose - - marker_pose = msg - centering_frame_ready = True - - def reset_marker_pose_blocking(): """ This function resets the marker pose to current "middle pose" of left and right arm EEs. :return: None """ - global centering_frame_ready global marker_pose - - centering_frame_ready = False - - centering_frame_pose_sub = rospy.Subscriber( - "dual_arm_cartesian_impedance_example_controller/centering_frame", - PoseStamped, centering_pose_callback) - - # Get initial pose for the interactive marker - while not centering_frame_ready and not rospy.is_shutdown(): - rospy.sleep(0.1) - - centering_frame_pose_sub.unregister() + marker_pose = rospy.wait_for_message( + "dual_arm_cartesian_impedance_example_controller/centering_frame", PoseStamped) def process_feedback(feedback): From 9a64dfbf2e1ee6e5cbd64dd23db6088a427ff7f3 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Mon, 8 Nov 2021 10:35:16 +0100 Subject: [PATCH 13/20] fix description on how to use robot_ips parameter --- .../dual_arm_cartesian_impedance_example_controller.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/franka_example_controllers/launch/dual_arm_cartesian_impedance_example_controller.launch b/franka_example_controllers/launch/dual_arm_cartesian_impedance_example_controller.launch index 9dafe440f..8320e6010 100644 --- a/franka_example_controllers/launch/dual_arm_cartesian_impedance_example_controller.launch +++ b/franka_example_controllers/launch/dual_arm_cartesian_impedance_example_controller.launch @@ -1,6 +1,6 @@ - + From 32f4337fe5f82ec54a919990ff31dab7a89881e5 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Mon, 8 Nov 2021 10:37:30 +0100 Subject: [PATCH 14/20] update CHANGELOG.md --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index c91311a82..74006cfc1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,7 +2,7 @@ ## 0.x - UNRELEASED -* Prevent interactive marker server from not shutting down when not initialized +* Fix: Allow interactive marker server to shut down if not initialized * Add realistic hand/finger collision geometries to the Gazebo robot description * Add `joint_state_desired` publisher to `franka_gazebo` * No further ROS Kinetic support, since [End-of-Life was in April 2021](http://wiki.ros.org/Distributions) From 2f770bed2d6ffcd33e4950442a1d51a96709aa4d Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Mon, 8 Nov 2021 15:34:45 +0100 Subject: [PATCH 15/20] CHANGE: Remove namespace from `franka_gazebo` launch file --- .../robots/panda_arm.urdf.xacro | 1 - franka_gazebo/launch/panda.launch | 103 +++++++++--------- 2 files changed, 50 insertions(+), 54 deletions(-) diff --git a/franka_description/robots/panda_arm.urdf.xacro b/franka_description/robots/panda_arm.urdf.xacro index 8dc6cd9bb..0b607f9cf 100644 --- a/franka_description/robots/panda_arm.urdf.xacro +++ b/franka_description/robots/panda_arm.urdf.xacro @@ -61,7 +61,6 @@ - $(arg arm_id) 0.001 franka_gazebo/FrankaHWSim diff --git a/franka_gazebo/launch/panda.launch b/franka_gazebo/launch/panda.launch index 39b287439..80c00a6e5 100644 --- a/franka_gazebo/launch/panda.launch +++ b/franka_gazebo/launch/panda.launch @@ -37,66 +37,63 @@ - - - + + - - + + - + - - + + - - + + - + - - - [franka_state_controller/joint_states, franka_gripper/joint_states] - - - - [franka_state_controller/joint_states_desired, franka_gripper/joint_states] - - - + + + [franka_state_controller/joint_states, franka_gripper/joint_states] + + + + [franka_state_controller/joint_states_desired, franka_gripper/joint_states] + + + - - - - - - - + + + + + From dcaebaf9d137125cbe6daa9a0a6a9ede65480100 Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Tue, 9 Nov 2021 15:53:28 +0100 Subject: [PATCH 16/20] ADD: Changelog entry --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 74006cfc1..eaf643599 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,7 @@ * No further ROS Kinetic support, since [End-of-Life was in April 2021](http://wiki.ros.org/Distributions) * Make position + orientation targets threadsafe in cartesian example controller * Add singularity warning to `franka_gazebo` if Jacobian becomes singular +* **BREAKING** Make `/panda` namespace of `franka_gazebo` optional ## 0.8.1 - 2021-09-08 From f1197b0fd7b3ee9cfce0b3181dec512033499e21 Mon Sep 17 00:00:00 2001 From: Marco Boneberger Date: Tue, 9 Nov 2021 17:02:07 +0100 Subject: [PATCH 17/20] use franka_description_with_marker.rviz for gazebo launch file --- .../franka_sim_description_with_marker.rviz | 195 ------------------ franka_gazebo/launch/panda.launch | 2 +- 2 files changed, 1 insertion(+), 196 deletions(-) delete mode 100644 franka_gazebo/config/franka_sim_description_with_marker.rviz diff --git a/franka_gazebo/config/franka_sim_description_with_marker.rviz b/franka_gazebo/config/franka_sim_description_with_marker.rviz deleted file mode 100644 index 89bc19a89..000000000 --- a/franka_gazebo/config/franka_sim_description_with_marker.rviz +++ /dev/null @@ -1,195 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /RobotModel1 - - /InteractiveMarkers1 - Splitter Ratio: 0.5 - Tree Height: 724 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - panda_link0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link8: - Alpha: 1 - Show Axes: false - Show Trail: false - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Name: RobotModel - Robot Description: /panda/robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/InteractiveMarkers - Enable Transparency: true - Enabled: true - Name: InteractiveMarkers - Show Axes: false - Show Descriptions: true - Show Visual Aids: false - Update Topic: /panda/equilibrium_pose_marker/update - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 3.1647839546203613 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.6603980660438538 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.4603980481624603 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1025 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a0000035ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035f000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d00000042fc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004b80000035f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1853 - X: 67 - Y: 455 diff --git a/franka_gazebo/launch/panda.launch b/franka_gazebo/launch/panda.launch index 80c00a6e5..b29844188 100644 --- a/franka_gazebo/launch/panda.launch +++ b/franka_gazebo/launch/panda.launch @@ -95,6 +95,6 @@ - + From 602e99a42d4624e29b8ce3e10fa420223a0b8b78 Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Wed, 10 Nov 2021 10:16:12 +0100 Subject: [PATCH 18/20] ADD: Effort Joint Trajectory Controller This makes the both `franka_control` and `franka_gazebo` offer the same controller interface for panda_moveit_config --- .../config/default_controllers.yaml | 49 +++++++++++++------ franka_gazebo/config/sim_controllers.yaml | 28 +++++++++++ 2 files changed, 63 insertions(+), 14 deletions(-) diff --git a/franka_control/config/default_controllers.yaml b/franka_control/config/default_controllers.yaml index 28b48056c..407f34059 100644 --- a/franka_control/config/default_controllers.yaml +++ b/franka_control/config/default_controllers.yaml @@ -10,20 +10,41 @@ position_joint_trajectory_controller: - panda_joint7 constraints: goal_time: 0.5 - panda_joint1: - goal: 0.05 - panda_joint2: - goal: 0.05 - panda_joint3: - goal: 0.05 - panda_joint4: - goal: 0.05 - panda_joint5: - goal: 0.05 - panda_joint6: - goal: 0.05 - panda_joint7: - goal: 0.05 + panda_joint1: { goal: 0.05} + panda_joint2: { goal: 0.05} + panda_joint3: { goal: 0.05} + panda_joint4: { goal: 0.05} + panda_joint5: { goal: 0.05} + panda_joint6: { goal: 0.05} + panda_joint7: { goal: 0.05} + +effort_joint_trajectory_controller: + type: effort_controllers/JointTrajectoryController + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 + gains: + panda_joint1: { p: 600, d: 30, i: 0 } + panda_joint2: { p: 600, d: 30, i: 0 } + panda_joint3: { p: 600, d: 30, i: 0 } + panda_joint4: { p: 600, d: 30, i: 0 } + panda_joint5: { p: 250, d: 10, i: 0 } + panda_joint6: { p: 150, d: 10, i: 0 } + panda_joint7: { p: 50, d: 5, i: 0 } + constraints: + goal_time: 0.5 + panda_joint1: { goal: 0.05} + panda_joint2: { goal: 0.05} + panda_joint3: { goal: 0.05} + panda_joint4: { goal: 0.05} + panda_joint5: { goal: 0.05} + panda_joint6: { goal: 0.05} + panda_joint7: { goal: 0.05} franka_state_controller: type: franka_control/FrankaStateController diff --git a/franka_gazebo/config/sim_controllers.yaml b/franka_gazebo/config/sim_controllers.yaml index f533560af..cb5d2fdde 100644 --- a/franka_gazebo/config/sim_controllers.yaml +++ b/franka_gazebo/config/sim_controllers.yaml @@ -38,3 +38,31 @@ cartesian_impedance_example_controller: - $(arg arm_id)_joint5 - $(arg arm_id)_joint6 - $(arg arm_id)_joint7 + +effort_joint_trajectory_controller: + type: effort_controllers/JointTrajectoryController + joints: + - $(arg arm_id)_joint1 + - $(arg arm_id)_joint2 + - $(arg arm_id)_joint3 + - $(arg arm_id)_joint4 + - $(arg arm_id)_joint5 + - $(arg arm_id)_joint6 + - $(arg arm_id)_joint7 + gains: + $(arg arm_id)_joint1: { p: 600, d: 30, i: 0 } + $(arg arm_id)_joint2: { p: 600, d: 30, i: 0 } + $(arg arm_id)_joint3: { p: 600, d: 30, i: 0 } + $(arg arm_id)_joint4: { p: 600, d: 30, i: 0 } + $(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.5 + $(arg arm_id)_joint1: { goal: 0.05 } + $(arg arm_id)_joint2: { goal: 0.05 } + $(arg arm_id)_joint3: { goal: 0.05 } + $(arg arm_id)_joint4: { goal: 0.05 } + $(arg arm_id)_joint5: { goal: 0.05 } + $(arg arm_id)_joint6: { goal: 0.05 } + $(arg arm_id)_joint7: { goal: 0.05 } From 10b05e2cd217981a2b795e68574fa71eb3be2d23 Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Wed, 10 Nov 2021 10:50:24 +0100 Subject: [PATCH 19/20] ADD: Changelog entry --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index eaf643599..f8629d2ea 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,7 @@ * Make position + orientation targets threadsafe in cartesian example controller * Add singularity warning to `franka_gazebo` if Jacobian becomes singular * **BREAKING** Make `/panda` namespace of `franka_gazebo` optional +* Add effort joint trajectory controller to be used by MoveIT ## 0.8.1 - 2021-09-08 From 6942e27a4b367bf4b9b040c0319481045de24542 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 27 Oct 2021 23:21:07 +0200 Subject: [PATCH 20/20] Drop re-publishing joint_states_desired --- franka_control/launch/franka_control.launch | 6 ------ franka_gazebo/launch/panda.launch | 5 ----- 2 files changed, 11 deletions(-) diff --git a/franka_control/launch/franka_control.launch b/franka_control/launch/franka_control.launch index 842353f6e..0afb5ffc0 100644 --- a/franka_control/launch/franka_control.launch +++ b/franka_control/launch/franka_control.launch @@ -22,10 +22,4 @@ [franka_state_controller/joint_states] - - [franka_state_controller/joint_states_desired, franka_gripper/joint_states] - [franka_state_controller/joint_states_desired] - - - diff --git a/franka_gazebo/launch/panda.launch b/franka_gazebo/launch/panda.launch index b29844188..eba70f46b 100644 --- a/franka_gazebo/launch/panda.launch +++ b/franka_gazebo/launch/panda.launch @@ -80,11 +80,6 @@ [franka_state_controller/joint_states, franka_gripper/joint_states] - - [franka_state_controller/joint_states_desired, franka_gripper/joint_states] - - -