00001 /* 00002 * CartImpTrajectory.h 00003 * 00004 * Created on: 31-08-2011 00005 * Author: konradb3 00006 */ 00007 00008 #ifndef CARTIMPTRAJECTORY_H_ 00009 #define CARTIMPTRAJECTORY_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/CartImpTrajectory.h> 00023 #include <lwr_impedance_controller/CartesianImpedance.h> 00024 00025 namespace lwr { 00026 00027 using namespace RTT; 00028 00029 class CartImpTrajectory : public RTT::TaskContext { 00030 public: 00031 CartImpTrajectory(const std::string& name); 00032 virtual ~CartImpTrajectory(); 00033 00034 virtual bool configureHook(); 00035 virtual bool startHook(); 00036 00037 virtual void updateHook(); 00038 virtual void stopHook(); 00039 virtual void cleanupHook(); 00040 private: 00041 00042 double linearlyInterpolate(double time, 00043 double startTime, 00044 double endTime, 00045 double startValue, 00046 double endValue); 00047 lwr_impedance_controller::CartImpTrajectoryPoint sampleInterpolation(); 00048 00049 InputPort<lwr_impedance_controller::CartImpTrajectory> port_cart_imp_trajectory_cmd; 00050 InputPort<geometry_msgs::Pose> port_cart_pos_msr; 00051 InputPort<geometry_msgs::Pose> port_desired_cartesian_position; 00052 00053 OutputPort<lwr_impedance_controller::CartesianImpedance> port_cartesian_impedance_cmd; 00054 OutputPort<geometry_msgs::Wrench> port_cart_wrench_cmd; 00055 OutputPort<geometry_msgs::Pose> port_cart_position_cmd; 00056 OutputPort<geometry_msgs::Pose> port_tool_frame; 00057 00058 lwr_impedance_controller::CartImpTrajectoryPoint setpoint_; 00059 lwr_impedance_controller::CartImpTrajectory trajectory_tmp_; 00060 lwr_impedance_controller::CartImpTrajectory trajectory_; 00061 00062 geometry_msgs::Pose cart_pos_msr; 00063 KDL::Frame cart_pos_old, tool_frame, cart_pos_cmd; 00064 00065 bool valid_trajectory_; 00066 unsigned int trajectory_index_; 00067 lwr_impedance_controller::CartImpTrajectoryPoint last_point_; 00068 unsigned int time_; 00069 double dt_; 00070 00071 }; 00072 00073 } 00074 00075 #endif /* CARTIMPTRAJECTORY_H_ */