diff --git a/src/QPTasks.cpp b/src/QPTasks.cpp index 86d60c34..f8f562ea 100644 --- a/src/QPTasks.cpp +++ b/src/QPTasks.cpp @@ -744,6 +744,7 @@ PostureTask::PostureTask(const std::vector & mbs, alphaDBegin_(0), jointDatas_(), Q_(mbs[rI].nrDof(), mbs[rI].nrDof()), C_(mbs[rI].nrDof()), alphaVec_(mbs[rI].nrDof()), refVel_(mbs[rI].nrDof()), refAccel_(mbs[rI].nrDof()) { + dimWeight_ = Eigen::VectorXd::Ones(C_.size()); refVel_.setZero(); refAccel_.setZero(); } @@ -808,7 +809,7 @@ void PostureTask::update(const std::vector & mbs, pt_.update(mb, mbc); rbd::paramToVector(mbc.alpha, alphaVec_); - Q_ = pt_.jac(); + Q_ = dimWeight_.asDiagonal() * pt_.jac(); C_.setZero(); int deb = mb.jointPosInDof(1); @@ -825,6 +826,7 @@ void PostureTask::update(const std::vector & mbs, + pjd.damping * (alphaVec_.segment(pjd.start, pjd.size) - refVel_.segment(pjd.start, pjd.size)) - refAccel_.segment(pjd.start, pjd.size); } + C_ = dimWeight_.asDiagonal() * C_; } const Eigen::MatrixXd & PostureTask::Q() const diff --git a/src/Tasks.cpp b/src/Tasks.cpp index 7bb9164d..27799af1 100644 --- a/src/Tasks.cpp +++ b/src/Tasks.cpp @@ -831,14 +831,14 @@ void PostureTask::update(const rbd::MultiBody & mb, const rbd::MultiBodyConfig & for(int i = 1; i < mb.nrJoints(); ++i) { // if dof == 1 is a prismatic/revolute joint - // else if dof == 4 is a spherical one + // else if dof == 3 is a spherical one // else is a fixed one if(mb.joint(i).dof() == 1) { eval_(pos) = q_[i][0] - mbc.q[i][0]; ++pos; } - else if(mb.joint(i).dof() == 4) + else if(mb.joint(i).dof() == 3) { Matrix3d orid(Quaterniond(q_[i][0], q_[i][1], q_[i][2], q_[i][3]).matrix()); diff --git a/src/Tasks/QPTasks.h b/src/Tasks/QPTasks.h index 7a05c73b..a82d41df 100644 --- a/src/Tasks/QPTasks.h +++ b/src/Tasks/QPTasks.h @@ -541,6 +541,17 @@ class TASKS_DLLAPI PostureTask : public Task return refAccel_; } + inline const Eigen::VectorXd & dimWeight() const noexcept + { + return dimWeight_; + } + + inline void dimWeight(const Eigen::VectorXd & dimW) noexcept + { + assert(dimW.size() == dimWeight_.size()); + dimWeight_ = dimW; + } + private: struct JointData { @@ -561,6 +572,7 @@ class TASKS_DLLAPI PostureTask : public Task Eigen::VectorXd C_; Eigen::VectorXd alphaVec_; Eigen::VectorXd refVel_, refAccel_; + Eigen::VectorXd dimWeight_; }; class TASKS_DLLAPI PositionTask : public HighLevelTask diff --git a/tests/QPSolverTest.cpp b/tests/QPSolverTest.cpp index c481e81b..c4da87f3 100644 --- a/tests/QPSolverTest.cpp +++ b/tests/QPSolverTest.cpp @@ -128,7 +128,7 @@ BOOST_AUTO_TEST_CASE(QPTaskTest) solver.addTask(&oriTaskSp); BOOST_CHECK_EQUAL(solver.nrTasks(), 1); - // Test OrientatioTask + // Test OrientationTask mbcs[0] = mbcInit; for(int i = 0; i < 10000; ++i) { @@ -162,6 +162,26 @@ BOOST_AUTO_TEST_CASE(QPTaskTest) BOOST_CHECK_SMALL(postureTask.task().eval().norm(), 0.00001); + // Test PostureTask with dimWeight + BOOST_CHECK_EQUAL(postureTask.dimWeight().size(), 3); + Eigen::VectorXd postureDimW = Eigen::VectorXd::Ones(3); + postureDimW(2) = 0.0; + postureTask.dimWeight(postureDimW); + postureTask.posture({{}, {0.2}, {0.4}, {-0.8}}); + + mbcs[0] = mbcInit; + for(int i = 0; i < 10000; ++i) + { + BOOST_REQUIRE(solver.solve(mbs, mbcs)); + eulerIntegration(mb, mbcs[0], 0.001); + + forwardKinematics(mb, mbcs[0]); + forwardVelocity(mb, mbcs[0]); + } + + BOOST_CHECK_SMALL(postureTask.task().eval().head(2).norm(), 0.00001); + BOOST_CHECK_SMALL(postureTask.task().eval().tail(1).norm() - 0.8, 0.00001); + solver.removeTask(&postureTask); Vector3d comD(RotZ(cst::pi() / 4.) * rbd::computeCoM(mb, mbcInit));