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); }