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