00001 /* 00002 * ImpedanceProjector.h 00003 * 00004 * Created on: 31-08-2011 00005 * Author: konradb3 00006 */ 00007 00008 #ifndef ImpedanceProjector_H_ 00009 #define ImpedanceProjector_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 <lwr_fri/typekit/Types.h> 00020 #include <sensor_msgs/typekit/Types.h> 00021 #include <geometry_msgs/typekit/Types.h> 00022 #include <lwr_impedance_controller/CartesianImpedance.h> 00023 00024 namespace lwr { 00025 00026 using namespace RTT; 00027 00028 class ImpedanceProjector : public RTT::TaskContext { 00029 public: 00030 ImpedanceProjector(const std::string& name); 00031 virtual ~ImpedanceProjector(); 00032 00033 virtual bool configureHook(); 00034 virtual bool startHook(); 00035 00036 virtual void updateHook(); 00037 virtual void stopHook(); 00038 virtual void cleanupHook(); 00039 private: 00040 00041 OutputPort<std::vector<double> > port_jnt_trq_cmd; 00042 OutputPort<lwr_fri::FriJointImpedance> port_joint_impedance_command; 00043 InputPort<std::vector<double> > port_joint_position_command; 00044 00045 InputPort<KDL::Jacobian> port_jacobian; 00046 InputPort<geometry_msgs::Pose> port_cart_pos_msr; 00047 InputPort<geometry_msgs::Pose> port_tool_frame; 00048 InputPort<lwr_impedance_controller::CartesianImpedance> port_cartesian_impedance_command; 00049 InputPort<double> port_command_period; 00050 InputPort<std::vector<double> > port_jnt_pos_msr; 00051 00052 double dt; 00053 std::vector<double> jnt_trq_cmd; 00054 std::vector<double> jnt_pos, jnt_pos_cmd; 00055 lwr_fri::FriJointImpedance imp; 00056 KDL::Jacobian jacobian; 00057 KDL::Frame tool_frame; 00058 lwr_impedance_controller::CartesianImpedance cartesian_impedance_command; 00059 00060 }; 00061 00062 } 00063 00064 #endif /* ImpedanceProjector_H_ */
