Classes | Public Member Functions | Private Member Functions | Private Attributes
r2_controller_ns::R2ImpedanceController Class Reference

#include <r2_impedance_controller.h>

Inheritance diagram for r2_controller_ns::R2ImpedanceController:
Inheritance graph
[legend]

List of all members.

Classes

class  CtrlCalc

Public Member Functions

bool init (pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &n)
virtual void starting ()
 occurs before first update() call
virtual void update ()
 performs one iteration of control, main function

Private Member Functions

std::vector< double > getGainParams (const std::vector< std::string > &param_names, const std::string &param_name)
 load gains from yaml file, used in load_params
void init_ros_msgs ()
void joint_command (const sensor_msgs::JointState::ConstPtr &msg)
void joint_command_entry (const std::string &name, double value, std::vector< double > &desired)
void joint_command_entry (const std::string &name, bool value, std::vector< int > &desired)
void joint_left_command (const sensor_msgs::JointState::ConstPtr &msg)
void joint_neck_command (const sensor_msgs::JointState::ConstPtr &msg)
void joint_right_command (const sensor_msgs::JointState::ConstPtr &msg)
void joint_waist_command (const sensor_msgs::JointState::ConstPtr &msg)
void load_params ()
 loads gains
void pose_left_command (const geometry_msgs::PoseStamped::ConstPtr &msg)
void pose_right_command (const geometry_msgs::PoseStamped::ConstPtr &msg)
void pose_vel_command_inner (const r2_msgs::PoseTwistStamped::ConstPtr &msg, Eigen::Matrix< double, 7, 1 > &cmd, KDL::Twist &velCmd, bool &cart_vel)
void pose_vel_left_command (const r2_msgs::PoseTwistStamped::ConstPtr &msg)
void pose_vel_right_command (const r2_msgs::PoseTwistStamped::ConstPtr &msg)
void publish_msgs ()
void set_gains (const r2_msgs::Gains::ConstPtr &msg)
bool set_joint_mode (r2_msgs::SetJointMode::Request &req, r2_msgs::SetJointMode::Response &res)
bool set_power (r2_msgs::Power::Request &req, r2_msgs::Power::Response &res)
bool set_servo (r2_msgs::Servo::Request &req, r2_msgs::Servo::Response &res)
bool set_tip_name (r2_msgs::SetTipName::Request &req, r2_msgs::SetTipName::Response &res)
KDL::Frame transformPoseMsg (const geometry_msgs::PoseStamped::ConstPtr &msg)

Private Attributes

CtrlCalc cc
boost::scoped_ptr
< realtime_tools::RealtimePublisher
< r2_msgs::Gains > > 		gains_publisher
ros::Subscriber joint_command_sub
ros::Subscriber left_joint_command_sub
boost::scoped_ptr
< tf::MessageFilter
< geometry_msgs::PoseStamped > > 		left_pose_command_filter
message_filters::Subscriber
< geometry_msgs::PoseStamped > 		left_pose_command_sub
boost::scoped_ptr
< realtime_tools::RealtimePublisher
< geometry_msgs::Twist > > 		left_pose_error_publisher
boost::scoped_ptr
< tf::MessageFilter
< r2_msgs::PoseTwistStamped > > 		left_pose_vel_command_filter
message_filters::Subscriber
< r2_msgs::PoseTwistStamped > 		left_pose_vel_command_sub
boost::scoped_ptr
< realtime_tools::RealtimePublisher
< geometry_msgs::PoseStamped > > 		left_tip_pose_publisher
ros::Subscriber neck_joint_command_sub
ros::NodeHandle node
ros::Subscriber right_joint_command_sub
boost::scoped_ptr
< tf::MessageFilter
< geometry_msgs::PoseStamped > > 		right_pose_command_filter
message_filters::Subscriber
< geometry_msgs::PoseStamped > 		right_pose_command_sub
boost::scoped_ptr
< realtime_tools::RealtimePublisher
< geometry_msgs::Twist > > 		right_pose_error_publisher
boost::scoped_ptr
< tf::MessageFilter
< r2_msgs::PoseTwistStamped > > 		right_pose_vel_command_filter
message_filters::Subscriber
< r2_msgs::PoseTwistStamped > 		right_pose_vel_command_sub
boost::scoped_ptr
< realtime_tools::RealtimePublisher
< geometry_msgs::PoseStamped > > 		right_tip_pose_publisher
pr2_mechanism_model::RobotState * robot_state
std::vector
< pr2_mechanism_model::JointState * > 		robotStateJoints
ros::Subscriber set_gains_sub
ros::ServiceServer srv_set_joint_mode
ros::ServiceServer srv_set_power
ros::ServiceServer srv_set_servo
ros::ServiceServer srv_set_tip_name
tf::TransformListener tfListener
boost::mutex thread_mutex
ros::Time time
ros::Subscriber waist_joint_command_sub

