diff --git a/src/tasks/eepos.cpp b/src/tasks/eepos.cpp
index 37aab95d89c3e2fb2bfd7ab727ad8094de817dd6..5de80b6b7fced6114206397b0955a06144a092dc 100755
--- a/src/tasks/eepos.cpp
+++ b/src/tasks/eepos.cpp
@@ -117,37 +117,25 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M
sigmaEEPOS.block(0,0,3,1) = eepos_des.block(0,0,3,1) - HT.cam_in_w.block(0,3,3,1);
// End-Effector Orientation Error
- // sigmaEEPOS.block(3,0,3,1) = eepos_des.block(3,0,3,1) - RToEulerContinuous(HT.cam_in_w.block(0,0,3,3),eepos_des.block(3,0,3,1));
+ sigmaEEPOS.block(3,0,3,1) = eepos_des.block(3,0,3,1) - RToEulerContinuous(HT.cam_in_w.block(0,0,3,3),eepos_des.block(3,0,3,1));
- // // Position and orientation error
+ // Position and orientation error
// Eigen::Translation<double,3> p_des(eepos_des(0,0), eepos_des(1,0), eepos_des(2,0));
// Eigen::Quaterniond q_des = Eigen::AngleAxisd(eepos_des(3,0), Eigen::Vector3d::UnitX())
// * Eigen::AngleAxisd(eepos_des(4,0), Eigen::Vector3d::UnitY())
// * Eigen::AngleAxisd(eepos_des(5,0), Eigen::Vector3d::UnitZ());
- // Eigen::Transform<double, 3, Eigen::Affine> HT_des_in_w = p_des*q_des;
+ // Eigen::Transform<double, 3, Eigen::Affine> HT_des_in_w = p_des*q_des; // OK
//
- // Eigen::Transform<double, 3, Eigen::Affine> HT_robot_in_w(HT.cam_in_w);
+ // Eigen::Transform<double, 3, Eigen::Affine> HT_robot_in_w(HT.cam_in_w); // OK
+ // Eigen::Transform<double, 3, Eigen::Affine> HT_w_in_robot = HT_robot_in_w.inverse();
//
- // Eigen::Transform<double, 3, Eigen::Affine> HT_w_in_des = HT_des_in_w.inverse();
+ // Eigen::Transform<double, 3, Eigen::Affine> HT_w_in_des = HT_des_in_w.inverse(); // OK
//
// Eigen::Transform<double, 3, Eigen::Affine> HT_robot_in_des = HT_robot_in_w*HT_w_in_des;
+ // Eigen::Transform<double, 3, Eigen::Affine> HT_des_in_robot = HT_des_in_w*HT_w_in_robot;
//
- //
-
-
- // Eigen::Transform<double, 3, Eigen::Affine> HT_aux = HT_des_in_w.inverse()*HT_robot_in_w;
- // Eigen::Transform<double, 3, Eigen::Affine> HT_error = HT_aux.inverse();
-
- // Eigen::Translation<double,3> p_curr_in_w(0.0,0.0,0.0);
- // Eigen::Transform<double, 3, Eigen::Affine> HT_robot_in_w_in_w;
- // HT_robot_in_w_in_w = HT_robot_in_w;
- // HT_robot_in_w_in_w.translation() = p_curr_in_w;
- //
- // HT_error = HT_robot_in_w_in_w*HT_error;
-
- // sigmaEEPOS.block(0,0,3,1) = HT_des_in_w.rotation() * HT_robot_in_des.translation();
- // std::cout << "Error" << '\n' << sigmaEEPOS.block(0,0,3,1) << '\n';
- // sigmaEEPOS.block(3,0,3,1) = HT_error.rotate(HT_robot_in_w.inverse().rotation()).rotation().eulerAngles(0, 1, 2);
+ // sigmaEEPOS.block(0,0,3,1) = HT_des_in_w.translation() - HT_robot_in_w.translation(); // OK
+ // sigmaEEPOS.block(3,0,3,1) = eepos_des.block(3,0,3,1) - HT_robot_in_w.rotation().eulerAngles(0,1,2);
// Task Jacobian _____________________________________________
diff --git a/src/tasks/jl_ineq.cpp b/src/tasks/jl_ineq.cpp
index 6105f96bc666d81357cabb822e25b622d038c8c8..41c27f913dad4a3b2d95fa5a646d8c3c488ecb3b 100644
--- a/src/tasks/jl_ineq.cpp
+++ b/src/tasks/jl_ineq.cpp
@@ -18,14 +18,10 @@ void CTaskJLIn::TaskErrorJacFull(UAM::CArm& arm,
JL = Eigen::MatrixXd::Zero(arm.nj*2,4+arm.nj);
JL.block(0, 4, arm.nj, arm.nj) = Eigen::MatrixXd::Identity(arm.nj, arm.nj);
JL.block(arm.nj, 4, arm.nj, arm.nj) = -1*Eigen::MatrixXd::Identity(arm.nj, arm.nj);
- // JL.block(0, 4, arm.nj, arm.nj) = -1*Eigen::MatrixXd::Identity(arm.nj, arm.nj);
- // JL.block(arm.nj, 4, arm.nj, arm.nj) = Eigen::MatrixXd::Identity(arm.nj, arm.nj);
for (unsigned int ii = 0; ii < arm.nj; ++ii)
{
// Task sigma
- // sigmaL(ii,0) = arm.jnt_pos(ii,0) - jntlim_ub(ii,0); // q-ub<=0
- // sigmaL(ii + arm.nj,0) = jntlim_lb(ii,0) - arm.jnt_pos(ii,0); // lb-q<=0
// Note: Although the ideal formulation should consider the above equations,
// we formulate the errors as e >= 0 to keep compatibility with the other tasks
// which are also defined as e >= 0.
diff --git a/src/tasks/jlvel_ineq.cpp b/src/tasks/jlvel_ineq.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b6eda1a43943edeb02286523be20b63fba39cbf6
--- /dev/null
+++ b/src/tasks/jlvel_ineq.cpp
@@ -0,0 +1,41 @@
+// Ownsr/include/eigen3/Eigen/src/Core/Product.h:133: Eigen::Product<Lhs, Rhs, Option>::Product(const Lhs&, const Rhs&) [with _Lhs = Eigen::Block<Eigen::Matrix<double, -1, -1>, 1, -1, false>; _Rh
+#include "jlvel_ineq.h"
+#include <common_fc.h>
+
+namespace UAM {
+
+CTaskJLVelIn::CTaskJLVelIn(){}
+
+CTaskJLVelIn::~CTaskJLVelIn(){}
+
+void CTaskJLVelIn::TaskErrorJacFull(const Eigen::MatrixXd& jnt_vel,
+ const Eigen::VectorXd& jntlim_ub,
+ const Eigen::VectorXd& jntlim_lb,
+ Eigen::MatrixXd& sigmaL,
+ Eigen::MatrixXd& JL,
+ Eigen::MatrixXd& JL_pseudo)
+{
+ int jnt_num = jnt_vel.rows();
+
+ sigmaL = Eigen::MatrixXd::Zero(2*jnt_num, 1);
+ JL = Eigen::MatrixXd::Zero(2*jnt_num, jnt_num);
+
+ JL.block(0, 0, jnt_num, jnt_num) = Eigen::MatrixXd::Identity(jnt_num, jnt_num);
+ JL.block(jnt_num, 0, jnt_num, jnt_num) = -1*Eigen::MatrixXd::Identity(jnt_num, jnt_num);
+
+ for (unsigned int ii = 0; ii < jnt_num; ++ii)
+ {
+ // Task sigma
+ // Note: Although the ideal formulation should consider the above equations,
+ // we formulate the errors as e >= 0 to keep compatibility with the other tasks
+ // which are also defined as e >= 0.
+ // Inside the solver code there is a fix to solve for e<=0.
+ sigmaL(ii,0) = jntlim_ub(ii) - jnt_vel(ii, 0); // ub-q>=0
+ sigmaL(ii + jnt_num,0) = jnt_vel(ii, 0) - jntlim_lb(ii); // q-lb>=0
+ }
+
+ // Task Jacobian Pseudo-inverse
+ JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL);
+}
+
+} // End of UAM namespace
diff --git a/src/tasks/jlvel_ineq.h b/src/tasks/jlvel_ineq.h
new file mode 100644
index 0000000000000000000000000000000000000000..e7fa0f1de17b32dc4b54dec937153b9a9700be9c
--- /dev/null
+++ b/src/tasks/jlvel_ineq.h
@@ -0,0 +1,50 @@
+#ifndef _TASK_JLVELIN_H
+#define _TASK_JLVELIN_H
+
+// Eigen
+#include <eigen3/Eigen/Dense>
+
+// Own
+#include <common_obj.h>
+
+namespace UAM {
+
+class CTaskJLVelIn
+{
+ private:
+
+ public:
+
+ /**
+ * \brief Constructor
+ *
+ * Class constructor.
+ *
+ */
+ CTaskJLVelIn();
+
+ /**
+ * \brief Destructor
+ *
+ * Class destructor.
+ *
+ */
+ ~CTaskJLVelIn();
+
+ /**
+ * \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.
+ * The returned Jacobian has full size (Arm DoF rows) and similarly the pseudo inverse of the jacobian.
+ *
+ */
+ static void TaskErrorJacFull(const Eigen::MatrixXd& jnt_vel,
+ const Eigen::VectorXd& jntlim_ub,
+ const Eigen::VectorXd& jntlim_lb,
+ Eigen::MatrixXd& sigmaL,
+ Eigen::MatrixXd& JL, Eigen::MatrixXd& JL_pseudo);
+};
+
+} // End of UAM namespace
+
+#endif