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