#include <joint_position_controller.h>
Public Member Functions | |
void | getCommand (double &cmd) |
Get latest position command to the joint: revolute (angle) and prismatic (position). More... | |
void | getGains (double &p, double &i, double &d, double &i_max, double &i_min) |
std::string | getJointName () |
bool | init (pr2_mechanism_model::RobotState *robot, const std::string &joint_name, const control_toolbox::Pid &pid) |
bool | init (pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n) |
JointPositionController () | |
void | setCommand (double cmd) |
Give set position of the joint for next update: revolute (angle) and prismatic (position) More... | |
void | setGains (const double &p, const double &i, const double &d, const double &i_max, const double &i_min) |
virtual void | starting () |
virtual void | update () |
Issues commands to the joint. Should be called at regular intervals. More... | |
~JointPositionController () | |
Public Member Functions inherited from pr2_controller_interface::Controller | |
Controller () | |
bool | getController (const std::string &name, int sched, ControllerType *&c) |
bool | initRequest (ControllerProvider *cp, pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n) |
bool | isRunning () |
void | starting (const ros::Time &time) |
bool | startRequest () |
void | stopping (const ros::Time &time) |
virtual void | stopping () |
bool | stopRequest () |
void | update (const ros::Time &time, const ros::Duration &period) |
void | updateRequest () |
virtual | ~Controller () |
Public Attributes | |
double | command_ |
ros::Duration | dt_ |
pr2_mechanism_model::JointState * | joint_state_ |
Public Attributes inherited from pr2_controller_interface::Controller | |
std::vector< std::string > | after_list_ |
AFTER_ME | |
std::vector< std::string > | before_list_ |
BEFORE_ME | |
CONSTRUCTED | |
INITIALIZED | |
RUNNING | |
enum pr2_controller_interface::Controller:: { ... } | state_ |
Private Member Functions | |
void | setCommandCB (const std_msgs::Float64ConstPtr &msg) |
Private Attributes | |
boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_controllers_msgs::JointControllerState > > | controller_state_publisher_ |
bool | initialized_ |
ros::Time | last_time_ |
int | loop_count_ |
ros::NodeHandle | node_ |
control_toolbox::Pid | pid_controller_ |
pr2_mechanism_model::RobotState * | robot_ |
ros::Subscriber | sub_command_ |
Definition at line 75 of file joint_position_controller.h.
controller::JointPositionController::JointPositionController | ( | ) |
Definition at line 45 of file joint_position_controller.cpp.
controller::JointPositionController::~JointPositionController | ( | ) |
Definition at line 51 of file joint_position_controller.cpp.
void controller::JointPositionController::getCommand | ( | double & | cmd | ) |
Get latest position command to the joint: revolute (angle) and prismatic (position).
Definition at line 128 of file joint_position_controller.cpp.
void controller::JointPositionController::getGains | ( | double & | p, |
double & | i, | ||
double & | d, | ||
double & | i_max, | ||
double & | i_min | ||
) |
Definition at line 111 of file joint_position_controller.cpp.
std::string controller::JointPositionController::getJointName | ( | ) |
Definition at line 116 of file joint_position_controller.cpp.
bool controller::JointPositionController::init | ( | pr2_mechanism_model::RobotState * | robot, |
const std::string & | joint_name, | ||
const control_toolbox::Pid & | pid | ||
) |
Definition at line 56 of file joint_position_controller.cpp.
|
virtual |
Implements pr2_controller_interface::Controller.
Definition at line 81 of file joint_position_controller.cpp.
void controller::JointPositionController::setCommand | ( | double | cmd | ) |
Give set position of the joint for next update: revolute (angle) and prismatic (position)
command |
Definition at line 122 of file joint_position_controller.cpp.
|
private |
Definition at line 200 of file joint_position_controller.cpp.
void controller::JointPositionController::setGains | ( | const double & | p, |
const double & | i, | ||
const double & | d, | ||
const double & | i_max, | ||
const double & | i_min | ||
) |
Definition at line 106 of file joint_position_controller.cpp.
|
inlinevirtual |
Reimplemented from pr2_controller_interface::Controller.
Definition at line 97 of file joint_position_controller.h.
|
virtual |
Issues commands to the joint. Should be called at regular intervals.
Implements pr2_controller_interface::Controller.
Definition at line 133 of file joint_position_controller.cpp.
double controller::JointPositionController::command_ |
Last commanded position.
Definition at line 113 of file joint_position_controller.h.
|
private |
Definition at line 127 of file joint_position_controller.h.
ros::Duration controller::JointPositionController::dt_ |
Definition at line 112 of file joint_position_controller.h.
|
private |
Definition at line 117 of file joint_position_controller.h.
pr2_mechanism_model::JointState* controller::JointPositionController::joint_state_ |
Joint we're controlling.
Definition at line 111 of file joint_position_controller.h.
|
private |
Last time stamp of update.
Definition at line 120 of file joint_position_controller.h.
|
private |
Definition at line 116 of file joint_position_controller.h.
|
private |
Definition at line 123 of file joint_position_controller.h.
|
private |
Internal PID controller.
Definition at line 119 of file joint_position_controller.h.
|
private |
Pointer to robot structure.
Definition at line 118 of file joint_position_controller.h.
|
private |
Definition at line 129 of file joint_position_controller.h.