Merge pull request #77 from gergondet/topic/DimWeightPostureTask

Dimensional weight for posture task
jrl-umi3218 · Jul 22, 2021 · a03cabf · a03cabf
2 parents d386707 + d984eb3
commit a03cabf
Show file tree

Hide file tree

Showing 4 changed files with 38 additions and 4 deletions.
diff --git a/src/QPTasks.cpp b/src/QPTasks.cpp
@@ -744,6 +744,7 @@ PostureTask::PostureTask(const std::vector<rbd::MultiBody> & 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<rbd::MultiBody> & 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<rbd::MultiBody> & 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
@@ -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
@@ -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
@@ -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<double>() / 4.) * rbd::computeCoM(mb, mbcInit));