Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef _kinton_arm_task_priority_control_alg_h_
00026 #define _kinton_arm_task_priority_control_alg_h_
00027
00028 #include <kinton_arm_task_priority_control/KintonArmTaskPriorityControlConfig.h>
00029 #include "mutex.h"
00030
00031 #include <tf/transform_listener.h>
00032 #include <kdl_parser/kdl_parser.hpp>
00033 #include <Eigen/Dense>
00034
00035 #include <quadarm_task_priority_ctrl.h>
00036
00037 using namespace KDL;
00038 using namespace Eigen;
00039 using namespace std;
00040
00041
00042
00048 class KintonArmTaskPriorityControlAlgorithm
00049 {
00050 protected:
00057 CMutex alg_mutex_;
00058
00059
00060
00061
00062 public:
00069 typedef kinton_arm_task_priority_control::KintonArmTaskPriorityControlConfig Config;
00070
00077 Config config_;
00078
00087 KintonArmTaskPriorityControlAlgorithm(void);
00088
00094 void lock(void) { alg_mutex_.enter(); };
00095
00101 void unlock(void) { alg_mutex_.exit(); };
00102
00110 bool try_enter(void) { return alg_mutex_.try_enter(); };
00111
00123 void config_update(Config& new_cfg, uint32_t level=0);
00124
00125
00126
00127
00134 ~KintonArmTaskPriorityControlAlgorithm(void);
00135
00142 void task_priority_ctrl(const MatrixXd& cam_vel,const bool& arm_unina,
00143 const bool& arm_catec,const bool& enable_sec_task,
00144 const Matrix4d& Tcam_in_tip,const Matrix4d& Ttag_in_cam,
00145 const MatrixXd& lambda_robot,
00146 const MatrixXd& gain_sec_task,
00147 const MatrixXd& q_rollpitch,const MatrixXd& v_rollpitch,
00148 const Chain& kdl_chain,const vector<arm_joint> joint_info,
00149 const double& dt, MatrixXd& q,
00150 MatrixXd& vel_quad);
00151
00152 CQuadarm_Task_Priority_Ctrl quadarm_task_priority_ctrl;
00153
00154 };
00155
00156 #endif