Classes | Public Member Functions | Public Attributes | Private Member Functions | Private Attributes | List of all members
effort_controllers::JointPositionController Class Reference

Joint Position Controller. More...

#include <joint_position_controller.h>

Inheritance diagram for effort_controllers::JointPositionController:
Inheritance graph
[legend]

Classes

struct  Commands
 Store position and velocity command in struct to allow easier realtime buffer usage. More...
 

Public Member Functions

void getGains (double &p, double &i, double &d, double &i_max, double &i_min)
 Get the PID parameters. More...
 
void getGains (double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup)
 Get the PID parameters. More...
 
std::string getJointName ()
 Get the name of the joint this controller uses. More...
 
double getPosition ()
 Get the current position of the joint. More...
 
bool init (hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
 The init function is called to initialize the controller from a non-realtime thread with a pointer to the hardware interface, itself, instead of a pointer to a RobotHW. More...
 
 JointPositionController ()
 
void printDebug ()
 Print debug info to console. More...
 
void setCommand (double pos_target)
 Give set position of the joint for next update: revolute (angle) and prismatic (position) More...
 
void setCommand (double pos_target, double vel_target)
 Give set position of the joint for next update: revolute (angle) and prismatic (position) Also supports a target velocity. More...
 
void setGains (const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup=false)
 Set the PID parameters. More...
 
void starting (const ros::Time &time)
 This is called from within the realtime thread just before the first call to update. More...
 
void update (const ros::Time &time, const ros::Duration &period)
 Issues commands to the joint. Should be called at regular intervals. More...
 
 ~JointPositionController ()
 
- Public Member Functions inherited from controller_interface::Controller< hardware_interface::EffortJointInterface >
virtual bool init (T *, ros::NodeHandle &)
 
virtual bool init (T *, ros::NodeHandle &, ros::NodeHandle &)
 
- Public Member Functions inherited from controller_interface::ControllerBase
virtual void aborting (const ros::Time &)
 
virtual void aborting (const ros::Time &)
 
bool abortRequest (const ros::Time &time)
 
bool abortRequest (const ros::Time &time)
 
 ControllerBase ()=default
 
 ControllerBase (const ControllerBase &)=delete
 
 ControllerBase (ControllerBase &&)=delete
 
bool isAborted () const
 
bool isAborted () const
 
bool isInitialized () const
 
bool isInitialized () const
 
bool isRunning () const
 
bool isRunning () const
 
bool isStopped () const
 
bool isStopped () const
 
bool isWaiting () const
 
bool isWaiting () const
 
ControllerBaseoperator= (const ControllerBase &)=delete
 
ControllerBaseoperator= (ControllerBase &&)=delete
 
bool startRequest (const ros::Time &time)
 
bool startRequest (const ros::Time &time)
 
virtual void stopping (const ros::Time &)
 
virtual void stopping (const ros::Time &)
 
bool stopRequest (const ros::Time &time)
 
bool stopRequest (const ros::Time &time)
 
void updateRequest (const ros::Time &time, const ros::Duration &period)
 
void updateRequest (const ros::Time &time, const ros::Duration &period)
 
virtual void waiting (const ros::Time &)
 
virtual void waiting (const ros::Time &)
 
bool waitRequest (const ros::Time &time)
 
bool waitRequest (const ros::Time &time)
 
virtual ~ControllerBase ()=default
 

Public Attributes

realtime_tools::RealtimeBuffer< Commandscommand_
 
Commands command_struct_
 
hardware_interface::JointHandle joint_
 
urdf::JointConstSharedPtr joint_urdf_
 
- Public Attributes inherited from controller_interface::ControllerBase
ControllerState state_
 

Private Member Functions

void enforceJointLimits (double &command)
 Check that the command is within the hard limits of the joint. Checks for joint type first. Sets command to limit if out of bounds. More...
 
void setCommandCB (const std_msgs::Float64ConstPtr &msg)
 Callback from /command subscriber for setpoint. More...
 

Private Attributes

std::unique_ptr< realtime_tools::RealtimePublisher< control_msgs::JointControllerState > > controller_state_publisher_
 
int loop_count_
 
control_toolbox::Pid pid_controller_
 
ros::Subscriber sub_command_
 

Additional Inherited Members

- Public Types inherited from controller_interface::ControllerBase
typedef std::vector< hardware_interface::InterfaceResourcesClaimedResources
 
enum  ControllerState {
  ControllerState::CONSTRUCTED, ControllerState::INITIALIZED, ControllerState::RUNNING, ControllerState::STOPPED,
  ControllerState::WAITING, ControllerState::ABORTED
}
 
- Protected Member Functions inherited from controller_interface::Controller< hardware_interface::EffortJointInterface >
std::string getHardwareInterfaceType () const
 
bool initRequest (hardware_interface::RobotHW *robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh, ClaimedResources &claimed_resources) override
 

Detailed Description

Joint Position Controller.

This class controls positon using a pid loop.

ROS interface

Parameters
typeMust be "effort_controllers::JointPositionController"
jointName of the joint to control.
pidContains the gains for the PID loop around position. See: control_toolbox::Pid

Subscribes to:

Publishes:

Definition at line 77 of file joint_position_controller.h.

Constructor & Destructor Documentation

◆ JointPositionController()

effort_controllers::JointPositionController::JointPositionController ( )

Definition at line 81 of file joint_position_controller.cpp.

◆ ~JointPositionController()

effort_controllers::JointPositionController::~JointPositionController ( )

Definition at line 85 of file joint_position_controller.cpp.

Member Function Documentation

◆ enforceJointLimits()

void effort_controllers::JointPositionController::enforceJointLimits ( double &  command)
private

Check that the command is within the hard limits of the joint. Checks for joint type first. Sets command to limit if out of bounds.

Parameters
command- the input to test

Definition at line 286 of file joint_position_controller.cpp.

◆ getGains() [1/2]

void effort_controllers::JointPositionController::getGains ( double &  p,
double &  i,
double &  d,
double &  i_max,
double &  i_min 
)

Get the PID parameters.

Definition at line 141 of file joint_position_controller.cpp.

◆ getGains() [2/2]

void effort_controllers::JointPositionController::getGains ( double &  p,
double &  i,
double &  d,
double &  i_max,
double &  i_min,
bool &  antiwindup 
)

Get the PID parameters.

Definition at line 136 of file joint_position_controller.cpp.

◆ getJointName()

std::string effort_controllers::JointPositionController::getJointName ( )

Get the name of the joint this controller uses.

Definition at line 152 of file joint_position_controller.cpp.

◆ getPosition()

double effort_controllers::JointPositionController::getPosition ( )

Get the current position of the joint.

Returns
current position

Definition at line 157 of file joint_position_controller.cpp.

◆ init()

bool effort_controllers::JointPositionController::init ( hardware_interface::EffortJointInterface robot,
ros::NodeHandle n 
)

The init function is called to initialize the controller from a non-realtime thread with a pointer to the hardware interface, itself, instead of a pointer to a RobotHW.

Parameters
robotThe specific hardware interface used by this controller.
nA NodeHandle in the namespace from which the controller should read its configuration, and where it should set up its ROS interface.
Returns
True if initialization was successful and the controller is ready to be started.

Definition at line 90 of file joint_position_controller.cpp.

◆ printDebug()

void effort_controllers::JointPositionController::printDebug ( )

Print debug info to console.

Definition at line 147 of file joint_position_controller.cpp.

◆ setCommand() [1/2]

void effort_controllers::JointPositionController::setCommand ( double  pos_target)

Give set position of the joint for next update: revolute (angle) and prismatic (position)

Parameters
command

Definition at line 163 of file joint_position_controller.cpp.

◆ setCommand() [2/2]

void effort_controllers::JointPositionController::setCommand ( double  pos_target,
double  vel_target 
)

Give set position of the joint for next update: revolute (angle) and prismatic (position) Also supports a target velocity.

Parameters
pos_target- position setpoint
vel_target- velocity setpoint

Definition at line 175 of file joint_position_controller.cpp.

◆ setCommandCB()

void effort_controllers::JointPositionController::setCommandCB ( const std_msgs::Float64ConstPtr &  msg)
private

Callback from /command subscriber for setpoint.

Definition at line 280 of file joint_position_controller.cpp.

◆ setGains()

void effort_controllers::JointPositionController::setGains ( const double &  p,
const double &  i,
const double &  d,
const double &  i_max,
const double &  i_min,
const bool &  antiwindup = false 
)

Set the PID parameters.

Definition at line 131 of file joint_position_controller.cpp.

◆ starting()

void effort_controllers::JointPositionController::starting ( const ros::Time time)
virtual

This is called from within the realtime thread just before the first call to update.

Parameters
timeThe current time

Reimplemented from controller_interface::ControllerBase.

Definition at line 184 of file joint_position_controller.cpp.

◆ update()

void effort_controllers::JointPositionController::update ( const ros::Time time,
const ros::Duration period 
)
virtual

Issues commands to the joint. Should be called at regular intervals.

Implements controller_interface::ControllerBase.

Definition at line 199 of file joint_position_controller.cpp.

Member Data Documentation

◆ command_

realtime_tools::RealtimeBuffer<Commands> effort_controllers::JointPositionController::command_

Definition at line 170 of file joint_position_controller.h.

◆ command_struct_

Commands effort_controllers::JointPositionController::command_struct_

Definition at line 171 of file joint_position_controller.h.

◆ controller_state_publisher_

std::unique_ptr< realtime_tools::RealtimePublisher< control_msgs::JointControllerState> > effort_controllers::JointPositionController::controller_state_publisher_
private

Definition at line 179 of file joint_position_controller.h.

◆ joint_

hardware_interface::JointHandle effort_controllers::JointPositionController::joint_

Definition at line 168 of file joint_position_controller.h.

◆ joint_urdf_

urdf::JointConstSharedPtr effort_controllers::JointPositionController::joint_urdf_

Definition at line 169 of file joint_position_controller.h.

◆ loop_count_

int effort_controllers::JointPositionController::loop_count_
private

Definition at line 174 of file joint_position_controller.h.

◆ pid_controller_

control_toolbox::Pid effort_controllers::JointPositionController::pid_controller_
private

Internal PID controller.

Definition at line 175 of file joint_position_controller.h.

◆ sub_command_

ros::Subscriber effort_controllers::JointPositionController::sub_command_
private

Definition at line 181 of file joint_position_controller.h.


The documentation for this class was generated from the following files:


effort_controllers
Author(s): Vijay Pradeep
autogenerated on Sun Mar 3 2024 03:19:24