diff --git a/.travis.yml b/.travis.yml
deleted file mode 100644
index 3a56a710..00000000
--- 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
- '
diff --git a/CHANGELOG.md b/CHANGELOG.md
index c83cbbea..f8629d2e 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -2,11 +2,14 @@
## 0.x - UNRELEASED
+* 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)
* 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
diff --git a/franka_control/config/default_controllers.yaml b/franka_control/config/default_controllers.yaml
index 28b48056..407f3405 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_control/launch/franka_control.launch b/franka_control/launch/franka_control.launch
index 842353f6..0afb5ffc 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_description/robots/hand.xacro b/franka_description/robots/hand.xacro
index 2f9ae8ce..868e1615 100644
--- a/franka_description/robots/hand.xacro
+++ b/franka_description/robots/hand.xacro
@@ -12,9 +12,16 @@
-
+
+
+
+
+
+
+
+
@@ -52,6 +59,10 @@
+
+
+
+
@@ -65,6 +76,11 @@
+
+
+
+
+
@@ -73,6 +89,12 @@
+
+
+
+
+
+
diff --git a/franka_description/robots/panda_arm.urdf.xacro b/franka_description/robots/panda_arm.urdf.xacro
index d5b8d006..0b607f9c 100644
--- a/franka_description/robots/panda_arm.urdf.xacro
+++ b/franka_description/robots/panda_arm.urdf.xacro
@@ -1,20 +1,21 @@
-
-
-
-
-
+
+
+
+
+
+
-
+
-
+
@@ -27,12 +28,12 @@
-
+
-
-
-
+
+
+
@@ -40,27 +41,26 @@
-
+
-
-
-
-
-
-
-
+
+
+
+
+
+
+
-
-
+
- ${arm_id}
0.001
franka_gazebo/FrankaHWSim
diff --git a/franka_description/robots/panda_gazebo.xacro b/franka_description/robots/panda_gazebo.xacro
index 98459754..0ab2d36b 100644
--- a/franka_description/robots/panda_gazebo.xacro
+++ b/franka_description/robots/panda_gazebo.xacro
@@ -8,7 +8,7 @@
-
+
@@ -300,6 +300,11 @@
izz="0.0017" />
+
+
+
+
+
@@ -351,6 +356,7 @@
+
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 9dafe440..8320e601 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 @@
-
+
diff --git a/franka_example_controllers/scripts/dual_arm_interactive_marker.py b/franka_example_controllers/scripts/dual_arm_interactive_marker.py
index 3119ce57..a26c2d90 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:
- 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):
diff --git a/franka_example_controllers/scripts/interactive_marker.py b/franka_example_controllers/scripts/interactive_marker.py
index 7755e4a6..1893debe 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 rospy.is_shutdown():
- rospy.sleep(1)
- state_sub.unregister()
+ wait_for_initial_pose()
pose_pub = rospy.Publisher(
"equilibrium_pose", PoseStamped, queue_size=10)
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 89bc19a8..00000000
--- 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/config/sim_controllers.yaml b/franka_gazebo/config/sim_controllers.yaml
index f533560a..cb5d2fdd 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 }
diff --git a/franka_gazebo/launch/panda.launch b/franka_gazebo/launch/panda.launch
index 52780202..eba70f46 100644
--- a/franka_gazebo/launch/panda.launch
+++ b/franka_gazebo/launch/panda.launch
@@ -31,83 +31,65 @@
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ [franka_state_controller/joint_states, franka_gripper/joint_states]
+
+
+
+
+
+
+
-
-
- [franka_state_controller/joint_states, franka_gripper/joint_states]
-
-
-
- [franka_state_controller/joint_states_desired, franka_gripper/joint_states]
-
-
-
-
-
-
-
-
-
-
-
-
+
diff --git a/franka_gazebo/test/launch/panda-gazebo.urdf b/franka_gazebo/test/launch/panda-gazebo.urdf
index cd262f36..3ef04c6c 100644
--- a/franka_gazebo/test/launch/panda-gazebo.urdf
+++ b/franka_gazebo/test/launch/panda-gazebo.urdf
@@ -7,7 +7,7 @@
-
+