#include <cartesian_pose_controller.h>
Public Member Functions | |
CartesianPoseController () | |
void | command (const geometry_msgs::PoseStamped::ConstPtr &pose_msg) |
bool | init (pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n) |
void | starting () |
void | update () |
~CartesianPoseController () | |
Public Attributes | |
KDL::Frame | pose_desi_ |
KDL::Frame | pose_meas_ |
KDL::Twist | twist_error_ |
KDL::Twist | twist_ff_ |
Private Member Functions | |
KDL::Frame | getPose () |
Private Attributes | |
pr2_mechanism_model::Chain | chain_ |
boost::scoped_ptr < tf::MessageFilter < geometry_msgs::PoseStamped > > | command_filter_ |
std::string | controller_name_ |
boost::scoped_ptr < KDL::ChainJntToJacSolver > | jac_solver_ |
KDL::Jacobian | jacobian_ |
KDL::JntArray | jnt_eff_ |
KDL::JntArray | jnt_pos_ |
boost::scoped_ptr < KDL::ChainFkSolverPos > | jnt_to_pose_solver_ |
KDL::Chain | kdl_chain_ |
ros::Time | last_time_ |
unsigned int | loop_count_ |
ros::NodeHandle | node_ |
std::vector< control_toolbox::Pid > | pid_controller_ |
pr2_mechanism_model::RobotState * | robot_state_ |
std::string | root_name_ |
boost::scoped_ptr < realtime_tools::RealtimePublisher < geometry_msgs::Twist > > | state_error_publisher_ |
boost::scoped_ptr < realtime_tools::RealtimePublisher < geometry_msgs::PoseStamped > > | state_pose_publisher_ |
message_filters::Subscriber < geometry_msgs::PoseStamped > | sub_command_ |
tf::TransformListener | tf_ |
Definition at line 88 of file cartesian_pose_controller.h.
controller::CartesianPoseController::CartesianPoseController | ( | ) |
Definition at line 51 of file cartesian_pose_controller.cpp.
controller::CartesianPoseController::~CartesianPoseController | ( | ) |
Definition at line 55 of file cartesian_pose_controller.cpp.
void controller::CartesianPoseController::command | ( | const geometry_msgs::PoseStamped::ConstPtr & | pose_msg | ) |
Definition at line 204 of file cartesian_pose_controller.cpp.
Frame controller::CartesianPoseController::getPose | ( | ) | [private] |
Definition at line 192 of file cartesian_pose_controller.cpp.
bool controller::CartesianPoseController::init | ( | pr2_mechanism_model::RobotState * | robot, | |
ros::NodeHandle & | n | |||
) |
Definition at line 61 of file cartesian_pose_controller.cpp.
void controller::CartesianPoseController::starting | ( | ) |
Definition at line 121 of file cartesian_pose_controller.cpp.
void controller::CartesianPoseController::update | ( | ) |
Definition at line 137 of file cartesian_pose_controller.cpp.
pr2_mechanism_model::Chain controller::CartesianPoseController::chain_ [private] |
Definition at line 113 of file cartesian_pose_controller.h.
boost::scoped_ptr<tf::MessageFilter<geometry_msgs::PoseStamped> > controller::CartesianPoseController::command_filter_ [private] |
Definition at line 133 of file cartesian_pose_controller.h.
std::string controller::CartesianPoseController::controller_name_ [private] |
Definition at line 108 of file cartesian_pose_controller.h.
boost::scoped_ptr<KDL::ChainJntToJacSolver> controller::CartesianPoseController::jac_solver_ [private] |
Definition at line 121 of file cartesian_pose_controller.h.
KDL::Jacobian controller::CartesianPoseController::jacobian_ [private] |
Definition at line 124 of file cartesian_pose_controller.h.
KDL::JntArray controller::CartesianPoseController::jnt_eff_ [private] |
Definition at line 123 of file cartesian_pose_controller.h.
KDL::JntArray controller::CartesianPoseController::jnt_pos_ [private] |
Definition at line 122 of file cartesian_pose_controller.h.
boost::scoped_ptr<KDL::ChainFkSolverPos> controller::CartesianPoseController::jnt_to_pose_solver_ [private] |
Definition at line 120 of file cartesian_pose_controller.h.
KDL::Chain controller::CartesianPoseController::kdl_chain_ [private] |
Definition at line 119 of file cartesian_pose_controller.h.
ros::Time controller::CartesianPoseController::last_time_ [private] |
Definition at line 109 of file cartesian_pose_controller.h.
unsigned int controller::CartesianPoseController::loop_count_ [private] |
Definition at line 129 of file cartesian_pose_controller.h.
ros::NodeHandle controller::CartesianPoseController::node_ [private] |
Definition at line 107 of file cartesian_pose_controller.h.
std::vector<control_toolbox::Pid> controller::CartesianPoseController::pid_controller_ [private] |
Definition at line 116 of file cartesian_pose_controller.h.
Definition at line 98 of file cartesian_pose_controller.h.
Definition at line 98 of file cartesian_pose_controller.h.
pr2_mechanism_model::RobotState* controller::CartesianPoseController::robot_state_ [private] |
Definition at line 112 of file cartesian_pose_controller.h.
std::string controller::CartesianPoseController::root_name_ [private] |
Definition at line 108 of file cartesian_pose_controller.h.
boost::scoped_ptr<realtime_tools::RealtimePublisher<geometry_msgs::Twist> > controller::CartesianPoseController::state_error_publisher_ [private] |
Definition at line 127 of file cartesian_pose_controller.h.
boost::scoped_ptr<realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped> > controller::CartesianPoseController::state_pose_publisher_ [private] |
Definition at line 128 of file cartesian_pose_controller.h.
message_filters::Subscriber<geometry_msgs::PoseStamped> controller::CartesianPoseController::sub_command_ [private] |
Definition at line 132 of file cartesian_pose_controller.h.
tf::TransformListener controller::CartesianPoseController::tf_ [private] |
Definition at line 131 of file cartesian_pose_controller.h.
Definition at line 102 of file cartesian_pose_controller.h.
Definition at line 99 of file cartesian_pose_controller.h.