Joint trajectory controller, which can be attached to a model with reference to one or more 1-axis joints in order to follow a trajectory. More...
#include <JointTrajectoryController.hh>
Public Member Functions | |
JointTrajectoryController () | |
Constructor. More... | |
~JointTrajectoryController () override=default | |
Destructor. More... | |
void | Configure (const Entity &_entity, const std::shared_ptr< const sdf::Element > &_sdf, EntityComponentManager &_ecm, EventManager &_eventMgr) override |
Configure the system. More... | |
void | PreUpdate (const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) override |
Public Member Functions inherited from System | |
System ()=default | |
Constructor. More... | |
virtual | ~System ()=default |
Destructor. More... | |
Detailed Description
Joint trajectory controller, which can be attached to a model with reference to one or more 1-axis joints in order to follow a trajectory.
Joint trajectories can be sent to this plugin via Ignition Transport. The default topic name is "/model/${MODEL_NAME}/joint_trajectory" that can be configured with the <topic>
system parameter.
This topic accepts ignition::msgs::JointTrajectory messages that represent a full trajectory, defined as temporal points
with their fields ordered according to joint_names
field. The fields under points
are positions
- Controlled by position PID controller for each joint velocities
- Controlled by velocity PID controller for each joint accelerations
- This field is currently ignored effort
- Controlled directly for each joint time_from_start
- Temporal information about the point
Forces/torques from position
, velocity
and effort
are summed and applied to the joint. Each PID controller can be configured with system parameters described below.
Input trajectory can be produced by a motion planning framework such as MoveIt2. For smooth execution of the trajectory, its points should to be interpolated before sending them via Ignition Transport (interpolation might already be implemented in the motion planning framework of your choice).
The progress of the current trajectory can be tracked on topic whose name is derived as <topic>_progress
. This progress is indicated in the range of (0.0, 1.0] and is currently based purely on time_from_start
contained in the trajectory points.
System Parameters
<topic>
The name of the topic that this plugin subscribes to. Optional parameter. Defaults to "/model/${MODEL_NAME}/joint_trajectory".
<use_header_start_time>
If enabled, trajectory execution begins at the timestamp contained in the header of received trajectory messages. Optional parameter. Defaults to false.
<joint_name>
Name of a joint to control. This parameter can be specified multiple times, i.e. once for each joint. Optional parameter. Defaults to all 1-axis joints contained in the model SDF (order is kept).
<initial_position>
Initial position of a joint (for position control). This parameter can be specified multiple times. Follows joint_name order. Optional parameter. Defaults to 0 for all joints.
<s_p_gain>
The proportional gain of the PID. Substitute 's' for "position" or "velocity" (e.g. "position_p_gain"). This parameter can be specified multiple times. Follows joint_name order. Optional parameter. The default value is 0 (disabled).
<s_i_gain>
The integral gain of the PID. Optional parameter. Substitute 's' for "position" or "velocity" (e.g. "position_p_gain"). This parameter can be specified multiple times. Follows joint_name order. Optional parameter. The default value is 0 (disabled).
<s_d_gain>
The derivative gain of the PID. Substitute 's' for "position" or "velocity" (e.g. "position_p_gain"). This parameter can be specified multiple times. Follows joint_name order. Optional parameter. The default value is 0 (disabled).
<s_i_min>
The integral lower limit of the PID. Substitute 's' for "position" or "velocity" (e.g. "position_p_gain"). This parameter can be specified multiple times. Follows joint_name order. Optional parameter. The default value is 0 (no limit if higher than s_i_max
).
<s_i_max>
The integral upper limit of the PID. Substitute 's' for "position" or "velocity" (e.g. "position_p_gain"). This parameter can be specified multiple times. Follows joint_name order. Optional parameter. The default value is -1 (no limit if lower than s_i_min
).
<s_cmd_min>
Output min value of the PID. Substitute 's' for "position" or "velocity" (e.g. "position_p_gain"). This parameter can be specified multiple times. Follows joint_name order. Optional parameter. The default value is 0 (no limit if higher than s_i_max
).
<s_cmd_max>
Output max value of the PID. Substitute 's' for "position" or "velocity" (e.g. "position_p_gain"). This parameter can be specified multiple times. Follows joint_name order. Optional parameter. The default value is -1 (no limit if lower than s_i_min
).
<s_cmd_offset>
Command offset (feed-forward) of the PID. Substitute 's' for "position" or "velocity" (e.g. "position_p_gain"). This parameter can be specified multiple times. Follows joint_name order. Optional parameter. The default value is 0 (no offset).
Constructor & Destructor Documentation
◆ JointTrajectoryController()
Constructor.
◆ ~JointTrajectoryController()
|
overridedefault |
Destructor.
Member Function Documentation
◆ Configure()
|
overridevirtual |
Configure the system.
- Parameters
-
[in] _entity The entity this plugin is attached to. [in] _sdf The SDF Element associated with this system plugin. [in] _ecm The EntityComponentManager of the given simulation instance. [in] _eventMgr The EventManager of the given simulation instance.
Implements ISystemConfigure.
◆ PreUpdate()
|
overridevirtual |
Implements ISystemPreUpdate.
The documentation for this class was generated from the following file: