Go to the documentation of this file.
36 #include "kdl/chainfksolvervel_recursive.hpp"
46 CartesianTwistController::CartesianTwistController()
48 jnt_to_twist_solver_(NULL)
64 std::string root_name, tip_name;
66 ROS_ERROR(
"CartesianTwistController: No root name found on parameter server (namespace: %s)",
71 ROS_ERROR(
"CartesianTwistController: No tip name found on parameter server (namespace: %s)",
81 if (!
chain_.
init(robot_state, root_name, tip_name))
100 for (
unsigned int i=0;
i<3;
i++)
105 for (
unsigned int i=0;
i<3;
i++)
122 for (
unsigned int i=0;
i<6;
i++)
158 for (
unsigned int i=0;
i<3;
i++)
161 for (
unsigned int i=0;
i<3;
i++)
165 for (
unsigned int i = 0;
i <
kdl_chain_.getNrOfJoints();
i++){
167 for (
unsigned int j=0; j<6; j++)
void addEfforts(const Vec &v)
void getVelocities(KDL::JntArrayVel &)
bool getParam(const std::string &key, bool &b) const
KDL::JntArrayVel jnt_posvel_
boost::scoped_ptr< KDL::ChainJntToJacSolver > jac_solver_
ros::Subscriber sub_command_
PLUGINLIB_EXPORT_CLASS(my_controller_ns::MyControllerClass, pr2_controller_interface::Controller)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
pr2_mechanism_model::Chain chain_
bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
void toKDL(KDL::Chain &chain)
std::vector< control_toolbox::Pid > fb_pid_controller_
pr2_mechanism_model::RobotState * robot_state_
~CartesianTwistController()
void command(const geometry_msgs::TwistConstPtr &twist_msg)
T param(const std::string ¶m_name, const T &default_val) const
const std::string & getNamespace() const
boost::scoped_ptr< KDL::ChainFkSolverVel > jnt_to_twist_solver_
bool init(RobotState *robot_state, const std::string &root, const std::string &tip)
def error(*args, **kwargs)