Go to the documentation of this file.
35 #include <boost/bind.hpp>
39 #include "kdl/chainfksolverpos_recursive.hpp"
52 CartesianPoseController::CartesianPoseController()
69 ROS_ERROR(
"CartesianPoseController: No root name found on parameter server (namespace: %s)",
74 ROS_ERROR(
"CartesianPoseController: No tip name found on parameter server (namespace: %s)",
103 for (
unsigned int i = 0;
i < 3;
i++)
106 for (
unsigned int i = 0;
i < 3;
i++)
125 for (
unsigned int i=0;
i<6;
i++)
150 KDL::Wrench wrench_desi;
151 for (
unsigned int i=0;
i<6;
i++)
158 for (
unsigned int i = 0;
i <
kdl_chain_.getNrOfJoints();
i++){
160 for (
unsigned int j=0; j<6; j++)
209 poseStampedMsgToTF(*pose_msg, pose_stamped);
boost::scoped_ptr< tf::MessageFilter< geometry_msgs::PoseStamped > > command_filter_
void addEfforts(const Vec &v)
void poseTFToKDL(const tf::Pose &t, KDL::Frame &k)
tf::TransformListener tf_
bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
message_filters::Subscriber< geometry_msgs::PoseStamped > sub_command_
bool getParam(const std::string &key, bool &b) const
pr2_mechanism_model::RobotState * robot_state_
pr2_mechanism_model::Chain chain_
~CartesianPoseController()
void getPositions(KDL::JntArray &)
boost::scoped_ptr< KDL::ChainFkSolverPos > jnt_to_pose_solver_
boost::scoped_ptr< realtime_tools::RealtimePublisher< geometry_msgs::PoseStamped > > state_pose_publisher_
boost::scoped_ptr< KDL::ChainJntToJacSolver > jac_solver_
std::vector< control_toolbox::Pid > pid_controller_
void poseKDLToTF(const KDL::Frame &k, tf::Pose &t)
PLUGINLIB_EXPORT_CLASS(my_controller_ns::MyControllerClass, pr2_controller_interface::Controller)
boost::scoped_ptr< realtime_tools::RealtimePublisher< geometry_msgs::Twist > > state_error_publisher_
void toKDL(KDL::Chain &chain)
const std::string & getNamespace() const
void command(const geometry_msgs::PoseStamped::ConstPtr &pose_msg)
bool init(RobotState *robot_state, const std::string &root, const std::string &tip)