diff --git a/joint_trajectory_controller/doc/new_trajectory.png b/joint_trajectory_controller/doc/new_trajectory.png new file mode 100644 index 0000000000..b7c07eeffe Binary files /dev/null and b/joint_trajectory_controller/doc/new_trajectory.png differ diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst new file mode 100644 index 0000000000..8be135ff4b --- /dev/null +++ b/joint_trajectory_controller/doc/parameters.rst @@ -0,0 +1,137 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/joint_trajectory_controller/doc/parameters.rst + +.. _parameters: + +Details about parameters +^^^^^^^^^^^^^^^^^^^^^^^^ + +joints (list(string)) + Joint names to control and listen to. + +command_joints (list(string)) + Joint names to control. This parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names. + +command_interface (list(string)) + Command interfaces provided by the hardware interface for all joints. + + Values: [position | velocity | acceleration] (multiple allowed) + +state_interfaces (list(string)) + State interfaces provided by the hardware for all joints. + + Values: position (mandatory) [velocity, [acceleration]]. + Acceleration interface can only be used in combination with position and velocity. + +state_publish_rate (double) + Publish-rate of the controller's "state" topic. + + Default: 50.0 + +action_monitor_rate (double) + Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory). + + Default: 20.0 + +allow_partial_joints_goal (boolean) + Allow joint goals defining trajectory for only some joints. + + Default: false + +allow_integration_in_goal_trajectories (boolean) + Allow integration in goal trajectories to accept goals without position or velocity specified + + Default: false + +interpolation_method (string) + The type of interpolation to use, if any. Can be "splines" or "none". + + Default: splines + +open_loop_control (boolean) + Use controller in open-loop control mode: + + * The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation. + * It deactivates the feedback control, see the ``gains`` structure. + + This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). + + .. Note:: + If this flag is set, the controller tries to read the values from the command interfaces on activation. + If they have real numeric values, those will be used instead of state interfaces. + Therefore it is important set command interfaces to NaN (i.e., ``std::numeric_limits::quiet_NaN()``) or state values when the hardware is started. + + Default: false + +allow_nonzero_velocity_at_trajectory_end (boolean) + If false, the last velocity point has to be zero or the goal will be rejected. + + Default: true + +constraints (structure) + Default values for tolerances if no explicit values are states in JointTrajectory message. + +constraints.stopped_velocity_tolerance (double) + Default value for end velocity deviation. + + Default: 0.01 + +constraints.goal_time (double) + Maximally allowed tolerance for not reaching the end of the trajectory in a predefined time. + + Default: 0.0 (not checked) + +constraints..trajectory (double) + Maximally allowed deviation from the target trajectory for a given joint. + + Default: 0.0 (tolerance is not enforced) + +constraints..goal (double) + Maximally allowed deviation from the goal (end of the trajectory) for a given joint. + + Default: 0.0 (tolerance is not enforced) + +gains (structure) + Only relevant, if ``open_loop_control`` is not set. + + If ``velocity`` is the only command interface for all joints or an ``effort`` command interface is configured, PID controllers are used for every joint. + This structure contains the controller gains for every joint with the control law + + .. math:: + + u = k_{ff} v_d + k_p e + k_i \sum e dt + k_d (v_d - v) + + with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see below), + the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively. + +gains..p (double) + Proportional gain :math:`k_p` for PID + + Default: 0.0 + +gains..i (double) + Integral gain :math:`k_i` for PID + + Default: 0.0 + +gains..d (double) + Derivative gain :math:`k_d` for PID + + Default: 0.0 + +gains..i_clamp (double) + Integral clamp. Symmetrical in both positive and negative direction. + + Default: 0.0 + +gains..ff_velocity_scale (double) + Feed-forward scaling :math:`k_{ff}` of velocity + + Default: 0.0 + +gains..normalize_error (bool) + If true, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`. + Otherwise :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured + position :math:`s` from the state interface. Use this for revolute joints without end stop, + where the shortest rotation to the target position is the desired motion. + + Default: false diff --git a/joint_trajectory_controller/doc/spline_acceleration.png b/joint_trajectory_controller/doc/spline_acceleration.png new file mode 100644 index 0000000000..98672beedd Binary files /dev/null and b/joint_trajectory_controller/doc/spline_acceleration.png differ diff --git a/joint_trajectory_controller/doc/spline_position.png b/joint_trajectory_controller/doc/spline_position.png new file mode 100644 index 0000000000..79167f8500 Binary files /dev/null and b/joint_trajectory_controller/doc/spline_position.png differ diff --git a/joint_trajectory_controller/doc/spline_position_velocity.png b/joint_trajectory_controller/doc/spline_position_velocity.png new file mode 100644 index 0000000000..0e959295fb Binary files /dev/null and b/joint_trajectory_controller/doc/spline_position_velocity.png differ diff --git a/joint_trajectory_controller/doc/spline_position_velocity_acceleration.png b/joint_trajectory_controller/doc/spline_position_velocity_acceleration.png new file mode 100644 index 0000000000..70152eec4e Binary files /dev/null and b/joint_trajectory_controller/doc/spline_position_velocity_acceleration.png differ diff --git a/joint_trajectory_controller/doc/spline_position_velocity_acceleration_initialpoint.png b/joint_trajectory_controller/doc/spline_position_velocity_acceleration_initialpoint.png new file mode 100644 index 0000000000..a0f06e33c5 Binary files /dev/null and b/joint_trajectory_controller/doc/spline_position_velocity_acceleration_initialpoint.png differ diff --git a/joint_trajectory_controller/doc/spline_position_velocity_acceleration_initialpoint_notime.png b/joint_trajectory_controller/doc/spline_position_velocity_acceleration_initialpoint_notime.png new file mode 100644 index 0000000000..a1e29de3fc Binary files /dev/null and b/joint_trajectory_controller/doc/spline_position_velocity_acceleration_initialpoint_notime.png differ diff --git a/joint_trajectory_controller/doc/spline_velocity.png b/joint_trajectory_controller/doc/spline_velocity.png new file mode 100644 index 0000000000..d427f4da88 Binary files /dev/null and b/joint_trajectory_controller/doc/spline_velocity.png differ diff --git a/joint_trajectory_controller/doc/spline_wrong_points.png b/joint_trajectory_controller/doc/spline_wrong_points.png new file mode 100644 index 0000000000..f73310c031 Binary files /dev/null and b/joint_trajectory_controller/doc/spline_wrong_points.png differ diff --git a/joint_trajectory_controller/doc/trajectory.rst b/joint_trajectory_controller/doc/trajectory.rst new file mode 100644 index 0000000000..44f385de27 --- /dev/null +++ b/joint_trajectory_controller/doc/trajectory.rst @@ -0,0 +1,168 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/joint_trajectory_controller/doc/trajectory.rst + +.. _joint_trajectory_controller_trajectory_representation: + +Trajectory Representation +--------------------------------- + +Trajectories are represented internally with ``trajectory_msgs/msg/JointTrajectory`` data structure. + +Currently, two interpolation methods are implemented: ``none`` and ``spline``. +By default, a spline interpolator is provided, but it's possible to support other representations. + +.. warning:: + The user has to ensure that the correct inputs are provided for the trajectory, which are needed + by the controller's setup of command interfaces and PID configuration. There is no sanity check and + missing fields in the sampled trajectory might cause segmentation faults. + +Interpolation Method ``none`` +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +It returns the initial point until the time for the first trajectory data point is reached. Then, it simply takes the next given datapoint. + +.. warning:: + It does not deduce (integrate) trajectory from derivatives, nor does it calculate derivatives. + I.e., one has to provide position and its derivatives as needed. + +Interpolation Method ``spline`` +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The spline interpolator uses the following interpolation strategies depending on the waypoint specification: + +* Linear: + + * Used, if only position is specified. + * Returns position and velocity + * Guarantees continuity at the position level. + * Discouraged because it yields trajectories with discontinuous velocities at the waypoints. + +* Cubic: + + * Used, if position and velocity are specified. + * Returns position, velocity, and acceleration. + * Guarantees continuity at the velocity level. + +* Quintic: + + * Used, if position, velocity and acceleration are specified + * Returns position, velocity, and acceleration. + * Guarantees continuity at the acceleration level. + +Trajectories with velocity fields only, velocity and acceleration only, or acceleration fields only can be processed and are accepted, if ``allow_integration_in_goal_trajectories`` is true. Position (and velocity) is then integrated from velocity (or acceleration, respectively) by Heun's method. + +Visualized Examples +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +To visualize the difference of the different interpolation methods and their inputs, different trajectories defined at a 0.5s grid and are sampled at a rate of 10ms. + +* Sampled trajectory with linear spline if position is given only: + +.. image:: spline_position.png + :alt: Sampled trajectory with splines if position is given only + +* Sampled trajectory with cubic splines if velocity is given only (no deduction for interpolation method ``none``): + +.. image:: spline_velocity.png + :alt: Sampled trajectory with splines if velocity is given only + +* Sampled trajectory if position and velocity is given: + +.. note:: + If the same integration method was used (``Trajectory`` class uses Heun's method), then the ``spline`` method this gives identical results as above where velocity only was given as input. + +.. image:: spline_position_velocity.png + :alt: Sampled trajectory if position and velocity is given + +* Sampled trajectory with quintic splines if acceleration is given only (no deduction for interpolation method ``none``): + +.. image:: spline_acceleration.png + :alt: Sampled trajectory with splines if acceleration is given only + +* Sampled trajectory if position, velocity, and acceleration points are given: + +.. note:: + If the same integration method was used (``Trajectory`` class uses Heun's method), then the ``spline`` method this gives identical results as above where acceleration only was given as input. + +.. image:: spline_position_velocity_acceleration.png + :alt: Sampled trajectory with splines if position, velocity, and acceleration is given + +* Sampled trajectory if the same position, velocity, and acceleration points as above are given, but with a nonzero initial point: + +.. image:: spline_position_velocity_acceleration_initialpoint.png + :alt: Sampled trajectory with splines if position, velocity, and acceleration is given with nonzero initial point + +* Sampled trajectory if the same position, velocity, and acceleration points as above are given but with the first point starting at ``t=0``: + +.. note:: + If the first point is starting at ``t=0``, there is no interpolation from the initial point to the trajectory. + +.. image:: spline_position_velocity_acceleration_initialpoint_notime.png + :alt: Sampled trajectory with splines if position, velocity, and acceleration is given with nonzero initial point and first point starting at ``t=0`` + +* Sampled trajectory with splines if inconsistent position, velocity, and acceleration points are given: + +.. note:: + Interpolation method ``none`` only gives the next input points, while the ``spline`` interpolation method shows high overshoot to match the given trajectory points. + +.. image:: spline_wrong_points.png + :alt: Sampled trajectory with splines if inconsistent position, velocity, and acceleration is given + +.. _joint_trajectory_controller_trajectory_replacement: + +Trajectory Replacement +--------------------------------- +*Parts of this documentation were originally published in the ROS 1 wiki under the* `CC BY 3.0 license `_. [#f1]_ + +Joint trajectory messages allow to specify the time at which a new trajectory should start executing by means of the header timestamp, where zero time (the default) means "start now". + +The arrival of a new trajectory command does not necessarily mean that the controller will completely discard the currently running trajectory and substitute it with the new one. +Rather, the controller will take the useful parts of both and combine them appropriately, yielding a smarter trajectory replacement strategy. + +The steps followed by the controller for trajectory replacement are as follows: + + + Get useful parts of the new trajectory: Preserve all waypoints whose time to be reached is in the future, and discard those with times in the past. + If there are no useful parts (ie. all waypoints are in the past) the new trajectory is rejected and the current one continues execution without changes. + + + Get useful parts of the current trajectory: Preserve the current trajectory up to the start time of the new trajectory, discard the later parts. + + + Combine the useful parts of the current and new trajectories. + +The following examples describe this behavior in detail. + +The first example shows a joint which is in hold position mode (flat grey line labeled *pos hold* in the figure below). +A new trajectory (shown in red) arrives at the current time (now), which contains three waypoints and a start time in the future (*traj start*). +The time at which waypoints should be reached (``time_from_start`` member of ``trajectory_msgs/JointTrajectoryPoint``) is relative to the trajectory start time. + +The controller splices the current hold trajectory at time *traj start* and appends the three waypoints. +Notice that between now and *traj start* the previous position hold is still maintained, as the new trajectory is not supposed to start yet. +After the last waypoint is reached, its position is held until new commands arrive. + +.. image:: new_trajectory.png + :alt: Receiving a new trajectory. + +| + +The controller guarantees that the transition between the current and new trajectories will be smooth. Longer times to reach the first waypoint mean slower transitions. + +The next examples discuss the effect of sending the same trajectory to the controller with different start times. +The scenario is that of a controller executing the trajectory from the previous example (shown in red), +and receiving a new command (shown in green) with a trajectory start time set to either zero (start now), +a future time, or a time in the past. + +.. image:: trajectory_replacement_future.png + :alt: Trajectory start time in the future. + +| + +.. image:: trajectory_replacement_now.png + :alt: Zero trajectory start time (start now). + +| + +Of special interest is the last example, where the new trajectory start time and first waypoint are in the past (before now). +In this case, the first waypoint is discarded and only the second one is realized. + +.. image:: trajectory_replacement_past.png + :alt: Trajectory start time in the past. + +| + +.. [#f1] Adolfo Rodriguez: `Understanding trajectory replacement `_ diff --git a/joint_trajectory_controller/doc/trajectory_replacement_future.png b/joint_trajectory_controller/doc/trajectory_replacement_future.png new file mode 100644 index 0000000000..582a6acce8 Binary files /dev/null and b/joint_trajectory_controller/doc/trajectory_replacement_future.png differ diff --git a/joint_trajectory_controller/doc/trajectory_replacement_now.png b/joint_trajectory_controller/doc/trajectory_replacement_now.png new file mode 100644 index 0000000000..a7cd2f6bc7 Binary files /dev/null and b/joint_trajectory_controller/doc/trajectory_replacement_now.png differ diff --git a/joint_trajectory_controller/doc/trajectory_replacement_past.png b/joint_trajectory_controller/doc/trajectory_replacement_past.png new file mode 100644 index 0000000000..a719f6fc66 Binary files /dev/null and b/joint_trajectory_controller/doc/trajectory_replacement_past.png differ diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 6125b9e385..281904e867 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -12,21 +12,7 @@ Trajectories are specified as a set of waypoints to be reached at specific time which the controller attempts to execute as well as the mechanism allows. Waypoints consist of positions, and optionally velocities and accelerations. - -*Parts of this documentation were originally published in the ROS 1 wiki under the* `CC BY 3.0 license `_. *Citations are given in the respective section, but were adapted for the ROS 2 implementation.* [#f1]_ [#f2]_ - -Trajectory representation [#f1]_ ---------------------------------- - -Trajectories are represented internally with ``trajectory_msgs/msg/JointTrajectory`` data structure. -By default, a spline interpolator is provided, but it's possible to support other representations. -The spline interpolator uses the following interpolation strategies depending on the waypoint specification: - -* Linear: Only position is specified. Guarantees continuity at the position level. Discouraged because it yields trajectories with discontinuous velocities at the waypoints. - -* Cubic: Position and velocity are specified. Guarantees continuity at the velocity level. - -* Quintic: Position, velocity and acceleration are specified: Guarantees continuity at the acceleration level. +*Parts of this documentation were originally published in the ROS 1 wiki under the* `CC BY 3.0 license `_. *Citations are given in the respective section, but were adapted for the ROS 2 implementation.* [#f1]_ Hardware interface type [#f1]_ ------------------------------- @@ -115,133 +101,10 @@ Preemption policy [#f1]_ Only one action goal can be active at any moment, or none if the topic interface is used. Path and goal tolerances are checked only for the trajectory segments of the active goal. -When an active action goal is preempted by another command coming from the action interface, the goal is canceled and the client is notified. +When an active action goal is preempted by another command coming from the action interface, the goal is canceled and the client is notified. The trajectory is replaced in a defined way, see :ref:`trajectory replacement `. Sending an empty trajectory message from the topic interface (not the action interface) will override the current action goal and not abort the action. -.. _parameters: - -Details about parameters -^^^^^^^^^^^^^^^^^^^^^^^^ - -joints (list(string)) - Joint names to control and listen to. - -command_joints (list(string)) - Joint names to control. This parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names. - -command_interface (list(string)) - Command interfaces provided by the hardware interface for all joints. - - Values: [position | velocity | acceleration] (multiple allowed) - -state_interfaces (list(string)) - State interfaces provided by the hardware for all joints. - - Values: position (mandatory) [velocity, [acceleration]]. - Acceleration interface can only be used in combination with position and velocity. - -state_publish_rate (double) - Publish-rate of the controller's "state" topic. - - Default: 50.0 - -action_monitor_rate (double) - Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory). - - Default: 20.0 - -allow_partial_joints_goal (boolean) - Allow joint goals defining trajectory for only some joints. - -open_loop_control (boolean) - Use controller in open-loop control mode: - + The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation. - + It deactivates the feedback control, see the ``gains`` structure. - - This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). - - .. Note:: - If this flag is set, the controller tries to read the values from the command interfaces on activation. - If they have real numeric values, those will be used instead of state interfaces. - Therefore it is important set command interfaces to NaN (i.e., ``std::numeric_limits::quiet_NaN()``) or state values when the hardware is started. - - Default: false - -allow_nonzero_velocity_at_trajectory_end (boolean) - If false, the last velocity point has to be zero or the goal will be rejected. - - Default: true - -constraints (structure) - Default values for tolerances if no explicit values are states in JointTrajectory message. - -constraints.stopped_velocity_tolerance (double) - Default value for end velocity deviation. - - Default: 0.01 - -constraints.goal_time (double) - Maximally allowed tolerance for not reaching the end of the trajectory in a predefined time. - - Default: 0.0 (not checked) - -constraints..trajectory (double) - Maximally allowed deviation from the target trajectory for a given joint. - - Default: 0.0 (tolerance is not enforced) - -constraints..goal (double) - Maximally allowed deviation from the goal (end of the trajectory) for a given joint. - - Default: 0.0 (tolerance is not enforced) - -gains (structure) - Only relevant, if ``open_loop_control`` is not set. - - If ``velocity`` is the only command interface for all joints or an ``effort`` command interface is configured, PID controllers are used for every joint. - This structure contains the controller gains for every joint with the control law - - .. math:: - - u = k_ff v_d + k_p (s_d - s) + k_i \sum(s_d - s) dt + k_d (v_d - v) - - with the desired velocity :math:`v_d` and position :math:`s_d`, - the measured velocity :math:`v` and position :math:`s`, the controller period :math:`dt`, - and the ``velocity`` or ``effort`` setpoint :math:`u`, respectively. - -gains..p (double) - Proportional gain :math:`k_p` for PID - - Default: 0.0 - -gains..i (double) - Integral gain :math:`k_i` for PID - - Default: 0.0 - -gains..d (double) - Derivative gain :math:`k_d` for PID - - Default: 0.0 - -gains..i_clamp (double) - Integral clamp. Symmetrical in both positive and negative direction. - - Default: 0.0 - -gains..ff_velocity_scale (double) - Feed-forward scaling :math:`k_ff` of velocity - - Default: 0.0 - -gains..normalize_error (bool) - If true, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`. - Otherwise :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured - position :math:`s` from the state interface. Use this for revolute joints without end stop, - where the shortest rotation to the target position is the desired motion. - - Default: false .. _ROS 2 interface: @@ -274,8 +137,7 @@ There are two mechanisms for sending trajectories to the controller: * via action, see :ref:`actions ` * via topic, see :ref:`subscriber ` -Both use the ``trajectory_msgs/JointTrajectory`` message to specify trajectories, and require specifying values for all the controller joints (as opposed to only a subset) if ``allow_partial_joints_goal`` is not set to ``True``. - +Both use the ``trajectory_msgs/msg/JointTrajectory`` message to specify trajectories, and require specifying values for all the controller joints (as opposed to only a subset) if ``allow_partial_joints_goal`` is not set to ``True``. For further information on the message format, see :ref:`trajectory representation `. .. _Actions: @@ -318,17 +180,27 @@ Services Query controller state at any future time -Specialized versions of JointTrajectoryController (TBD in ...) +Specialized versions of JointTrajectoryController -------------------------------------------------------------- +(TBD in ...) -The controller types are placed into namespaces according to their command types for the hardware (see `general introduction into controllers <../../index.rst>`_). +The controller types are placed into namespaces according to their command types for the hardware (see :ref:`controllers`). The following version of the Joint Trajectory Controller are available mapping the following interfaces: * position_controllers::JointTrajectoryController +Further information +-------------------------------------------------------------- + +.. toctree:: + :titlesonly: + + Trajectory Representation + joint_trajectory_controller Parameters + + .. rubric:: Footnote .. [#f1] Adolfo Rodriguez: `joint_trajectory_controller `_ -.. [#f2] Adolfo Rodriguez: `Understanding trajectory replacement `_