Detailed Description

Definition at line 79 of file r2_impedance_controller.h.

Member Function Documentation

vector< double > R2ImpedanceController::getGainParams ( const std::vector< std::string > &  param_names,
const std::string &  param_name 
) [private]

load gains from yaml file, used in load_params

Definition at line 319 of file r2_impedance_controller.cpp.

bool R2ImpedanceController::init ( pr2_mechanism_model::RobotState *  robot_state,
ros::NodeHandle &  n 
) [virtual]

Implements pr2_controller_interface::Controller.

Definition at line 93 of file r2_impedance_controller.cpp.

void R2ImpedanceController::init_ros_msgs ( ) [private]

Definition at line 348 of file r2_impedance_controller.cpp.

void R2ImpedanceController::joint_command ( const sensor_msgs::JointState::ConstPtr &  msg) [private]

Definition at line 541 of file r2_impedance_controller.cpp.

void r2_controller_ns::R2ImpedanceController::joint_command_entry ( const std::string &  name,
double  value,
std::vector< double > &  desired 
) [private]
void r2_controller_ns::R2ImpedanceController::joint_command_entry ( const std::string &  name,
bool  value,
std::vector< int > &  desired 
) [private]
void R2ImpedanceController::joint_left_command ( const sensor_msgs::JointState::ConstPtr &  msg) [private]

Definition at line 399 of file r2_impedance_controller.cpp.

void R2ImpedanceController::joint_neck_command ( const sensor_msgs::JointState::ConstPtr &  msg) [private]

Definition at line 401 of file r2_impedance_controller.cpp.

void R2ImpedanceController::joint_right_command ( const sensor_msgs::JointState::ConstPtr &  msg) [private]

Definition at line 400 of file r2_impedance_controller.cpp.

void R2ImpedanceController::joint_waist_command ( const sensor_msgs::JointState::ConstPtr &  msg) [private]

Definition at line 402 of file r2_impedance_controller.cpp.

void R2ImpedanceController::load_params ( ) [private]

loads gains

helps put the named values from getGainParams into the correct index

Definition at line 252 of file r2_impedance_controller.cpp.

void R2ImpedanceController::pose_left_command ( const geometry_msgs::PoseStamped::ConstPtr &  msg) [private]

Definition at line 403 of file r2_impedance_controller.cpp.

void R2ImpedanceController::pose_right_command ( const geometry_msgs::PoseStamped::ConstPtr &  msg) [private]

Definition at line 416 of file r2_impedance_controller.cpp.

void R2ImpedanceController::pose_vel_command_inner ( const r2_msgs::PoseTwistStamped::ConstPtr &  msg,
Eigen::Matrix< double, 7, 1 > &  cmd,
KDL::Twist &  velCmd,
bool &  cart_vel 
) [private]

Definition at line 435 of file r2_impedance_controller.cpp.

void R2ImpedanceController::pose_vel_left_command ( const r2_msgs::PoseTwistStamped::ConstPtr &  msg) [private]

Definition at line 429 of file r2_impedance_controller.cpp.

