Skip to content

Commit

Permalink
Merge pull request #82 from gergondet/topic/CollisionJointSelection
Browse files Browse the repository at this point in the history
  • Loading branch information
@gergondet
gergondet authored Jun 14, 2022
2 parents a3debfe + c84d810 commit 36a3527
Show file tree
Hide file tree
Showing 2 changed files with 43 additions and 22 deletions.
54 changes: 34 additions & 20 deletions src/QPConstr.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -360,8 +360,9 @@ CollisionConstr::BodyCollData::BodyCollData(const rbd::MultiBody & mb,
int rI,
const std::string & bName,
sch::S_Object * h,
const sva::PTransformd & X)
: hull(h), jac(mb, bName), X_op_o(X), rIndex(rI), bIndex(mb.bodyIndexByName(bName)), bodyName(bName)
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)
{
}

Expand Down Expand Up @@ -400,18 +401,22 @@ void CollisionConstr::addCollision(const std::vector<rbd::MultiBody> & mbs,
double di,
double ds,
double damping,
double dampingOff)
double dampingOff,
const Eigen::VectorXd & r1Selector,
const Eigen::VectorXd & r2Selector)
{
const rbd::MultiBody mb1 = mbs[r1Index];
const rbd::MultiBody mb2 = mbs[r2Index];
const rbd::MultiBody mb1 = mbs[static_cast<size_t>(r1Index)];
const rbd::MultiBody mb2 = mbs[static_cast<size_t>(r2Index)];
std::vector<BodyCollData> bodies;
if(mb1.nrDof() > 0)
{
bodies.emplace_back(mb1, r1Index, r1BodyName, body1, X_op1_o1);
assert(r1Selector.size() == 0 || r1Selector.size() == mb1.nrDof());
bodies.emplace_back(mb1, r1Index, r1BodyName, body1, X_op1_o1, r1Selector);
}
if(mb2.nrDof() > 0)
{
bodies.emplace_back(mb2, r2Index, r2BodyName, body2, X_op2_o2);
assert(r2Selector.size() == 0 || r2Selector.size() == mb2.nrDof());
bodies.emplace_back(mb2, r2Index, r2BodyName, body2, X_op2_o2, r1Index == r2Index ? r1Selector : r2Selector);
}
dataVec_.emplace_back(std::move(bodies), collId, body1, body2, di, ds, damping, dampingOff);
}
Expand Down Expand Up @@ -452,8 +457,8 @@ void CollisionConstr::reset()

