TableRopeScene::TableRopeScene(fs::path ropeFile, bool telekinesis) : GrabbingScene(telekinesis) { // vector<double> firstJoints = doubleVecFromFile((KNOT_DATA / "init_joints_train.txt").string()); // ValuesInds vi = getValuesInds(firstJoints); // setupDefaultROSRave(); // pr2m->pr2->setDOFValues(vi.second, vi.first); vector<int> indices(1,pr2m->pr2->robot->GetJointIndex("torso_lift_joint")); vector<double> values(1, .31); pr2m->pr2->setDOFValues(indices, values); vector<btVector3> tableCornersWorld = toBulletVectors(floatMatFromFile((KNOT_DATA / "table_corners.txt").string())) * METERS; vector<btVector3> controlPointsWorld = toBulletVectors(floatMatFromFile(ropeFile.string())) * METERS; PlotPoints::Ptr corners(new PlotPoints(20)); corners->setPoints(tableCornersWorld); env->add(corners); m_table = makeTable(tableCornersWorld, .1*GeneralConfig::scale); float seglen = controlPointsWorld[0].distance(controlPointsWorld[1]); // m_rope.reset(new CapsuleRope(controlPointsWorld, fmin(seglen/4.1,.0075*METERS))); m_rope.reset(new CapsuleRope(controlPointsWorld, .0075*METERS)); env->add(m_rope); env->add(m_table); setGrabBodies(m_rope->children); }
TableRopeScene::TableRopeScene(const vector<btVector3> &tableCornersWorld_, const vector<btVector3>& controlPointsWorld, bool telekinesis) : GrabbingScene(telekinesis), tableCornersWorld(tableCornersWorld_) { vector<int> indices(1,pr2m->pr2->robot->GetJointIndex("torso_lift_joint")); vector<double> values(1, .31); pr2m->pr2->setDOFValues(indices, values); PlotPoints::Ptr corners(new PlotPoints(20)); corners->setPoints(tableCornersWorld); env->add(corners); m_table = makeTable(tableCornersWorld, .1*GeneralConfig::scale); float seglen = controlPointsWorld[0].distance(controlPointsWorld[1]); m_rope.reset(new CapsuleRope( controlPointsWorld, .005*METERS, // radius .5, // angStiffness 1, // angDamping .9, // linDamping .8, // angLimit .9 // linStopErp )); env->add(m_rope); env->add(m_table); setGrabBodies(m_rope->children); }