#include <sr_controller.hpp>
Public Member Functions | |
void | getCommand (double &cmd) |
Get latest position command to the joint: revolute (angle) and prismatic (position). | |
virtual void | getGains (double &p, double &i, double &d, double &i_max, double &i_min) |
std::string | getJointName () |
virtual bool | init (pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n) |
virtual bool | resetGains (std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp) |
void | setCommand (double cmd) |
Give set position of the joint for next update: revolute (angle) and prismatic (position) | |
virtual void | setCommandCB (const std_msgs::Float64ConstPtr &msg) |
SrController () | |
virtual void | starting () |
virtual void | update () |
Issues commands to the joint. Should be called at regular intervals. | |
virtual | ~SrController () |
Public Attributes | |
double | command_ |
ros::Duration | dt_ |
bool | has_j2 |
pr2_mechanism_model::JointState * | joint_state_ |
pr2_mechanism_model::JointState * | joint_state_2 |
Protected Member Functions | |
void | after_init () |
call this function at the end of the init function in the inheriting classes. | |
double | clamp_command (double cmd) |
void | get_min_max (urdf::Model model, std::string joint_name) |
void | maxForceFactorCB (const std_msgs::Float64ConstPtr &msg) |
Protected Attributes | |
boost::scoped_ptr < realtime_tools::RealtimePublisher < pr2_controllers_msgs::JointControllerState > > | controller_state_publisher_ |
boost::shared_ptr < sr_friction_compensation::SrFrictionCompensator > | friction_compensator |
int | friction_deadband |
the deadband for the friction compensation algorithm | |
sr_deadband::HysteresisDeadband < double > | hysteresis_deadband |
We're using an hysteresis deadband. | |
bool | initialized_ |
ros::Time | last_time_ |
int | loop_count_ |
double | max_ |
double | max_force_demand |
clamps the force demand to this value | |
double | max_force_factor_ |
double | min_ |
Min and max range of the joint, used to clamp the command. | |
ros::NodeHandle | n_tilde_ |
ros::NodeHandle | node_ |
pr2_mechanism_model::RobotState * | robot_ |
ros::ServiceServer | serve_reset_gains_ |
ros::ServiceServer | serve_set_gains_ |
ros::Subscriber | sub_command_ |
ros::Subscriber | sub_max_force_factor_ |
Definition at line 53 of file sr_controller.hpp.
Definition at line 39 of file sr_controller.cpp.
controller::SrController::~SrController | ( | ) | [virtual] |
Definition at line 48 of file sr_controller.cpp.
void controller::SrController::after_init | ( | ) | [protected] |
call this function at the end of the init function in the inheriting classes.
Definition at line 53 of file sr_controller.cpp.
double controller::SrController::clamp_command | ( | double | cmd | ) | [protected] |
Clamps the command to the correct range (between min and max).
cmd | the command we want to clamp |
Definition at line 124 of file sr_controller.cpp.
void controller::SrController::get_min_max | ( | urdf::Model | model, |
std::string | joint_name | ||
) | [protected] |
Reads the min and max from the robot model for the current joint.
model | the urdf description of the robot |
joint_name | the name of the joint |
Definition at line 102 of file sr_controller.cpp.
void controller::SrController::getCommand | ( | double & | cmd | ) |
Get latest position command to the joint: revolute (angle) and prismatic (position).
Definition at line 72 of file sr_controller.cpp.
void controller::SrController::getGains | ( | double & | p, |
double & | i, | ||
double & | d, | ||
double & | i_max, | ||
double & | i_min | ||
) | [virtual] |
Reimplemented in controller::SrhMixedPositionVelocityJointController, controller::SrhJointMuscleValveController, controller::SrhJointVelocityController, controller::SrhEffortJointController, controller::SrhJointPositionController, and controller::SrhMuscleJointPositionController.
Definition at line 99 of file sr_controller.cpp.
std::string controller::SrController::getJointName | ( | ) |
Definition at line 60 of file sr_controller.cpp.
bool controller::SrController::init | ( | pr2_mechanism_model::RobotState * | robot, |
ros::NodeHandle & | n | ||
) | [virtual] |
Implements pr2_controller_interface::Controller.
Reimplemented in controller::SrhExampleController, controller::SrhMixedPositionVelocityJointController, controller::SrhJointMuscleValveController, controller::SrhJointVelocityController, controller::SrhSyntouchController, controller::SrhEffortJointController, controller::SrhJointPositionController, and controller::SrhMuscleJointPositionController.
Definition at line 82 of file sr_controller.cpp.
void controller::SrController::maxForceFactorCB | ( | const std_msgs::Float64ConstPtr & | msg | ) | [protected] |
Callback function for the max force factor topic
msg | the message receiver over the topic |
Definition at line 135 of file sr_controller.cpp.
bool controller::SrController::resetGains | ( | std_srvs::Empty::Request & | req, |
std_srvs::Empty::Response & | resp | ||
) | [virtual] |
Reimplemented in controller::SrhMixedPositionVelocityJointController, controller::SrhJointMuscleValveController, controller::SrhJointVelocityController, controller::SrhEffortJointController, controller::SrhJointPositionController, and controller::SrhMuscleJointPositionController.
Definition at line 94 of file sr_controller.cpp.
void controller::SrController::setCommand | ( | double | cmd | ) |
Give set position of the joint for next update: revolute (angle) and prismatic (position)
cmd | the received angle |
Definition at line 66 of file sr_controller.cpp.
void controller::SrController::setCommandCB | ( | const std_msgs::Float64ConstPtr & | msg | ) | [virtual] |
Definition at line 77 of file sr_controller.cpp.
void controller::SrController::starting | ( | ) | [virtual] |
Reimplemented from pr2_controller_interface::Controller.
Reimplemented in controller::SrhExampleController, controller::SrhMixedPositionVelocityJointController, controller::SrhJointMuscleValveController, controller::SrhJointVelocityController, controller::SrhSyntouchController, controller::SrhEffortJointController, controller::SrhJointPositionController, and controller::SrhMuscleJointPositionController.
Definition at line 91 of file sr_controller.cpp.
void controller::SrController::update | ( | void | ) | [virtual] |
Issues commands to the joint. Should be called at regular intervals.
Implements pr2_controller_interface::Controller.
Reimplemented in controller::SrhExampleController, controller::SrhMixedPositionVelocityJointController, controller::SrhJointMuscleValveController, controller::SrhJointVelocityController, controller::SrhSyntouchController, controller::SrhEffortJointController, controller::SrhJointPositionController, and controller::SrhMuscleJointPositionController.
Definition at line 87 of file sr_controller.cpp.
Last commanded position.
Definition at line 91 of file sr_controller.hpp.
boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_controllers_msgs::JointControllerState> > controller::SrController::controller_state_publisher_ [protected] |
Reimplemented in controller::SrhJointMuscleValveController, controller::SrhMixedPositionVelocityJointController, controller::SrhMuscleJointPositionController, and controller::SrhSyntouchController.
Definition at line 128 of file sr_controller.hpp.
Definition at line 90 of file sr_controller.hpp.
boost::shared_ptr<sr_friction_compensation::SrFrictionCompensator> controller::SrController::friction_compensator [protected] |
Definition at line 130 of file sr_controller.hpp.
int controller::SrController::friction_deadband [protected] |
the deadband for the friction compensation algorithm
Definition at line 139 of file sr_controller.hpp.
true if this is a joint 0.
Definition at line 89 of file sr_controller.hpp.
sr_deadband::HysteresisDeadband<double> controller::SrController::hysteresis_deadband [protected] |
We're using an hysteresis deadband.
Reimplemented in controller::SrhMixedPositionVelocityJointController, controller::SrhMuscleJointPositionController, controller::SrhJointPositionController, and controller::SrhJointVelocityController.
Definition at line 142 of file sr_controller.hpp.
bool controller::SrController::initialized_ [protected] |
Definition at line 120 of file sr_controller.hpp.
Joint we're controlling.
Definition at line 87 of file sr_controller.hpp.
2ndJoint we're controlling if joint 0.
Definition at line 88 of file sr_controller.hpp.
ros::Time controller::SrController::last_time_ [protected] |
Last time stamp of update.
Definition at line 122 of file sr_controller.hpp.
int controller::SrController::loop_count_ [protected] |
Definition at line 119 of file sr_controller.hpp.
double controller::SrController::max_ [protected] |
Definition at line 117 of file sr_controller.hpp.
double controller::SrController::max_force_demand [protected] |
clamps the force demand to this value
Reimplemented in controller::SrhMuscleJointPositionController, and controller::SrhJointPositionController.
Definition at line 137 of file sr_controller.hpp.
double controller::SrController::max_force_factor_ [protected] |
Definition at line 147 of file sr_controller.hpp.
double controller::SrController::min_ [protected] |
Min and max range of the joint, used to clamp the command.
Definition at line 117 of file sr_controller.hpp.
ros::NodeHandle controller::SrController::n_tilde_ [protected] |
Definition at line 124 of file sr_controller.hpp.
ros::NodeHandle controller::SrController::node_ [protected] |
Definition at line 124 of file sr_controller.hpp.
Pointer to robot structure.
Definition at line 121 of file sr_controller.hpp.
Definition at line 134 of file sr_controller.hpp.
Reimplemented in controller::SrhMixedPositionVelocityJointController.
Definition at line 133 of file sr_controller.hpp.
Reimplemented in controller::SrhJointMuscleValveController.
Definition at line 132 of file sr_controller.hpp.
Definition at line 148 of file sr_controller.hpp.