void CollisionConstr::updateNrCollisions()
{
AInEq_.setZero(dataVec_.size(), nrVars_);
bInEq_.setZero(dataVec_.size());
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)
Expand All @@ -478,8 +483,8 @@ void CollisionConstr::update(const std::vector<rbd::MultiBody> & mbs,
// update moving hull position
for(BodyCollData & bcd : d.bodies)
{
const rbd::MultiBodyConfig & mbc = mbcs[bcd.rIndex];
bcd.hull->setTransformation(tosch(bcd.X_op_o * mbc.bodyPosW[bcd.bIndex]));
const rbd::MultiBodyConfig & mbc = mbcs[static_cast<size_t>(bcd.rIndex)];
bcd.hull->setTransformation(tosch(bcd.X_op_o * mbc.bodyPosW[static_cast<size_t>(bcd.bIndex)]));
}

d.distance = d.pair->getClosestPoints(pb1Tmp, pb2Tmp);
Expand All @@ -495,8 +500,9 @@ void CollisionConstr::update(const std::vector<rbd::MultiBody> & mbs,
for(std::size_t i = 0; i < d.bodies.size(); ++i)
{
BodyCollData & bcd = d.bodies[i];
const rbd::MultiBodyConfig & mbc = mbcs[bcd.rIndex];
nearestPoint = (sva::PTransformd(nearestPoint) * mbc.bodyPosW[bcd.bIndex].inv()).translation();
const rbd::MultiBodyConfig & mbc = mbcs[static_cast<size_t>(bcd.rIndex)];
nearestPoint =
(sva::PTransformd(nearestPoint) * mbc.bodyPosW[static_cast<size_t>(bcd.bIndex)].inv()).translation();

// change the jacobian end point
bcd.jac.point(nearestPoint);
Expand Down Expand Up @@ -524,8 +530,8 @@ void CollisionConstr::update(const std::vector<rbd::MultiBody> & mbs,
for(std::size_t i = 0; i < d.bodies.size(); ++i)
{
BodyCollData & bcd = d.bodies[i];
const rbd::MultiBody & mb = mbs[bcd.rIndex];
const rbd::MultiBodyConfig & mbc = mbcs[bcd.rIndex];
const rbd::MultiBody & mb = mbs[static_cast<size_t>(bcd.rIndex)];
const rbd::MultiBodyConfig & mbc = mbcs[static_cast<size_t>(bcd.rIndex)];

// Compute body1
const MatrixXd & jac = bcd.jac.jacobian(mb, mbc);
Expand All @@ -541,8 +547,16 @@ void CollisionConstr::update(const std::vector<rbd::MultiBody> & mbs,
double jqdnd = pSpeed.dot(dnf * step_);
double jdqdn = pNormalAcc.dot(nf * step_);

AInEq_.block(nrActivated_, data.alphaDBegin(bcd.rIndex), 1, mb.nrDof()).noalias() -=
fullJac_.block(0, 0, 1, mb.nrDof());
if(bcd.selector.size() == 0)
{
AInEq_.block(nrActivated_, data.alphaDBegin(bcd.rIndex), 1, mb.nrDof()).noalias() -=
fullJac_.block(0, 0, 1, mb.nrDof());
}
else
{
AInEq_.block(nrActivated_, data.alphaDBegin(bcd.rIndex), 1, mb.nrDof()).noalias() -=
fullJac_.block(0, 0, 1, mb.nrDof()) * bcd.selector.asDiagonal();
}
bInEq_(nrActivated_) += sign * (jqdn + jqdnd + jdqdn);
// little hack
// the max iteration number is two, so at the second iteration
Expand Down Expand Up @@ -582,7 +596,7 @@ std::string CollisionConstr::descInEq(const std::vector<rbd::MultiBody> & mbs, i
std::stringstream ss;
for(const BodyCollData & bcd : d.bodies)
{
const rbd::MultiBody & mb = mbs[bcd.rIndex];
const rbd::MultiBody & mb = mbs[static_cast<size_t>(bcd.rIndex)];
ss << "robot: " << bcd.rIndex << std::endl;
ss << "body: " << mb.body(bcd.bIndex).name() << std::endl;
}
Expand Down Expand Up @@ -630,8 +644,8 @@ double CollisionConstr::computeDamping(const std::vector<rbd::MultiBody> & mbs,
for(std::size_t i = 0; i < cd.bodies.size(); ++i)
{
const BodyCollData & bcd = cd.bodies[i];
const rbd::MultiBody & mb = mbs[bcd.rIndex];
const rbd::MultiBodyConfig & mbc = mbcs[bcd.rIndex];
const rbd::MultiBody & mb = mbs[static_cast<size_t>(bcd.rIndex)];
const rbd::MultiBodyConfig & mbc = mbcs[static_cast<size_t>(bcd.rIndex)];

Eigen::Vector3d velW = bcd.jac.velocity(mb, mbc).linear();

Expand Down
11 changes: 9 additions & 2 deletions src/Tasks/QPConstr.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -317,6 +317,9 @@ class TASKS_DLLAPI CollisionConstr : public ConstraintFunction<Inequality>
* @param ds \f$ d_s \f$.
* @param damping \f$ \xi \f$, if set to 0 the damping is computed automatically.
* @param dampingOff \f$ \xi_{\text{off}} \f$.
* @param r1Selector A joint selection vector for \p r1Index the default selects all joints
* @param r2Selector A joint selection vector for \p r2Index the default selects all joints,
* ignored if r1Index == r2Index
*/
void addCollision(const std::vector<rbd::MultiBody> & mbs,
int collId,
Expand All @@ -331,7 +334,9 @@ class TASKS_DLLAPI CollisionConstr : public ConstraintFunction<Inequality>
double di,
double ds,
double damping,
double dampingOff = 0.);
double dampingOff = 0.,
const Eigen::VectorXd & r1Selector = Eigen::VectorXd::Zero(0),
const Eigen::VectorXd & r2Selector = Eigen::VectorXd::Zero(0));

/**
* Remove a collision avoidance constraint.
Expand Down Expand Up @@ -374,13 +379,15 @@ class TASKS_DLLAPI CollisionConstr : public ConstraintFunction<Inequality>
int rIndex,
const std::string & bodyName,
sch::S_Object * hull,
const sva::PTransformd & X_op_o);
const sva::PTransformd & X_op_o,
const Eigen::VectorXd & selector);

sch::S_Object * hull;
rbd::Jacobian jac;
sva::PTransformd X_op_o;
int rIndex, bIndex;
std::string bodyName;
Eigen::VectorXd selector;
};

struct CollData
Expand Down

0 comments on commit 36a3527

Please sign in to comment.