void R2ImpedanceController::pose_vel_right_command ( const r2_msgs::PoseTwistStamped::ConstPtr &  msg) [private]

Definition at line 432 of file r2_impedance_controller.cpp.

void R2ImpedanceController::publish_msgs ( ) [private]

Definition at line 895 of file r2_impedance_controller.cpp.

void R2ImpedanceController::set_gains ( const r2_msgs::Gains::ConstPtr &  msg) [private]

need to handle cart gains differently since they aren't indexed into the usual arrays

Definition at line 494 of file r2_impedance_controller.cpp.

bool R2ImpedanceController::set_joint_mode ( r2_msgs::SetJointMode::Request &  req,
r2_msgs::SetJointMode::Response &  res 
) [private]

Definition at line 703 of file r2_impedance_controller.cpp.

bool R2ImpedanceController::set_power ( r2_msgs::Power::Request &  req,
r2_msgs::Power::Response &  res 
) [private]

Definition at line 748 of file r2_impedance_controller.cpp.

bool R2ImpedanceController::set_servo ( r2_msgs::Servo::Request &  req,
r2_msgs::Servo::Response &  res 
) [private]

Definition at line 749 of file r2_impedance_controller.cpp.

bool R2ImpedanceController::set_tip_name ( r2_msgs::SetTipName::Request &  req,
r2_msgs::SetTipName::Response &  res 
) [private]

Definition at line 733 of file r2_impedance_controller.cpp.

virtual void r2_controller_ns::R2ImpedanceController::starting ( ) [inline, virtual]

occurs before first update() call

Reimplemented from pr2_controller_interface::Controller.

Definition at line 278 of file r2_impedance_controller.h.

KDL::Frame R2ImpedanceController::transformPoseMsg ( const geometry_msgs::PoseStamped::ConstPtr &  msg) [private]

Definition at line 485 of file r2_impedance_controller.cpp.

void R2ImpedanceController::update ( void  ) [virtual]

performs one iteration of control, main function

update section

read current joint values

seems like j5,j6 have some instability in position. explicit calculation seems to cause large swings in velocity

we can get NaN here for some reason

calculate section

send commands to joints

Implements pr2_controller_interface::Controller.

Definition at line 798 of file r2_impedance_controller.cpp.

Member Data Documentation

CtrlCalc r2_controller_ns::R2ImpedanceController::cc [private]

Definition at line 217 of file r2_impedance_controller.h.

boost::scoped_ptr<realtime_tools::RealtimePublisher<r2_msgs::Gains> > r2_controller_ns::R2ImpedanceController::gains_publisher [private]

Definition at line 109 of file r2_impedance_controller.h.

ros::Subscriber r2_controller_ns::R2ImpedanceController::joint_command_sub [private]

Definition at line 86 of file r2_impedance_controller.h.

ros::Subscriber r2_controller_ns::R2ImpedanceController::left_joint_command_sub [private]

Definition at line 87 of file r2_impedance_controller.h.

boost::scoped_ptr<tf::MessageFilter<geometry_msgs::PoseStamped> > r2_controller_ns::R2ImpedanceController::left_pose_command_filter [private]

Definition at line 90 of file r2_impedance_controller.h.

message_filters::Subscriber<geometry_msgs::PoseStamped> r2_controller_ns::R2ImpedanceController::left_pose_command_sub [private]

Definition at line 89 of file r2_impedance_controller.h.

boost::scoped_ptr<realtime_tools::RealtimePublisher<geometry_msgs::Twist> > r2_controller_ns::R2ImpedanceController::left_pose_error_publisher [private]

Definition at line 85 of file r2_impedance_controller.h.

boost::scoped_ptr<tf::MessageFilter<r2_msgs::PoseTwistStamped> > r2_controller_ns::R2ImpedanceController::left_pose_vel_command_filter [private]

Definition at line 92 of file r2_impedance_controller.h.

message_filters::Subscriber<r2_msgs::PoseTwistStamped> r2_controller_ns::R2ImpedanceController::left_pose_vel_command_sub [private]

