// 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;
}
bool velocityConstraint(rw::math::Q &dq, rw::models::Device::Ptr &device, double ×tep, double &tau){
if(device->getDOF() != dq.size()){
rw::common::Log::log().error() << "ERROR: Dimensions of the input of dq and device dof must agree in velocityConstraint.\n";
rw::common::Log::log().error() << " - dq: " << dq.size() << ", dof: " << device->getDOF() << "\n";
}
if(!(timestep > 0)){
rw::common::Log::log().error() << "ERROR: Timestep must be greater than 0.\n";
rw::common::Log::log().error() << " - dt: " << timestep << "\n";
}
bool ret = false;
rw::math::Q vC = device->getVelocityLimits();
// rw::common::Log::log().info() << " dq:\n" << dq << "\n";
// rw::common::Log::log().info() << " dq_act:\n" << dq/timestep << "\n";
// rw::common::Log::log().info() << " dq_vec:\n" << vC << "\n";
// find how much to fast it's moving
rw::math::Q timescale(vC.size());
double maxscale = 0;
for(unsigned int i = 0; i < timescale.size(); i++){
timescale(i) = fabs((dq(i) / timestep) / vC(i));
if(timescale(i) > maxscale){
maxscale = timescale(i);
}
}
// apply timescaling to make it go within the bounds
if(maxscale > 1){
tau = timestep * maxscale;
ret = true;
} else{
tau = timestep;
}
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;
}
