00001 /* 00002 * VelocityProjector.h 00003 * 00004 * Created on: 31-08-2011 00005 * Author: konradb3 00006 */ 00007 00008 #ifndef VelocityProjector_H_ 00009 #define VelocityProjector_H_ 00010 00011 #include <rtt/TaskContext.hpp> 00012 #include <rtt/Port.hpp> 00013 #include <rtt/Logger.hpp> 00014 00015 #include <kdl/frames.hpp> 00016 #include <kdl/jntarray.hpp> 00017 #include <kdl/jacobian.hpp> 00018 00019 #include <sensor_msgs/typekit/Types.h> 00020 #include <geometry_msgs/typekit/Types.h> 00021 00022 typedef Eigen::Matrix<double, 7, 6> Matrix76d; 00023 typedef Eigen::Matrix<double, 7, 7> Matrix77d; 00024 typedef Eigen::Matrix<double, 7, 1> Vector7d; 00025 typedef Eigen::Matrix<double, 6, 1> Vector6d; 00026 00027 namespace lwr { 00028 00029 using namespace RTT; 00030 00031 class VelocityProjector : public RTT::TaskContext { 00032 public: 00033 VelocityProjector(const std::string& name); 00034 virtual ~VelocityProjector(); 00035 00036 virtual bool configureHook(); 00037 virtual bool startHook(); 00038 00039 virtual void updateHook(); 00040 virtual void stopHook(); 00041 virtual void cleanupHook(); 00042 private: 00043 OutputPort<std::vector<double> > port_joint_position_command; 00044 00045 OutputPort<geometry_msgs::Pose> port_desired_cartesian_position; 00046 00047 InputPort<KDL::Jacobian> port_jacobian; 00048 InputPort<geometry_msgs::Twist> port_cartesian_velocity_command; 00049 InputPort<geometry_msgs::Pose> port_tool_frame; 00050 InputPort<double> port_command_period; 00051 InputPort<std::vector<double> > port_jnt_pos_des; 00052 InputPort<std::vector<double> > port_nullspace_velocity_command; 00053 InputPort<Matrix77d> port_mass_matrix; 00054 00055 double dt; 00056 std::vector<double> jnt_pos_des; 00057 std::vector<double> jnt_pos_cmd; 00058 std::vector<double> null_vel_cmd; 00059 geometry_msgs::Twist cart_vel_cmd; 00060 KDL::Twist cart_vel; 00061 KDL::Jacobian jacobian; 00062 KDL::Frame tool_frame; 00063 00064 }; 00065 00066 } 00067 00068 #endif /* VelocityProjector_H_ */
