Go to the documentation of this file.
37 #ifndef PR2_CONTROLLERS_JT_CARTESIAN_CONTROLLER_H
38 #define PR2_CONTROLLERS_JT_CARTESIAN_CONTROLLER_H
40 #include <boost/scoped_ptr.hpp>
43 #include <Eigen/Geometry>
50 #include <kdl/chainfksolver.hpp>
51 #include <kdl/chainfksolverpos_recursive.hpp>
52 #include <kdl/chain.hpp>
53 #include <kdl/chainjnttojacsolver.hpp>
54 #include <kdl/frames.hpp>
60 #include <geometry_msgs/PoseStamped.h>
61 #include <geometry_msgs/TwistStamped.h>
62 #include <robot_mechanism_controllers/JTCartesianControllerState.h>
70 typedef Eigen::Matrix<double, Joints, 1>
JointVec;
71 typedef Eigen::Matrix<double, 6, Joints>
Jacobian;
73 Kin(
const KDL::Chain &kdl_chain) :
110 typedef Eigen::Matrix<double, Joints, 1>
JointVec;
111 typedef Eigen::Matrix<double, 6, 1>
CartVec;
112 typedef Eigen::Matrix<double, 6, Joints>
Jacobian;
113 typedef robot_mechanism_controllers::JTCartesianControllerState
StateMsg;
140 boost::scoped_ptr<Kin<Joints> >
kin_;
141 Eigen::Matrix<double,6,1>
Kp,
Kd;
161 void setGains(
const std_msgs::Float64MultiArray::ConstPtr &msg)
163 if (
msg->data.size() >= 6)
164 for (
size_t i = 0;
i < 6; ++
i)
165 Kp[i] =
msg->data[i];
167 for (
size_t i = 0;
i < 6; ++
i)
170 ROS_INFO(
"New gains: %.1lf, %.1lf, %.1lf %.1lf, %.1lf, %.1lf",
176 if (
msg->data.size() == 0) {
181 ROS_ERROR(
"Posture message had the wrong size: %d", (
int)
msg->data.size());
187 for (
int j = 0; j <
Joints; ++j)
192 void commandPose(
const geometry_msgs::PoseStamped::ConstPtr &command)
194 geometry_msgs::PoseStamped in_root;
201 ROS_ERROR(
"Failed to transform: %s", ex.what());
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
KDL::ChainJntToJacSolver jac_solver_
boost::scoped_ptr< Kin< Joints > > kin_
realtime_tools::RealtimePublisher< geometry_msgs::PoseStamped > pub_x_desi_
realtime_tools::RealtimePublisher< StateMsg > pub_state_
void commandPose(const geometry_msgs::PoseStamped::ConstPtr &command)
Eigen::Affine3d x_desi_filtered_
void jac(const JointVec &q, Jacobian &J)
ROSLIB_DECL std::string command(const std::string &cmd)
Eigen::Matrix< double, 6, 1 > Kd
bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
ros::Subscriber sub_gains_
void setGains(const std_msgs::Float64MultiArray::ConstPtr &msg)
KDL::ChainFkSolverPos_recursive fk_solver_
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
pr2_mechanism_model::Chain chain_
void fk(const JointVec &q, Eigen::Affine3d &x)
double pose_command_filter_
double vel_saturation_trans_
double jacobian_inverse_damping_
Eigen::Matrix< double, Joints, 1 > JointVec
void commandPosture(const std_msgs::Float64MultiArray::ConstPtr &msg)
Eigen::Matrix< double, 6, 1 > Kp
tf::TransformListener tf_
Eigen::Matrix< double, 6, Joints > Jacobian
Eigen::Matrix< double, 6, 1 > CartVec
double vel_saturation_rot_
robot_mechanism_controllers::JTCartesianControllerState StateMsg
pr2_mechanism_model::RobotState * robot_state_
Eigen::Matrix< double, 6, Joints > Jacobian
void transformKDLToEigen(const KDL::Frame &k, Eigen::Affine3d &e)
Eigen::Affine3d last_pose_
Eigen::Matrix< double, Joints, 1 > JointVec
if((endptr==val_str)||(endptr<(val_str+strlen(val_str))))
ros::Subscriber sub_posture_
Kin(const KDL::Chain &kdl_chain)
ros::Subscriber sub_pose_