diff --git a/binding/python/tasks/qp/c_qp.pxd b/binding/python/tasks/qp/c_qp.pxd index 979dafc8..2da57fca 100644 --- a/binding/python/tasks/qp/c_qp.pxd +++ b/binding/python/tasks/qp/c_qp.pxd @@ -562,8 +562,8 @@ cdef extern from "" namespace "tasks::qp": void removeFromSolver(QPSolver &) cdef extern from "" namespace "tasks::qp": - cdef cppclass CollisionConstr(ConstraintFunction[Inequality], Inequality, Constraint): - CollisionConstr(const vector[MultiBody]&, double) + cdef cppclass DistanceConstr(ConstraintFunction[Inequality], Inequality, Constraint): + DistanceConstr(const vector[MultiBody]&, double) void addCollision(const vector[MultiBody]&, int, int, const string&, S_Object*, const PTransformd&, int, const string&, S_Object*, const PTransformd&, double, double, double, double) bool rmCollision(int) int nrCollisions() const diff --git a/binding/python/tasks/qp/qp.pxd b/binding/python/tasks/qp/qp.pxd index 10eb1ea9..4f7b2da9 100644 --- a/binding/python/tasks/qp/qp.pxd +++ b/binding/python/tasks/qp/qp.pxd @@ -196,11 +196,11 @@ cdef class ContactPosConstr(ContactConstrCommon): cdef ContactPosConstr ContactPosConstrFromPtr(c_qp.ContactPosConstr *) -cdef class CollisionConstr(Inequality): - cdef c_qp.CollisionConstr * impl +cdef class DistanceConstr(Inequality): + cdef c_qp.DistanceConstr * impl cdef cppbool __own_impl -cdef CollisionConstr CollisionConstrFromPtr(c_qp.CollisionConstr*) +cdef DistanceConstr DistanceConstrFromPtr(c_qp.DistanceConstr*) cdef class CoMIncPlaneConstr(Inequality): cdef c_qp.CoMIncPlaneConstr * impl diff --git a/binding/python/tasks/qp/qp.pyx b/binding/python/tasks/qp/qp.pyx index 8f96b3fd..9ce2bf71 100644 --- a/binding/python/tasks/qp/qp.pyx +++ b/binding/python/tasks/qp/qp.pyx @@ -1878,14 +1878,14 @@ cdef ContactPosConstr ContactPosConstrFromPtr(c_qp.ContactPosConstr * p): ret.constraint_base = ret.impl return ret -cdef class CollisionConstr(Inequality): +cdef class DistanceConstr(Inequality): def __dealloc__(self): if self.__own_impl: del self.impl def __cinit__(self, MultiBodyVector mbs, double step, skip_alloc = False): self.__own_impl = True if not skip_alloc: - self.impl = new c_qp.CollisionConstr(deref(mbs.v), step) + self.impl = new c_qp.DistanceConstr(deref(mbs.v), step) self.cf_base = self.impl self.ineq_base = self.impl self.constraint_base = self.impl @@ -1915,8 +1915,8 @@ cdef class CollisionConstr(Inequality): def removeFromSolver(self, QPSolver solver): self.impl.removeFromSolver(deref(solver.impl)) -cdef CollisionConstr CollisionConstrFromPtr(c_qp.CollisionConstr * p): - cdef CollisionConstr ret = CollisionConstr(None, 0, skip_alloc = True) +cdef DistanceConstr DistanceConstrFromPtr(c_qp.DistanceConstr * p): + cdef DistanceConstr ret = DistanceConstr(None, 0, skip_alloc = True) ret.__own_impl = False ret.impl = ret.ineq_base = ret.constraint_base = p return ret diff --git a/binding/python/tests/test_qp_solver.py b/binding/python/tests/test_qp_solver.py index 2d521abd..6e018927 100644 --- a/binding/python/tests/test_qp_solver.py +++ b/binding/python/tests/test_qp_solver.py @@ -646,7 +646,7 @@ def test(self): pair = sch.CD_Pair(b0, b3) identity = sva.PTransformd.Identity() - autoCollConstr = tasks.qp.CollisionConstr(mbs, 0.001) + autoCollConstr = tasks.qp.DistanceConstr(mbs, 0.001) collId1 = 10 autoCollConstr.addCollision( mbs, collId1, 0, "b0", b0, identity, 0, "b3", b3, identity, 0.01, 0.005, 1 @@ -709,7 +709,7 @@ def test(self): b0.transform(mbcInit.bodyPosW[0]) identity = sva.PTransformd.Identity() - seCollConstr = tasks.qp.CollisionConstr(mbs, 0.001) + seCollConstr = tasks.qp.DistanceConstr(mbs, 0.001) collId1 = 10 seCollConstr.addCollision( mbs, collId1, 0, "b3", b3, identity, 1, "b0", b0, identity, 0.01, 0.005, 1 diff --git a/src/QPConstr.cpp b/src/QPConstr.cpp index e4803d92..62dfb013 100644 --- a/src/QPConstr.cpp +++ b/src/QPConstr.cpp @@ -309,7 +309,7 @@ double DamperJointLimitsConstr::computeDamper(double dist, double iDist, double { return damping * ((dist - sDist) / (iDist - sDist)); } /** - * CollisionConstr + * DistanceConstr */ sch::Matrix4x4 tosch(const sva::PTransformd & t) @@ -330,31 +330,31 @@ sch::Matrix4x4 tosch(const sva::PTransformd & t) return m; } -CollisionConstr::BodyCollData::BodyCollData(const rbd::MultiBody & mb, - int rI, - const std::string & bName, - sch::S_Object * h, - const sva::PTransformd & X, - const Eigen::VectorXd & selector) +DistanceConstr::BodyCollData::BodyCollData(const rbd::MultiBody & mb, + int rI, + const std::string & bName, + sch::S_Object * h, + const sva::PTransformd & X, + const Eigen::VectorXd & selector) : hull(h), jac(mb, bName), X_op_o(X), rIndex(rI), bIndex(mb.bodyIndexByName(bName)), bodyName(bName), selector(selector) { } -CollisionConstr::CollData::CollData(std::vector bcds, - int collId, - sch::S_Object * body1, - sch::S_Object * body2, - double di, - double ds, - double damp, - double dampOff) +DistanceConstr::CollData::CollData(std::vector bcds, + int collId, + sch::S_Object * body1, + sch::S_Object * body2, + double di, + double ds, + double damp, + double dampOff) : pair(new sch::CD_Pair(body1, body2)), distance(2 * di), normVecDist(Eigen::Vector3d::Zero()), di(di), ds(ds), damping(damp), bodies(std::move(bcds)), dampingType(damping > 0. ? DampingType::Hard : DampingType::Free), dampingOff(dampOff), collId(collId) { } -CollisionConstr::CollisionConstr(const std::vector & mbs, double step) +DistanceConstr::DistanceConstr(const std::vector & mbs, double step) : dataVec_(), step_(step), nrActivated_(0), totalAlphaD_(-1), AInEq_(), bInEq_(), fullJac_(), distJac_() { int maxDof = std::max_element(mbs.begin(), mbs.end(), compareDof)->nrDof(); @@ -362,22 +362,22 @@ CollisionConstr::CollisionConstr(const std::vector & mbs, double distJac_.resize(1, maxDof); } -void CollisionConstr::addCollision(const std::vector & mbs, - int collId, - int r1Index, - const std::string & r1BodyName, - sch::S_Object * body1, - const sva::PTransformd & X_op1_o1, - int r2Index, - const std::string & r2BodyName, - sch::S_Object * body2, - const sva::PTransformd & X_op2_o2, - double di, - double ds, - double damping, - double dampingOff, - const Eigen::VectorXd & r1Selector, - const Eigen::VectorXd & r2Selector) +void DistanceConstr::addCollision(const std::vector & mbs, + int collId, + int r1Index, + const std::string & r1BodyName, + sch::S_Object * body1, + const sva::PTransformd & X_op1_o1, + int r2Index, + const std::string & r2BodyName, + sch::S_Object * body2, + const sva::PTransformd & X_op2_o2, + double di, + double ds, + double damping, + double dampingOff, + const Eigen::VectorXd & r1Selector, + const Eigen::VectorXd & r2Selector) { const rbd::MultiBody mb1 = mbs[static_cast(r1Index)]; const rbd::MultiBody mb2 = mbs[static_cast(r2Index)]; @@ -395,7 +395,7 @@ void CollisionConstr::addCollision(const std::vector & mbs, dataVec_.emplace_back(std::move(bodies), collId, body1, body2, di, ds, damping, dampingOff); } -bool CollisionConstr::rmCollision(int collId) +bool DistanceConstr::rmCollision(int collId) { auto it = std::find_if(dataVec_.begin(), dataVec_.end(), [collId](const CollData & data) { return data.collId == collId; }); @@ -408,7 +408,7 @@ bool CollisionConstr::rmCollision(int collId) return false; } -auto CollisionConstr::getCollisionData(int collId) const -> const CollData & +auto DistanceConstr::getCollisionData(int collId) const -> const CollData & { auto it = std::find_if(dataVec_.begin(), dataVec_.end(), [&](const CollData & data) { return data.collId == collId; }); @@ -416,28 +416,28 @@ auto CollisionConstr::getCollisionData(int collId) const -> const CollData & throw std::runtime_error("No collision with the requested id"); } -std::size_t CollisionConstr::nrCollisions() const +std::size_t DistanceConstr::nrCollisions() const { return dataVec_.size(); } -void CollisionConstr::reset() +void DistanceConstr::reset() { dataVec_.clear(); } -void CollisionConstr::updateNrCollisions() +void DistanceConstr::updateNrCollisions() { AInEq_.setZero(static_cast(dataVec_.size()), nrVars_); bInEq_.setZero(static_cast(dataVec_.size())); } -void CollisionConstr::updateNrVars(const std::vector & /* mb */, const SolverData & data) +void DistanceConstr::updateNrVars(const std::vector & /* mb */, const SolverData & data) { totalAlphaD_ = data.totalAlphaD(); nrVars_ = data.nrVars(); updateNrCollisions(); } -void CollisionConstr::update(const std::vector & mbs, - const std::vector & mbcs, - const SolverData & data) +void DistanceConstr::update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data) { using namespace Eigen; @@ -476,7 +476,7 @@ void CollisionConstr::update(const std::vector & mbs, nearestPoint = d.p2; } - if(d.distance < d.di) + if((d.distance < d.di && d.di > d.ds) || (d.distance > d.di && d.di < d.ds)) { // automatic damping computation if needed if(d.dampingType == CollData::DampingType::Free) @@ -491,7 +491,7 @@ void CollisionConstr::update(const std::vector & mbs, Vector3d onf = d.normVecDist; Vector3d dnf = (nf - onf) / step_; - double sign = 1.; + double sign = (d.di > d.ds) ? 1.0 : -1.0; bInEq_(nrActivated_) = dampers; AInEq_.block(nrActivated_, 0, 1, totalAlphaD_).setZero(); for(std::size_t i = 0; i < d.bodies.size(); ++i) @@ -527,8 +527,8 @@ void CollisionConstr::update(const std::vector & mbs, bInEq_(nrActivated_) += sign * (jqdn + jqdnd + jdqdn); // little hack // the max iteration number is two, so at the second iteration - // sign will be -1 - sign = -1.; + // sign will be -sign + sign = -sign; } ++nrActivated_; } @@ -541,10 +541,10 @@ void CollisionConstr::update(const std::vector & mbs, } } -std::string CollisionConstr::nameInEq() const -{ return "SelfCollisionConstr"; } +std::string DistanceConstr::nameInEq() const +{ return "DistanceConstr"; } -std::string CollisionConstr::descInEq(const std::vector & mbs, int line) +std::string DistanceConstr::descInEq(const std::vector & mbs, int line) { int curLine = 0; for(CollData & d : dataVec_) @@ -575,23 +575,23 @@ std::string CollisionConstr::descInEq(const std::vector & mbs, i return ""; } -int CollisionConstr::nrInEq() const +int DistanceConstr::nrInEq() const { return nrActivated_; } -int CollisionConstr::maxInEq() const +int DistanceConstr::maxInEq() const { return int(dataVec_.size()); } -const Eigen::MatrixXd & CollisionConstr::AInEq() const +const Eigen::MatrixXd & DistanceConstr::AInEq() const { return AInEq_; } -const Eigen::VectorXd & CollisionConstr::bInEq() const +const Eigen::VectorXd & DistanceConstr::bInEq() const { return bInEq_; } -double CollisionConstr::computeDamping(const std::vector & mbs, - const std::vector & mbcs, - const CollData & cd, - const Eigen::Vector3d & normVecDist, - double dist) const +double DistanceConstr::computeDamping(const std::vector & mbs, + const std::vector & mbcs, + const CollData & cd, + const Eigen::Vector3d & normVecDist, + double dist) const { Eigen::Vector3d diffVel(Eigen::Vector3d::Zero()); double sign = 1.; diff --git a/src/Tasks/QPConstr.h b/src/Tasks/QPConstr.h index 0a104e94..b9d86933 100644 --- a/src/Tasks/QPConstr.h +++ b/src/Tasks/QPConstr.h @@ -285,14 +285,14 @@ class TASKS_DLLAPI DamperJointLimitsConstr : public ConstraintFunction * the following formula: * \f[ \xi = -\frac{d_i - d_s}{d - d_s}\alpha + \xi_{\text{off}} \f] */ -class TASKS_DLLAPI CollisionConstr : public ConstraintFunction +class TASKS_DLLAPI DistanceConstr : public ConstraintFunction { public: /** * @param mbs Multi-robot system. * @param step Time step in second. */ - CollisionConstr(const std::vector & mbs, double step); + DistanceConstr(const std::vector & mbs, double step); /** * Add a collision avoidance constraint. @@ -448,8 +448,8 @@ class TASKS_DLLAPI CollisionConstr : public ConstraintFunction int nrVars_; - CollisionConstr(const CollisionConstr &) = delete; - CollisionConstr & operator=(const CollisionConstr &) = delete; + DistanceConstr(const DistanceConstr &) = delete; + DistanceConstr & operator=(const DistanceConstr &) = delete; }; /** diff --git a/tests/QPSolverTest.cpp b/tests/QPSolverTest.cpp index 1c79863d..f6ea6dbe 100644 --- a/tests/QPSolverTest.cpp +++ b/tests/QPSolverTest.cpp @@ -900,7 +900,7 @@ BOOST_AUTO_TEST_CASE(QPAutoCollTest) sch::CD_Pair pair(&b0, &b3); PTransformd I = PTransformd::Identity(); - qp::CollisionConstr autoCollConstr(mbs, 0.001); + qp::DistanceConstr autoCollConstr(mbs, 0.001); int collId1 = 10; autoCollConstr.addCollision(mbs, collId1, 0, "b0", &b0, I, 0, "b3", &b3, I, 0.01, 0.005, 1.); BOOST_CHECK_EQUAL(autoCollConstr.nrCollisions(), 1); @@ -1011,7 +1011,7 @@ BOOST_AUTO_TEST_CASE(QPStaticEnvCollTest) b0.setTransformation(qp::tosch(mbcInit.bodyPosW[0])); PTransformd I = PTransformd::Identity(); - qp::CollisionConstr seCollConstr(mbs, 0.001); + qp::DistanceConstr seCollConstr(mbs, 0.001); int collId1 = 10; seCollConstr.addCollision(mbs, collId1, 0, "b3", &b3, I, 1, "b0", &b0, I, 0.01, 0.005, 1.); BOOST_CHECK_EQUAL(seCollConstr.nrCollisions(), 1);