Matrix3d toMatrix3d(const btMatrix3x3& basis)
{
Matrix3d rotation;
btVector3 col0 = basis.getColumn(0);
btVector3 col1 = basis.getColumn(1);
btVector3 col2 = basis.getColumn(2);
rotation.col(0) = toVector3d(col0);
rotation.col(1) = toVector3d(col1);
rotation.col(2) = toVector3d(col2);
return rotation;
}
bool BulletModel::collisionPointsAllToAll(const bool use_margins,
vector<PointPair>& collision_points)
{
BulletResultCollector c;
MatrixXd normals;
vector<double> distance;
BulletCollisionWorldWrapper& bt_world = getBulletWorld(use_margins);
bt_world.bt_collision_world->performDiscreteCollisionDetection();
int numManifolds = bt_world.bt_collision_world->getDispatcher()->getNumManifolds();
for (int i=0;i<numManifolds;i++)
{
btPersistentManifold* contactManifold = bt_world.bt_collision_world->getDispatcher()->getManifoldByIndexInternal(i);
const btCollisionObject* obA = contactManifold->getBody0();
const btCollisionObject* obB = contactManifold->getBody1();
auto elementA = static_cast< Element* >(obA->getUserPointer());
auto elementB = static_cast< Element* >(obB->getUserPointer());
DrakeShapes::Shape shapeA = elementA->getShape();
DrakeShapes::Shape shapeB = elementB->getShape();
double marginA = 0;
double marginB = 0;
if (shapeA == DrakeShapes::MESH ||
shapeA == DrakeShapes::MESH_POINTS ||
shapeA == DrakeShapes::BOX) {
marginA = obA->getCollisionShape()->getMargin();
}
if (shapeB == DrakeShapes::MESH ||
shapeB == DrakeShapes::MESH_POINTS ||
shapeB == DrakeShapes::BOX) {
marginB = obB->getCollisionShape()->getMargin();
}
int numContacts = contactManifold->getNumContacts();
for (int j=0;j<numContacts;j++)
{ btManifoldPoint& pt = contactManifold->getContactPoint(j);
if (pt.getDistance()+marginA+marginB<0.f)
{
const btVector3& normalOnB = pt.m_normalWorldOnB;
const btVector3& ptA = pt.getPositionWorldOnA() + normalOnB*marginA;
const btVector3& ptB = pt.getPositionWorldOnB() - normalOnB*marginB;
c.addSingleResult(elementA->getId(),elementB->getId(),toVector3d(ptA),toVector3d(ptB),
toVector3d(normalOnB),(double) pt.getDistance());
}
}
}
collision_points = c.getResults();
return c.pts.size() > 0;
}
// CartPoseCostCalculator(const CartPoseCostCalculator& other) : pose_(other.pose_), manip_(other.manip_), rs_(other.rs_) {}
VectorXd operator()(const VectorXd& dof_vals) const {
manip_->SetDOFValues(toDblVec(dof_vals));
OR::Transform newpose = link_->GetTransform();
OR::Transform pose_err = pose_inv_ * newpose;
VectorXd err = concat(rotVec(pose_err.rot), toVector3d(pose_err.trans));
return err;
}
Eigen::Vector3d getValueVec3(tinyxml2::XMLElement* _parentElement, const std::string& _name)
{
assert(_parentElement != nullptr);
assert(!_name.empty());
std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText();
return toVector3d(str);
}
se3::SE3 toPinocchioSE3(const fcl::Transform3f & tf)
{
return se3::SE3(toMatrix3d(tf.getRotation()), toVector3d(tf.getTranslation()));
}
// Get closest point on outer object in global frame,
Eigen::Vector3d closestPointOuter () const { return toVector3d(fcl_distance_result.nearest_points [1]); }
bool BulletModel::findClosestPointsBtwElements(const ElementId idA,
const ElementId idB,
const bool use_margins,
std::unique_ptr<ResultCollector>& c)
{
btConvexShape* shapeA;
btConvexShape* shapeB;
btGjkPairDetector::ClosestPointInput input;
btPointCollector gjkOutput;
BulletCollisionWorldWrapper& bt_world = getBulletWorld(use_margins);
auto bt_objA_iter = bt_world.bt_collision_objects.find(idA);
if (bt_objA_iter == bt_world.bt_collision_objects.end())
return false;
auto bt_objB_iter = bt_world.bt_collision_objects.find(idB);
if (bt_objB_iter == bt_world.bt_collision_objects.end())
return false;
unique_ptr<btCollisionObject>& bt_objA = bt_objA_iter->second;
unique_ptr<btCollisionObject>& bt_objB = bt_objB_iter->second;
shapeA = (btConvexShape*) bt_objA->getCollisionShape();
shapeB = (btConvexShape*) bt_objB->getCollisionShape();
btGjkEpaPenetrationDepthSolver epa;
btVoronoiSimplexSolver sGjkSimplexSolver;
sGjkSimplexSolver.setEqualVertexThreshold(0.f);
btGjkPairDetector convexConvex(shapeA,shapeB,&sGjkSimplexSolver,&epa);
input.m_transformA = bt_objA->getWorldTransform();
input.m_transformB = bt_objB->getWorldTransform();
convexConvex.getClosestPoints(input ,gjkOutput,0);
btVector3 pointOnAinWorld;
btVector3 pointOnBinWorld;
if (elements[idA]->getShape() == DrakeShapes::MESH ||
elements[idA]->getShape() == DrakeShapes::MESH_POINTS ||
elements[idA]->getShape() == DrakeShapes::BOX) {
pointOnAinWorld = gjkOutput.m_pointInWorld +
gjkOutput.m_normalOnBInWorld*(gjkOutput.m_distance+shapeA->getMargin());
} else {
pointOnAinWorld = gjkOutput.m_pointInWorld +
gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;
}
if (elements[idB]->getShape() == DrakeShapes::MESH ||
elements[idB]->getShape() == DrakeShapes::MESH_POINTS ||
elements[idB]->getShape() == DrakeShapes::BOX) {
pointOnBinWorld = gjkOutput.m_pointInWorld - gjkOutput.m_normalOnBInWorld*shapeB->getMargin();
} else {
pointOnBinWorld = gjkOutput.m_pointInWorld;
}
btVector3 pointOnElemA = input.m_transformA.invXform(pointOnAinWorld);
btVector3 pointOnElemB = input.m_transformB.invXform(pointOnBinWorld);
VectorXd pointOnA_1(4);
VectorXd pointOnB_1(4);
pointOnA_1 << toVector3d(pointOnElemA), 1;
pointOnB_1 << toVector3d(pointOnElemB), 1;
Vector3d pointOnA;
Vector3d pointOnB;
pointOnA << elements[idA]->getLocalTransform().topRows(3)*pointOnA_1;
pointOnB << elements[idB]->getLocalTransform().topRows(3)*pointOnB_1;
btScalar distance = gjkOutput.m_normalOnBInWorld.dot(pointOnAinWorld-pointOnBinWorld);
if (gjkOutput.m_hasResult) {
c->addSingleResult(idA,idB,pointOnA,pointOnB, toVector3d(gjkOutput.m_normalOnBInWorld),(double) distance);
} else {
c->addSingleResult(idA,
idB,
Vector3d::Zero(),
Vector3d::Zero(),
Vector3d::Zero(),1.0);
cerr << "In BulletModel::findClosestPointsBtwElements: No closest point found!" << endl;
}
return (c->pts.size() > 0);
}
// CartPoseCostCalculator(const CartPoseCostCalculator& other) : pose_(other.pose_), manip_(other.manip_), rs_(other.rs_) {}
VectorXd operator()(const VectorXd& dof_vals) {
manip_->SetDOFValues(toDblVec(dof_vals));
OR::Transform newpose = link_->GetTransform();
return pt_world_ - toVector3d(newpose.trans);
}
