MoveIt support

This library implements the FollowJointTrajectory action server (actionlib) low level controller used typically by Moveit to command motions via the JointTrajectoryController

For the FollowJointTrajectory controller interface, the communication will take place under the name /controller_name/action_namespace configurable from the launch files

Launch parameters

Launch parameters for MoveIt support
Launch parameter	Description	Type	Default value
`controller_name`	The name of the controller	`str`	`"iiwa_controller"`
`action_namespace`	The action namespace for the controller	`str`	`"follow_joint_trajectory"`
`follow_all_trajectory`	Whether to follow the whole planned trajectory or go directly to the goal by skipping the intermediate trajectory points	`bool`	`true`
`trajectory_update_threshold`	Threshold used to progressively traverse trajectory points (see details below)	`double`	`0.5`

Launch file: libiiwa_ros/launch/default.launch

<launch>
    <!-- FollowJointTrajectory action: /controller_name/action_namespace -->
    <arg name="controller_name"                 default="iiwa_controller"/>
    <arg name="action_namespace"                default="follow_joint_trajectory"/>
    <arg name="follow_all_trajectory"           default="true"/>
    <arg name="trajectory_update_threshold"     default="0.5"/>

    <!-- libiiwa configuration (see LibIiwa class) -->
    <arg name="libiiwa_ip"                      default="0.0.0.0"/>
    <arg name="libiiwa_port"                    default="12225"/>

    <arg name="servo_interface"                 default="true"/>

    <!-- node configuration -->
    <arg name="verbose"                         default="false"/>

    <!-- launch ROS node -->
    <node name="libiiwa_ros" pkg="libiiwa_ros" type="node.py" output="screen" cwd="node">
        <param name="controller_name"               type="str"      value="$(arg controller_name)"/>
        <param name="action_namespace"              type="str"      value="$(arg action_namespace)"/>
        <param name="follow_all_trajectory"         type="bool"     value="$(arg follow_all_trajectory)"/>
        <param name="trajectory_update_threshold"   type="double"   value="$(arg trajectory_update_threshold)"/>

        <param name="libiiwa_ip"                    type="str"      value="$(arg libiiwa_ip)"/>
        <param name="libiiwa_port"                  type="int"      value="$(arg libiiwa_port)"/>

        <param name="servo_interface"               type="bool"     value="$(arg servo_interface)"/>

        <param name="verbose"                       type="bool"     value="$(arg verbose)"/>
    </node>
</launch>

The implementation will be included in upcoming releases. Open a new discussion if you need to use this functionality ahead of time.

Launch file: libiiwa_ros2/launch/default.py

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import TextSubstitution
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():

    namespace_launch_arg = DeclareLaunchArgument(
        "__ns", default_value=TextSubstitution(text="/")
    )

    # FollowJointTrajectory action: /controller_name/action_namespace
    controller_name_launch_arg = DeclareLaunchArgument(
        "controller_name", default_value=TextSubstitution(text="iiwa_controller")
    )
    action_namespace_launch_arg = DeclareLaunchArgument(
        "action_namespace", default_value=TextSubstitution(text="follow_joint_trajectory")
    )
    follow_all_trajectory_launch_arg = DeclareLaunchArgument(
        "follow_all_trajectory", default_value=TextSubstitution(text="True")
    )
    trajectory_update_threshold_launch_arg = DeclareLaunchArgument(
        "trajectory_update_threshold", default_value=TextSubstitution(text="0.5")
    )

    # libiiwa configuration (see LibIiwa class)
    libiiwa_ip_launch_arg = DeclareLaunchArgument(
        "libiiwa_ip", default_value=TextSubstitution(text="0.0.0.0")
    )
    libiiwa_port_launch_arg = DeclareLaunchArgument(
        "libiiwa_port", default_value=TextSubstitution(text="12225")
    )

    servo_interface_launch_arg = DeclareLaunchArgument(
        "servo_interface", default_value=TextSubstitution(text="True")
    )
    
    # node configuration
    verbose_launch_arg = DeclareLaunchArgument(
        "verbose", default_value=TextSubstitution(text="False")
    )

    # ROS2 node
    node = Node(
        package='libiiwa_ros2',
        namespace=LaunchConfiguration('__ns'),
        executable='node',
        name='iiwa',
        parameters=[{
            "__ns": LaunchConfiguration('__ns'),
            "controller_name": LaunchConfiguration('controller_name'),
            "action_namespace": LaunchConfiguration('action_namespace'),
            "follow_all_trajectory": LaunchConfiguration('follow_all_trajectory'),
            "trajectory_update_threshold": LaunchConfiguration('trajectory_update_threshold'),
            "libiiwa_ip": LaunchConfiguration('libiiwa_ip'),
            "libiiwa_port": LaunchConfiguration('libiiwa_port'),
            "servo_interface": LaunchConfiguration('servo_interface'),
            "verbose": LaunchConfiguration('verbose')
        }],
        output='screen',
    )

    return LaunchDescription([
        namespace_launch_arg,
        controller_name_launch_arg,
        action_namespace_launch_arg,
        follow_all_trajectory_launch_arg,
        trajectory_update_threshold_launch_arg,
        libiiwa_ip_launch_arg,
        libiiwa_port_launch_arg,
        servo_interface_launch_arg,
        verbose_launch_arg,
        node
    ])

Trajectory execution implementation

The execution of the sequence of trajectories parameterized in the FollowJointTrajectory actionlib is updated according to the following formula:

\(\sum_{j}^{J=7} || q_j - q_{j_{trajectory}} || <=\) trajectory_update_threshold

Where \(q\) is the position of the joint of the robot

The execution is sensitive to different velocity, acceleration, and jerk values. Adjust the trajectory_update_threshold parameter (in launch file) to control the behavior of the trajectory execution (in Servoing mode) in the following way:

intermittent trajectory: increase the parameter to update the target trajectory more quickly
missing trajectories: decrease the parameter to update the target trajectory slower

Troubleshooting

Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 1.000000 seconds). Stopping trajectory.

Edit the MoveIt configuration package launch/trajectory_execution.launch.xml

Disable execution duration monitoring

<param name="trajectory_execution/execution_duration_monitoring" value="false" />

Adjust the execution duration scaling (e.g. 5)

<param name="trajectory_execution/allowed_execution_duration_scaling" value="5.0"/>