diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index e48f3f1b016c8f32e29297966c5cd359db512304..4a4a201164e0d2b02eea1ebac51cdb5feb5e16e2 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,8 +1,8 @@
# driver source files
-SET(sources common_fc.cpp kinematics.cpp uam_task_ctrl.cpp)
+SET(sources common_fc.cpp kinematics.cpp uam_task_ctrl.cpp tasks/ir.cpp tasks/vs.cpp tasks/cog.cpp tasks/jl.cpp)
# application header files
-SET(headers common_obj.h common_fc.h kinematics.h uam_task_ctrl.h)
+SET(headers common_obj.h common_fc.h kinematics.h uam_task_ctrl.h tasks/ir.h tasks/vs.h tasks/cog.h tasks/jl.h)
# locate the necessary dependencies
diff --git a/src/common_fc.cpp b/src/common_fc.cpp
index 4f8fb48365ac80238c0580ca9854601785062676..b09584760d3f927e0e99bec147331364ae2d27fa 100644
--- a/src/common_fc.cpp
+++ b/src/common_fc.cpp
@@ -2,11 +2,9 @@
namespace UAM {
-CCommon_Fc::CCommon_Fc()
-{}
+CCommon_Fc::CCommon_Fc(){}
-CCommon_Fc::~CCommon_Fc()
-{}
+CCommon_Fc::~CCommon_Fc(){}
Eigen::MatrixXd CCommon_Fc::WeightInvJac(const Eigen::MatrixXd& J, const Eigen::MatrixXd& invjac_delta_gain, const double& invjac_alpha_min, const double& target_dist)
{
diff --git a/src/common_obj.h b/src/common_obj.h
index b0223adf5b4ccaf17d61b2995798465ba3e3e852..e8752fb46d0cd132c4f727c6b29e5a491c2890b4 100644
--- a/src/common_obj.h
+++ b/src/common_obj.h
@@ -24,12 +24,6 @@
namespace UAM {
-// Arm CoG data
-typedef struct{
- std::vector<Eigen::MatrixXd> link_cog; // Vector of link CoG relative to base
- Eigen::MatrixXd arm_cog; // Arm CoG coordinates w.r.t. arm base link;
-}ArmCogData;
-
// Arm joint
typedef struct{
std::string link_parent; // Link parent name.
@@ -39,6 +33,12 @@ typedef struct{
double mass; // Joint mass.
}CArmJoint;
+// Arm CoG data
+typedef struct{
+ std::vector<Eigen::MatrixXd> link_cog; // Vector of link CoG relative to base
+ Eigen::MatrixXd arm_cog; // Arm CoG coordinates w.r.t. arm base link;
+}ArmCogData;
+
// Arm
typedef struct{
KDL::Chain chain; // KDL chain.
diff --git a/src/kinematics.cpp b/src/kinematics.cpp
index 673408400811ce25ffa3e7c136b722621ed25015..d4c450387a8b565ee05177208d83fa53b02deb88 100644
--- a/src/kinematics.cpp
+++ b/src/kinematics.cpp
@@ -2,6 +2,10 @@
namespace UAM {
+CKinematics::CKinematics(){}
+
+CKinematics::~CKinematics(){}
+
void CKinematics::UAMKine(const UAM::CQuad& quad, UAM::CArm& arm, UAM::CHT& HT, UAM::CUAM& uam)
{
// Get arm Homogenous transform, arm tip in base_link
diff --git a/src/kinematics.h b/src/kinematics.h
index 514fe3a73479e61d1c922a6bc7a2db73f6f40eb8..27999f69808c35f82ca4c01c5effd86b19d42c00 100644
--- a/src/kinematics.h
+++ b/src/kinematics.h
@@ -32,7 +32,7 @@ class CKinematics
/**
* \brief Constructor
*
- * Quadarm Task Priority Control Class constructor.
+ * Class constructor.
*
*/
CKinematics();
@@ -40,7 +40,7 @@ class CKinematics
/**
* \brief Destructor
*
- * Quadarm Task Priority Control Class destructor.
+ * Class destructor.
*
*/
~CKinematics();
diff --git a/src/tasks/cog.cpp b/src/tasks/cog.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..667ea4652056dc64aa033da7b858299157ff8346
--- /dev/null
+++ b/src/tasks/cog.cpp
@@ -0,0 +1,181 @@
+// MISC
+#include <iostream> /* input, output operations */
+
+// KDL stuff
+#include <kdl/frames.hpp>
+#include <kdl/chain.hpp>
+
+// Own
+#include "cog.h"
+#include <common_fc.h>
+
+namespace UAM {
+
+CTaskCOG::CTaskCOG(){}
+
+CTaskCOG::~CTaskCOG(){}
+
+void CTaskCOG::TaskErrorJac(UAM::CArm& arm, const UAM::CHT& HT, Eigen::MatrixXd& cog_PGxy, Eigen::MatrixXd& cog_arm, Eigen::MatrixXd& sigmaG, Eigen::MatrixXd& JG, Eigen::MatrixXd& JG_pseudo, UAM::ArmCogData& arm_cog_data)
+{
+ // KDL stuff
+ std::vector<KDL::Frame> joint_pose(arm.nj); // Vector of Joint frames
+ joint_pose.resize(arm.nj);
+
+ // This should be done, otherwise an eigen assertion arises in the robot computer
+ std::vector<Eigen::Matrix4d,Eigen::aligned_allocator<Eigen::Matrix4d> > Transf(arm.nj);
+ //vector<Matrix4d> Transf(arm.nj); // Vector of HT of frames relative to each links
+ Transf.resize(arm.nj);
+ // Eigen stuff
+ std::vector<Eigen::MatrixXd> T_base_to_joint(arm.nj+1); // Vector of HT of frame links relative to base
+ T_base_to_joint.resize(arm.nj+1);
+ std::vector<Eigen::MatrixXd> IIIrdcolRot_b_x(arm.nj+1); // Vector of r3d column of HT
+ IIIrdcolRot_b_x.resize(arm.nj+1);
+ std::vector<Eigen::MatrixXd> link_origin(arm.nj+1); // Origin of each link's frame
+ link_origin.resize(arm.nj+1);
+
+ // CoG data
+ arm.mass = 0; // Total arm mass
+ Eigen::MatrixXd arm_cg = Eigen::MatrixXd::Zero(4,1); // Sum of CoG vector
+ arm_cog_data.arm_cog = Eigen::MatrixXd::Zero(3,1); // Arm CoG
+ arm_cog_data.link_cog.resize(arm.nj);
+ std::vector<Eigen::MatrixXd> link_cg(arm.nj); // Vector of link CoG relative to base
+ link_cg.resize(arm.nj);
+
+ // Get arm segments
+ unsigned int num_seg = arm.chain.getNrOfSegments();
+ std::vector<KDL::Segment> arm_segment(num_seg);
+ arm_segment.resize(num_seg);
+ for (unsigned int ii = 1; ii < num_seg; ++ii)
+ arm_segment.at(ii) = arm.chain.getSegment(ii);
+
+ // Initial transform
+ T_base_to_joint.at(0) = HT.armbase_in_baselink;
+ // 3rd column of rotation matrix
+ IIIrdcolRot_b_x.at(0) = T_base_to_joint.at(0).block(0,2,3,1);
+ // Origin of the initial frame
+ link_origin.at(0) = T_base_to_joint.at(0).col(3);
+
+ // Debug
+ arm.T_base_to_joint.resize(arm.nj+1);
+ arm.T_base_to_joint.at(0) = T_base_to_joint.at(0);
+
+ // std::cout << "**************" << std::endl;
+ // std::cout << "______base_link to arm_base: " << std::endl;
+ // std::cout << T_base_to_joint.at(0).block(0,3,3,1) << std::endl;
+ // Matrix3d Rot = T_base_to_joint.at(0).block(0,0,3,3);
+ // std::cout << Rot.eulerAngles(0, 1, 2) << std::endl;
+
+ for (unsigned int jj = 0; jj < arm.nj; ++jj)
+ {
+ // Get mass of each link
+ arm.joint_info.at(jj).mass = arm_segment.at(jj+1).getInertia().getMass();
+ // Joint frames
+ joint_pose.at(jj) = arm_segment.at(jj+1).pose(arm.jnt_pos(jj,0));
+ // relative Homogeneous transforms (HT)
+ Transf.at(jj) = UAM::CCommon_Fc::GetTransform(joint_pose.at(jj));
+ // HT r.t. base link
+ T_base_to_joint.at(jj+1) = T_base_to_joint.at(jj) * Transf.at(jj);
+ // 3rd column of rotation matrix
+ IIIrdcolRot_b_x.at(jj+1) = T_base_to_joint.at(jj+1).inverse().block(0,2,3,1);
+ // IIIrdcolRot_b_x.at(jj+1) = T_base_to_joint.at(jj+1).block(0,2,3,1);
+ // Origin of the link's frame r.t. base_link
+ link_origin.at(jj+1) = T_base_to_joint.at(jj+1).col(3);
+ // Distance to the middle of the link jj
+ Eigen::MatrixXd link_mid = (link_origin.at(jj+1)-link_origin.at(jj))/2;
+ // Link CoG coordinates
+ arm_cog_data.link_cog.at(jj) = link_origin.at(jj)+link_mid;
+ // gravity for the link jj
+ link_cg.at(jj) = arm.joint_info.at(jj).mass * arm_cog_data.link_cog.at(jj);
+ // Sum of link CoGs
+ arm_cg = arm_cg + link_cg.at(jj);
+ // Sum of link mass
+ arm.mass = arm.mass + arm.joint_info.at(jj).mass;
+
+ //Debug
+ // std::cout << "num segments: " << arm_segment.size() << std::endl;
+ // std::cout << "Link: " << jj << " name:";
+ // std::cout << arm_segment.at(jj+1).getName();
+ // std::cout << " mass: " << arm.joint_info.at(jj).mass;
+ // std::cout << " link_cog:" << std::endl;
+ // std::cout << link_origin.at(jj)+arm_cog_data.cog_joint.at(jj) << std::endl;
+ // std::cout << "Transform: " << std::endl;
+ // std::cout << T_base_to_joint.at(jj+1) <<std::endl;
+
+ // std::cout << "______joint: " << jj << std::endl;
+ // std::cout << ">> KDL: " << jj << std::endl;
+ // std::cout << joint_pose.at(jj) << std::endl;
+ // std::cout << "<< EIGEN matrix: " << jj << std::endl;
+ // std::cout << Transf.at(jj) << std::endl;
+ // std::cout << "trans: " << jj << std::endl;
+ // std::cout << Transf.at(jj).block(0,3,3,1) << std::endl;
+ // std::cout << "rot: " << jj << std::endl;
+ // Matrix3d Rot = Transf.at(jj).block(0,0,3,3);
+ // std::cout << Rot.eulerAngles(0,1,2) << std::endl;
+
+ // Fill link information
+ arm.T_base_to_joint.at(jj+1) = T_base_to_joint.at(jj+1);
+ }
+
+ // vector from arm base to CoG
+ arm_cog_data.arm_cog = arm_cg/arm.mass;
+
+ // Debug
+ // std::cout << "mass: " << arm.mass << std::endl;
+ // std::cout << "arm_cg: " << arm_cg << std::endl;
+ //std::cout << "cog: " << arm_cog_data.arm_cog << std::endl;
+
+ // Rotation between quadrotor body and inertial frames
+ // TODO: If attitude is estimated, then it could be roll and pitch
+ Eigen::Matrix3d Rib;
+ // Rib = AngleAxisd(0, Vector3d::UnitZ()) * AngleAxisd(this->ctrl_params_.q_rollpitch(1,0), Vector3d::UnitY()) * AngleAxisd(this->ctrl_params_.q_rollpitch(0,0), Vector3d::UnitX());
+ Rib = Eigen::MatrixXd::Identity(3,3);
+
+ // X and Y values of CoG in quadrotor inertial frame
+ cog_arm = arm_cog_data.arm_cog.block(0,0,3,1);
+ Eigen::MatrixXd cog_arm_in_qi = Rib * cog_arm;
+ cog_PGxy = cog_arm_in_qi.block(0,0,2,1);
+
+ // Partial CoG and jacobian of each link (augmented links: from current to end-effector)
+ Eigen::MatrixXd CoG_partial;
+ Eigen::MatrixXd Jj_cog(3,arm.nj);
+ for (unsigned int jj = 0; jj < arm.nj; ++jj)
+ {
+ // Get the center of gravity from the current link to the end effector
+ double partial_mass = 0;
+ Eigen::MatrixXd partial_arm_cg = Eigen::MatrixXd::Zero(4,1);
+ for (unsigned int ii = jj; ii < arm.nj; ++ii)
+ {
+ partial_mass = partial_mass + arm.joint_info.at(ii).mass;
+ partial_arm_cg = partial_arm_cg + link_cg.at(ii);
+ }
+
+ if(partial_mass!=0){
+ CoG_partial = partial_arm_cg/partial_mass;
+
+ Eigen::Matrix3d screw_rot = Eigen::Matrix3d::Zero();
+ screw_rot(0,1) = -IIIrdcolRot_b_x.at(jj)(2);
+ screw_rot(0,2) = IIIrdcolRot_b_x.at(jj)(1);
+ screw_rot(1,0) = IIIrdcolRot_b_x.at(jj)(2);
+ screw_rot(0,2) = -IIIrdcolRot_b_x.at(jj)(0);
+ screw_rot(2,0) = -IIIrdcolRot_b_x.at(jj)(1);
+ screw_rot(2,1) = IIIrdcolRot_b_x.at(jj)(0);
+
+ Jj_cog.col(jj) = (partial_mass/arm.mass)*screw_rot*CoG_partial.block(0,0,3,1);
+ }
+ else Jj_cog.col(jj) = Eigen::MatrixXd::Zero(3,1);
+ }
+
+ JG = Eigen::MatrixXd::Zero(1,4+arm.nj);
+ JG.block(0,0,1,4) = Eigen::MatrixXd::Zero(1,4);
+ Eigen::MatrixXd Jacob_temp(3,arm.nj);
+ for (unsigned int ii = 0; ii < arm.nj; ++ii)
+ Jacob_temp.col(ii) = Rib * Jj_cog.col(ii);
+
+ JG.block(0,4,1,arm.nj)=2*cog_PGxy.transpose()*Jacob_temp.block(0,0,2,arm.nj);
+
+ JG_pseudo = UAM::CCommon_Fc::CalcPinv(JG);
+
+ sigmaG = -(cog_PGxy.transpose()*cog_PGxy);
+}
+
+} // End of UAM namespace
\ No newline at end of file
diff --git a/src/tasks/cog.h b/src/tasks/cog.h
new file mode 100644
index 0000000000000000000000000000000000000000..f74bb6b8e2d3e840befae047ab1ca26fdc10f30e
--- /dev/null
+++ b/src/tasks/cog.h
@@ -0,0 +1,45 @@
+#ifndef _TASK_COG_H
+#define _TASK_COG_H
+
+// Eigen
+#include <eigen3/Eigen/Dense>
+
+// Own
+#include <common_obj.h>
+
+namespace UAM {
+
+class CTaskCOG
+{
+ private:
+
+ public:
+
+ /**
+ * \brief Constructor
+ *
+ * Class constructor.
+ *
+ */
+ CTaskCOG();
+
+ /**
+ * \brief Destructor
+ *
+ * Class destructor.
+ *
+ */
+ ~CTaskCOG();
+
+ /**
+ * \brief Task Error and Jacobians
+ *
+ * Returns the task error and Jacobians. Both error and Jacobians are computed in the same functions to avoid repeated operations.
+ *
+ */
+ static void TaskErrorJac(UAM::CArm& arm, const UAM::CHT& HT, Eigen::MatrixXd& cog_PGxy, Eigen::MatrixXd& cog_arm, Eigen::MatrixXd& sigmaG, Eigen::MatrixXd& JG, Eigen::MatrixXd& JG_pseudo, UAM::ArmCogData& arm_cog_data);
+};
+
+} // End of UAM namespace
+
+#endif
diff --git a/src/tasks/ir.cpp b/src/tasks/ir.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..78019224f0bcb1228d5dbc32399e3d9d07ab7b56
--- /dev/null
+++ b/src/tasks/ir.cpp
@@ -0,0 +1,63 @@
+// MISC
+#include <iostream> /* input, output operations */
+
+// Own
+#include <common_fc.h>
+#include "ir.h"
+
+namespace UAM {
+
+CTaskIR::CTaskIR(){}
+
+CTaskIR::~CTaskIR(){}
+
+void CTaskIR::TaskErrorJac(const int& arm_dof, const double& inf_rad, const Eigen::MatrixXd& d_obs, Eigen::MatrixXd& sigmaIR, Eigen::MatrixXd& JIR, Eigen::MatrixXd& JIR_pseudo)
+{
+ // Inflation radius
+ Eigen::MatrixXd inf_radius = Eigen::MatrixXd::Zero(1,1);
+ inf_radius(0,0) = inf_rad;
+
+ // Task vector
+ Eigen::MatrixXd euc_d_obs = (d_obs.transpose()*d_obs).array().sqrt();
+ sigmaIR = euc_d_obs-inf_radius;
+
+ // Task Jacobian
+ JIR = Eigen::MatrixXd::Zero(1,4+arm_dof);
+ Eigen::MatrixXd Jtemp = d_obs;
+ JIR.block(0,0,1,3) = 2*Jtemp.transpose();
+
+ // Bloquing matrix
+ Eigen::MatrixXd H = Eigen::MatrixXd::Zero(4+arm_dof,4+arm_dof);
+
+ // TODO: Currently checking each dimension (x,y,z) separately
+ // Euclidean distance to the obstacle
+ // if (sqrt(pow(d_obs(0,0),2)+pow(d_obs(1,0),2)+pow(d_obs(2,0),2))<inf_radius(0,0))
+ // {
+ // DOF to block
+ for (unsigned int ii = 0; ii < 3; ++ii)
+ {
+ if (d_obs(ii,0) < inf_radius(0,0) && d_obs(ii,0) > 0.0)
+ {
+ std::cout << "WARNING!! Inflation Radius violation. DOF: " << ii << " Inf. radius: " << inf_radius(0,0) << " Obstacle distance: " << d_obs(ii,0) << std::endl;
+ H(ii,ii) = 1.0;
+ }
+ }
+ // }
+
+ Eigen::MatrixXd Hinv = UAM::CCommon_Fc::CalcPinv(H);
+
+ // weighting matrix
+ Eigen::MatrixXd temp = JIR*Hinv*JIR.transpose();
+ JIR_pseudo = Hinv*JIR.transpose()*UAM::CCommon_Fc::CalcPinv(temp);
+
+ // DEBUG:
+ // std::cout << "xxxxxx Task velocities xxxxxx" << std::endl;
+ // std::cout << JIR_pseudo*sigmaIR << std::endl;
+ // std::cout << "______ Task Jacobian _____" << std::endl;
+ // std::cout << JIR << std::endl;
+ // std::cout << "_______ Pseudo Inverse of Task Jacobian ____" << std::endl;
+ // std::cout << JIR_pseudo << std::endl;
+ // std::cout << "+++++++++" << std::endl;
+}
+
+} // End of UAM namespace
\ No newline at end of file
diff --git a/src/tasks/ir.h b/src/tasks/ir.h
new file mode 100644
index 0000000000000000000000000000000000000000..f5a1ff4275301092c19a93e5277101304ff3f4e9
--- /dev/null
+++ b/src/tasks/ir.h
@@ -0,0 +1,42 @@
+#ifndef _TASK_IR_H
+#define _TASK_IR_H
+
+// Eigen
+#include <eigen3/Eigen/Dense>
+
+namespace UAM {
+
+class CTaskIR
+{
+ private:
+
+ public:
+
+ /**
+ * \brief Constructor
+ *
+ * Class constructor.
+ *
+ */
+ CTaskIR();
+
+ /**
+ * \brief Destructor
+ *
+ * Class destructor.
+ *
+ */
+ ~CTaskIR();
+
+ /**
+ * \brief Task Error and Jacobians
+ *
+ * Returns the task error and Jacobians. Both error and Jacobians are computed in the same functions to avoid repeated operations.
+ *
+ */
+ static void TaskErrorJac(const int& arm_dof, const double& inf_rad, const Eigen::MatrixXd& d_obs, Eigen::MatrixXd& sigmaIR, Eigen::MatrixXd& JIR, Eigen::MatrixXd& JIR_pseudo);
+};
+
+} // End of UAM namespace
+
+#endif
diff --git a/src/tasks/jl.cpp b/src/tasks/jl.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..838eac03cb3eeee81a4b4dc218cfe72c1acb8da1
--- /dev/null
+++ b/src/tasks/jl.cpp
@@ -0,0 +1,40 @@
+// Own
+#include <common_fc.h>
+#include "jl.h"
+
+namespace UAM {
+
+CTaskJL::CTaskJL(){}
+
+CTaskJL::~CTaskJL(){}
+
+void CTaskJL::TaskErrorJac(UAM::CArm& arm, const Eigen::MatrixXd& jntlim_pos_des, Eigen::MatrixXd& jntlim_pos_error, Eigen::MatrixXd& sigmaL, Eigen::MatrixXd& JL, Eigen::MatrixXd& JL_pseudo)
+{
+ // Get diagonal gain matrix
+ Eigen::MatrixXd AAL = Eigen::MatrixXd::Zero(arm.nj,arm.nj);
+ for (unsigned int ii = 0; ii < arm.nj; ++ii)
+ {
+ double min,max;
+ min = arm.joint_info.at(ii).joint_min;
+ max = arm.joint_info.at(ii).joint_max;
+ // middle joints
+ // this->ctrl_params_.jntlim_pos_des(ii,0) = min+0.5*(max-min);
+ AAL(ii,ii) = 1/((max-min)*(max-min));
+ }
+
+ // Get current arm joint errors
+ jntlim_pos_error = arm.jnt_pos-jntlim_pos_des;
+
+ // Task sigma
+ sigmaL = jntlim_pos_error.transpose()*AAL*jntlim_pos_error;
+
+ // Task Jacobian
+ JL = Eigen::MatrixXd::Zero(1,4+arm.nj);
+ JL.block(0,0,1,4) = Eigen::MatrixXd::Zero(1,4);
+ Eigen::MatrixXd Jtemp = AAL*jntlim_pos_error;
+ JL.block(0,4,1,arm.nj) = -2*Jtemp.transpose();
+
+ JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL);
+}
+
+} // End of UAM namespace
\ No newline at end of file
diff --git a/src/tasks/jl.h b/src/tasks/jl.h
new file mode 100644
index 0000000000000000000000000000000000000000..5a536bd9c55266ee5b063fc7e464de791ad52904
--- /dev/null
+++ b/src/tasks/jl.h
@@ -0,0 +1,45 @@
+#ifndef _TASK_JL_H
+#define _TASK_JL_H
+
+// Eigen
+#include <eigen3/Eigen/Dense>
+
+// Own
+#include <common_obj.h>
+
+namespace UAM {
+
+class CTaskJL
+{
+ private:
+
+ public:
+
+ /**
+ * \brief Constructor
+ *
+ * Class constructor.
+ *
+ */
+ CTaskJL();
+
+ /**
+ * \brief Destructor
+ *
+ * Class destructor.
+ *
+ */
+ ~CTaskJL();
+
+ /**
+ * \brief Task Error and Jacobians
+ *
+ * Returns the task error and Jacobians. Both error and Jacobians are computed in the same functions to avoid repeated operations.
+ *
+ */
+ static void TaskErrorJac(UAM::CArm& arm, const Eigen::MatrixXd& jntlim_pos_des, Eigen::MatrixXd& jntlim_pos_error, Eigen::MatrixXd& sigmaL, Eigen::MatrixXd& JL, Eigen::MatrixXd& JL_pseudo);
+};
+
+} // End of UAM namespace
+
+#endif
diff --git a/src/tasks/vs.cpp b/src/tasks/vs.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6cf18aaba4c3b9d11365695bb1b9edbd0562d756
--- /dev/null
+++ b/src/tasks/vs.cpp
@@ -0,0 +1,37 @@
+
+// MISC
+#include <iostream> /* input, output operations */
+
+// Own
+#include "vs.h"
+#include <common_fc.h>
+
+namespace UAM {
+
+CTaskVS::CTaskVS(){}
+
+CTaskVS::~CTaskVS(){}
+
+void CTaskVS::TaskErrorJac(const int& arm_dof, const Eigen::MatrixXd& vs_delta_gain, const double& vs_alpha_min, const double& target_dist, const Eigen::MatrixXd& v_rollpitch, const Eigen::MatrixXd& cam_vel, const Eigen::MatrixXd& uam_jac, Eigen::MatrixXd& sigmaVS, Eigen::MatrixXd& JVS, Eigen::MatrixXd& JVS_pseudo)
+{
+ // Underactuated Quadrotor
+ // 9 DOF Extracting wx and wy from quadrotor
+ Eigen::MatrixXd J1(8,4),J2(8,2),Jqa1(6,4+arm_dof);
+ Jqa1.block(0,0,6,3) = uam_jac.block(0,0,6,3);
+ Jqa1.block(0,3,6,1+arm_dof) = uam_jac.block(0,5,6,1+arm_dof);
+ Eigen::MatrixXd Jqa2 = uam_jac.block(0,3,6,2);
+ Jqa2 = uam_jac.block(0,3,6,2);
+
+ // task velocity vector
+ sigmaVS = cam_vel-Jqa2*v_rollpitch;
+
+ // task jacobian
+ JVS = Jqa1;
+
+ // task jacobian pseudo inverse
+ // JVS_pseudo = UAM::CCommon_Fc::CalcPinv(JVS);
+ // JVS_pseudo = UAM::CCommon_Fc::WeightInvJac(JVS);
+ JVS_pseudo = UAM::CCommon_Fc::WeightInvJac(JVS,vs_delta_gain,vs_alpha_min,target_dist);
+}
+
+} // End of UAM namespace
\ No newline at end of file
diff --git a/src/tasks/vs.h b/src/tasks/vs.h
new file mode 100644
index 0000000000000000000000000000000000000000..cedd0b42ac1ad89f7323f809275172694d7ccf72
--- /dev/null
+++ b/src/tasks/vs.h
@@ -0,0 +1,42 @@
+#ifndef _TASK_VS_H
+#define _TASK_VS_H
+
+// Eigen
+#include <eigen3/Eigen/Dense>
+
+namespace UAM {
+
+class CTaskVS
+{
+ private:
+
+ public:
+
+ /**
+ * \brief Constructor
+ *
+ * Class constructor.
+ *
+ */
+ CTaskVS();
+
+ /**
+ * \brief Destructor
+ *
+ * Class destructor.
+ *
+ */
+ ~CTaskVS();
+
+ /**
+ * \brief Task Error and Jacobians
+ *
+ * Returns the task error and Jacobians. Both error and Jacobians are computed in the same functions to avoid repeated operations.
+ *
+ */
+ static void TaskErrorJac(const int& arm_dof, const Eigen::MatrixXd& vs_delta_gain, const double& vs_alpha_min, const double& target_dist, const Eigen::MatrixXd& v_rollpitch, const Eigen::MatrixXd& cam_vel, const Eigen::MatrixXd& uam_jac, Eigen::MatrixXd& sigmaVS, Eigen::MatrixXd& JVS, Eigen::MatrixXd& JVS_pseudo);
+};
+
+} // End of UAM namespace
+
+#endif
diff --git a/src/uam_task_ctrl.cpp b/src/uam_task_ctrl.cpp
index 4e971e58dc5e42244f5e8281ed93d7f308c26a38..367dbc93d9f1894962b57016902f9c8785de8d5e 100644
--- a/src/uam_task_ctrl.cpp
+++ b/src/uam_task_ctrl.cpp
@@ -1,31 +1,34 @@
#include "uam_task_ctrl.h"
-#include<Eigen/StdVector>
+// Tasks specific
+#include <tasks/ir.h>
+#include <tasks/vs.h>
+#include <tasks/cog.h>
+#include <tasks/jl.h>
using namespace Eigen;
using namespace KDL;
using namespace std;
-CQuadarm_Task_Priority_Ctrl::CQuadarm_Task_Priority_Ctrl()
+CHierarchTaskPCtrl::CHierarchTaskPCtrl()
{
// this->datetime_ = UAM::CCommon_Fc::get_datetime();
}
-CQuadarm_Task_Priority_Ctrl::~CQuadarm_Task_Priority_Ctrl()
+CHierarchTaskPCtrl::~CHierarchTaskPCtrl()
{
}
// Main public function
-void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const MatrixXd& quad_dist_obs,
- const goal_obj& goal,
- UAM::CHT& HT,
- UAM::CArm& arm,
- const UAM::CQuad& quad,
- UAM::CCtrlParams& ctrl_params,
- MatrixXd& robot_pos,
- MatrixXd& robot_vel)
+void CHierarchTaskPCtrl::htpc(const MatrixXd& quad_dist_obs,
+ const goal_obj& goal,
+ UAM::CHT& HT,
+ UAM::CArm& arm,
+ const UAM::CQuad& quad,
+ UAM::CCtrlParams& ctrl_params,
+ MatrixXd& robot_pos,
+ MatrixXd& robot_vel)
{
-
// Current quadrotor height
this->quad_dist_obs_ = quad_dist_obs;
@@ -63,14 +66,14 @@ void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const MatrixXd& qua
}
// Control
-void CQuadarm_Task_Priority_Ctrl::uam_hierarchical_ctrl()
+void CHierarchTaskPCtrl::uam_hierarchical_ctrl()
{
// TASK 0: Security task (Inflation radius) _____________________
-
- // task Jacobian and sigma
+
+ // Task error and Jacobians
MatrixXd JIR,JIR_pseudo,sigmaIR;
- task_infrad(JIR,JIR_pseudo,sigmaIR);
-
+ UAM::CTaskIR::TaskErrorJac(this->arm_.nj,this->ctrl_params_.inf_radius,this->quad_dist_obs_,sigmaIR,JIR,JIR_pseudo);
+
// Gain
double lambdaIR = this->ctrl_params_.ir_gain;
@@ -79,14 +82,14 @@ void CQuadarm_Task_Priority_Ctrl::uam_hierarchical_ctrl()
// TASK 1: Visual Servo _________________________________________
+ // Task error and Jacobians
+ MatrixXd JVS,JVS_wpseudo,sigmaVS;
+ UAM::CTaskVS::TaskErrorJac(this->arm_.nj,this->ctrl_params_.vs_delta_gain,this->ctrl_params_.vs_alpha_min,this->target_dist_,this->ctrl_params_.v_rollpitch,this->cam_vel_,this->uam_.jacob,sigmaVS,JVS,JVS_wpseudo);
+
// Null space projector
MatrixXd eye = MatrixXd::Identity(4+this->arm_.nj,4+this->arm_.nj);
MatrixXd NIR = (eye-(JIR_pseudo*JIR));
- // task Jacobian and sigma
- MatrixXd JVS,JVS_wpseudo,sigmaVS;
- task_vs(JVS,JVS_wpseudo,sigmaVS);
-
// task velocity
this->ctrl_params_.vs_vel = NIR * JVS_wpseudo * sigmaVS;
@@ -100,10 +103,10 @@ void CQuadarm_Task_Priority_Ctrl::uam_hierarchical_ctrl()
MatrixXd JIR_VS_pseudo = UAM::CCommon_Fc::CalcPinv(JIR_VS);
NIR_VS = (eye-(JIR_VS_pseudo*JIR_VS));
- // task Jacobian and sigma
+ // Task error and Jacobians
MatrixXd JG,JG_pseudo,sigmaG;
- task_cog(JG,JG_pseudo,sigmaG);
-
+ UAM::CTaskCOG::TaskErrorJac(this->arm_,this->T_,this->ctrl_params_.cog_PGxy,this->ctrl_params_.cog_arm,sigmaG,JG,JG_pseudo,this->arm_cog_data_);
+
// Gain
double lambdaG = this->ctrl_params_.cog_gain;
@@ -124,7 +127,7 @@ void CQuadarm_Task_Priority_Ctrl::uam_hierarchical_ctrl()
// task Jacobian and sigma
MatrixXd JL,JL_pseudo,sigmaL;
- task_jl(JL,JL_pseudo,sigmaL);
+ UAM::CTaskJL::TaskErrorJac(this->arm_, this->ctrl_params_.jntlim_pos_des, this->ctrl_params_.jntlim_pos_error, sigmaL, JL, JL_pseudo);
// Gain
double lambdaL = this->ctrl_params_.jntlim_gain;
@@ -194,265 +197,3 @@ void CQuadarm_Task_Priority_Ctrl::uam_hierarchical_ctrl()
this->jacobians_.cog = JG;
this->jacobians_.jl = JL;
}
-void CQuadarm_Task_Priority_Ctrl::task_infrad(MatrixXd& JIR,MatrixXd& JIR_pseudo,MatrixXd& sigmaIR)
-{
- // Distance to the obstacle (currently detecting only the ground)
- MatrixXd d_obs = this->quad_dist_obs_;
-
- // Inflation radius
- MatrixXd inf_radius = MatrixXd::Zero(1,1);
- inf_radius(0,0) = this->ctrl_params_.inf_radius;
-
- // Task vector
- Eigen::MatrixXd euc_d_obs = (d_obs.transpose()*d_obs).array().sqrt();
- // sigmaIR = inf_radius-euc_d_obs;
- sigmaIR = euc_d_obs-inf_radius;
-
- // Task Jacobian
- JIR = MatrixXd::Zero(1,4+this->arm_.nj);
- MatrixXd Jtemp = d_obs;
- JIR.block(0,0,1,3) = 2*Jtemp.transpose();
-
- // Bloquing matrix
- MatrixXd H = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
-
- // Euclidean distance to the obstacle
- // if (sqrt(pow(d_obs(0,0),2)+pow(d_obs(1,0),2)+pow(d_obs(2,0),2))<inf_radius(0,0))
- // {
- // DOF to block
- for (unsigned int ii = 0; ii < 3; ++ii)
- {
- if (d_obs(ii,0) < inf_radius(0,0) && d_obs(ii,0) > 0.0)
- {
- cout << "WARNING!! Inflation Radius violation. DOF: " << ii << " Inf. radius: " << inf_radius(0,0) << " Obstacle distance: " << d_obs(ii,0) << endl;
- H(ii,ii) = 1.0;
- }
- }
- // }
-
- MatrixXd Hinv = UAM::CCommon_Fc::CalcPinv(H);
-
- // weighting matrix
- MatrixXd temp = JIR*Hinv*JIR.transpose();
- JIR_pseudo = Hinv*JIR.transpose()*UAM::CCommon_Fc::CalcPinv(temp);
-
- // cout << "xxxxxx Task velocities xxxxxx" << endl;
- // cout << JIR_pseudo*sigmaIR << endl;
- // cout << "______ Task Jacobian _____" << endl;
- // cout << JIR << endl;
- // cout << "_______ Pseudo Inverse of Task Jacobian ____" << endl;
- // cout << JIR_pseudo << endl;
- // cout << "+++++++++" << endl;
-}
-void CQuadarm_Task_Priority_Ctrl::task_vs(MatrixXd& JVS,MatrixXd& JVS_pseudo,MatrixXd& sigmaVS)
-{
- // Underactuated Quadrotor
- 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->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);
-
- // task jacobian
- JVS = Jqa1;
-
- // task jacobian pseudo inverse
- //JVS_pseudo = UAM::CCommon_Fc::CalcPinv(Jqa1);
- // JVS_pseudo = UAM::CCommon_Fc::WeightInvJac(Jqa1);
- JVS_pseudo = UAM::CCommon_Fc::WeightInvJac(Jqa1,this->ctrl_params_.vs_delta_gain,this->ctrl_params_.vs_alpha_min,this->target_dist_);
-
- // task velocity vector
- sigmaVS = this->cam_vel_-Jqa2*this->ctrl_params_.v_rollpitch;
-}
-void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,MatrixXd& sigmaG)
-{
- // KDL stuff
- vector<Frame> joint_pose(this->arm_.nj); // Vector of Joint frames
- joint_pose.resize(this->arm_.nj);
-
- // This should be done, otherwise an eigen assertion arises in the robot computer
- vector<Eigen::Matrix4d,Eigen::aligned_allocator<Eigen::Matrix4d> > Transf(this->arm_.nj);
- //vector<Matrix4d> Transf(this->arm_.nj); // Vector of HT of frames relative to each links
- Transf.resize(this->arm_.nj);
- // Eigen stuff
- vector<MatrixXd> T_base_to_joint(this->arm_.nj+1); // Vector of HT of frame links relative to base
- T_base_to_joint.resize(this->arm_.nj+1);
- vector<MatrixXd> IIIrdcolRot_b_x(this->arm_.nj+1); // Vector of r3d column of HT
- IIIrdcolRot_b_x.resize(this->arm_.nj+1);
- vector<MatrixXd> link_origin(this->arm_.nj+1); // Origin of each link's frame
- link_origin.resize(this->arm_.nj+1);
-
- // CoG data
- this->arm_.mass = 0; // Total arm mass
- MatrixXd arm_cg = MatrixXd::Zero(4,1); // Sum of CoG vector
- this->arm_cog_data_.arm_cog = MatrixXd::Zero(3,1); // Arm CoG
- this->arm_cog_data_.link_cog.resize(this->arm_.nj);
- vector<MatrixXd> link_cg(this->arm_.nj); // Vector of link CoG relative to base
- link_cg.resize(this->arm_.nj);
-
- // Get arm segments
- unsigned int num_seg = this->arm_.chain.getNrOfSegments();
- vector<Segment> arm_segment(num_seg);
- arm_segment.resize(num_seg);
- for (unsigned int ii = 1; ii < num_seg; ++ii)
- arm_segment.at(ii) = this->arm_.chain.getSegment(ii);
-
- // Initial transform
- T_base_to_joint.at(0) = this->T_.armbase_in_baselink;
- // 3rd column of rotation matrix
- IIIrdcolRot_b_x.at(0) = T_base_to_joint.at(0).block(0,2,3,1);
- // Origin of the initial frame
- link_origin.at(0) = T_base_to_joint.at(0).col(3);
-
- // Debug
- this->arm_.T_base_to_joint.resize(this->arm_.nj+1);
- this->arm_.T_base_to_joint.at(0) = T_base_to_joint.at(0);
-
- // std::cout << "**************" << std::endl;
- // std::cout << "______base_link to arm_base: " << std::endl;
- // std::cout << T_base_to_joint.at(0).block(0,3,3,1) << std::endl;
- // Matrix3d Rot = T_base_to_joint.at(0).block(0,0,3,3);
- // std::cout << Rot.eulerAngles(0, 1, 2) << std::endl;
-
- for (unsigned int jj = 0; jj < this->arm_.nj; ++jj)
- {
- // Get mass of each link
- this->arm_.joint_info.at(jj).mass = arm_segment.at(jj+1).getInertia().getMass();
- // Joint frames
- joint_pose.at(jj) = arm_segment.at(jj+1).pose(this->arm_.jnt_pos(jj,0));
- // relative Homogeneous transforms (HT)
- Transf.at(jj) = UAM::CCommon_Fc::GetTransform(joint_pose.at(jj));
- // HT r.t. base link
- T_base_to_joint.at(jj+1) = T_base_to_joint.at(jj) * Transf.at(jj);
- // 3rd column of rotation matrix
- IIIrdcolRot_b_x.at(jj+1) = T_base_to_joint.at(jj+1).inverse().block(0,2,3,1);
- // IIIrdcolRot_b_x.at(jj+1) = T_base_to_joint.at(jj+1).block(0,2,3,1);
- // Origin of the link's frame r.t. base_link
- link_origin.at(jj+1) = T_base_to_joint.at(jj+1).col(3);
- // Distance to the middle of the link jj
- MatrixXd link_mid = (link_origin.at(jj+1)-link_origin.at(jj))/2;
- // Link CoG coordinates
- this->arm_cog_data_.link_cog.at(jj) = link_origin.at(jj)+link_mid;
- // gravity for the link jj
- link_cg.at(jj) = this->arm_.joint_info.at(jj).mass * this->arm_cog_data_.link_cog.at(jj);
- // Sum of link CoGs
- arm_cg = arm_cg + link_cg.at(jj);
- // Sum of link mass
- this->arm_.mass = this->arm_.mass + this->arm_.joint_info.at(jj).mass;
-
- //Debug
- // std::cout << "num segments: " << arm_segment.size() << std::endl;
- // std::cout << "Link: " << jj << " name:";
- // std::cout << arm_segment.at(jj+1).getName();
- // std::cout << " mass: " << this->arm_.joint_info.at(jj).mass;
- // std::cout << " link_cog:" << std::endl;
- // std::cout << link_origin.at(jj)+this->arm_cog_data_.cog_joint.at(jj) << std::endl;
- // std::cout << "Transform: " << std::endl;
- // std::cout << T_base_to_joint.at(jj+1) <<std::endl;
-
- // std::cout << "______joint: " << jj << std::endl;
- // std::cout << ">> KDL: " << jj << std::endl;
- // std::cout << joint_pose.at(jj) << std::endl;
- // std::cout << "<< EIGEN matrix: " << jj << std::endl;
- // std::cout << Transf.at(jj) << std::endl;
- // std::cout << "trans: " << jj << std::endl;
- // std::cout << Transf.at(jj).block(0,3,3,1) << std::endl;
- // std::cout << "rot: " << jj << std::endl;
- // Matrix3d Rot = Transf.at(jj).block(0,0,3,3);
- // std::cout << Rot.eulerAngles(0,1,2) << std::endl;
-
- // Fill link information
- this->arm_.T_base_to_joint.at(jj+1) = T_base_to_joint.at(jj+1);
- }
-
- // vector from arm base to CoG
- this->arm_cog_data_.arm_cog = arm_cg/this->arm_.mass;
-
- // Debug
- // std::cout << "mass: " << this->arm_.mass << std::endl;
- // std::cout << "arm_cg: " << arm_cg << std::endl;
- //std::cout << "cog: " << this->arm_cog_data_.arm_cog << std::endl;
-
- // Rotation between quadrotor body and inertial frames
- // TODO: If attitude is estimated, then it could be roll and pitch
- Matrix3d Rib;
- // Rib = AngleAxisd(0, Vector3d::UnitZ()) * AngleAxisd(this->ctrl_params_.q_rollpitch(1,0), Vector3d::UnitY()) * AngleAxisd(this->ctrl_params_.q_rollpitch(0,0), Vector3d::UnitX());
- Rib = MatrixXd::Identity(3,3);
-
- // X and Y values of CoG in quadrotor inertial frame
- this->ctrl_params_.cog_arm = this->arm_cog_data_.arm_cog.block(0,0,3,1);
- MatrixXd cog_arm_in_qi = Rib * this->ctrl_params_.cog_arm;
- this->ctrl_params_.cog_PGxy = cog_arm_in_qi.block(0,0,2,1);
-
- // Partial CoG and jacobian of each link (augmented links: from current to end-effector)
- MatrixXd CoG_partial;
- MatrixXd Jj_cog(3,this->arm_.nj);
- for (unsigned int jj = 0; jj < this->arm_.nj; ++jj)
- {
- // Get the center of gravity from the current link to the end effector
- double partial_mass = 0;
- MatrixXd partial_arm_cg = MatrixXd::Zero(4,1);
- for (unsigned int ii = jj; ii < this->arm_.nj; ++ii)
- {
- partial_mass = partial_mass + this->arm_.joint_info.at(ii).mass;
- partial_arm_cg = partial_arm_cg + link_cg.at(ii);
- }
-
- if(partial_mass!=0){
- CoG_partial = partial_arm_cg/partial_mass;
-
- Matrix3d screw_rot=Matrix3d::Zero();
- screw_rot(0,1) = -IIIrdcolRot_b_x.at(jj)(2);
- screw_rot(0,2) = IIIrdcolRot_b_x.at(jj)(1);
- screw_rot(1,0) = IIIrdcolRot_b_x.at(jj)(2);
- screw_rot(0,2) = -IIIrdcolRot_b_x.at(jj)(0);
- screw_rot(2,0) = -IIIrdcolRot_b_x.at(jj)(1);
- screw_rot(2,1) = IIIrdcolRot_b_x.at(jj)(0);
-
- Jj_cog.col(jj) = (partial_mass/this->arm_.mass)*screw_rot*CoG_partial.block(0,0,3,1);
- }
- else Jj_cog.col(jj) = MatrixXd::Zero(3,1);
- }
-
- JG=MatrixXd::Zero(1,4+this->arm_.nj);
- JG.block(0,0,1,4)=MatrixXd::Zero(1,4);
- MatrixXd Jacob_temp(3,this->arm_.nj);
- for (unsigned int ii = 0; ii < this->arm_.nj; ++ii)
- Jacob_temp.col(ii) = Rib * Jj_cog.col(ii);
-
- JG.block(0,4,1,this->arm_.nj)=2*this->ctrl_params_.cog_PGxy.transpose()*Jacob_temp.block(0,0,2,this->arm_.nj);
-
- JG_pseudo = UAM::CCommon_Fc::CalcPinv(JG);
-
- sigmaG = -(this->ctrl_params_.cog_PGxy.transpose()*this->ctrl_params_.cog_PGxy);
-}
-void CQuadarm_Task_Priority_Ctrl::task_jl(MatrixXd& JL,MatrixXd& JL_pseudo,MatrixXd& sigmaL)
-{
- // Get diagonal gain matrix
- MatrixXd AAL=MatrixXd::Zero(this->arm_.nj,this->arm_.nj);
- for (unsigned int ii = 0; ii < this->arm_.nj; ++ii)
- {
- double min,max;
- min=this->arm_.joint_info.at(ii).joint_min;
- max=this->arm_.joint_info.at(ii).joint_max;
- // middle joints
- // this->ctrl_params_.jntlim_pos_des(ii,0) = min+0.5*(max-min);
- AAL(ii,ii) = 1/((max-min)*(max-min));
- }
-
- // Get current arm joint errors
- 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;
-
- // Task Jacobian
- JL = MatrixXd::Zero(1,4+this->arm_.nj);
- JL.block(0,0,1,4) = MatrixXd::Zero(1,4);
- MatrixXd Jtemp = AAL*this->ctrl_params_.jntlim_pos_error;
- JL.block(0,4,1,this->arm_.nj) = -2*Jtemp.transpose();
-
- JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL);
-}
-
diff --git a/src/uam_task_ctrl.h b/src/uam_task_ctrl.h
index 91efd72a31de5ce4f696da54c04dbfa595f519ce..af8881c8163607da479eb3800298b92f2b3c1f06 100644
--- a/src/uam_task_ctrl.h
+++ b/src/uam_task_ctrl.h
@@ -43,7 +43,7 @@ typedef struct{
double target_dist; // Target distance
}goal_obj;
-class CQuadarm_Task_Priority_Ctrl
+class CHierarchTaskPCtrl
{
private:
@@ -71,43 +71,6 @@ class CQuadarm_Task_Priority_Ctrl
*/
void uam_hierarchical_ctrl();
- /**
- * \brief Task: Visual Servoing
- *
- * Compute vector and jacobians of the task corresponding to the visual servoing.
- *
- */
- void task_vs(MatrixXd& JVS,MatrixXd& JVS_pseudo,MatrixXd& sigmaVS);
-
- /**
- * \brief Task: Security inflation radius
- *
- * Compute vector and jacobians of the task to ensure no obstacles will
- *
- * fall inside the inflation radius (reactive behaviour).
- *
- */
- void task_infrad(MatrixXd& JIR,MatrixXd& JIR_pseudo,MatrixXd& sigmaIR);
-
- /**
- * \brief Task: CoG alignment
- *
- * Compute vector and jacobians of the task to vertically align the arm center of gravity
- *
- * with the quadrotor gravitational vector
- *
- */
- void task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,MatrixXd& sigmaG);
-
- /**
- * \brief Task: Arm joint limits avoidance
- *
- * Compute vector and jacobians of the task to avoid arm joint limits
- *
- */
- void task_jl(MatrixXd& JL,MatrixXd& JL_pseudo,MatrixXd& sigmaL);
-
-
public:
//TODO: these variables are public for debugging purposes
@@ -121,33 +84,33 @@ class CQuadarm_Task_Priority_Ctrl
/**
* \brief Constructor
*
- * Quadarm Task Priority Control Class constructor.
+ * Class constructor.
*
*/
- CQuadarm_Task_Priority_Ctrl();
+ CHierarchTaskPCtrl();
/**
* \brief Destructor
*
- * Quadarm Task Priority Control Class destructor.
+ * Class destructor.
*
*/
- ~CQuadarm_Task_Priority_Ctrl();
+ ~CHierarchTaskPCtrl();
/**
- * \brief Quadarm Task Priority Control Main API Function
+ * \brief Hierarchical Task Priority Control Main API Function
*
* Main public function call of Quadarm Task Priority Control.
*
*/
- void quadarm_task_priority_ctrl(const MatrixXd& quad_dist_obs,
- const goal_obj& goal,
- UAM::CHT& HT,
- UAM::CArm& arm,
- const UAM::CQuad& quad,
- UAM::CCtrlParams& CtrlParams,
- MatrixXd& robot_pos,
- MatrixXd& robot_vel);
+ void htpc(const MatrixXd& quad_dist_obs,
+ const goal_obj& goal,
+ UAM::CHT& HT,
+ UAM::CArm& arm,
+ const UAM::CQuad& quad,
+ UAM::CCtrlParams& CtrlParams,
+ MatrixXd& robot_pos,
+ MatrixXd& robot_vel);
};
#endif