diff --git a/src/tasks/jl.cpp b/src/tasks/jl.cpp index b63d5c9724548bdd67db603e233375f038d4299b..9a99a9a176e6ae4c3c59290e8c9a961e2fabe42b 100644 --- a/src/tasks/jl.cpp +++ b/src/tasks/jl.cpp @@ -60,9 +60,9 @@ void CTaskJL::TaskErrorJacFull(UAM::CArm& arm, const Eigen::MatrixXd& jntlim_pos for (unsigned int ii = 0; ii < arm.nj; ++ii) { // Task sigma - sigmaL(ii,0) = AAL(ii,ii)*std::pow(jntlim_pos_error(ii,0),pow_n); + sigmaL(ii,0) = -AAL(ii,ii)*std::pow(jntlim_pos_error(ii,0),pow_n); // Task Jacobian - JL(ii,4+ii) = pow_n*AAL(ii,ii)*std::pow(jntlim_pos_error(ii,0),pow_n-1); + JL(ii,4+ii) = -pow_n*AAL(ii,ii)*std::pow(jntlim_pos_error(ii,0),pow_n-1); } // Task Jacobian Pseudo-inverse