Merge pull request #201 in SWDEV/franka_ros from SRR-1576 to develop

* commit 'fa1815657fabfc51980c268433190c58d98a4b88':
  CHANGE: Use `pass_all_args` in move_to_start.launch for franka_control.launch
  CHANGE: Better estimate move_to_start duration
  FIX: Make pyformat happy
  FIX: Let move_to_start motion be smooth at start & end
  FIX: Remove exec_depend on MoveIt
  ADD: CHANGELOG entry
  CHANGE: Use `joint_trajectory_controller` directly instead of MoveIt

CHANGELOG.md

@@ -2,8 +2,11 @@
 
 ## 0.10.0 - UNRELEASED
 
+Requires `libfranka` >= 0.8.0
+
   * `franka_example_controllers`: Normalize rotations before usage
   * `franka_example_controllers`: Extend the `teleop_joint_pd_example_controller` with markers indicating leader and follower roles + consistently use leader and follower as robot names in the example.
+  * `franka_example_controllers`: Don't require MoveIt for `move_to_start.launch`
 
 ## 0.9.1 - 2022-08-29
 

franka_control/config/start_pose.yaml

@@ -0,0 +1,8 @@
+joint_pose:
+  $(arg arm_id)_joint1: 0
+  $(arg arm_id)_joint2: -0.785398163397
+  $(arg arm_id)_joint3: 0
+  $(arg arm_id)_joint4: -2.35619449019
+  $(arg arm_id)_joint5: 0
+  $(arg arm_id)_joint6: 1.57079632679
+  $(arg arm_id)_joint7: 0.785398163397

franka_example_controllers/launch/move_to_start.launch

@@ -1,13 +1,30 @@
 <?xml version="1.0" ?>
 <launch>
   <arg name="robot_ip" />
+  <arg name="arm_id" default="panda" />
+  <arg name="transmission" default="effort" doc="The type of position control to use (either 'position' or 'effort')" />
 
-  <include file="$(find franka_control)/launch/franka_control.launch">
-    <arg name="robot_ip" value="$(arg robot_ip)" />
+  <include file="$(find franka_control)/launch/franka_control.launch" pass_all_args="true">
     <arg name="load_gripper" value="false" />
   </include>
-  <include file="$(find panda_moveit_config)/launch/panda_moveit.launch">
-    <arg name="load_gripper" value="false" />
-  </include>
-  <node name="move_to_start" pkg="franka_example_controllers" type="move_to_start.py" output="screen" required="true" />
+
+  <node name="controller_spawner"
+        pkg="controller_manager"
+        type="spawner"
+        respawn="false"
+        output="screen"
+        args="$(arg transmission)_joint_trajectory_controller">
+  </node>
+
+  <node name="move_to_start"
+        pkg="franka_example_controllers"
+        type="move_to_start.py"
+        output="screen"
+        required="true">
+    <rosparam file="$(find franka_control)/config/start_pose.yaml" subst_value="true" />
+    <remap from="~follow_joint_trajectory" to="$(arg transmission)_joint_trajectory_controller/follow_joint_trajectory" />
+    <remap from="~joint_states" to="franka_state_controller/joint_states" />
+    <param name="max_dq" value="0.2" /> <!-- [rad/s] -->
+  </node>
+
 </launch>

franka_example_controllers/package.xml

@@ -38,11 +38,9 @@
   <exec_depend>franka_control</exec_depend>
   <exec_depend>franka_description</exec_depend>
   <exec_depend>message_runtime</exec_depend>
-  <exec_depend>panda_moveit_config</exec_depend>
-  <exec_depend>moveit_commander</exec_depend>
   <exec_depend>rospy</exec_depend>
 
   <export>
     <controller_interface plugin="${prefix}/franka_example_controllers_plugin.xml" />
   </export>
-</package>
+</package>

franka_example_controllers/scripts/move_to_start.py

@@ -1,12 +1,78 @@
 #!/usr/bin/env python
 
-import rospy
-from moveit_commander import MoveGroupCommander
-from actionlib_msgs.msg import GoalStatusArray
-
-if __name__ == '__main__':
-    rospy.init_node('move_to_start')
-    rospy.wait_for_message('move_group/status', GoalStatusArray)
-    commander = MoveGroupCommander('panda_arm')
-    commander.set_named_target('ready')
-    commander.go()
+import sys
+import rospy as ros
+
+from actionlib import SimpleActionClient
+from sensor_msgs.msg import JointState
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from control_msgs.msg import FollowJointTrajectoryAction, \
+                             FollowJointTrajectoryGoal, FollowJointTrajectoryResult
+
+ros.init_node('move_to_start')
+
+action = ros.resolve_name('~follow_joint_trajectory')
+client = SimpleActionClient(action, FollowJointTrajectoryAction)
+ros.loginfo("move_to_start: Waiting for '" + action + "' action to come up")
+client.wait_for_server()
+
+param = ros.resolve_name('~joint_pose')
+pose = ros.get_param(param, None)
+if pose is None:
+    ros.logerr('move_to_start: Could not find required parameter "' + param + '"')
+    sys.exit(1)
+
+topic = ros.resolve_name('~joint_states')
+ros.loginfo("move_to_start: Waiting for message on topic '" + topic + "'")
+joint_state = ros.wait_for_message(topic, JointState)
+initial_pose = dict(zip(joint_state.name, joint_state.position))
+
+max_movement = max(abs(pose[joint] - initial_pose[joint]) for joint in pose)
+
+point = JointTrajectoryPoint()
+point.time_from_start = ros.Duration.from_sec(
+    # Use either the time to move the furthest joint with 'max_dq' or 500ms,
+    # whatever is greater
+    max(max_movement / ros.get_param('~max_dq', 0.5), 0.5)
+)
+goal = FollowJointTrajectoryGoal()
+
+goal.trajectory.joint_names, point.positions = [list(x) for x in zip(*pose.items())]
+point.velocities = [0] * len(pose)
+
+goal.trajectory.points.append(point)
+goal.goal_time_tolerance = ros.Duration.from_sec(0.5)
+
+ros.loginfo('Sending trajectory Goal to move into initial config')
+client.send_goal_and_wait(goal)
+
+result = client.get_result()
+if result.error_code != FollowJointTrajectoryResult.SUCCESSFUL:
+    ros.logerr('move_to_start: Movement was not successful: ' + {
+        FollowJointTrajectoryResult.INVALID_GOAL:
+        """
+        The joint pose you want to move to is invalid (e.g. unreachable, singularity...).
+        Is the 'joint_pose' reachable?
+        """,
+
+        FollowJointTrajectoryResult.INVALID_JOINTS:
+        """
+        The joint pose you specified is for different joints than the joint trajectory controller
+        is claiming. Does you 'joint_pose' include all 7 joints of the robot?
+        """,
+
+        FollowJointTrajectoryResult.PATH_TOLERANCE_VIOLATED:
+        """
+        During the motion the robot deviated from the planned path too much. Is something blocking
+        the robot?
+        """,
+
+        FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED:
+        """
+        After the motion the robot deviated from the desired goal pose too much. Probably the robot
+        didn't reach the joint_pose properly
+        """,
+    }[result.error_code])
+
+else:
+    ros.loginfo('move_to_start: Successfully moved into start pose')

franka_ros/package.xml

@@ -21,7 +21,6 @@
   <exec_depend>franka_msgs</exec_depend>
   <exec_depend>franka_visualization</exec_depend>
   <exec_depend>franka_gazebo</exec_depend>
-  <exec_depend>panda_moveit_config</exec_depend>
 
   <export>
     <metapackage />