00001 /* 00002 * ForceController.h 00003 * 00004 * Created on: 31-08-2011 00005 * Author: konradb3 00006 */ 00007 00008 #ifndef ForceController_H_ 00009 #define ForceController_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 ForceController : public RTT::TaskContext { 00032 public: 00033 ForceController(const std::string& name); 00034 virtual ~ForceController(); 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 00044 InputPort<geometry_msgs::Wrench> port_cartesian_wrench; 00045 OutputPort<geometry_msgs::Twist> port_cartesian_velocity_command; 00046 00047 double dt; 00048 geometry_msgs::Wrench cartesian_wrench; 00049 geometry_msgs::Twist cartesian_twist; 00050 00051 }; 00052 00053 } 00054 00055 #endif /* ForceController_H_ */