Skip to content

DistanceConstr #131

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
anastasiabolotnikova wants to merge 2 commits into
base: master
Choose a base branch
from
Open

DistanceConstr #131

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
9a4861c
Adding reversed update for collision/distance constraint
anastasiabolotnikova Jul 22, 2026
29ae472
Renamed CollisionConstr to be DistanceConstr
anastasiabolotnikova Jul 22, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions binding/python/tasks/qp/c_qp.pxd
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -562,8 +562,8 @@ cdef extern from "<Tasks/QPContactConstr.h>" namespace "tasks::qp":
void removeFromSolver(QPSolver &)

cdef extern from "<Tasks/QPConstr.h>" 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
Expand Down
6 changes: 3 additions & 3 deletions binding/python/tasks/qp/qp.pxd
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
8 changes: 4 additions & 4 deletions binding/python/tasks/qp/qp.pyx
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
4 changes: 2 additions & 2 deletions binding/python/tests/test_qp_solver.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
114 changes: 57 additions & 57 deletions src/QPConstr.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -330,54 +330,54 @@ 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<BodyCollData> bcds,
int collId,
sch::S_Object * body1,
sch::S_Object * body2,
double di,
double ds,
double damp,
double dampOff)
DistanceConstr::CollData::CollData(std::vector<BodyCollData> 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<rbd::MultiBody> & mbs, double step)
DistanceConstr::DistanceConstr(const std::vector<rbd::MultiBody> & 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();
fullJac_.resize(1, maxDof);
distJac_.resize(1, maxDof);
}

void CollisionConstr::addCollision(const std::vector<rbd::MultiBody> & 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<rbd::MultiBody> & 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<size_t>(r1Index)];
const rbd::MultiBody mb2 = mbs[static_cast<size_t>(r2Index)];
Expand All @@ -395,7 +395,7 @@ void CollisionConstr::addCollision(const std::vector<rbd::MultiBody> & 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; });
Expand All @@ -408,36 +408,36 @@ 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; });
if(it != dataVec_.end()) { return *it; }
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<Eigen::DenseIndex>(dataVec_.size()), nrVars_);
bInEq_.setZero(static_cast<Eigen::DenseIndex>(dataVec_.size()));
}

void CollisionConstr::updateNrVars(const std::vector<rbd::MultiBody> & /* mb */, const SolverData & data)
void DistanceConstr::updateNrVars(const std::vector<rbd::MultiBody> & /* mb */, const SolverData & data)
{
totalAlphaD_ = data.totalAlphaD();
nrVars_ = data.nrVars();
updateNrCollisions();
}

void CollisionConstr::update(const std::vector<rbd::MultiBody> & mbs,
const std::vector<rbd::MultiBodyConfig> & mbcs,
const SolverData & data)
void DistanceConstr::update(const std::vector<rbd::MultiBody> & mbs,
const std::vector<rbd::MultiBodyConfig> & mbcs,
const SolverData & data)
{
using namespace Eigen;

Expand Down Expand Up @@ -476,7 +476,7 @@ void CollisionConstr::update(const std::vector<rbd::MultiBody> & 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))

@arntanguy arntanguy Jul 24, 2026

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Could you document this behavior?

If I understand correctly:

  • if di > ds this is a collision avoidance constraint
  • di < ds this is a max distance constraint that gets activated when we are di distance away from the surface and we cannot go more than ds distance away from it

This is correct here, but if di == ds the constraint makes no sense and we risk dividing by 0.

      double dampers = d.damping * ((d.distance - d.ds) / (d.di - d.ds));

I haven't checked if we have unit tests for that, if we don't it'd be a good idea to add one.

{
// automatic damping computation if needed
if(d.dampingType == CollData::DampingType::Free)
Expand All @@ -491,7 +491,7 @@ void CollisionConstr::update(const std::vector<rbd::MultiBody> & 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)
Expand Down Expand Up @@ -527,8 +527,8 @@ void CollisionConstr::update(const std::vector<rbd::MultiBody> & 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;

@arntanguy arntanguy Jul 24, 2026

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

So it's a loop on d.bodies.size() which is always 2 I guess? Confusing, but if it ain't broke...

}
++nrActivated_;
}
Expand All @@ -541,10 +541,10 @@ void CollisionConstr::update(const std::vector<rbd::MultiBody> & mbs,
}
}

std::string CollisionConstr::nameInEq() const
{ return "SelfCollisionConstr"; }
std::string DistanceConstr::nameInEq() const
{ return "DistanceConstr"; }

std::string CollisionConstr::descInEq(const std::vector<rbd::MultiBody> & mbs, int line)
std::string DistanceConstr::descInEq(const std::vector<rbd::MultiBody> & mbs, int line)
{
int curLine = 0;
for(CollData & d : dataVec_)
Expand Down Expand Up @@ -575,23 +575,23 @@ std::string CollisionConstr::descInEq(const std::vector<rbd::MultiBody> & 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<rbd::MultiBody> & mbs,
const std::vector<rbd::MultiBodyConfig> & mbcs,
const CollData & cd,
const Eigen::Vector3d & normVecDist,
double dist) const
double DistanceConstr::computeDamping(const std::vector<rbd::MultiBody> & mbs,

@arntanguy arntanguy Jul 24, 2026

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We should maybe ensure that the damping heuristics here behave well for unusual cases, e.g large distances. This has AFAIK only been used in contexts where we wish to avoid collisions on rather small distances, with an interaction distance rather close to the safety distance.

const std::vector<rbd::MultiBodyConfig> & mbcs,
const CollData & cd,
const Eigen::Vector3d & normVecDist,
double dist) const
{
Eigen::Vector3d diffVel(Eigen::Vector3d::Zero());
double sign = 1.;
Expand Down
8 changes: 4 additions & 4 deletions src/Tasks/QPConstr.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -285,14 +285,14 @@ class TASKS_DLLAPI DamperJointLimitsConstr : public ConstraintFunction<Bound>
* the following formula:
* \f[ \xi = -\frac{d_i - d_s}{d - d_s}\alpha + \xi_{\text{off}} \f]
*/
class TASKS_DLLAPI CollisionConstr : public ConstraintFunction<Inequality>
class TASKS_DLLAPI DistanceConstr : public ConstraintFunction<Inequality>

@arntanguy arntanguy Jul 24, 2026 •
edited
Loading

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You can add

using CollisionConstr [[deprecated("Use DistanceConstr instead.")]] = DistanceConstr;

after the class to keep the old name around for backwards compatibility.

Note: with this, this PR can be merged independently of mc_rtc

{
public:
/**
* @param mbs Multi-robot system.
* @param step Time step in second.
*/
CollisionConstr(const std::vector<rbd::MultiBody> & mbs, double step);
DistanceConstr(const std::vector<rbd::MultiBody> & mbs, double step);

/**
* Add a collision avoidance constraint.
Expand Down Expand Up @@ -448,8 +448,8 @@ class TASKS_DLLAPI CollisionConstr : public ConstraintFunction<Inequality>

int nrVars_;

CollisionConstr(const CollisionConstr &) = delete;
CollisionConstr & operator=(const CollisionConstr &) = delete;
DistanceConstr(const DistanceConstr &) = delete;
DistanceConstr & operator=(const DistanceConstr &) = delete;
};

/**
Expand Down
4 changes: 2 additions & 2 deletions tests/QPSolverTest.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down