simplified input vars

src/CMakeLists.txt

@@ -7,10 +7,11 @@ SET(headers quadarm_task_priority_ctrl.h)
 # locate the necessary dependencies
 
 # KDL #########
-INCLUDE (${PROJECT_SOURCE_DIR}/FindPkgConfig.cmake)
-
-SET( KDL_INSTALL ${CMAKE_INSTALL_PREFIX} CACHE PATH "The KDL installation directory.")
-INCLUDE (${PROJECT_SOURCE_DIR}/FindKDL.cmake)
+#INCLUDE (${PROJECT_SOURCE_DIR}/FindPkgConfig.cmake)
+ 
+#SET( KDL_INSTALL ${CMAKE_INSTALL_PREFIX} CACHE PATH "The KDL installation directory.")
+#INCLUDE (${PROJECT_SOURCE_DIR}/FindKDL.cmake)
+find_package(Orocos-KDL REQUIRED)
 
 # Boost #######
 set(Boost_USE_STATIC_LIBS ON) 
@@ -27,6 +28,7 @@ INCLUDE_DIRECTORIES(. ${EIGEN_INCLUDE_DIR} ${Boost_INCLUDE_DIRS})
 # create the shared library
 ADD_LIBRARY(quadarm_task_priority_ctrl SHARED ${sources})
 
+#TARGET_LINK_LIBRARIES(quadarm_task_priority_ctrl ${EIGEN_LIBRARY} ${Boost_LIBRARIES} orocos-kdl)
 TARGET_LINK_LIBRARIES(quadarm_task_priority_ctrl ${EIGEN_LIBRARY} ${Boost_LIBRARIES}) 
 
 

src/quadarm_task_priority_ctrl.cpp

@@ -13,24 +13,32 @@ CQuadarm_Task_Priority_Ctrl::~CQuadarm_Task_Priority_Ctrl()
 {
 }
 
-void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const MatrixXd& cam_vel,
-                                                             const Matrix4d& Tcam_in_tip,
-                                                             const Matrix4d& Ttag_in_cam,
-                                                             const double& target_dist,
-                                                             const Chain& kdl_chain, 
+/*void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const goal_obj& goal,
+                                                             const ht& HT,
+                                                             const Chain& kdl_chain,
                                                              const vector<arm_joint> joint_info,
                                                              ctrl_params& ctrl_params,
-                                                             MatrixXd& joint_pos, 
-                                                             MatrixXd& robot_vels)
+                                                             MatrixXd& joint_pos,
+                                                             MatrixXd& robot_vels)*/
+void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const goal_obj& goal,
+                                                             const ht& HT,
+                                                             const arm& arm,
+                                                             ctrl_params& ctrl_params,
+																														 MatrixXd& joint_pos,
+                                                             MatrixXd& robot_vel)
 {
-  this->vel_.cam = cam_vel;
-  this->T_.cam_in_tip = Tcam_in_tip;
-  this->T_.tag_in_cam = Ttag_in_cam;
-  this->arm_.chain = kdl_chain;
-  this->arm_.joint_info=joint_info;
-  this->arm_.jnt_pos = joint_pos;
-  this->target_dist_ = target_dist;
+	// Goal related objects
+  this->vel_.cam = goal.cam_vel;
+  this->target_dist_ = goal.target_dist;
+  // Homogeneous transformations
+  this->T_ = HT;
+  // Control parameters
   this->ctrl_params_ = ctrl_params;
+  // Arm data
+  this->arm_.chain = arm.chain;
+  this->arm_.joint_info= arm.joint_info;
+  this->arm_.jnt_pos = arm.jnt_pos;
+
 
   // Arm DOFs
   this->arm_.nj = this->arm_.chain.getNrOfJoints();
@@ -42,10 +50,11 @@ void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const MatrixXd& cam
   this->arm_.jnt_pos.block(6,0,this->arm_.nj,1) = this->arm_.jnt_pos.block(6,0,this->arm_.nj,1)+(this->vel_.quadarm.block(6,0,this->arm_.nj,1) * this->ctrl_params_.dt);
   // Quadrotor positions increment
   this->arm_.jnt_pos.block(0,0,6,1) = this->arm_.jnt_pos.block(0,0,6,1)+(this->vel_.quadarm.block(0,0,6,1) * this->ctrl_params_.dt);
+
   // return values
   ctrl_params = this->ctrl_params_;
   joint_pos = this->arm_.jnt_pos;
-  robot_vels = this->vel_.quadarm;
+  robot_vel = this->vel_.quadarm;
 }
 
 void CQuadarm_Task_Priority_Ctrl::qa_kinematics(){
@@ -873,4 +882,4 @@ void CQuadarm_Task_Priority_Ctrl::write_to_file(const string& folder_name, const
   File << data(data.rows()-1,data.cols()-1);
   File << "\n";
   File.close();
-}
+}
\ No newline at end of file

src/quadarm_task_priority_ctrl.h

@@ -35,7 +35,13 @@ using namespace Eigen;
 using namespace KDL;
 using namespace std;
 
-// Homogenous Transforms
+// Goal related objects
+typedef struct{
+		MatrixXd cam_vel; // Desired camera velocity (end-effector)
+    double target_dist; // Target distance
+}goal_obj;
+
+// Homogeneous Transforms
 typedef struct{
     Matrix4d tag_in_cam; // Tag in camera frames.
     Matrix4d tag_in_baselink; // Tag in base_link frames.
@@ -273,16 +279,12 @@ class CQuadarm_Task_Priority_Ctrl
     * Main public function call of Quadarm Task Priority Control.
     *
     */     
-    void quadarm_task_priority_ctrl(const MatrixXd& cam_vel,
-                                    const Matrix4d& Tcam_in_tip,
-                                    const Matrix4d& Ttag_in_cam,
-                                    const double& target_dist,
-                                    const Chain& kdl_chain, 
-                                    const vector<arm_joint> joint_info,
+    void quadarm_task_priority_ctrl(const goal_obj& goal,
+                                    const ht& HT,
+                                    const arm& arm,
                                     ctrl_params& ctrl_params,
-                                    MatrixXd& joint_pos, 
-                                    MatrixXd& joint_vels);
-
+																		MatrixXd& joint_pos,
+                                    MatrixXd& robot_vel);
 
     /**
     * \brief Write to file