Definition at line 91 of file r2_impedance_controller.h.

boost::scoped_ptr<realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped> > r2_controller_ns::R2ImpedanceController::left_tip_pose_publisher [private]

Definition at line 84 of file r2_impedance_controller.h.

ros::Subscriber r2_controller_ns::R2ImpedanceController::neck_joint_command_sub [private]

Definition at line 105 of file r2_impedance_controller.h.

ros::NodeHandle r2_controller_ns::R2ImpedanceController::node [private]

Definition at line 82 of file r2_impedance_controller.h.

ros::Subscriber r2_controller_ns::R2ImpedanceController::right_joint_command_sub [private]

Definition at line 96 of file r2_impedance_controller.h.

boost::scoped_ptr<tf::MessageFilter<geometry_msgs::PoseStamped> > r2_controller_ns::R2ImpedanceController::right_pose_command_filter [private]

Definition at line 99 of file r2_impedance_controller.h.

message_filters::Subscriber<geometry_msgs::PoseStamped> r2_controller_ns::R2ImpedanceController::right_pose_command_sub [private]

Definition at line 98 of file r2_impedance_controller.h.

boost::scoped_ptr<realtime_tools::RealtimePublisher<geometry_msgs::Twist> > r2_controller_ns::R2ImpedanceController::right_pose_error_publisher [private]

Definition at line 95 of file r2_impedance_controller.h.

boost::scoped_ptr<tf::MessageFilter<r2_msgs::PoseTwistStamped> > r2_controller_ns::R2ImpedanceController::right_pose_vel_command_filter [private]

Definition at line 101 of file r2_impedance_controller.h.

message_filters::Subscriber<r2_msgs::PoseTwistStamped> r2_controller_ns::R2ImpedanceController::right_pose_vel_command_sub [private]

Definition at line 100 of file r2_impedance_controller.h.

boost::scoped_ptr<realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped> > r2_controller_ns::R2ImpedanceController::right_tip_pose_publisher [private]

Definition at line 94 of file r2_impedance_controller.h.

pr2_mechanism_model::RobotState* r2_controller_ns::R2ImpedanceController::robot_state [private]

Definition at line 225 of file r2_impedance_controller.h.

std::vector<pr2_mechanism_model::JointState*> r2_controller_ns::R2ImpedanceController::robotStateJoints [private]

Definition at line 222 of file r2_impedance_controller.h.

ros::Subscriber r2_controller_ns::R2ImpedanceController::set_gains_sub [private]

Definition at line 108 of file r2_impedance_controller.h.

ros::ServiceServer r2_controller_ns::R2ImpedanceController::srv_set_joint_mode [private]

Definition at line 111 of file r2_impedance_controller.h.

ros::ServiceServer r2_controller_ns::R2ImpedanceController::srv_set_power [private]

Definition at line 113 of file r2_impedance_controller.h.

ros::ServiceServer r2_controller_ns::R2ImpedanceController::srv_set_servo [private]

Definition at line 114 of file r2_impedance_controller.h.

ros::ServiceServer r2_controller_ns::R2ImpedanceController::srv_set_tip_name [private]

Definition at line 112 of file r2_impedance_controller.h.

tf::TransformListener r2_controller_ns::R2ImpedanceController::tfListener [private]

Definition at line 83 of file r2_impedance_controller.h.

boost::mutex r2_controller_ns::R2ImpedanceController::thread_mutex [private]

Definition at line 269 of file r2_impedance_controller.h.

ros::Time r2_controller_ns::R2ImpedanceController::time [private]

Definition at line 226 of file r2_impedance_controller.h.

ros::Subscriber r2_controller_ns::R2ImpedanceController::waist_joint_command_sub [private]

Definition at line 106 of file r2_impedance_controller.h.

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

r2_controllers_gazebo
Author(s): Stephen Hart
autogenerated on Thu Jan 2 2014 11:31:58