Beispiel #1
0
// this check collision function was taken from Lars's code example, I think to remember...
bool checkCollisions(rw::models::Device::Ptr device, const rw::kinematics::State &state, const rw::proximity::CollisionDetector &detector, const rw::math::Q &q) {
    rw::kinematics::State testState = state;
    rw::proximity::CollisionDetector::QueryResult data;
    bool ret = true;

    device->setQ(q,testState);
    if (detector.inCollision(testState,&data)) {
        ret = false;
    }
    return ret;
}
Beispiel #2
0
void OldGripper::updateGripper(
                               rw::models::WorkCell::Ptr wc,
                               rwsim::dynamics::DynamicWorkCell::Ptr dwc,
                               rw::models::Device::Ptr dev,
                               rwsim::dynamics::RigidDevice::Ptr ddev,
                               rw::kinematics::State& state,
                               MovableFrame::Ptr tcpFrame
                               ) {

    Geometry::Ptr baseGeometry = getBaseGeometry();
    Geometry::Ptr leftGeometry = getFingerGeometry();
    Geometry::Ptr rightGeometry = getFingerGeometry();

    // remove existing objects
    DEBUG << "- Removing objects..." << endl;
    //wc->removeObject(wc->findObject("gripper.Base").get());
    wc->removeObject(wc->findObject("gripper.LeftFinger").get());
    wc->removeObject(wc->findObject("gripper.RightFinger").get());
    DEBUG << "- Objects removed." << endl;

    // create and add new objects
    DEBUG << "- Adding new objects..." << endl;

    // if base is parametrized, the box has to be moved from origin by half its height
    /*Transform3D<> baseT;
    baseT = Transform3D<>(-0.5 * _basez * Vector3D<>::z());

    RigidObject* baseobj = new RigidObject(wc->findFrame("gripper.Base"));
    Model3D* basemodel = new Model3D("BaseModel");
    basemodel->addTriMesh(Model3D::Material("stlmat", 0.4f, 0.4f, 0.4f), *baseGeometry->getGeometryData()->getTriMesh());
    basemodel->setTransform(baseT);
    baseGeometry->setTransform(baseT);
    baseobj->addModel(basemodel);
    baseobj->addGeometry(baseGeometry);
    wc->add(baseobj);
    dwc->findBody("gripper.Base")->setObject(baseobj);*/

    RigidObject* leftobj = new RigidObject(wc->findFrame("gripper.LeftFinger"));
    Model3D* leftmodel = new Model3D("LeftModel");
    leftmodel->addTriMesh(Model3D::Material("stlmat", 0.4f, 0.4f, 0.4f), *leftGeometry->getGeometryData()->getTriMesh());
    leftmodel->setTransform(Transform3D<>());
    leftGeometry->setTransform(Transform3D<>());
    leftobj->addModel(leftmodel);
    leftobj->addGeometry(leftGeometry);
    wc->add(leftobj);
    dwc->findBody("gripper.LeftFinger")->setObject(leftobj);

    RigidObject* rightobj = new RigidObject(wc->findFrame("gripper.RightFinger"));
    Model3D* rightmodel = new Model3D("RightModel");
    rightmodel->addTriMesh(Model3D::Material("stlmat", 0.4f, 0.4f, 0.4f), *rightGeometry->getGeometryData()->getTriMesh());
    rightmodel->setTransform(Transform3D<>(Vector3D<>(), Rotation3D<>(1, 0, 0, 0, 1, 0, 0, 0, -1)));
    rightGeometry->setTransform(Transform3D<>(Vector3D<>(), Rotation3D<>(1, 0, 0, 0, 1, 0, 0, 0, -1)));
    rightobj->addModel(rightmodel);
    rightobj->addGeometry(rightGeometry);
    wc->add(rightobj);
    dwc->findBody("gripper.RightFinger")->setObject(rightobj);
    DEBUG << "Objects added." << endl;

    // set tcp
    tcpFrame->setTransform(Transform3D<>(Vector3D<>(0, 0, _length - _tcpoffset)), state);

    // set bounds
    double minOpening = 0.5 * _jawdist;

    dev->setBounds(make_pair(Q(1, minOpening), Q(1, minOpening + 0.5 * _stroke)));
    dev->setQ(Q(1, minOpening), state);

    // set force
    ddev->setMotorForceLimits(Q(2, _force, _force));

    DEBUG << "Gripper updated!" << endl;
}