diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index ee936a5aa260900c10fd4ad28379ea8c73331ac9..8d5322c48074b90575162a2fd6a0a3e238277a2d 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/src/quadarm_task_priority_ctrl.cpp
@@ -18,8 +18,9 @@ void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const double& quad_
const goal_obj& goal,
const ht& HT,
const arm& arm,
+ const quadrotor& quad,
ctrl_params& ctrl_params,
- MatrixXd& joint_pos,
+ MatrixXd& robot_pos,
MatrixXd& robot_vel)
{
@@ -27,7 +28,7 @@ void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const double& quad_
this->quad_height_ = quad_height;
// Goal related objects
- this->vel_.cam = goal.cam_vel;
+ this->cam_vel_ = goal.cam_vel;
this->target_dist_ = goal.target_dist;
// Homogeneous transformations
this->T_ = HT;
@@ -37,26 +38,27 @@ void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const double& quad_
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();
+ // Quad data
+ this->quad_.pos = quad.pos;
- qa_kinematics();
+ uam_kinematics();
qa_control();
// Arm positions increment
- 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);
+ this->arm_.jnt_pos = this->arm_.jnt_pos+(this->uam_.vel.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);
+ this->quad_.pos = this->quad_.pos+(this->uam_.vel.block(0,0,6,1) * this->ctrl_params_.dt);
// return values
ctrl_params = this->ctrl_params_;
- joint_pos = this->arm_.jnt_pos;
- robot_vel = this->vel_.quadarm;
+ robot_pos.block(0,0,6,1) = this->quad_.pos;
+ robot_pos.block(6,0,this->arm_.nj,1) = this->arm_.jnt_pos;
+ robot_vel = this->uam_.vel;
}
// Kinematics
-void CQuadarm_Task_Priority_Ctrl::qa_kinematics(){
+void CQuadarm_Task_Priority_Ctrl::uam_kinematics(){
// Get arm Homogenous transform, arm tip in base_link
this->T_.tip_in_baselink = arm_f_kinematics();
@@ -98,10 +100,10 @@ void CQuadarm_Task_Priority_Ctrl::qa_kinematics(){
Rot.block(3,3,3,3)=Rb_in_c;
// Whole robot Jacobian
- this->Jqa_ = MatrixXd::Zero(6,6+this->arm_.nj);
+ this->uam_.jacob = MatrixXd::Zero(6,6+this->arm_.nj);
- this->Jqa_.block(0,0,6,6) = Rot*Jb2c*Ji2b;
- this->Jqa_.block(0,6,6,this->arm_.nj)=Rot*Ja;
+ this->uam_.jacob.block(0,0,6,6) = Rot*Jb2c*Ji2b;
+ this->uam_.jacob.block(0,6,6,this->arm_.nj)=Rot*Ja;
}
Matrix4d CQuadarm_Task_Priority_Ctrl::arm_f_kinematics()
{
@@ -115,8 +117,8 @@ Matrix4d CQuadarm_Task_Priority_Ctrl::arm_f_kinematics()
// resize the joint state vector in non-realtime
this->arm_.jnt_pos_kdl.resize(this->arm_.nj);
- for(unsigned int i=0; i < this->arm_.nj; i++){
- this->arm_.jnt_pos_kdl.data(i) = this->arm_.jnt_pos(6+i,0);
+ for(unsigned int ii=0; ii < this->arm_.nj; ii++){
+ this->arm_.jnt_pos_kdl.data(ii) = this->arm_.jnt_pos(ii,0);
}
// computes Cartesian pose in realtime From Base_link to Arm tip
@@ -166,9 +168,9 @@ MatrixXd CQuadarm_Task_Priority_Ctrl::quadrotor_jacobian()
{
double psi,theta,phi;
- phi = this->arm_.jnt_pos(3,0);
- theta = this->arm_.jnt_pos(4,0);
- // psi = this->arm_.jnt_pos(5,0);
+ phi = this->quad_.pos(3,0);
+ theta = this->quad_.pos(4,0);
+ // psi = this->quad_.pos(5,0);
psi = 0;
MatrixXd Jq(6,6);
@@ -305,9 +307,9 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
}
// Rearranging terms
- this->vel_.quadarm = MatrixXd::Zero(6+this->arm_.nj,1);
- this->vel_.quadarm.block(0,0,3,1) = Vtotal.block(0,0,3,1);
- this->vel_.quadarm.block(5,0,1+this->arm_.nj,1) = Vtotal.block(3,0,1+this->arm_.nj,1);
+ this->uam_.vel = MatrixXd::Zero(6+this->arm_.nj,1);
+ this->uam_.vel.block(0,0,3,1) = Vtotal.block(0,0,3,1);
+ this->uam_.vel.block(5,0,1+this->arm_.nj,1) = Vtotal.block(3,0,1+this->arm_.nj,1);
}
void CQuadarm_Task_Priority_Ctrl::task_infrad(MatrixXd& JIR,MatrixXd& JIR_pseudo,MatrixXd& sigmaIR)
{
@@ -357,9 +359,9 @@ void CQuadarm_Task_Priority_Ctrl::task_vs(MatrixXd& JVS,MatrixXd& JVS_pseudo,Mat
MatrixXd J1(8,4),J2(8,2),Jqa1(6,4+this->arm_.nj),Jqa2(6,2);
// 9 DOF Extracting wx and wy from quadrotor
- Jqa1.block(0,0,6,3) = this->Jqa_.block(0,0,6,3);
- Jqa1.block(0,3,6,1+this->arm_.nj) = this->Jqa_.block(0,5,6,1+this->arm_.nj);
- Jqa2 = this->Jqa_.block(0,3,6,2);
+ Jqa1.block(0,0,6,3) = this->uam_.jacob.block(0,0,6,3);
+ Jqa1.block(0,3,6,1+this->arm_.nj) = this->uam_.jacob.block(0,5,6,1+this->arm_.nj);
+ Jqa2 = this->uam_.jacob.block(0,3,6,2);
MatrixXd eye = MatrixXd::Identity(4+this->arm_.nj,4+this->arm_.nj);
// task jacobian
@@ -370,7 +372,7 @@ void CQuadarm_Task_Priority_Ctrl::task_vs(MatrixXd& JVS,MatrixXd& JVS_pseudo,Mat
JVS_pseudo = weight_jacvs_inverse(Jqa1);
// task velocity vector
- sigmaVS = this->vel_.cam-Jqa2*this->ctrl_params_.v_rollpitch;
+ sigmaVS = this->cam_vel_-Jqa2*this->ctrl_params_.v_rollpitch;
}
void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,MatrixXd& sigmaG)
{
@@ -404,7 +406,7 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
arm_segment.at(ii) = this->arm_.chain.getSegment(ii);
// Get joint positions
- qa = this->arm_.jnt_pos.block(6,0,this->arm_.nj,1);
+ qa = this->arm_.jnt_pos;
//Fixed transform from arm_base to base_link
// Matrix4d Tbase = Matrix4d::Zero();
@@ -558,7 +560,7 @@ void CQuadarm_Task_Priority_Ctrl::task_jl(MatrixXd& JL,MatrixXd& JL_pseudo,Matri
}
// Get current arm joint errors
- this->ctrl_params_.jntlim_pos_error = this->arm_.jnt_pos.block(6,0,this->arm_.nj,1)-this->ctrl_params_.jntlim_pos_des;
+ this->ctrl_params_.jntlim_pos_error = this->arm_.jnt_pos-this->ctrl_params_.jntlim_pos_des;
// Task sigma
sigmaL = this->ctrl_params_.jntlim_pos_error.transpose()*AAL*this->ctrl_params_.jntlim_pos_error;
diff --git a/src/quadarm_task_priority_ctrl.h b/src/quadarm_task_priority_ctrl.h
index ecad8cfd4663519977aa29ed6295988f0e68145d..3749fd8bd443c486405c69387cd05d4c64596638 100644
--- a/src/quadarm_task_priority_ctrl.h
+++ b/src/quadarm_task_priority_ctrl.h
@@ -50,13 +50,6 @@ typedef struct{
Matrix4d cam_in_baselink; // Camera in base_link frames.
}ht;
-// Object velocities
-typedef struct{
- MatrixXd cam; // Camera (end-effector).
- MatrixXd quadarm; // Quadrotor and Arm joint velocities.
- MatrixXd old_quadarm; // Old: secondary task velocities.
-}obj_vel;
-
// Arm joint definition
typedef struct{
string link_parent; // Link parent name.
@@ -78,6 +71,17 @@ typedef struct{
Jacobian jacobian; // Jacobian.
}arm;
+// Quadrotor definition
+typedef struct{
+ MatrixXd pos; // position values (joints).
+}quadrotor;
+
+// UAM definition
+typedef struct{
+ MatrixXd vel; // Velocities (joints).
+ MatrixXd jacob; // Jacobian.
+}uam;
+
// Control Law parameters
typedef struct{
bool enable_sec_task; // Enable secondary task.
@@ -118,27 +122,29 @@ class CQuadarm_Task_Priority_Ctrl
private:
- ht T_; // Homogeneous Transforms
+ uam uam_; // Unmanned Aerial Manipulator
- obj_vel vel_; // Robot velocities
+ arm arm_; // Arm
- MatrixXd Jqa_; // Quadrotor and arm jacobian.
+ quadrotor quad_; // Quadrotor
- double target_dist_; // Euclidean distance to target.
+ ctrl_params ctrl_params_; // Control Law parameters.
- arm arm_; // Arm
+ ht T_; // Homogeneous Transforms
- ctrl_params ctrl_params_; // Control Law parameters.
+ MatrixXd cam_vel_; // Camera velocities
+
+ double target_dist_; // Euclidean distance to target.
double quad_height_; // Quadrotor ground distance.
/**
- * \brief Compute Quadrotor and Arm Kinematics
+ * \brief Compute Unmanned Aerial Manipulator Kinematics
*
- * Compute the quadrotor and arm Jacobian and the position of the quadrotor using forward kinematics
+ * Compute the UAM Jacobian and the position of the quadrotor using forward kinematics
*
*/
- void qa_kinematics();
+ void uam_kinematics();
/**
* \brief Compute Quadrotor Jacobian
@@ -260,8 +266,9 @@ class CQuadarm_Task_Priority_Ctrl
const goal_obj& goal,
const ht& HT,
const arm& arm,
+ const quadrotor& quad,
ctrl_params& ctrl_params,
- MatrixXd& joint_pos,
+ MatrixXd& robot_pos,
MatrixXd& robot_vel);
/**
* \brief Write to file