void ViewerTest::SetViewer(OpenRAVE::EnvironmentBasePtr &penv, const std::string &viewername)
{
OpenRAVE::ViewerBasePtr viewer = OpenRAVE::RaveCreateViewer(penv, viewername);
BOOST_ASSERT(!!viewer);
// attach it to the environment:
penv->Add(viewer);
// finally call the viewer's infinite loop (this is why a separate thread is needed)
bool showgui = true;
viewer->main(showgui);
}
void run() {
foldnodeplot.reset(new PlotPoints());
env->add(foldnodeplot);
folddirplot.reset(new PlotLines(3));
env->add(folddirplot);
pickplot.reset(new PlotSpheres());
env->add(pickplot);
pickedNode = NULL;
leftManipAxes.reset(new PlotAxes);
env->add(leftManipAxes);
rightManipAxes.reset(new PlotAxes);
env->add(rightManipAxes);
// load the robot
pr2m.reset(new PR2Manager(*this));
if (FlatteningConfig::useFakeGripper) {
TelekineticGripper::Ptr fakeLeft(new TelekineticGripper(pr2m->pr2Left));
fakeLeft->setTransform(pr2m->pr2Left->getTransform());
env->add(fakeLeft);
gleft.reset(new GenManip(fakeLeft));
TelekineticGripper::Ptr fakeRight(new TelekineticGripper(pr2m->pr2Right));
fakeRight->setTransform(pr2m->pr2Right->getTransform());
gright.reset(new GenManip(fakeRight));
env->add(fakeRight);
pr2m->pr2->setTransform(btTransform(btQuaternion::getIdentity(), btVector3(0, 0, -100))); // out of view
} else {
gleft.reset(new GenManip(pr2m->pr2Left));
gright.reset(new GenManip(pr2m->pr2Right));
}
// create the table
const float table_height = .5;
const float table_thickness = .05;
tableExtents = GeneralConfig::scale * btVector3(.75,.75,table_thickness/2);
tableTrans = btTransform(btQuaternion(0, 0, 0, 1), GeneralConfig::scale * btVector3(0.8, 0, table_height-table_thickness/2));
BoxObject::Ptr table(new BoxObject(0, tableExtents, tableTrans));
table->rigidBody->setFriction(0.1);
env->add(table);
cout << "table margin: " << table->rigidBody->getCollisionShape()->getMargin() << endl;
// put the table in openrave
/*
OpenRAVE::KinBodyPtr raveTable = OpenRAVE::RaveCreateKinBody(rave->env);
raveTable->SetName("table");
vector<OpenRAVE::AABB> v;
v.push_back(OpenRAVE::AABB(util::toRaveTransform(table_trans, 1./pr2m->pr2->scale).trans, 1./pr2m->pr2->scale * util::toRaveVector(table_extents)));
raveTable->InitFromBoxes(v, true);
rave->env->AddKinBody(raveTable);
*/
#if 0
OpenRAVE::ViewerBasePtr raveViewer = OpenRAVE::RaveCreateViewer(rave->env, "qtcoin");
rave->env->AddViewer(raveViewer);
raveViewer->main(true);
#endif
const int resx = 45, resy = 31;
// const btScalar lenx = GeneralConfig::scale * 0.7, leny = GeneralConfig::scale * 0.5;
const btScalar lenx = GeneralConfig::scale * 0.7/2, leny = GeneralConfig::scale * 0.5/2;
// const btVector3 clothcenter = GeneralConfig::scale * btVector3(0.5, 0, table_height+0.01);
const btVector3 clothcenter = GeneralConfig::scale * btVector3(0.3, 0.1, table_height+0.01);
// cloth = makeSelfCollidingTowel(clothcenter, lenx, leny, resx, resy, env->bullet->softBodyWorldInfo);
cloth.reset(new Cloth(resx, resy, lenx, leny, clothcenter, env->bullet->softBodyWorldInfo));
env->add(cloth);
facepicker.reset(new SoftBodyFacePicker(*this, viewer.getCamera(), cloth->softBody.get()));
facepicker->setPickCallback(boost::bind(&CustomScene::pickCallback, this, _1));
sbgripperleft.reset(new GenPR2SoftGripper(pr2m->pr2, gleft, true));
sbgripperleft->setGrabOnlyOnContact(true);
sbgripperleft->setTarget(cloth);
GenPR2SoftGripperAction leftAction(pr2m->pr2, gleft->baseManip()->manip, sbgripperleft);
leftAction.setTarget(cloth);
leftAction.setExecTime(1.);
addVoidKeyCallback('a', boost::bind(&CustomScene::runGripperAction, this, leftAction));
addVoidKeyCallback('c', boost::bind(&CustomScene::graspPickedNode, this));
addVoidKeyCallback('f', boost::bind(&CustomScene::greedyFlattenSingle, this));
addVoidKeyCallback('F', boost::bind(&CustomScene::deepFlattenSingle, this));
addVoidKeyCallback('g', boost::bind(&CustomScene::liftCloth, this));
addPreDrawCallback(boost::bind(&CustomScene::markFolds, this));
addPreDrawCallback(boost::bind(&CustomScene::drawPick, this));
addPreDrawCallback(boost::bind(&CustomScene::drawManipAxes, this));
addPreDrawCallback(boost::bind(&GenPR2SoftGripper::dbgDraw, sbgripperleft.get(), this));
startViewer();
startFixedTimestepLoop(BulletConfig::dt);
}
