Vector3d latticeVectorHelper( Vector3d difference_along_normal, Vector3d v_dir ){
double length = difference_along_normal.length() / (difference_along_normal.normalize().dot(v_dir));
return v_dir * length;
}
/*Animate the camera in the timer event function
Note: La transformation de la camera est le seul parametre a etre animee.
Changer la position (ou le vecteur lateral ou up) ne fera rien lors de
l'animation.*/
void Widget3d::timerEvent( QTimerEvent* ipE )
{
if ( ipE->timerId() == mAnimationTimerId )
{
double animationTime = min( mAnimationDuration, mAnimationTimer.elapsed() );
double t = animationTime / (double)mAnimationDuration;
t = inversePower(t, 3);
/*Ici, on bircole une matrice (main droite) orthonormale qui réprésente
l'orientation de la caméra (pos, lat, up, look). On interpolera
la vielle orientation avec la nouvelle afin d'obtenir les positions,
ainsi que les vecteurs lat, up et look intermédiaires.*/
Vector3d lookVector( mOldCam.getLook(), mOldCam.getPos() );
lookVector.normalise();
Matrix4 m1( mOldCam.getLat(), mOldCam.getUp(), lookVector );
m1 = Matrix4( toVector(mOldCam.getPos()) ) * m1;
lookVector.set( mNewCam.getLook(), mNewCam.getPos() );
lookVector.normalise();
Matrix4 m2( mNewCam.getLat(), mNewCam.getUp(), lookVector );
m2 = Matrix4( toVector(mNewCam.getPos()) ) * m2;
Matrix4 iterationMatrix =
math::interpolate( m1, m2, t );
Vector3d interpolatedLook = toVector(mOldCam.getLook()) * (1 - t) +
toVector(mNewCam.getLook()) * t;
mCam.set( toPoint(iterationMatrix.getTranslationAsVector()),
toPoint(interpolatedLook),
Vector3d( iterationMatrix(1, 0), iterationMatrix(1, 1), iterationMatrix(1, 2) ) );
//--- animation de la projection
Camera::Projection iProj = mNewCam.getProjection();
Camera::Projection oldProj = mOldCam.getProjection();
Camera::Projection newProj = mNewCam.getProjection();
iProj.mRight = oldProj.mRight * (1-t) + newProj.mRight * t;
iProj.mLeft = oldProj.mLeft * (1-t) + newProj.mLeft * t;
iProj.mBottom = oldProj.mBottom * (1-t) + newProj.mBottom * t;
iProj.mTop = oldProj.mTop * (1-t) + newProj.mTop * t;
iProj.mNear = oldProj.mNear * (1-t) + newProj.mNear * t;
iProj.mFar = oldProj.mFar * (1-t) + newProj.mFar * t;
iProj.mZoomFactor = oldProj.mZoomFactor * (1-t) + newProj.mZoomFactor * t;
iProj.mType = newProj.mType;
iProj.mProportionalToWindow = newProj.mProportionalToWindow;
mCam.setProjection( iProj );
mCam.applyProjectionTransformation();
if ( animationTime >= mAnimationDuration )
{
killTimer( mAnimationTimerId );
mAnimationTimerId = 0;
}
update();
}
else if( ipE->timerId() == mCameraControlTimerId && !isAnimatingCamera() )
{
switch ( getControlType() )
{
case ctRotateAround:
{
Vector3d v; double a = 1.0 * PI / 180;
if( isKeyPressed( Qt::Key_W ) )
{ v += mCam.getLat(); a *= -1; }
if( isKeyPressed( Qt::Key_S ) )
{ v += mCam.getLat(); }
if( isKeyPressed( Qt::Key_A ) )
{ v += Vector3d(0, 1, 0); a *= -1; }
if( isKeyPressed( Qt::Key_D ) )
{ v += Vector3d(0, 1, 0); }
mCam.rotate( a, v, mCam.getLook() );
}break;
case ctFree:
{
Vector3d v;
if( isKeyPressed( Qt::Key_W ) )
{ v += Vector3d( mCam.getPos(), mCam.getLook() ); }
if( isKeyPressed( Qt::Key_S ) )
{ v += -Vector3d( mCam.getPos(), mCam.getLook() ); }
if( isKeyPressed( Qt::Key_A ) )
{ v += mCam.getLat() * -1; }
if( isKeyPressed( Qt::Key_D ) )
{ v += mCam.getLat(); }
if( isKeyPressed( Qt::Key_Q ) )
{ v += mCam.getUp(); }
if( isKeyPressed( Qt::Key_E ) )
{ v += mCam.getUp() * -1; }
v.normalise();
mCam.translate( v * getCameraSpeed() );
} break;
default: break;
}
update();
}
else
{
QWidget::timerEvent( ipE );
}
}
void ParticleState::setDirection(const Vector3d &dir) {
direction = dir / dir.getR();
}
char* MyController::sendSceneInfo(std::string header, int camID) {
m_my->setWheelVelocity(0.0, 0.0);
Vector3d myPos;
m_my->getPosition(myPos);
double x = myPos.x();
double z = myPos.z();
double theta = calcHeadingAngle(); // y方向の回転は無しと考える
// カメラがついているリンク名取得
std::string link = ""; //"WAIST_LINK0"; //m_my->getCameraLinkName(3);
if (camID < 3) {
link = m_my->getCameraLinkName(camID);
} else {
link = "WAIST_LINK0";
}
//const dReal *lpos = m_my->getParts(link.c_str())->getPosition();
Vector3d lpos;
m_my->getParts(link.c_str())->getPosition(lpos);
// カメラの位置取得(リンク座標系)
Vector3d cpos;
m_my->getCamPos(cpos, camID);
// カメラの位置(絶対座標系, ロボットの回転はないものとする)
//printf("linkpos: %lf %lf %lf \n", lpos.x(), lpos.y(), lpos.z());
//printf("camerapos: %lf %lf %lf \n", cpos.x(), cpos.y(), cpos.z());
Vector3d campos(lpos.x() + cpos.z() * sin(DEG2RAD(theta)),
lpos.y() + cpos.y(),
lpos.z() + cpos.z() * cos(DEG2RAD(theta)));
//Vector3d campos(cpos.x(), cpos.y(), cpos.z());
// カメラの方向取得(ロボットの回転,関節の回転はないものとする)
Vector3d cdir;
m_my->getCamDir(cdir, camID);
char *replyMsg = new char[1024];
sprintf(replyMsg, "%s %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf",
header.c_str(), x, z, theta, campos.x(), campos.y(), campos.z(), cdir.x(), cdir.y(), cdir.z());
printf("%s \n", replyMsg);
m_srv->sendMsgToSrv(replyMsg);
return replyMsg;
}
void MyController::onRecvMsg(RecvMsgEvent &evt)
{
// 送信者取得
std::string sender = evt.getSender();
std::cout << "sender: " << sender << std::endl;
char *all_msg = (char*)evt.getMsg();
printf("all_msg: %s \n", all_msg);
char *delim = (char *)(" ");
char *ctx;
char *header = strtok_r(all_msg, delim, &ctx);
if (strcmp(header, "RESET") == 0) {
printf("Received RESET \n");
setRobotPosition(0, -50);
setRobotHeadingAngle(0);
setCameraPosition(0, 3);
printf("Reseted RobotPosition \n");
//char* replyMsg = sendSceneInfo();
sendSceneInfo("Start");
m_executed = true;
return;
}
// 送信者がゴミ認識サービスの場合
if(sender == "RecogTrash") {
double x = 0, y = 0, z = 0; // range = 0;
if(strcmp(header, "RandomRouteStart") == 0) {
m_state = 800;
m_executed = false;
return;
}
if(strcmp(header, "RandomRouteArrived") == 0) {
//printf("onRecv RandomRouteArrived \n");
m_state = 800;
m_executed = false;
return;
}
if(strcmp(header, "RandomRoute") == 0) {
x = atof(strtok_r(NULL, delim, &ctx));
z = atof(strtok_r(NULL, delim, &ctx));
m_range = atof(strtok_r(NULL, delim, &ctx));
nextPos.set(x, y, z);
double lookingX = atof(strtok_r(NULL, delim, &ctx));
double lookingZ = atof(strtok_r(NULL, delim, &ctx));
m_lookingPos.set(lookingX, 0, lookingZ);
m_lookObjFlg = atof(strtok_r(NULL, delim, &ctx));
printf("m_lookObjFlg: %lf \n", m_lookObjFlg);
m_state = 805;
m_executed = false;
return;
}
if(strcmp(header, "GoForwardVelocity") == 0) {
double wheelVel = atof(strtok_r(NULL, delim, &ctx));
printf("GoForwardVelocity coef: %lf \n", wheelVel);
wheelVel *= m_vel;
m_my->setWheelVelocity(wheelVel * 10., wheelVel * 10.);
//sendSceneInfo();
return;
}
if(strcmp(header, "RotateVelocity") == 0) {
double wheelVel = atof(strtok_r(NULL, delim, &ctx));
printf("RotateVelocity coef: %lf \n", wheelVel);
wheelVel *= m_vel;
m_my->setWheelVelocity(-wheelVel, wheelVel);
//sendSceneInfo();
return;
}
if(strcmp(header, "Stop") == 0) {
printf("Stop joyStick \n");
m_my->setWheelVelocity(0.0, 0.0);
sendSceneInfo();
return;
}
if (strcmp(header, "CamID") == 0) {
printf("Setting CameraID \n");
m_CamID = atoi(strtok_r(NULL, delim, &ctx));
return;
}
if (strcmp(header, "CaptureData") == 0) {
printf("SetRobotPosition \n");
double x = atof(strtok_r(NULL, delim, &ctx));
double z = atof(strtok_r(NULL, delim, &ctx));
setRobotPosition(x, z);
double angle = atof(strtok_r(NULL, delim, &ctx));
printf("setRobotHeadingAngle: %lf \n", angle);
setRobotHeadingAngle(angle);
char* replyMsg = sendSceneInfo();
//m_srv->sendMsgToSrv(replyMsg);
m_executed = true;
return;
}
} else if (sender == "SIGViewer") {
printf("mess from SIGViewer \n");
if(strcmp(header, "CameraAngle") == 0) {
double angle = atof(strtok_r(NULL, delim, &ctx));
printf("CameraAngle: %lf \n", angle);
setCameraPosition(angle, 3);
char* replyMsg = sendSceneInfo();
m_executed = true;
return;
} else if (strcmp(header, "RobotAngle") == 0) {
printf("rorate robor start \n");
double angle = atof(strtok_r(NULL, delim, &ctx));
printf("RobotHeadingAngle: %lf \n", angle);
setRobotHeadingAngle(angle);
char* replyMsg = sendSceneInfo();
//m_srv->sendMsgToSrv(replyMsg);
m_executed = true;
return;
} else if (strcmp(header, "RotateDir") == 0) {
// 次に移動する座標を位置を取り出す
double x = atof(strtok_r(NULL, delim, &ctx));
double z = atof(strtok_r(NULL, delim, &ctx));
nextPos.set(x, 0, z);
printf("RotateDir %lf %lf \n", nextPos.x(), nextPos.z());
// ロボットのステートを更新
m_state = 920;
m_executed = false;
return;
} else if(strcmp(header, "RobotPosition") == 0) {
double x = atof(strtok_r(NULL, delim, &ctx));
double z = atof(strtok_r(NULL, delim, &ctx));
setRobotPosition(x, z);
return;
}
}
}
void MomentumCorrFunc::correlateFrames(int frame1, int frame2) {
SimInfo::MoleculeIterator mi1;
SimInfo::MoleculeIterator mi2;
Molecule* mol1;
Molecule* mol2;
Molecule::AtomIterator ai1;
Molecule::AtomIterator ai2;
Atom* atom1;
Atom* atom2;
std::vector<Vector3d> atomPositions1;
std::vector<Vector3d> atomPositions2;
std::vector<Vector3d> atomVelocity1;
std::vector<Vector3d> atomVelocity2;
int thisAtom1, thisAtom2;
Snapshot* snapshot1 = bsMan_->getSnapshot(frame1);
Snapshot* snapshot2 = bsMan_->getSnapshot(frame2);
assert(snapshot1 && snapshot2);
RealType time1 = snapshot1->getTime();
RealType time2 = snapshot2->getTime();
int timeBin = int ((time2 - time1) /deltaTime_ + 0.5);
updateFrame(frame1);
atomPositions1.clear();
for (mol1 = info_->beginMolecule(mi1); mol1 != NULL;
mol1 = info_->nextMolecule(mi1)) {
for(atom1 = mol1->beginAtom(ai1); atom1 != NULL;
atom1 = mol1->nextAtom(ai1)) {
atomPositions1.push_back(atom1->getPos(frame1));
atomVelocity1.push_back(atom1->getVel(frame1));
}
}
updateFrame(frame2);
atomPositions2.clear();
for (mol2 = info_->beginMolecule(mi2); mol2 != NULL;
mol2 = info_->nextMolecule(mi2)) {
for(atom2 = mol2->beginAtom(ai2); atom2 != NULL;
atom2 = mol2->nextAtom(ai2)) {
atomPositions2.push_back(atom2->getPos(frame2));
atomVelocity2.push_back(atom2->getVel(frame2));
}
}
thisAtom1 = 0;
for (mol1 = info_->beginMolecule(mi1); mol1 != NULL;
mol1 = info_->nextMolecule(mi1)) {
for(atom1 = mol1->beginAtom(ai1); atom1 != NULL;
atom1 = mol1->nextAtom(ai1)) {
Vector3d r1 = atomPositions1[thisAtom1];
Vector3d p1 = atom1->getMass() * atomVelocity1[thisAtom1];
thisAtom2 = 0;
for (mol2 = info_->beginMolecule(mi2); mol2 != NULL;
mol2 = info_->nextMolecule(mi2)) {
for(atom2 = mol2->beginAtom(ai2); atom2 != NULL;
atom2 = mol2->nextAtom(ai2)) {
Vector3d r2 = atomPositions2[thisAtom2];
Vector3d p2 = atom2->getMass() * atomVelocity1[thisAtom1];
Vector3d deltaPos = (r2-r1);
Vector3d dp2( deltaPos.x() * deltaPos.x(),
deltaPos.y() * deltaPos.y(),
deltaPos.z() * deltaPos.z());
Vector3d pprod( p1.x() * p2.x(),
p1.y() * p2.y(),
p1.z() * p2.z());
histogram_[timeBin] += outProduct(dp2, pprod);
thisAtom2++;
}
}
thisAtom1++;
}
}
count_[timeBin]++;
}
double vectorAngle(const Vector3d &r1,const Vector3d &r2){
return(acos(r1.dot(r2)/r1.norm()/r2.norm()));
}
bool ContainedByBox::Inside(const Vector3d& point) const
{
return ((point.x() >= corner1.x()) && (point.x() <= corner2.x()) &&
(point.y() >= corner1.y()) && (point.y() <= corner2.y()) &&
(point.z() >= corner1.z()) && (point.z() <= corner2.z()));
}
bool ContainedBySphere::Inside(const Vector3d& point) const
{
Vector3d originToCenter = center - point;
DBL ocSquared = originToCenter.lengthSqr();
return (ocSquared <= Sqr(radius));
}
void drawInfo()
{
if ( visibleInfo )
{
glDisable(GL_COLOR_MATERIAL);
glDisable(GL_BLEND);
glDisable(GL_LIGHTING);
GLText text;
if ( gameMode )
text.init(SCREEN_WIDTH,SCREEN_HEIGHT,glWhite,GLUT_BITMAP_HELVETICA_18);
else
text.init(640,480,glWhite,GLUT_BITMAP_HELVETICA_12);
text.enterTextInputMode();
switch ( headCalibrationDone )
{
case 0:
{
if ( allVisibleHead )
text.draw("==== Head Calibration OK ==== Press Spacebar to continue");
else
text.draw("Be visible with the head and glasses");
}
break;
case 1:
case 2:
{
if ( allVisiblePatch )
text.draw("Move the head in the center");
else
text.draw("Be visible with the patch");
}
break;
case 3: // When the head calibration is done then calibrate the fingers
{
switch ( fingerCalibrationDone )
{
case 0:
text.draw("Press F to record platform markers");
break;
case 1:
text.draw("Move index and thumb on platform markers to record ghost finger tips, then press F");
break;
case 2:
text.draw("Move index and thumb to rigidbody tip to define starting position, then press F");
break;
case 3:
text.draw("Finger calibration successfull!");
break;
}
}
break;
}
text.draw("####### SUBJECT #######");
text.draw("#");
text.draw("# Name: " +parameters.find("SubjectName"));
text.draw("# IOD: " +stringify<double>(interoculardistance));
text.draw("#");
text.draw("#######################");
text.draw("HeadCalibration= " + stringify<int>(headCalibrationDone) );
text.draw("FingerCalibration= " + stringify<int>(fingerCalibrationDone) );
if ( isVisible(markers[1].p) && isVisible(markers[2].p) )
glColor3fv(glGreen);
else
glColor3fv(glRed);
text.draw("Marker "+ stringify<int>(1)+stringify< Eigen::Matrix<double,1,3> > (markers[1].p.transpose())+ " [mm]" );
text.draw("Marker "+ stringify<int>(2)+stringify< Eigen::Matrix<double,1,3> > (markers[2].p.transpose())+ " [mm]" );
glColor3fv(glWhite);
text.draw("Marker "+ stringify<int>(3)+stringify< Eigen::Matrix<double,1,3> > (markers[3].p.transpose())+ " [mm]" );
text.draw("Marker "+ stringify<int>(4)+stringify< Eigen::Matrix<double,1,3> > (markers[4].p.transpose())+ " [mm]" );
if ( isVisible(markers[5].p) && isVisible(markers[6].p) && isVisible(markers[7].p) )
glColor3fv(glGreen);
else
glColor3fv(glRed);
text.draw("Marker "+ stringify<int>(5)+stringify< Eigen::Matrix<double,1,3> > (markers[5].p.transpose())+ " [mm]" );
text.draw("Marker "+ stringify<int>(6)+stringify< Eigen::Matrix<double,1,3> > (markers[6].p.transpose())+ " [mm]" );
text.draw("Marker "+ stringify<int>(7)+stringify< Eigen::Matrix<double,1,3> > (markers[7].p.transpose())+ " [mm]" );
text.draw("Marker "+ stringify<int>(8)+stringify< Eigen::Matrix<double,1,3> > (markers[8].p.transpose())+ " [mm]" );
if ( isVisible(markers[13].p) && isVisible(markers[14].p) && isVisible(markers[16].p) )
glColor3fv(glGreen);
else
glColor3fv(glRed);
text.draw("Marker "+ stringify<int>(13)+stringify< Eigen::Matrix<double,1,3> > (markers[13].p.transpose())+ " [mm]" );
text.draw("Marker "+ stringify<int>(14)+stringify< Eigen::Matrix<double,1,3> > (markers[14].p.transpose())+ " [mm]" );
text.draw("Marker "+ stringify<int>(16)+stringify< Eigen::Matrix<double,1,3> > (markers[16].p.transpose())+ " [mm]" );
if ( isVisible(markers[15].p) && isVisible(markers[17].p) && isVisible(markers[18].p) )
glColor3fv(glGreen);
else
glColor3fv(glRed);
text.draw("Marker "+ stringify<int>(15)+stringify< Eigen::Matrix<double,1,3> > (markers[15].p.transpose())+ " [mm]" );
text.draw("Marker "+ stringify<int>(17)+stringify< Eigen::Matrix<double,1,3> > (markers[17].p.transpose())+ " [mm]" );
text.draw("Marker "+ stringify<int>(18)+stringify< Eigen::Matrix<double,1,3> > (markers[18].p.transpose())+ " [mm]" );
glColor3fv(glWhite);
text.draw("EyeRight= "+stringify< Eigen::Matrix<double,1,3> > (eyeRight.transpose())+ " [mm]" );
text.draw("EyeLeft= "+stringify< Eigen::Matrix<double,1,3> > (eyeLeft.transpose())+ " [mm]" );
text.draw("Alignment(X,Y)= " +stringify<double>(alignmentX)+","+stringify<double>(alignmentY));
text.draw("Index = " +stringify< Eigen::Matrix<double,1,3> >(index.transpose()));
text.draw("Thumb = " +stringify< Eigen::Matrix<double,1,3> >(thumb.transpose()));
text.draw("Inside Time= " + stringify<int>(timer.getElapsedTimeInMilliSec()) );
text.draw("Trial = " + stringify<int>(trialNumber));
text.draw("Condition = " + stringify<int>(factors["Condition"]));
text.draw("Fingers at start = " + fingersAtStart);
text.leaveTextInputMode();
}
}
// Questa funzione e' quella che in background fa tutti i conti matematici, quindi qui devi inserire
// 1) Scrittura su file continua delle coordinate che vuoi salvare
// 2) Estrazione delle coordinate a partire dai corpi rigidi precedentemente definiti vedi ad esempio
// come e' fatto per eyeLeft e eyeRight oppure per thumb ed index
void idle()
{
optotrak->updateMarkers();
//cerr << deltaT << endl;
markers = optotrak->getAllMarkers();
// Coordinates picker
allVisiblePlatform = isVisible(markers[1].p);
allVisibleThumb = isVisible(markers[15].p) && isVisible(markers[17].p) && isVisible(markers[18].p);
allVisibleIndex = isVisible(markers[13].p) && isVisible(markers[14].p) && isVisible(markers[16].p);
allVisibleFingers = allVisibleThumb && allVisibleIndex;
allVisiblePatch = isVisible(markers[5].p) && isVisible(markers[6].p) && isVisible(markers[7].p);
allVisibleHead = allVisiblePatch && isVisible(markers[1].p);
if ( allVisiblePatch )
headEyeCoords.update(markers[5].p,markers[6].p,markers[7].p);
if ( allVisibleThumb )
thumbCoords.update(markers[15].p,markers[17].p,markers[18].p);
if ( allVisibleIndex )
indexCoords.update(markers[13].p, markers[14].p, markers[16].p );
if ( headCalibrationDone==3 && fingerCalibrationDone==3 )
{
if ( !allVisibleIndex )
occludedFrames++;
if ( !allVisibleThumb )
occludedFrames++;
}
if(headCalibration)
{
eyeLeft = headEyeCoords.getLeftEye();
eyeRight = headEyeCoords.getRightEye();
} else {
eyeRight = Vector3d(interoculardistance/2,0,0);
eyeLeft = -eyeRight;
}
thumb = thumbCoords.getP1();
index = indexCoords.getP1();
singleMarker = markers.at(4).p.transpose();
if( abs(index.z()-thumb.z()) > 15.0 )
fingersAtStart=false;
else
fingersAtStart=true;
if( endTrial && fingersAtStart)
{
advanceTrial();
}
#ifdef WRITE
// Write to file
if ( headCalibrationDone==3 && fingerCalibrationDone==3 )
{
markersFile << fixed << trialNumber << "\t" <<
eyeLeft.transpose() << "\t" << eyeRight.transpose() << "\t" <<
//markers.at(4).p.transpose() << "\t" <<
index.transpose() << "\t" <<
thumb.transpose() << "\t" <<
fingersAtStart << "\t" <<
isStimulusDrawn
;
markersFile << endl;
}
#endif
}
void Gelatin::step(double h, const Vector3d &grav, const vector< shared_ptr<FreakFace> > faces)
{
aTrips.clear();
//v.setZero();
//f.setZero();
//
// vTrips.clear();
// fTrips.clear();
VectorXd b(n);
b.setZero();
for (int i = 0; i < particles.size(); i++) {
if (!particles[i]->fixed) {
int index = particles[i]->i;
double m = particles[i]->m;
double val = m + h * damping(0) * m;
aTrips.push_back(Trip(index, index, val));
aTrips.push_back(Trip(index+1, index+1, val));
aTrips.push_back(Trip(index+2, index+2, val));
Vector3d v = particles[i]->v;
Vector3d fg = h * m * grav;
b(index) = m * v(0) + fg(0);
b(index+1) = m * v(1) + fg(1);
b(index+2) = m * v(2) + fg(2);
}
}
for (int i = 0; i < springs.size(); i++) {
double E = springs[i]->E;
double L = springs[i]->L;
auto p0 = springs[i]->p0;
auto p1 = springs[i]->p1;
//MatrixXd delta(3, 1);
Vector3d delta = p1->x - p0->x;
double l = delta.norm();
Vector3d fs = E * (l - L) * (delta / l);
if (!springs[i]->p0->fixed) {
b(p0->i) += h * fs(0);
b(p0->i+1) += h * fs(1);
b(p0->i+2) += h * fs(2);
}
if (!springs[i]->p1->fixed) {
b(p1->i) -= h * fs(0);
b(p1->i+1) -= h * fs(1);
b(p1->i+2) -= h * fs(2);
}
if (!springs[i]->p0->fixed && !springs[i]->p1->fixed) {
Matrix3d ks = (E / (l * l)) * ((1.0 - (l - L)/l) * (delta * delta.transpose())
+ ((l - L)/l) * double(delta.transpose() * delta) * Matrix3d::Identity());
int i0 = p0->i;
int i1 = p1->i;
addKs(ks, i0, i1, h);
}
}
Eigen::SparseMatrix<double> sparseA(n,n);
sparseA.setFromTriplets(aTrips.begin(), aTrips.end());
ConjugateGradient< SparseMatrix<double> > cg;
cg.setMaxIterations(25);
cg.setTolerance(1e-3);
cg.compute(sparseA);
//v = cg.solve(b);
v = cg.solveWithGuess(b, v);
//v = A.ldlt().solve(b);
for (int i = 0; i < particles.size(); i++) {
if (!particles[i]->fixed) {
int index = particles[i]->i;
particles[i]->v = v.block<3,1>(index, 0);
particles[i]->x += h * particles[i]->v;
}
}
collide(faces);
// Update position and normal buffers
updatePosNor();
}
void ot_index_box(const Vector3d& min_point, const Vector3d& max_point, OT_ID *id)
{
// TODO OPTIMIZE
DBL dx, dy, dz, desiredSize;
DBL bsized, maxord;
POW2OP_DECLARE()
// Calculate the absolute minimum required size of the node, assuming it is perfectly centered within the node;
// Node size must be a power of 2, and be large enough to accomodate box's biggest dimensions with maximum overhang to all sides
// compute ideal size of the node for a perfect fit without any overhang
dx = max_point.x() - min_point.x();
dy = max_point.y() - min_point.y();
dz = max_point.z() - min_point.z();
desiredSize = max3(dx, dy, dz);
// compute ideal size of the node for a perfect fit with full overhang to all sides
// desiredSize /= (1 + 2 * 0.5);
// compute best-matching power-of-two size for a perfect fit with overhang
// (Note: theoretically this might pick a size larger than required if desiredSize is already a power of two)
// desiredSize *= 2.0;
POW2OP_FLOOR(bsized,desiredSize)
// avoid divisions by zero
if(bsized == 0.0)
bsized = 1.0;
#ifdef SAFE_METHOD
// This block checks for the case where the node id would cause integer
// overflow, since it is a small buffer far away
maxord = max3(fabs(min_point[X]), fabs(min_point[Y]), fabs(min_point[Z]));
maxord += OT_BIAS;
while (maxord / bsized > 1000000000.0)
{
#ifdef RADSTATS
overflows++;
#endif
bsized *= 2.0;
}
#endif // SAFE_METHOD
// The node we chose so far would be ideal for a box of identical size positioned at the node's center,
// but the actual box is probably somewhat off-center and therefore may have excessive overhang in some directions;
// check and possibly fix this.
Vector3d center = (min_point + max_point) / 2;
id->x = (int) floor((center[X] + OT_BIAS) / bsized);
id->y = (int) floor((center[Y] + OT_BIAS) / bsized);
id->z = (int) floor((center[Z] + OT_BIAS) / bsized);
POW2OP_ENCODE(id->Size, bsized)
#ifdef RADSTATS
thisloops = 0;
#endif
while (!ot_point_in_node(min_point, id) || !ot_point_in_node(max_point, id))
{
// Debug_Info("looping %d,%d,%d,%d min=%d, max=%d\n", test_id.x, test_id.y,
// test_id.z, test_id.Size, ot_point_in_node(min_point, &test_id),
// ot_point_in_node(max_point, &test_id));
ot_parent(id, id);
#ifdef RADSTATS
totloops++;
thisloops++;
#endif
}
#ifdef RADSTATS
if (thisloops < minloops)
minloops = thisloops;
if (thisloops > maxloops)
maxloops = thisloops;
#endif
#ifdef OT_DEBUG
if (id->Size > 139)
{
Debug_Info("unusually large id, maxdel=%.4f, bsized=%.4f, isize=%d\n",
maxdel, bsized, id->Size);
}
#endif
}
void MotionModelScanMatcher::estimateCovariance(RobotPose max_pose, PoseGrid& grid, int use_fine_grid){
int posegrid_x_dim = pose_x_dim;
int posegrid_y_dim = pose_y_dim;
if(use_fine_grid) posegrid_x_dim = fine_pose_x_dim;
if(use_fine_grid) posegrid_y_dim = fine_pose_y_dim;
Matrix3d cov_temp;
Matrix3d K = Matrix3d::Constant(0.0);
Vector3d u(0.0, 0.0, 0.0);
// first normalize grid
double sum = 0;
double offset = grid.getVal(max_pose_ind.theta, max_pose_ind.y, max_pose_ind.x) - 25;
double phat;
double logp;
RobotPose p, norm_p;
for (int theta_ind = 0; theta_ind < pose_theta_dim; theta_ind++) {
p.theta = theta_ind*theta_step;
for (int x_ind = 0; x_ind < posegrid_x_dim; x_ind++) {
p.x = (x_ind + pose_grid_offset_x - occ_x_dim/2)*x_step;
for (int y_ind = 0; y_ind < posegrid_y_dim; y_ind++) {
p.y = (y_ind + pose_grid_offset_y - occ_x_dim/2)*y_step;
//p is now in the same frame as max_pose
logp = grid.getVal(theta_ind, y_ind, x_ind);
if(logp == -DBL_MAX){
continue;
}
if(sum <= -DBL_MAX/2 || sum > DBL_MAX/2){
ROS_WARN("[MotionModelScanMatcher::estimateCovariance] - SUM NEAR OVERFLOW!!!!!!!!!!!");
}
//re-scale the log prob to an actual prob
//ROS_INFO("[MotionModelScanMatcher::estimateCovariance]\tlogp=%f",logp);
phat = exp(logp - offset); // legit fudge but overflows still
//phat = exp((logp / 10) + offset); // fudge
//calc running sums
Vector3d pose = p.toEigenVector();
Matrix3d plus_k = phat * (pose * pose.transpose());
K = K + plus_k;
u = u + (pose * phat);
sum += phat;
/*
if(phat != 0){
ROS_INFO("[MotionModelScanMatcher::estimateCovariance] - phat = %f,\t logp = %f, offset = %f", phat, logp, offset);
ROS_INFO("[MotionModelScanMatcher::estimateCovariance] - u = [%f %f %f]", u(0,0), u(1,0), u(2,0));
}
*/
}
}
}
cov_temp = K / sum + (u * u.transpose())/(pow(sum,2));
//move cov_temp's values to cov
for(int i = 0; i < 9; i ++){
int x_ind = i % 3;
int y_ind = i / 3;
cov[i] = cov_temp(x_ind, y_ind);
}
ROS_INFO("[MotionModelScanMatcher::estimateCovariance]\tSigma=[%e\t%e\t%e]",cov[0],cov[1],cov[2]);
ROS_INFO(" \t [%e\t%e\t%e]",cov[3],cov[4],cov[5]);
ROS_INFO(" \t [%e\t%e\t%e]",cov[6],cov[7],cov[8]);
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
int error;
if (nrhs<1) mexErrMsgTxt("usage: ptr = QPControllermex(0,control_obj,robot_obj,...); alpha=QPControllermex(ptr,...,...)");
if (nlhs<1) mexErrMsgTxt("take at least one output... please.");
struct QPControllerData* pdata;
mxArray* pm;
double* pr;
int i,j;
if (mxGetScalar(prhs[0])==0) { // then construct the data object and return
pdata = new struct QPControllerData;
// get control object properties
const mxArray* pobj = prhs[1];
pm = myGetProperty(pobj,"slack_limit");
pdata->slack_limit = mxGetScalar(pm);
pm = myGetProperty(pobj,"W_kdot");
assert(mxGetM(pm)==3); assert(mxGetN(pm)==3);
pdata->W_kdot.resize(mxGetM(pm),mxGetN(pm));
memcpy(pdata->W_kdot.data(),mxGetPr(pm),sizeof(double)*mxGetM(pm)*mxGetN(pm));
pm= myGetProperty(pobj,"w_grf");
pdata->w_grf = mxGetScalar(pm);
pm= myGetProperty(pobj,"w_slack");
pdata->w_slack = mxGetScalar(pm);
pm = myGetProperty(pobj,"Kp_ang");
pdata->Kp_ang = mxGetScalar(pm);
pm = myGetProperty(pobj,"Kp_accel");
pdata->Kp_accel = mxGetScalar(pm);
pm= myGetProperty(pobj,"n_body_accel_inputs");
pdata->n_body_accel_inputs = mxGetScalar(pm);
pm = myGetProperty(pobj,"body_accel_input_weights");
pdata->body_accel_input_weights.resize(pdata->n_body_accel_inputs);
memcpy(pdata->body_accel_input_weights.data(),mxGetPr(pm),sizeof(double)*pdata->n_body_accel_inputs);
pdata->n_body_accel_constraints = 0;
for (int i=0; i<pdata->n_body_accel_inputs; i++) {
if (pdata->body_accel_input_weights(i) < 0)
pdata->n_body_accel_constraints++;
}
// get robot mex model ptr
if (!mxIsNumeric(prhs[2]) || mxGetNumberOfElements(prhs[2])!=1)
mexErrMsgIdAndTxt("Drake:QPControllermex:BadInputs","the third argument should be the robot mex ptr");
memcpy(&(pdata->r),mxGetData(prhs[2]),sizeof(pdata->r));
pdata->B.resize(mxGetM(prhs[3]),mxGetN(prhs[3]));
memcpy(pdata->B.data(),mxGetPr(prhs[3]),sizeof(double)*mxGetM(prhs[3])*mxGetN(prhs[3]));
int nq = pdata->r->num_dof, nu = pdata->B.cols();
pm = myGetProperty(pobj,"w_qdd");
pdata->w_qdd.resize(nq);
memcpy(pdata->w_qdd.data(),mxGetPr(pm),sizeof(double)*nq);
pdata->umin.resize(nu);
pdata->umax.resize(nu);
memcpy(pdata->umin.data(),mxGetPr(prhs[4]),sizeof(double)*nu);
memcpy(pdata->umax.data(),mxGetPr(prhs[5]),sizeof(double)*nu);
pdata->B_act.resize(nu,nu);
pdata->B_act = pdata->B.bottomRows(nu);
// get the map ptr back from matlab
if (!mxIsNumeric(prhs[6]) || mxGetNumberOfElements(prhs[6])!=1)
mexErrMsgIdAndTxt("Drake:QPControllermex:BadInputs","the seventh argument should be the map ptr");
memcpy(&pdata->map_ptr,mxGetPr(prhs[6]),sizeof(pdata->map_ptr));
// pdata->map_ptr = NULL;
if (!pdata->map_ptr)
mexWarnMsgTxt("Map ptr is NULL. Assuming flat terrain at z=0");
// create gurobi environment
error = GRBloadenv(&(pdata->env),NULL);
// set solver params (http://www.gurobi.com/documentation/5.5/reference-manual/node798#sec:Parameters)
mxArray* psolveropts = myGetProperty(pobj,"gurobi_options");
int method = (int) mxGetScalar(myGetField(psolveropts,"method"));
CGE ( GRBsetintparam(pdata->env,"outputflag",0), pdata->env );
CGE ( GRBsetintparam(pdata->env,"method",method), pdata->env );
// CGE ( GRBsetintparam(pdata->env,"method",method), pdata->env );
CGE ( GRBsetintparam(pdata->env,"presolve",0), pdata->env );
if (method==2) {
CGE ( GRBsetintparam(pdata->env,"bariterlimit",20), pdata->env );
CGE ( GRBsetintparam(pdata->env,"barhomogeneous",0), pdata->env );
CGE ( GRBsetdblparam(pdata->env,"barconvtol",0.0005), pdata->env );
}
mxClassID cid;
if (sizeof(pdata)==4) cid = mxUINT32_CLASS;
else if (sizeof(pdata)==8) cid = mxUINT64_CLASS;
else mexErrMsgIdAndTxt("Drake:constructModelmex:PointerSize","Are you on a 32-bit machine or 64-bit machine??");
plhs[0] = mxCreateNumericMatrix(1,1,cid,mxREAL);
memcpy(mxGetData(plhs[0]),&pdata,sizeof(pdata));
// preallocate some memory
pdata->H.resize(nq,nq);
pdata->H_float.resize(6,nq);
pdata->H_act.resize(nu,nq);
pdata->C.resize(nq);
pdata->C_float.resize(6);
pdata->C_act.resize(nu);
pdata->J.resize(3,nq);
pdata->Jdot.resize(3,nq);
pdata->J_xy.resize(2,nq);
pdata->Jdot_xy.resize(2,nq);
pdata->Hqp.resize(nq,nq);
pdata->fqp.resize(nq);
pdata->Ag.resize(6,nq);
pdata->Agdot.resize(6,nq);
pdata->Ak.resize(3,nq);
pdata->Akdot.resize(3,nq);
pdata->vbasis_len = 0;
pdata->cbasis_len = 0;
pdata->vbasis = NULL;
pdata->cbasis = NULL;
return;
}
// first get the ptr back from matlab
if (!mxIsNumeric(prhs[0]) || mxGetNumberOfElements(prhs[0])!=1)
mexErrMsgIdAndTxt("Drake:QPControllermex:BadInputs","the first argument should be the ptr");
memcpy(&pdata,mxGetData(prhs[0]),sizeof(pdata));
// for (i=0; i<pdata->r->num_bodies; i++)
// mexPrintf("body %d (%s) has %d contact points\n", i, pdata->r->bodies[i].linkname.c_str(), pdata->r->bodies[i].contact_pts.cols());
int nu = pdata->B.cols(), nq = pdata->r->num_dof;
const int dim = 3, // 3D
nd = 2*m_surface_tangents; // for friction cone approx, hard coded for now
assert(nu+6 == nq);
int narg=1;
int use_fast_qp = (int) mxGetScalar(prhs[narg++]);
Map< VectorXd > qddot_des(mxGetPr(prhs[narg++]),nq);
double *q = mxGetPr(prhs[narg++]);
double *qd = &q[nq];
vector<VectorXd,aligned_allocator<VectorXd>> body_accel_inputs;
for (int i=0; i<pdata->n_body_accel_inputs; i++) {
assert(mxGetM(prhs[narg])==7); assert(mxGetN(prhs[narg])==1);
VectorXd v = VectorXd::Zero(7,1);
memcpy(v.data(),mxGetPr(prhs[narg++]),sizeof(double)*7);
body_accel_inputs.push_back(v);
}
int num_condof;
VectorXd condof;
if (!mxIsEmpty(prhs[narg])) {
assert(mxGetN(prhs[narg])==1);
num_condof=mxGetM(prhs[narg]);
condof = VectorXd::Zero(num_condof);
memcpy(condof.data(),mxGetPr(prhs[narg++]),sizeof(double)*num_condof);
}
else {
num_condof=0;
narg++; // skip over empty vector
}
int desired_support_argid = narg++;
Map<MatrixXd> A_ls(mxGetPr(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
Map<MatrixXd> B_ls(mxGetPr(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
Map<MatrixXd> Qy (mxGetPr(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
Map<MatrixXd> R_ls(mxGetPr(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
Map<MatrixXd> C_ls(mxGetPr(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
Map<MatrixXd> D_ls(mxGetPr(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
Map<MatrixXd> S (mxGetPr(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
Map<VectorXd> s1(mxGetPr(prhs[narg]),mxGetM(prhs[narg])); narg++;
Map<VectorXd> s1dot(mxGetPr(prhs[narg]),mxGetM(prhs[narg])); narg++;
double s2dot = mxGetScalar(prhs[narg++]);
Map<VectorXd> x0(mxGetPr(prhs[narg]),mxGetM(prhs[narg])); narg++;
Map<VectorXd> u0(mxGetPr(prhs[narg]),mxGetM(prhs[narg])); narg++;
Map<VectorXd> y0(mxGetPr(prhs[narg]),mxGetM(prhs[narg])); narg++;
double mu = mxGetScalar(prhs[narg++]);
double terrain_height = mxGetScalar(prhs[narg++]); // nonzero if we're using DRCFlatTerrainMap
MatrixXd R_DQyD_ls = R_ls + D_ls.transpose()*Qy*D_ls;
pdata->r->doKinematics(q,false,qd);
//---------------------------------------------------------------------
// Compute active support from desired supports -----------------------
vector<SupportStateElement> active_supports;
set<int> contact_bodies; // redundant, clean up later
int num_active_contact_pts=0;
if (!mxIsEmpty(prhs[desired_support_argid])) {
VectorXd phi;
mxArray* mxBodies = myGetField(prhs[desired_support_argid],"bodies");
if (!mxBodies) mexErrMsgTxt("couldn't get bodies");
double* pBodies = mxGetPr(mxBodies);
mxArray* mxContactPts = myGetField(prhs[desired_support_argid],"contact_pts");
if (!mxContactPts) mexErrMsgTxt("couldn't get contact points");
mxArray* mxContactSurfaces = myGetField(prhs[desired_support_argid],"contact_surfaces");
if (!mxContactSurfaces) mexErrMsgTxt("couldn't get contact surfaces");
double* pContactSurfaces = mxGetPr(mxContactSurfaces);
for (i=0; i<mxGetNumberOfElements(mxBodies);i++) {
mxArray* mxBodyContactPts = mxGetCell(mxContactPts,i);
int nc = mxGetNumberOfElements(mxBodyContactPts);
if (nc<1) continue;
SupportStateElement se;
se.body_idx = (int) pBodies[i]-1;
pr = mxGetPr(mxBodyContactPts);
for (j=0; j<nc; j++) {
se.contact_pt_inds.insert((int)pr[j]-1);
}
se.contact_surface = (int) pContactSurfaces[i]-1;
active_supports.push_back(se);
num_active_contact_pts += nc;
contact_bodies.insert((int)se.body_idx);
}
}
pdata->r->HandC(q,qd,(MatrixXd*)NULL,pdata->H,pdata->C,(MatrixXd*)NULL,(MatrixXd*)NULL,(MatrixXd*)NULL);
pdata->H_float = pdata->H.topRows(6);
pdata->H_act = pdata->H.bottomRows(nu);
pdata->C_float = pdata->C.head(6);
pdata->C_act = pdata->C.tail(nu);
bool include_angular_momentum = (pdata->W_kdot.array().maxCoeff() > 1e-10);
if (include_angular_momentum) {
pdata->r->getCMM(q,qd,pdata->Ag,pdata->Agdot);
pdata->Ak = pdata->Ag.topRows(3);
pdata->Akdot = pdata->Agdot.topRows(3);
}
Vector3d xcom;
// consider making all J's into row-major
pdata->r->getCOM(xcom);
pdata->r->getCOMJac(pdata->J);
pdata->r->getCOMJacDot(pdata->Jdot);
pdata->J_xy = pdata->J.topRows(2);
pdata->Jdot_xy = pdata->Jdot.topRows(2);
MatrixXd Jcom,Jcomdot;
if (x0.size()==6) {
Jcom = pdata->J;
Jcomdot = pdata->Jdot;
}
else {
Jcom = pdata->J_xy;
Jcomdot = pdata->Jdot_xy;
}
Map<VectorXd> qdvec(qd,nq);
MatrixXd B,JB,Jp,Jpdot,normals;
int nc = contactConstraintsBV(pdata->r,num_active_contact_pts,mu,active_supports,pdata->map_ptr,B,JB,Jp,Jpdot,normals,terrain_height);
int neps = nc*dim;
VectorXd x_bar,xlimp;
MatrixXd D_float(6,JB.cols()), D_act(nu,JB.cols());
if (nc>0) {
if (x0.size()==6) {
// x,y,z com
xlimp.resize(6);
xlimp.topRows(3) = xcom;
xlimp.bottomRows(3) = Jcom*qdvec;
}
else {
xlimp.resize(4);
xlimp.topRows(2) = xcom.topRows(2);
xlimp.bottomRows(2) = Jcom*qdvec;
}
x_bar = xlimp-x0;
D_float = JB.topRows(6);
D_act = JB.bottomRows(nu);
}
int nf = nc*nd; // number of contact force variables
int nparams = nq+nf+neps;
Vector3d kdot_des;
if (include_angular_momentum) {
VectorXd k = pdata->Ak*qdvec;
kdot_des = -pdata->Kp_ang*k; // TODO: parameterize
}
//----------------------------------------------------------------------
// QP cost function ----------------------------------------------------
//
// min: ybar*Qy*ybar + ubar*R*ubar + (2*S*xbar + s1)*(A*x + B*u) +
// w_qdd*quad(qddot_ref - qdd) + w_eps*quad(epsilon) +
// w_grf*quad(beta) + quad(kdot_des - (A*qdd + Adot*qd))
VectorXd f(nparams);
{
if (nc > 0) {
// NOTE: moved Hqp calcs below, because I compute the inverse directly for FastQP (and sparse Hqp for gurobi)
VectorXd tmp = C_ls*xlimp;
VectorXd tmp1 = Jcomdot*qdvec;
MatrixXd tmp2 = R_DQyD_ls*Jcom;
pdata->fqp = tmp.transpose()*Qy*D_ls*Jcom;
pdata->fqp += tmp1.transpose()*tmp2;
pdata->fqp += (S*x_bar + 0.5*s1).transpose()*B_ls*Jcom;
pdata->fqp -= u0.transpose()*tmp2;
pdata->fqp -= y0.transpose()*Qy*D_ls*Jcom;
pdata->fqp -= (pdata->w_qdd.array()*qddot_des.array()).matrix().transpose();
if (include_angular_momentum) {
pdata->fqp += qdvec.transpose()*pdata->Akdot.transpose()*pdata->W_kdot*pdata->Ak;
pdata->fqp -= kdot_des.transpose()*pdata->W_kdot*pdata->Ak;
}
f.head(nq) = pdata->fqp.transpose();
} else {
f.head(nq) = -qddot_des;
}
}
f.tail(nf+neps) = VectorXd::Zero(nf+neps);
int neq = 6+neps+6*pdata->n_body_accel_constraints+num_condof;
MatrixXd Aeq = MatrixXd::Zero(neq,nparams);
VectorXd beq = VectorXd::Zero(neq);
// constrained floating base dynamics
// H_float*qdd - J_float'*lambda - Dbar_float*beta = -C_float
Aeq.topLeftCorner(6,nq) = pdata->H_float;
beq.topRows(6) = -pdata->C_float;
if (nc>0) {
Aeq.block(0,nq,6,nc*nd) = -D_float;
}
if (nc > 0) {
// relative acceleration constraint
Aeq.block(6,0,neps,nq) = Jp;
Aeq.block(6,nq,neps,nf) = MatrixXd::Zero(neps,nf); // note: obvious sparsity here
Aeq.block(6,nq+nf,neps,neps) = MatrixXd::Identity(neps,neps); // note: obvious sparsity here
beq.segment(6,neps) = (-Jpdot -pdata->Kp_accel*Jp)*qdvec;
}
// add in body spatial equality constraints
VectorXd body_vdot;
MatrixXd orig = MatrixXd::Zero(4,1);
orig(3,0) = 1;
int body_idx;
int equality_ind = 6+neps;
MatrixXd Jb(6,nq);
MatrixXd Jbdot(6,nq);
for (int i=0; i<pdata->n_body_accel_inputs; i++) {
if (pdata->body_accel_input_weights(i) < 0) {
// negative implies constraint
body_vdot = body_accel_inputs[i].bottomRows(6);
body_idx = (int)(body_accel_inputs[i][0])-1;
if (!inSupport(active_supports,body_idx)) {
pdata->r->forwardJac(body_idx,orig,1,Jb);
pdata->r->forwardJacDot(body_idx,orig,1,Jbdot);
for (int j=0; j<6; j++) {
if (!std::isnan(body_vdot[j])) {
Aeq.block(equality_ind,0,1,nq) = Jb.row(j);
beq[equality_ind++] = -Jbdot.row(j)*qdvec + body_vdot[j];
}
}
}
}
}
if (num_condof>0) {
// add joint acceleration constraints
for (int i=0; i<num_condof; i++) {
Aeq(equality_ind,(int)condof[i]-1) = 1;
beq[equality_ind++] = qddot_des[(int)condof[i]-1];
}
}
MatrixXd Ain = MatrixXd::Zero(2*nu,nparams); // note: obvious sparsity here
VectorXd bin = VectorXd::Zero(2*nu);
// linear input saturation constraints
// u=B_act'*(H_act*qdd + C_act - Jz_act'*z - Dbar_act*beta)
// using transpose instead of inverse because B is orthogonal
Ain.topLeftCorner(nu,nq) = pdata->B_act.transpose()*pdata->H_act;
Ain.block(0,nq,nu,nc*nd) = -pdata->B_act.transpose()*D_act;
bin.head(nu) = -pdata->B_act.transpose()*pdata->C_act + pdata->umax;
Ain.block(nu,0,nu,nparams) = -1*Ain.block(0,0,nu,nparams);
bin.segment(nu,nu) = pdata->B_act.transpose()*pdata->C_act - pdata->umin;
for (int i=0; i<2*nu; i++) {
// remove inf constraints---needed by gurobi
if (std::isinf(double(bin(i)))) {
Ain.row(i) = 0*Ain.row(i);
bin(i)=0;
}
}
GRBmodel * model = NULL;
int info=-1;
// set obj,lb,up
VectorXd lb(nparams), ub(nparams);
lb.head(nq) = -1e3*VectorXd::Ones(nq);
ub.head(nq) = 1e3*VectorXd::Ones(nq);
lb.segment(nq,nf) = VectorXd::Zero(nf);
ub.segment(nq,nf) = 1e3*VectorXd::Ones(nf);
lb.tail(neps) = -pdata->slack_limit*VectorXd::Ones(neps);
ub.tail(neps) = pdata->slack_limit*VectorXd::Ones(neps);
VectorXd alpha(nparams);
MatrixXd Qnfdiag(nf,1), Qneps(neps,1);
vector<MatrixXd*> QBlkDiag( nc>0 ? 3 : 1 ); // nq, nf, neps // this one is for gurobi
VectorXd w = (pdata->w_qdd.array() + REG).matrix();
#ifdef USE_MATRIX_INVERSION_LEMMA
bool include_body_accel_cost_terms = pdata->n_body_accel_inputs > 0 && pdata->body_accel_input_weights.array().maxCoeff() > 1e-10;
if (use_fast_qp > 0 && !include_angular_momentum && !include_body_accel_cost_terms)
{
// TODO: update to include angular momentum, body accel objectives.
// We want Hqp inverse, which I can compute efficiently using the
// matrix inversion lemma (see wikipedia):
// inv(A + U'CV) = inv(A) - inv(A)*U* inv([ inv(C)+ V*inv(A)*U ]) V inv(A)
if (nc>0) {
MatrixXd Wi = ((1/(pdata->w_qdd.array() + REG)).matrix()).asDiagonal();
if (R_DQyD_ls.trace()>1e-15) { // R_DQyD_ls is not zero
pdata->Hqp = Wi - Wi*Jcom.transpose()*(R_DQyD_ls.inverse() + Jcom*Wi*Jcom.transpose()).inverse()*Jcom*Wi;
}
}
else {
pdata->Hqp = MatrixXd::Constant(nq,1,1/(1+REG));
}
#ifdef TEST_FAST_QP
if (nc>0) {
MatrixXd Hqp_test(nq,nq);
MatrixXd W = w.asDiagonal();
Hqp_test = (Jcom.transpose()*R_DQyD_ls*Jcom + W).inverse();
if (((Hqp_test-pdata->Hqp).array().abs()).maxCoeff() > 1e-6) {
mexErrMsgTxt("Q submatrix inverse from matrix inversion lemma does not match direct Q inverse.");
}
}
#endif
Qnfdiag = MatrixXd::Constant(nf,1,1/REG);
Qneps = MatrixXd::Constant(neps,1,1/(.001+REG));
QBlkDiag[0] = &pdata->Hqp;
if (nc>0) {
QBlkDiag[1] = &Qnfdiag;
QBlkDiag[2] = &Qneps; // quadratic slack var cost, Q(nparams-neps:end,nparams-neps:end)=eye(neps)
}
MatrixXd Ain_lb_ub(2*nu+2*nparams,nparams);
VectorXd bin_lb_ub(2*nu+2*nparams);
Ain_lb_ub << Ain, // note: obvious sparsity here
-MatrixXd::Identity(nparams,nparams),
MatrixXd::Identity(nparams,nparams);
bin_lb_ub << bin, -lb, ub;
info = fastQPThatTakesQinv(QBlkDiag, f, Aeq, beq, Ain_lb_ub, bin_lb_ub, pdata->active, alpha);
if (info<0) mexPrintf("fastQP info = %d. Calling gurobi.\n", info);
}
else {
#endif
if (nc>0) {
pdata->Hqp = Jcom.transpose()*R_DQyD_ls*Jcom;
if (include_angular_momentum) {
pdata->Hqp += pdata->Ak.transpose()*pdata->W_kdot*pdata->Ak;
}
pdata->Hqp += pdata->w_qdd.asDiagonal();
pdata->Hqp += REG*MatrixXd::Identity(nq,nq);
} else {
pdata->Hqp = (1+REG)*MatrixXd::Identity(nq,nq);
}
// add in body spatial acceleration cost terms
double w_i;
for (int i=0; i<pdata->n_body_accel_inputs; i++) {
w_i=pdata->body_accel_input_weights(i);
if (w_i > 0) {
body_vdot = body_accel_inputs[i].bottomRows(6);
body_idx = (int)(body_accel_inputs[i][0])-1;
if (!inSupport(active_supports,body_idx)) {
pdata->r->forwardJac(body_idx,orig,1,Jb);
pdata->r->forwardJacDot(body_idx,orig,1,Jbdot);
for (int j=0; j<6; j++) {
if (!std::isnan(body_vdot[j])) {
pdata->Hqp += w_i*(Jb.row(j)).transpose()*Jb.row(j);
f.head(nq) += w_i*(qdvec.transpose()*Jbdot.row(j).transpose() - body_vdot[j])*Jb.row(j).transpose();
}
}
}
}
}
Qnfdiag = MatrixXd::Constant(nf,1,pdata->w_grf+REG);
Qneps = MatrixXd::Constant(neps,1,pdata->w_slack+REG);
QBlkDiag[0] = &pdata->Hqp;
if (nc>0) {
QBlkDiag[1] = &Qnfdiag;
QBlkDiag[2] = &Qneps; // quadratic slack var cost, Q(nparams-neps:end,nparams-neps:end)=eye(neps)
}
MatrixXd Ain_lb_ub(2*nu+2*nparams,nparams);
VectorXd bin_lb_ub(2*nu+2*nparams);
Ain_lb_ub << Ain, // note: obvious sparsity here
-MatrixXd::Identity(nparams,nparams),
MatrixXd::Identity(nparams,nparams);
bin_lb_ub << bin, -lb, ub;
if (use_fast_qp > 0)
{ // set up and call fastqp
info = fastQP(QBlkDiag, f, Aeq, beq, Ain_lb_ub, bin_lb_ub, pdata->active, alpha);
if (info<0) mexPrintf("fastQP info=%d... calling Gurobi.\n", info);
}
else {
// use gurobi active set
model = gurobiActiveSetQP(pdata->env,QBlkDiag,f,Aeq,beq,Ain,bin,lb,ub,pdata->vbasis,pdata->vbasis_len,pdata->cbasis,pdata->cbasis_len,alpha);
CGE(GRBgetintattr(model,"NumVars",&pdata->vbasis_len), pdata->env);
CGE(GRBgetintattr(model,"NumConstrs",&pdata->cbasis_len), pdata->env);
info=66;
//info = -1;
}
if (info<0) {
model = gurobiQP(pdata->env,QBlkDiag,f,Aeq,beq,Ain,bin,lb,ub,pdata->active,alpha);
int status; CGE(GRBgetintattr(model, "Status", &status), pdata->env);
if (status!=2) mexPrintf("Gurobi reports non-optimal status = %d\n", status);
}
#ifdef USE_MATRIX_INVERSION_LEMMA
}
#endif
//----------------------------------------------------------------------
// Solve for inputs ----------------------------------------------------
VectorXd y(nu);
VectorXd qdd = alpha.head(nq);
VectorXd beta = alpha.segment(nq,nc*nd);
// use transpose because B_act is orthogonal
y = pdata->B_act.transpose()*(pdata->H_act*qdd + pdata->C_act - D_act*beta);
//y = pdata->B_act.jacobiSvd(ComputeThinU|ComputeThinV).solve(pdata->H_act*qdd + pdata->C_act - Jz_act.transpose()*lambda - D_act*beta);
if (nlhs>0) {
plhs[0] = eigenToMatlab(y);
}
if (nlhs>1) {
plhs[1] = eigenToMatlab(qdd);
}
if (nlhs>2) {
plhs[2] = mxCreateNumericMatrix(1,1,mxINT32_CLASS,mxREAL);
memcpy(mxGetData(plhs[2]),&info,sizeof(int));
}
if (nlhs>3) {
plhs[3] = mxCreateDoubleMatrix(1,active_supports.size(),mxREAL);
pr = mxGetPr(plhs[3]);
int i=0;
for (vector<SupportStateElement>::iterator iter = active_supports.begin(); iter!=active_supports.end(); iter++) {
pr[i++] = (double) (iter->body_idx + 1);
}
}
if (nlhs>4) {
plhs[4] = eigenToMatlab(alpha);
}
if (nlhs>5) {
plhs[5] = eigenToMatlab(pdata->Hqp);
}
if (nlhs>6) {
plhs[6] = eigenToMatlab(f);
}
if (nlhs>7) {
plhs[7] = eigenToMatlab(Aeq);
}
if (nlhs>8) {
plhs[8] = eigenToMatlab(beq);
}
if (nlhs>9) {
plhs[9] = eigenToMatlab(Ain_lb_ub);
}
if (nlhs>10) {
plhs[10] = eigenToMatlab(bin_lb_ub);
}
if (nlhs>11) {
plhs[11] = eigenToMatlab(Qnfdiag);
}
if (nlhs>12) {
plhs[12] = eigenToMatlab(Qneps);
}
if (nlhs>13) {
double Vdot;
if (nc>0)
// note: Sdot is 0 for ZMP/double integrator dynamics, so we omit that term here
Vdot = ((2*x_bar.transpose()*S + s1.transpose())*(A_ls*x_bar + B_ls*(Jcomdot*qdvec + Jcom*qdd)) + s1dot.transpose()*x_bar)(0) + s2dot;
else
Vdot = 0;
plhs[13] = mxCreateDoubleScalar(Vdot);
}
if (nlhs>14) {
RigidBodyManipulator* r = pdata->r;
VectorXd individual_cops = individualSupportCOPs(r, active_supports, normals, B, beta);
plhs[14] = eigenToMatlab(individual_cops);
}
if (model) {
GRBfreemodel(model);
}
// GRBfreeenv(env);
}
int main( int /*argc*/, char* argv[] )
{
const static string ui_file = "app.clicks";
// Load configuration data
pangolin::ParseVarsFile("app.cfg");
InputRecordRepeat input("ui.");
input.LoadBuffer(ui_file);
// Target to track from
Target target;
target.GenerateRandom(60,25/(842.0/297.0),75/(842.0/297.0),15/(842.0/297.0),Vector2d(297,210));
// target.GenerateCircular(60,20,50,15,makeVector(210,210));
// target.GenerateEmptyCircle(60,25,75,15,200,makeVector(297,210));
target.SaveEPS("target_A4.eps");
cout << "Calibration target saved as: target_A4.eps" << endl;
// Setup Video
Var<string> video_uri("video_uri");
VideoRecordRepeat video(video_uri, "video.pvn", 1024*1024*200);
// VideoInput video(video_uri);
const unsigned w = video.Width();
const unsigned h = video.Height();
// Create Glut window
pangolin::CreateGlutWindowAndBind("Main",2*w+PANEL_WIDTH,h);
// Pangolin 3D Render state
pangolin::OpenGlRenderState s_cam;
s_cam.Set(ProjectionMatrixRDF_TopLeft(640,480,420,420,320,240,1,1E6));
s_cam.Set(FromTooN(toTooN(Sophus::SE3(Sophus::SO3(),Vector3d(-target.Size()[0]/2,-target.Size()[1]/2,500) ))));
pangolin::Handler3D handler(s_cam);
// Create viewport for video with fixed aspect
View& vPanel = pangolin::CreatePanel("ui").SetBounds(1.0,0.0,0,PANEL_WIDTH);
View& vVideo = pangolin::Display("Video").SetAspect((float)w/h);
View& v3D = pangolin::Display("3D").SetAspect((float)w/h).SetHandler(&handler);
// View& vDebug = pangolin::Display("Debug").SetAspect((float)w/h);
Display("Container")
.SetBounds(1.0,0.0,PANEL_WIDTH,1.0,false)
.SetLayout(LayoutEqual)
.AddDisplay(vVideo)
.AddDisplay(v3D)
// .AddDisplay(vDebug)
;
// OpenGl Texture for video frame
GlTexture texRGB(w,h,GL_RGBA8);
GlTexture tex(w,h,GL_LUMINANCE8);
// Declare Image buffers
CVD::ImageRef size(w,h);
Image<Rgb<byte> > Irgb(size);
Image<byte> I(size);
Image<float> intI(size);
Image<short> lI(size);
Image<std::array<float,2> > dI(size);
Image<byte> tI(size);
// Camera parameters
Matrix<float,5,1> cam_params; // = Var<Matrix<float,5,1> >("cam_params");
cerr << "Not loading params properly" << endl;
cam_params << 0.694621, 0.925258, 0.505055, 0.484551, 0.968455;
FovCamera cam( w,h, w*cam_params[0],h*cam_params[1], w*cam_params[2],h*cam_params[3], cam_params[4] );
// Last good pose
Sophus::SE3 T_gw;
std::clock_t last_good;
int good_frames;
// Pose hypothesis
Sophus::SE3 T_hw;
// Fixed mirrored pose
Sophus::SE3 T_0w;
// Stored keyframes
boost::ptr_vector<Keyframe> keyframes;
// Variables
Var<bool> record("ui.Record",false,false);
Var<bool> play("ui.Play",false,false);
Var<bool> source("ui.Source",false,false);
Var<bool> add_keyframe("ui.Add Keyframe",false,false);
Var<bool> use_mirror("ui.Use Mirror",false,true);
Var<bool> draw_mirror("ui.Draw Mirror",false,true);
// Var<bool> calc_mirror_pose("ui.Calculate Mirrored Pose",false,false);
Var<bool> disp_thresh("ui.Display Thresh",false);
Var<float> at_threshold("ui.Adap Threshold",1.0,0,1.0);
Var<int> at_window("ui.Adapt Window",w/3,1,200);
Var<float> conic_min_area(".Conic min area",25, 0, 100);
Var<float> conic_max_area(".Conic max area",4E4, 0, 1E5);
Var<float> conic_min_density(".Conic min density",0.4, 0, 1.0);
Var<float> conic_min_aspect(".Conic min aspect",0.1, 0, 1.0);
Var<int> target_match_neighbours(".Match Descriptor Neighbours",10, 5, 20);
Var<int> target_ransac_its("ui.Ransac Its", 100, 20, 500);
Var<int> target_ransac_min_pts("ui.Ransac Min Pts", 5, 5, 10);
Var<float> target_ransac_max_inlier_err_mm("ui.Ransac Max Inlier Err (mm)", 15, 0, 50);
Var<float> target_plane_inlier_thresh("ui.Plane inlier thresh", 1.5, 0.1, 10);
Var<double> rms("ui.RMS", 0);
Var<double> max_rms("ui.max RMS", 1.0, 0.01, 10);
Var<double> robust_4pt_inlier_tol("ui.Ransac 4Pt Inlier", 0.006, 1E-3, 1E-2);
Var<int> robust_4pt_its("ui.Ransac 4pt its", 100, 10, 500);
Var<double> max_mmps("ui.max mms per sec", 1500, 100, 2000);
Var<bool> lock_to_cam("ui.AR",false);
for(int frame=0; !pangolin::ShouldQuit(); ++frame)
{
Viewport::DisableScissor();
glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
// Get newest frame from camera and upload to GPU as texture
video.GrabNewest((byte*)Irgb.data(),true);
// Associate input with this video frame
input.SetIndex(video.FrameId());
// Generic processing
ConvertRGBtoI(Irgb,I);
gradient(w,h,I.data(),dI.data());
integral_image(w,h,I.data(),intI.data());
// Threshold and label image
AdaptiveThreshold(w,h,I.data(),intI.data(),tI.data(),at_threshold,at_window,(byte)0,(byte)255);
vector<PixelClass> labels;
Label(w,h,tI.data(),lI.data(),labels);
// Find conics
vector<PixelClass> candidates;
vector<Conic> conics;
FindCandidateConicsFromLabels(
w,h,labels,candidates,
conic_min_area,conic_max_area,conic_min_density, conic_min_aspect
);
FindConics(w,h,candidates,dI.data(),conics );
// Generate map and point structures
vector<int> conics_target_map(conics.size(),-1);
vector<Vector2d > ellipses;
for( size_t i=0; i < conics.size(); ++i )
ellipses.push_back(conics[i].center);
// Update pose given correspondences
T_hw = FindPose(cam,target.circles3D(),ellipses,conics_target_map,robust_4pt_inlier_tol,robust_4pt_its);
rms = ReprojectionErrorRMS(cam,T_hw,target.circles3D(),ellipses,conics_target_map);
int inliers =0;
for( int i=0; i < conics_target_map.size(); ++i)
if( conics_target_map[i] >=0 ) inliers++;
if( !isfinite((double)rms) || rms > max_rms || (good_frames < 5 && inliers < 6) )
{
// Undistort Conics
vector<Conic> conics_camframe;
for( unsigned int i=0; i<conics.size(); ++i )
conics_camframe.push_back(UnmapConic(conics[i],cam));
// Find target given (approximately) undistorted conics
const static LinearCamera idcam(-1,-1,1,1,0,0);
target.FindTarget(
idcam,conics_camframe, conics_target_map, target_match_neighbours,
target_ransac_its, target_ransac_min_pts, target_ransac_max_inlier_err_mm,
target_plane_inlier_thresh,use_mirror
);
// Estimate camera pose relative to target coordinate system
T_hw = FindPose(cam,target.circles3D(),ellipses,conics_target_map,robust_4pt_inlier_tol,robust_4pt_its);
rms = ReprojectionErrorRMS(cam,T_hw,target.circles3D(),ellipses,conics_target_map);
}
if( isfinite((double)rms) && rms < max_rms )
{
// seconds since good
const double t = std::difftime(clock(),last_good) / (double) CLOCKS_PER_SEC;
const double dx = (T_hw.inverse().translation() - T_gw.inverse().translation()).norm();
if( dx / t < max_mmps || last_good == 0) {
good_frames++;
}else{
good_frames = 0;
}
if( good_frames > 5 )
{
T_gw = T_hw;
last_good = clock();
}
}else{
good_frames = 0;
}
if( good_frames > 5 )
{
if( lock_to_cam ) {
s_cam.Set(FromTooN(toTooN(T_gw)));
}
if( pangolin::Pushed(add_keyframe) ) {
Keyframe* kf = new Keyframe();
kf->T_kw = T_gw;
kf->conics.insert(kf->conics.begin(),conics.begin(),conics.end());
kf->conics_target_map.insert(kf->conics_target_map.begin(),conics_target_map.begin(),conics_target_map.end());
keyframes.push_back(kf);
}
}
// Display Live Image
glColor3f(1,1,1);
vVideo.ActivateScissorAndClear();
if(!disp_thresh) {
texRGB.Upload(Irgb.data(),GL_RGB,GL_UNSIGNED_BYTE);
texRGB.RenderToViewportFlipY();
}else{
tex.Upload(tI.data(),GL_LUMINANCE,GL_UNSIGNED_BYTE);
tex.RenderToViewportFlipY();
}
// Display detected ellipses
glOrtho(-0.5,w-0.5,h-0.5,-0.5,0,1.0);
for( int i=0; i<ellipses.size(); ++i ) {
glColorBin(conics_target_map[i],target.circles().size());
DrawCross(ellipses[i],2);
}
glColor3f(0,1,0);
DrawLabels(candidates);
// Display 3D Vis
glEnable(GL_DEPTH_TEST);
v3D.ActivateScissorAndClear(s_cam);
glDepthFunc(GL_LEQUAL);
glDrawAxis(30);
DrawTarget(target,Vector2d(0,0),1,0.2,0.2);
DrawTarget(conics_target_map,target,Vector2d(0,0),1);
// Draw Camera
glColor3f(1,0,0);
glDrawFrustrum(cam.Kinv(),w,h,T_gw.inverse(),10);
Vector3d r_w = Vector3d::Zero();
if( keyframes.size() >= 3 )
{
// Method 1
// As described in this paper
// "Camera Pose Estimation using Images of Planar Mirror Reflections" ECCV 2010
// Rui Rodrigues, Joao Barreto, Urbano Nunes
const unsigned Nkf = keyframes.size() -1;
const Sophus::SE3 T_w0 = keyframes[0].T_kw.inverse();
MatrixXd As(Nkf*3,4);
As.setZero();
for( int i=0; i<Nkf; ++i )
{
const Sophus::SE3 T_iw = keyframes[i+1].T_kw * T_w0;
const Vector3d t = T_iw.translation();
const Vector3d theta_omega = T_iw.so3().log();
const double theta = theta_omega.norm();
const Vector3d omega = theta_omega / theta;
const double tanthby2 = tan(theta/2.0);
const Matrix3d skew_t = SkewSym(t);
As.block<3,3>(3*i,0) = skew_t + tanthby2 * omega * t.transpose();
As.block<3,1>(3*i,3) = -2 * tanthby2 * omega;
}
// Solve system using SVD
Eigen::JacobiSVD<MatrixXd> svd(As, ComputeFullV);
// Get mirror plane for virtual camera 0 in cam0 FoR
const Vector4d _N_0 = svd.matrixV().col(3);
Vector4d N_0 = _N_0 / (_N_0.head<3>()).norm();
// d has different meaning in paper. Negate to match my thesis.
N_0(3) *= -1;
// Render plane corresponding to first keyframe
glSetFrameOfReferenceF(keyframes[0].T_kw.inverse());
glColor3f(0.2,0,0);
DrawPlane(N_0,10,100);
glUnsetFrameOfReference();
// Attempt to render real camera.
// We seem to have the correct translation. Rotation is wrong.
// It's because the real camera (from the paper), relates to the real
// points, Q, but we're using Qhat which doesn't match the real Q.
{
const Vector4d Nz = Vector4d(0,0,1,0);
const Matrix4d T_wr(SymmetryTransform(Nz) * T_w0.matrix() * SymmetryTransform(N_0));
glColor3f(0,0,1);
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glMultMatrix( T_wr );
glDrawFrustrum(cam.Kinv(),w,h,30);
glPopMatrix();
}
// Draw live mirror
if( draw_mirror )
{
Vector3d l_w = T_gw.inverse().translation();
l_w[2] *= -1;
const Vector3d N = l_w - r_w;
const double dist = N.norm();
const Vector3d n = N / dist;
const double d = -(r_w + N/2.0).dot(n);
glColor3f(1,0,0);
DrawCross(l_w);
glColor4f(0.2,0.2,0.2,0.2);
DrawPlane(Vector4d(n[0],n[1],n[2],d),10,100);
}
}
// Keyframes
glColor3f(0.5,0.5,0.5);
// foreach (Keyframe& kf, keyframes) {
// glDrawFrustrum(cam.Kinv(),w,h,kf.T_kw.inverse(),30);
// }
if(keyframes.size() > 0 )
{
glSetFrameOfReferenceF(keyframes[0].T_kw.inverse());
glDrawAxis(30);
glUnsetFrameOfReference();
glColor3f(1,0.5,0.5);
glDrawFrustrum(cam.Kinv(),w,h,keyframes[0].T_kw.inverse(),30);
}
if(pangolin::Pushed(record)) {
video.Record();
input.Record();
}
if(pangolin::Pushed(play)) {
video.Play(true);
input.PlayBuffer(0,input.Size()-1);
input.SaveBuffer(ui_file);
}
if(pangolin::Pushed(source)) {
video.Source();
input.Stop();
input.SaveBuffer(ui_file);
}
vPanel.Render();
// Swap back buffer with front
glutSwapBuffers();
// Process window events via GLUT
glutMainLoopEvent();
}
return 0;
}
void MapInitializer::addObservation(Vector3d & X, Vector2d pt, Transformation<double> & pose,
const ICamera * cam, const Transformation<double> & TbaseCam)
{
CostFunction * costFunc = new ReprojectionErrorStereo(pt, TbaseCam, cam);
problem.AddResidualBlock(costFunc, NULL, X.data(), pose.transData(), pose.rotData());
}
void BondAngleDistribution::process() {
Molecule* mol;
Atom* atom;
RigidBody* rb;
int myIndex;
SimInfo::MoleculeIterator mi;
Molecule::RigidBodyIterator rbIter;
Molecule::AtomIterator ai;
StuntDouble* sd;
Vector3d vec;
std::vector<Vector3d> bondvec;
RealType r;
int nBonds;
int i;
DumpReader reader(info_, dumpFilename_);
int nFrames = reader.getNFrames();
frameCounter_ = 0;
nTotBonds_ = 0;
for (int istep = 0; istep < nFrames; istep += step_) {
reader.readFrame(istep);
frameCounter_++;
currentSnapshot_ = info_->getSnapshotManager()->getCurrentSnapshot();
if (evaluator_.isDynamic()) {
seleMan_.setSelectionSet(evaluator_.evaluate());
}
// update the positions of atoms which belong to the rigidbodies
for (mol = info_->beginMolecule(mi); mol != NULL;
mol = info_->nextMolecule(mi)) {
for (rb = mol->beginRigidBody(rbIter); rb != NULL;
rb = mol->nextRigidBody(rbIter)) {
rb->updateAtoms();
}
}
// outer loop is over the selected StuntDoubles:
for (sd = seleMan_.beginSelected(i); sd != NULL;
sd = seleMan_.nextSelected(i)) {
myIndex = sd->getGlobalIndex();
nBonds = 0;
bondvec.clear();
// inner loop is over all other atoms in the system:
for (mol = info_->beginMolecule(mi); mol != NULL;
mol = info_->nextMolecule(mi)) {
for (atom = mol->beginAtom(ai); atom != NULL;
atom = mol->nextAtom(ai)) {
if (atom->getGlobalIndex() != myIndex) {
vec = sd->getPos() - atom->getPos();
if (usePeriodicBoundaryConditions_)
currentSnapshot_->wrapVector(vec);
// Calculate "bonds" and make a pair list
r = vec.length();
// Check to see if neighbor is in bond cutoff
if (r < rCut_) {
// Add neighbor to bond list's
bondvec.push_back(vec);
nBonds++;
nTotBonds_++;
}
}
}
for (int i = 0; i < nBonds-1; i++ ){
Vector3d vec1 = bondvec[i];
vec1.normalize();
for(int j = i+1; j < nBonds; j++){
Vector3d vec2 = bondvec[j];
vec2.normalize();
RealType theta = acos(dot(vec1,vec2))*180.0/NumericConstant::PI;
if (theta > 180.0){
theta = 360.0 - theta;
}
int whichBin = int(theta/deltaTheta_);
histogram_[whichBin] += 2;
}
}
}
}
}
writeBondAngleDistribution();
}
// Convert to Printlines
void Layer::MakePrintlines(Vector3d &lastPos, //GCodeState &state,
vector<PLine3> &lines3,
double offsetZ,
Settings &settings) const
{
const double linewidth = settings.GetExtrudedMaterialWidth(thickness);
const double cornerradius = linewidth*settings.get_double("Slicing","CornerRadius");
const bool clipnearest = settings.get_boolean("Slicing","MoveNearest");
const uint supportExtruder = settings.GetSupportExtruder();
const double minshelltime = settings.get_double("Slicing","MinShelltime");
const double maxshellspeed = settings.get_double("Extruder","MaxShellSpeed");
const bool ZliftAlways = settings.get_boolean("Extruder","ZliftAlways");
Vector2d startPoint(lastPos.x(),lastPos.y());
const double extr_per_mm = settings.GetExtrusionPerMM(thickness);
//vector<PLine3> lines3;
Printlines printlines(this, &settings, offsetZ);
vector<PLine2> lines;
vector<Poly> polys; // intermediate collection
// polys to keep line movements inside
//const vector<Poly> * clippolys = &polygons;
const vector<Poly> * clippolys = GetOuterShell();
// 1. Skins, all but last, because they are the lowest lines, below layer Z
if (skins > 1) {
for(uint s = 0; s < skins; s++) {
// z offset from bottom to top:
double skin_z = Z - thickness + (s+1)*thickness/skins;
if ( skin_z < 0 ){
cerr << "Skin Z<0! " << s << " -- " << Z << " -- "<<skin_z <<" -- " << thickness << endl;
continue;
}
// skin infill polys:
if (skinFullInfills[s])
polys.insert(polys.end(),
skinFullInfills[s]->infillpolys.begin(),
skinFullInfills[s]->infillpolys.end());
// add skin infill to lines
printlines.addPolys(INFILL, polys, false);
polys.clear();
// make polygons at skin_z:
for(size_t p = 0; p < skinPolygons.size(); p++) {
polys.push_back(Poly(skinPolygons[p], skin_z));
}
// add skin to lines
printlines.addPolys(SKIN, polys, (s==0), // displace at first skin
maxshellspeed * 60,
minshelltime);
if (s < skins-1) { // not on the last layer, this handle with all other lines
// have to get all these separately because z changes
printlines.makeLines(startPoint, lines);
if (!ZliftAlways)
printlines.clipMovements(*clippolys, lines, clipnearest, linewidth);
printlines.optimize(linewidth,
minshelltime, cornerradius, lines);
printlines.getLines(lines, lines3, extr_per_mm);
printlines.clear();
lines.clear();
}
polys.clear();
}
} // last skin layer now still in lines
lines.clear();
// 2. Skirt
printlines.addPolys(SKIRT, skirtPolygons, false,
maxshellspeed * 60,
minshelltime);
// 3. Support
if (supportInfill) {
uint extruderbefore = settings.selectedExtruder;
settings.SelectExtruder(supportExtruder);
printlines.addPolys(SUPPORT, supportInfill->infillpolys, false);
settings.SelectExtruder(extruderbefore);
}
// 4. all other polygons:
// Shells
for(int p=shellPolygons.size()-1; p>=0; p--) { // inner to outer
printlines.addPolys(SHELL, shellPolygons[p],
(p==(int)(shellPolygons.size())-1),
maxshellspeed * 60,
minshelltime);
}
// Infill
if (normalInfill)
printlines.addPolys(INFILL, normalInfill->infillpolys, false);
if (thinInfill)
printlines.addPolys(INFILL, thinInfill->infillpolys, false);
if (fullInfill)
printlines.addPolys(INFILL, fullInfill->infillpolys, false);
if (skirtInfill)
printlines.addPolys(INFILL, skirtInfill->infillpolys, false);
if (decorInfill)
printlines.addPolys(INFILL, decorInfill->infillpolys, false);
for (uint b=0; b < bridgeInfills.size(); b++)
if (bridgeInfills[b])
printlines.addPolys(INFILL, bridgeInfills[b]->infillpolys, false);
double polyspeedfactor = printlines.makeLines(startPoint, lines);
// FINISH
Command lchange(LAYERCHANGE, LayerNo);
lchange.where = Vector3d(0.,0.,Z);
lchange.comment += info();
lines3.push_back(PLine3(lchange));
if (!ZliftAlways)
printlines.clipMovements(*clippolys, lines, clipnearest, linewidth);
printlines.optimize(linewidth,
settings.get_double("Slicing","MinLayertime"),
cornerradius, lines);
if ((guint)LayerNo < (guint)settings.get_integer("Slicing","FirstLayersNum"))
printlines.setSpeedFactor(settings.get_double("Slicing","FirstLayersSpeed"), lines);
double slowdownfactor = printlines.getSlowdownFactor() * polyspeedfactor;
if (settings.get_boolean("Slicing","FanControl")) {
int fanspeed = settings.get_integer("Slicing","MinFanSpeed");
if (slowdownfactor < 1 && slowdownfactor > 0) {
double fanfactor = 1-slowdownfactor;
fanspeed +=
int(fanfactor * (settings.get_integer("Slicing","MaxFanSpeed")-settings.get_integer("Slicing","MinFanSpeed")));
fanspeed = CLAMP(fanspeed, settings.get_integer("Slicing","MinFanSpeed"),
settings.get_integer("Slicing","MaxFanSpeed"));
//cerr << slowdownfactor << " - " << fanfactor << " - " << fanspeed << " - " << endl;
}
Command fancommand(FANON, fanspeed);
lines3.push_back(PLine3(fancommand));
}
printlines.getLines(lines, lines3, extr_per_mm);
if (lines3.size()>0)
lastPos = lines3.back().to;
}
bool se2bracket_tests()
{
bool failed = false;
vector<Vector3d> vecs;
Vector3d tmp;
// tmp << 0,0,0;
// vecs.push_back(tmp);
// tmp << 1,0,0;
// vecs.push_back(tmp);
// tmp << 0,1,1;
// vecs.push_back(tmp);
// tmp << -1,1,0;
// vecs.push_back(tmp);
tmp << 20,-1,-1;
vecs.push_back(tmp);
// tmp << 30,5,20;
// vecs.push_back(tmp);
for (unsigned int i=0; i<vecs.size(); ++i)
{
Vector3d resDiff = vecs[i] - SE2::vee(SE2::hat(vecs[i]));
if (resDiff.norm()>SMALL_EPS)
{
cerr << "Hat-vee Test" << endl;
cerr << "Test case: " << i << endl;
cerr << resDiff.transpose() << endl;
cerr << endl;
}
for (unsigned int j=0; j<vecs.size(); ++j)
{
Vector3d res1 = SE2::lieBracket(vecs[i],vecs[j]);
Matrix3d hati = SE2::hat(vecs[i]);
Matrix3d hatj = SE2::hat(vecs[j]);
Vector3d res2 = SE2::vee(hati*hatj-hatj*hati);
Vector3d resDiff = res1-res2;
if (resDiff.norm()>SMALL_EPS)
{
cerr << "SE2 Lie Bracket Test" << endl;
cerr << "Test case: " << i << ", " <<j<< endl;
cerr << vecs[i].transpose() << endl;
cerr << vecs[j].transpose() << endl;
cerr << res1 << endl;
cerr << res2 << endl;
cerr << resDiff.transpose() << endl;
cerr << endl;
failed = true;
}
}
Vector3d omega = vecs[i];
Matrix3d exp_x = SE2::exp(omega).matrix();
Matrix3d expmap_hat_x = (SE2::hat(omega)).exp();
Matrix3d DiffR = exp_x-expmap_hat_x;
double nrm = DiffR.norm();
if (isnan(nrm) || nrm>SMALL_EPS)
{
cerr << "expmap(hat(x)) - exp(x)" << endl;
cerr << "Test case: " << i << endl;
cerr << exp_x <<endl;
cerr << expmap_hat_x <<endl;
cerr << DiffR <<endl;
cerr << endl;
failed = true;
}
}
return failed;
}
GLThread::GLThread() {
int numInit = (NUM_POINTS-3)/2;
double noise_factor = 0.0;
display_start_pos.setZero();
vector<Vector3d> vertices;
vector<double> angles;
vertices.push_back(Vector3d::Zero());
angles.push_back(0.0);
//push back unitx so first tangent matches start_frame
vertices.push_back(Vector3d::UnitX()*DEFAULT_REST_LENGTH);
angles.push_back(0.0);
Vector3d direction;
direction(0) = 1.0;
direction(1) = 0.0;
direction(2) = -1.0;
direction.normalize();
for (int i=0; i < numInit; i++)
{
Vector3d noise( ((double)(rand()%10000)) / 10000.0, ((double)(rand()%10000)) / 10000.0, ((double)(rand()%10000)) / 10000.0);
noise *= noise_factor;
Vector3d next_Vec = vertices.back()+(direction+noise).normalized()*DEFAULT_REST_LENGTH;
vertices.push_back(next_Vec);
angles.push_back(0.0);
//std::cout << positions[i] << std::endl << std::endl;
}
//change direction
direction(0) = 1.0;
direction(1) = 0.0;
direction(2) = 1.0;
direction.normalize();
for (int i=0; i < numInit; i++)
{
Vector3d noise( ((double)(rand()%10000)) / 10000.0, ((double)(rand()%10000)) / 10000.0, ((double)(rand()%10000)) / 10000.0);
noise *= noise_factor;
Vector3d next_Vec = vertices.back()+(direction+noise).normalized()*DEFAULT_REST_LENGTH;
vertices.push_back(next_Vec);
angles.push_back(0.0);
}
//push back unitx so last tangent matches end_frame
vertices.push_back(vertices.back()+Vector3d::UnitX()*DEFAULT_REST_LENGTH);
angles.push_back(0.0);
angles.resize(vertices.size());
rotations[0] = Matrix3d::Identity();
rotations[1] = Matrix3d::Identity();
_thread = new Thread(vertices, angles, rotations[0], rotations[1]);
//_thread->set_start_rest_length(1);
//_thread->set_end_rest_length(1);
//_thread->set_rest_length(DEFAULT_REST_LENGTH);
#ifdef ISOTROPIC
to_set_bend = _thread->get_bend_coeff();
#else
to_set_B = _thread->get_bend_matrix();
#endif
to_set_twist = _thread->get_twist_coeff();
to_set_grav = _thread->get_grav_coeff();
#ifdef ISOTROPIC
_thread->set_coeffs_normalized(to_set_bend, to_set_twist, to_set_grav);
#else
_thread->set_coeffs_normalized(to_set_B, to_set_twist, to_set_grav);
#endif
InitContour();
strcpy(_display_name, "");
}
// Questa funzione e' quella che in background fa tutti i conti matematici, quindi qui devi inserire
// 1) Scrittura su file continua delle coordinate che vuoi salvare
// 2) Estrazione delle coordinate a partire dai corpi rigidi precedentemente definiti vedi ad esempio
// come e' fatto per eyeLeft e eyeRight oppure per thumb ed index
void idle()
{
optotrak.updateMarkers();
markers = optotrak.getAllMarkers();
// Coordinates picker //is this correct? (are the marker identities the same?)
allVisiblePlatform = isVisible(markers[15].p) && isVisible(markers[16].p);
allVisibleThumb = isVisible(markers[20].p) && isVisible(markers[21].p) && isVisible(markers[22].p);
allVisibleIndex = isVisible(markers[22].p) && isVisible(markers[23].p) && isVisible(markers[24].p);
allVisibleFingers = allVisibleIndex && allVisibleThumb;
if ( allVisibleFingers )
{
thumbCoords.update(markers[20].p, markers[21].p, markers[22].p );
indexCoords.update(markers[19].p, markers[23].p, markers[24].p );
}
eyeRight = Vector3d(0,0,0);
index = indexCoords.getP1();
thumb = thumbCoords.getP1();
//////////////////////////
/// Real Stuff
//////////////////////////
if (fingerCalibrationDone==3) { // After we've calibrated
if ( !allVisibleFingers ) { // Check for finger occlusion
beepOk(0);
if (started && !reachedObject) // Increment counter if hand is in flight
numLostFrames += 1;
}
frameNumber++; // Advance frame number
// Check that we're at the start position
if( (index.y() < startPos_top) && // index below ceiling
(index.y() > startPos_bottom) && // index above floor
(index.x() < startPos_right) && // index left of right wall
(index.x() > startPos_left) && // index right of left wall
(index.z() < startPos_rear) && // index in front of rear wall
(index.z() > startPos_front) ) { // index behind front wall
// if so, keep resetting timer (need this outside the "fingers together" loop!)
timer.start();
// distance from index to thumb less than 1.5cm in each direction
if ((abs(index.y() - thumb.y()) < 15) &&
(abs(index.x() - thumb.x()) < 15) &&
(abs(index.z() - thumb.z()) < 15) ) {
// if so, we are in the start position
handAtStart = true;
}
} else if (allVisibleFingers) { // we've moved from the start, begin counting
handAtStart = false;
started = true;
start_frame = frameNumber;
if (!reachedObject) //(trial.getCurrent().at("OpenLoop") && !reachedObject)
plato_write(3);
}
// Find component distances
grip_X = (index.x()+thumb.x())/2;
grip_Y = (index.y()+thumb.y())/2;
grip_Z = (index.z()+thumb.z())/2;
x_dist = abs(grip_X - target_X);
y_dist = abs(grip_Y - target_Y);
z_dist = abs(grip_Z - target_Z);
plato_trigger = index.z() - (target_Z+20);
// Distance formulas
grip_aperture = sqrt(
(abs(index.x() - thumb.x())*abs(index.x() - thumb.x())) +
(abs(index.y() - thumb.y())*abs(index.y() - thumb.y())) +
(abs(index.z() - thumb.z())*abs(index.z() - thumb.z())));
grip_distanceToTarget = sqrt((x_dist*x_dist)+(y_dist*y_dist)+(z_dist*z_dist));
// If hand is in flight
if (!reachedObject && started && allVisibleFingers) {
// PLATO Threshold
if ( (plato_trigger < 0) && (plato_trigger > -1000) )
{
reachedObject = true;
//if (trial.getCurrent().at("OpenLoop"))
plato_write(0);
TGA_frame = frameNumber - start_frame;
TGA_time = timer.getElapsedTimeInMilliSec();
}
}
if(handAtStart && started)
advanceTrial();
}
// Write to trialFile
if (fingerCalibrationDone==3 )
{
int TN;
if (controlCondition)
TN = controlTrial;
else
TN = trialNumber;
trialFile << fixed <<
parameters.find("SubjectName") << "\t" << //subjName
TN << "\t" << //trialN
timer.getElapsedTimeInMilliSec() << "\t" << //time
frameNumber << "\t" << //frameN
index.transpose() << "\t" << //indexXraw, indexYraw, indexZraw
thumb.transpose() << "\t" << //thumbXraw, thumbYraw, thumbZraw
grip_distanceToTarget << "\t" <<
allVisibleFingers << "\t" << //allVisibleFingers
reachedObject << endl;
}
}
double MyController::onAction(ActionEvent &evt)
{
/* if(!checkService("RecogTrash")){
m_srv == NULL;
m_state = 0;
return UPDATE_INTERVAL;
}
if(m_srv == NULL){
// ゴミ認識サービスが利用可能か調べる
if(checkService("RecogTrash")){
// ゴミ認識サービスに接続
m_srv = connectToService("RecogTrash");
return UPDATE_INTERVAL;
}
}*/
//if(evt.time() < m_time) printf("state: %d \n", m_state);
switch(m_state) {
// 初期状態
case 0: {
if(m_srv == NULL){
// ゴミ認識サービスが利用可能か調べる
if(checkService("RecogTrash")){
// ゴミ認識サービスに接続
m_srv = connectToService("RecogTrash");
}
} else if(m_srv != NULL && m_executed == false){
//rotate toward upper
m_my->setJointVelocity("LARM_JOINT4", -m_jvel, 0.0);
m_my->setJointVelocity("RARM_JOINT4", -m_jvel, 0.0);
// 50°回転
m_time = DEG2RAD(ROTATE_ANG) / m_jvel + evt.time();
m_state = 5;
m_executed = false;
}
break;
}
case 5: {
if(evt.time() > m_time && m_executed == false) {
m_my->setJointVelocity("LARM_JOINT4", 0.0, 0.0);
m_my->setJointVelocity("RARM_JOINT4", 0.0, 0.0);
sendSceneInfo("Start");
printf("Started! \n");
m_executed = true;
}
break;
}
case 800: {
if(evt.time() > m_time && m_executed == false) {
sendSceneInfo();
m_executed = true;
}
break;
}
case 805: {
if(evt.time() > m_time && m_executed == false) {
// 送られた座標に移動する
double range = 0;
m_time = rotateTowardObj(nextPos, m_rotateVel, evt.time());
m_state = 807;
m_executed = false;
}
break;
}
case 807: {
if(evt.time() > m_time && m_executed == false) {
m_my->setWheelVelocity(0.0, 0.0);
printf("移動先 x: %lf, z: %lf \n", nextPos.x(), nextPos.z());
m_time = goToObj(nextPos, m_vel*4, m_range, evt.time());
if (m_lookObjFlg == 1.0) {
printf("looking to Obj \n");
m_state = 810;
} else {
printf("go to next node \n");
m_state = 815;
}
m_executed = false;
}
break;
}
case 810: {
// 送られた座標に移動中
if(evt.time() > m_time && m_executed == false) {
m_my->setWheelVelocity(0.0, 0.0);
m_time = rotateTowardObj(m_lookingPos, m_rotateVel, evt.time());
m_executed = false;
m_state = 815;
}
break;
}
case 815: {
// 送られた座標に移動中
if(evt.time() > m_time && m_executed == false) {
m_my->setWheelVelocity(0.0, 0.0);
sendSceneInfo();
printf("sent data to SIGViewer \n");
m_executed = true;
}
break;
}
case 920: {
// 送られた座標に回転する
m_time = rotateTowardObj(nextPos, m_rotateVel, evt.time());
m_state = 921;
m_executed = false;
break;
}
case 921: {
// ロボットが回転中
if(evt.time() > m_time && m_executed == false) {
m_my->setWheelVelocity(0.0, 0.0);
m_executed = false;
}
break;
}
default: {
break;
}
}
return UPDATE_INTERVAL;
}
/*************************** FUNCTIONS ***********************************/
void drawInfo()
{
if (paused)
visibleInfo = true;
if ( visibleInfo )
{
glDisable(GL_COLOR_MATERIAL);
glDisable(GL_BLEND);
glDisable(GL_LIGHTING);
GLText text;
text.init(SCREEN_WIDTH,SCREEN_HEIGHT,glWhite,GLUT_BITMAP_HELVETICA_18);
text.enterTextInputMode();
glColor3fv(glWhite);
text.draw("####### EXPERIMENTER INSTRUCTIONS #######");
if (!objectsForward)
{
text.draw("Press F to bring platform forward");
}
if ( objectsForward && (startPosCalibration==0) && (fingerCalibrationDone==0))
{
text.draw("Press F to record platform markers");
}
if ( objectsForward && (startPosCalibration==0) && (fingerCalibrationDone==1) )
{
text.draw("Move index and thumb on platform markers to record ghost finger tips, then press F");
}
if ( objectsForward && (startPosCalibration==0) && (fingerCalibrationDone==2) && allVisibleFingers)
{
text.draw("Move index and thumb to starting position, then press F");
}
if ( objectsForward && (startPosCalibration==1) && (fingerCalibrationDone==2))
{
text.draw("Mount the display board on the platform, then press F.");
}
if ( objectsForward && (startPosCalibration==2) && (fingerCalibrationDone==2))
{
text.draw("Press F to record position of left object (use straw).");
}
if ( objectsForward && (startPosCalibration==3) && (fingerCalibrationDone==2))
{
text.draw("Press F to record position of right object (use straw).");
}
if ( objectsForward && (startPosCalibration==4) && (fingerCalibrationDone==2) && allVisibleFingers)
{
text.draw("Press F to begin!");
}
text.draw("#########################################");
text.draw(" ");
text.draw("####### SUBJECT #######");
text.draw("#");
text.draw("# Name: " +parameters.find("SubjectName"));
text.draw("#");
text.draw("# Trial: " +stringify<int>(trialNumber));
text.draw("#");
text.draw("#######################");
text.draw("StartPosCalibration = " + stringify<int>(startPosCalibration) );
text.draw("FingerCalibration = " + stringify<int>(fingerCalibrationDone) );
text.draw(" ");
text.draw("Calibration Platform Markers" );
if ( isVisible(markers[15].p) )
glColor3fv(glGreen);
else
glColor3fv(glRed);
text.draw("Thumb Calibration Point (15) "+stringify< Eigen::Matrix<double,1,3> >(markers[15].p.transpose())+ " [mm]" );
if ( isVisible(markers[16].p) )
glColor3fv(glGreen);
else
glColor3fv(glRed);
text.draw("Index Calibration Point (16) "+stringify< Eigen::Matrix<double,1,3> >(markers[16].p.transpose())+ " [mm]" );
if ( isVisible(markers[10].p) )
glColor3fv(glGreen);
else
glColor3fv(glRed);
text.draw("Straw Marker (10) "+stringify< Eigen::Matrix<double,1,3> >(markers[10].p.transpose())+ " [mm]" );
glColor3fv(glWhite);
text.draw(" ");
text.draw("Thumb" );
if ( isVisible(markers[20].p) && isVisible(markers[21].p) && isVisible(markers[22].p) )
glColor3fv(glGreen);
else
glColor3fv(glRed);
text.draw("Marker "+ stringify<int>(20)+stringify< Eigen::Matrix<double,1,3> > (markers[20].p.transpose())+ " [mm]" );
text.draw("Marker "+ stringify<int>(21)+stringify< Eigen::Matrix<double,1,3> > (markers[21].p.transpose())+ " [mm]" );
text.draw("Marker "+ stringify<int>(22)+stringify< Eigen::Matrix<double,1,3> > (markers[22].p.transpose())+ " [mm]" );
glColor3fv(glWhite);
text.draw(" ");
text.draw("Index" );
if ( isVisible(markers[19].p) && isVisible(markers[23].p) && isVisible(markers[24].p) )
glColor3fv(glGreen);
else
glColor3fv(glRed);
text.draw("Marker "+ stringify<int>(11)+stringify< Eigen::Matrix<double,1,3> > (markers[19].p.transpose())+ " [mm]" );
text.draw("Marker "+ stringify<int>(12)+stringify< Eigen::Matrix<double,1,3> > (markers[23].p.transpose())+ " [mm]" );
text.draw("Marker "+ stringify<int>(13)+stringify< Eigen::Matrix<double,1,3> > (markers[24].p.transpose())+ " [mm]" );
glColor3fv(glGreen);
text.draw(" ");
text.draw("Index= " +stringify< Eigen::Matrix<double,1,3> >(index.transpose()));
text.draw("Thumb= " +stringify< Eigen::Matrix<double,1,3> >(thumb.transpose()));
text.draw(" ");
text.draw("Trial= " + stringify<int>(trialNumber));
text.draw("Frame= " + stringify<double>(frameNumber));
text.draw("Timer= " + stringify<int>(timer.getElapsedTimeInMilliSec()) );
text.draw(" ");
text.draw("Target Object : " + stringify<int>(currentObj));
if (handAtStart)
{
glColor3fv(glGreen);
text.draw("Hand at start.");
}
else
{
glColor3fv(glWhite);
text.draw("Hand in flight.");
}
text.draw(" ");
if(reachedObject)
{
glColor3fv(glGreen);
text.draw("Object Reached!");
}
else
{
glColor3fv(glRed);
text.draw("Distance to object= " + stringify<double>(grip_distanceToTarget));
}
text.leaveTextInputMode();
}
}
double MyController::rotateTowardObj(Vector3d pos, double velocity, double now)
{
// 自分の位置の取得
Vector3d myPos;
m_my->getPosition(myPos);
// 自分の位置からターゲットを結ぶベクトル
Vector3d tmpp = pos;
tmpp -= myPos;
// y方向は考えない
tmpp.y(0);
// 自分の回転を得る
Rotation myRot;
m_my->getRotation(myRot);
printf("ロボットの現在位置: x: %lf, z %lf \n", myPos.x(), myPos.z());
// エンティティの初期方向
Vector3d iniVec(0.0, 0.0, 1.0);
// y軸の回転角度を得る(x,z方向の回転は無いと仮定)
double qw = myRot.qw();
double qy = myRot.qy();
double theta = 2*acos(fabs(qw));
if(qw*qy < 0)
theta = -1*theta;
printf("ロボットが向いている角度 current theta: %lf(deg) \n", theta * 180 / PI);
// z方向からの角度
double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
double targetAngle = acos(tmp);
// 方向
if(tmpp.x() > 0) targetAngle = -1*targetAngle;
targetAngle += theta;
printf("targetAngle: %lf(deg) currentAngle: %lf(deg) \n", targetAngle*180.0/PI, theta * 180.0 / PI);
if(targetAngle == 0.0){
return 0.0;
}
else {
// 回転すべき円周距離
double distance = m_distance*PI*fabs(targetAngle)/(2*PI);
printf("distance: %lf \n", distance);
// 車輪の半径から移動速度を得る
double vel = m_radius*velocity;
printf("radius: %lf, velocity: %lf, vel: %lf \n", m_radius, velocity, vel);
// 回転時間(u秒)
double time = distance / vel;
printf("rotateTime: %lf = dis: %lf / vel: %lf\n", time, distance, vel);
// 車輪回転開始
if(targetAngle > 0.0){
m_my->setWheelVelocity(velocity, -velocity);
}
else{
m_my->setWheelVelocity(-velocity, velocity);
}
return now + time;
}
}
// Funzione di callback per gestire pressioni dei tasti
void handleKeypress(unsigned char key, int x, int y)
{
switch (key)
{
case 'x':
{
}
break;
case 'i':
{
visibleInfo=!visibleInfo;
}
break;
case 't':
{
training = !training;
}
break;
case '*':
{
}
break;
case 'q':
{
}
break;
case 27: //ESC
{
exit(0);
}
break;
case 'f':
{
if (!objectsForward)
{
beepOk(2);
objectsForward = true;
RoveretoMotorFunctions::moveObjectAbsolute(mountDisplayPosn, motorHomePosn, 3000);
break;
}
if ( objectsForward && (startPosCalibration==0) && (fingerCalibrationDone==0) && allVisiblePlatform)
{
platformIndex=markers[16].p;
platformThumb=markers[15].p;
fingerCalibrationDone=1;
beepOk(2);
break;
}
if ( (startPosCalibration==0) && (fingerCalibrationDone==1) && allVisibleFingers )
{
indexCoords.init(platformIndex, markers.at(19).p, markers.at(23).p, markers.at(24).p );
thumbCoords.init(platformThumb, markers.at(20).p, markers.at(21).p, markers.at(22).p );
fingerCalibrationDone=2;
beepOk(2);
break;
}
if ( (startPosCalibration==0) && (fingerCalibrationDone==2) && allVisibleFingers)
{
startPos_front = index.z()-20;
startPos_right = index.x()+20;
startPos_top = index.y()+20;
startPos_rear = thumb.z()+20;
startPos_left = thumb.x()-20;
startPos_bottom = thumb.y()-20;
startPosCalibration=1;
beepOk(2);
break;
}
if ( (startPosCalibration==1) && (fingerCalibrationDone==2))
{
startPosCalibration = 2;
beepOk(2);
RoveretoMotorFunctions::moveObjectAbsolute(showTopMidBoard,motorHomePosn,4000);
break;
}
if ( (startPosCalibration==2) && (fingerCalibrationDone==2) )
{
objL_X = markers[10].p.x();
objL_Y = markers[10].p.y();
objL_Z = markers[10].p.z();
startPosCalibration=3;
beepOk(2);
break;
}
if ( (startPosCalibration==3) && (fingerCalibrationDone==2) )
{
objR_X = markers[10].p.x();
objR_Y = markers[10].p.y();
objR_Z = markers[10].p.z();
startPosCalibration=4;
beepOk(2);
break;
}
if ( (startPosCalibration==4) && (fingerCalibrationDone==2) && allVisibleFingers)
{
fingerCalibrationDone=3;
visibleInfo=false;
training=false;
factors = trial.getNext();
initTrial();
break;
}
}
break;
case 's':
{
advanceTrial();
}
break;
case '0':
{
}
break;
case 'e':
{
cout << "Hand at start: " << handAtStart << endl;
cout << "Started: " << started << endl;
cout << "Reached Object: " << reachedObject << endl;
}
break;
case 'j':
{
cout << "Distance to Object: sqrt(" << x_dist << "^2 + " << y_dist << "^2 + " << z_dist << "^2) = " << grip_distanceToTarget << endl;
cout << "Start Pos Corner: " << startPos_right << "\t" << startPos_top << "\t" << startPos_front << endl;
}
break;
case '5': //down
{
}
break;
case '8': //up
{
}
break;
case '4': //left
{
}
break;
case '6': //right
{
}
break;
case '1': //left 2
{
}
break;
case '3': //right 2
{
}
break;
}
}
void ikTest()
{
cout << "--------------------------------------------" << endl;
cout << "| Testing Standard Damped Least Squares IK |" << endl;
cout << "--------------------------------------------" << endl;
Robot parseTest("../urdf/huboplus.urdf");
parseTest.linkage("Body_RSP").name("RightArm");
parseTest.linkage("Body_LSP").name("LeftArm");
parseTest.linkage("Body_RHY").name("RightLeg");
parseTest.linkage("Body_LHY").name("LeftLeg");
string limb = "LeftArm";
VectorXd targetValues, jointValues, restValues, storedVals;
targetValues.resize(parseTest.linkage(limb).nJoints());
jointValues.resize(parseTest.linkage(limb).nJoints());
restValues.resize(parseTest.linkage(limb).nJoints());
restValues.setZero();
if(limb == "RightLeg" || limb == "LeftLeg")
{
restValues[0] = 0;
restValues[1] = 0;
restValues[2] = -0.15;
restValues[3] = 0.3;
restValues[4] = -0.15;
restValues[5] = 0;
}
else if(limb == "RightArm" || limb == "LeftArm")
{
if(limb=="RightArm")
restValues[0] = -30*M_PI/180;
else
restValues[0] = 30*M_PI/180;
restValues[1] = 0;
restValues[2] = 0;
restValues[3] = -30*M_PI/180;
restValues[4] = 0;
restValues[5] = 0;
}
bool totallyRandom = false;
int resolution = 1000;
double scatterScale = 0.05;
int tests = 10000;
Constraints constraints;
constraints.restingValues(restValues);
constraints.performNullSpaceTask = false;
constraints.maxAttempts = 1;
constraints.maxIterations = 50;
constraints.convergenceTolerance = 0.001;
// constraints.wrapToJointLimits = true;
// constraints.wrapSolutionToJointLimits = true;
constraints.wrapToJointLimits = false;
constraints.wrapSolutionToJointLimits = false;
TRANSFORM target;
int wins = 0, jumps = 0;
srand(time(NULL));
clock_t time;
time = clock();
for(int k=0; k<tests; k++)
{
for(int i=0; i<parseTest.linkage(limb).nJoints(); i++)
{
int randomVal = rand();
targetValues(i) = ((double)(randomVal%resolution))/((double)resolution-1)
*(parseTest.linkage(limb).joint(i).max() - parseTest.linkage(limb).joint(i).min())
+ parseTest.linkage(limb).joint(i).min();
// cout << "vals: " << randomVal;
randomVal = rand();
// cout << "\t:\t" << randomVal << endl;
/////////////////////// TOTALLY RANDOM TEST ///////////////////////////
if(totallyRandom)
jointValues(i) = ((double)(randomVal%resolution))/((double)resolution-1)
*(parseTest.linkage(limb).joint(i).max() - parseTest.linkage(limb).joint(i).min())
+ parseTest.linkage(limb).joint(i).min();
////////////////////// NOT COMPLETELY RANDOM TEST //////////////////////
else
// jointValues(i) = targetValues(i) +
// ((double)(2*rand()%resolution)/((double)resolution-1)-1)*scatterScale
// *(parseTest.linkage(limb).joint(i).max() - parseTest.linkage(limb).joint(i).min());
jointValues(i) = targetValues(i) +
((double)(2*rand()%resolution)/((double)resolution-1)-1)*scatterScale;
parseTest.linkage(limb).joint(i).value(targetValues(i));
}
target = parseTest.linkage(limb).tool().respectToRobot();
parseTest.linkage(limb).values(jointValues);
// cout << "Start:" << endl;
// cout << parseTest.linkage(limb).tool().respectToRobot().matrix() << endl;
// cout << "Target:" << endl;
// cout << target.matrix() << endl;
storedVals = jointValues;
rk_result_t result = parseTest.dampedLeastSquaresIK_linkage(limb, jointValues, target, constraints);
if( result == RK_SOLVED )
wins++;
// if(false)
if(!totallyRandom && (result != RK_SOLVED || (storedVals-jointValues).norm() > 1.1*(storedVals-targetValues).norm()) )
{
if( result == RK_SOLVED )
cout << "SOLVED" << endl;
else
{
Vector3d Terr = target.translation() - parseTest.linkage(limb).tool().respectToRobot().translation();
AngleAxisd aaerr;
aaerr = target.rotation()*parseTest.linkage(limb).tool().respectToRobot().rotation().transpose();
Vector3d Rerr;
if(fabs(aaerr.angle()) <= M_PI)
Rerr = aaerr.angle()*aaerr.axis();
else
Rerr = (aaerr.angle()-2*M_PI)*aaerr.axis();
cout << "FAILED (" << Terr.norm() << ", " << Rerr.norm() << ")" << endl;
}
cout << "Start: " << storedVals.transpose() << endl;
cout << "Solve: " << jointValues.transpose() << endl;
cout << "Goal : " << targetValues.transpose() << endl;
cout << "Delta: " << (storedVals-targetValues).transpose() << "\t(" << (storedVals-targetValues).norm() << ")" << endl;
cout << "Diff : " << (storedVals-jointValues).transpose() << "\t(" << (storedVals-jointValues).norm() << ")" << endl << endl;
if( result == RK_SOLVED )
jumps++;
}
// cout << "End:" << endl;
// cout << parseTest.linkage(limb).tool().respectToRobot().matrix() << endl;
// cout << "Target Joint Values: " << targetValues.transpose() << endl;
// if(result != RK_SOLVED)
if(false)
{
cout << "Start values: ";
for(int p=0; p<storedVals.size(); p++)
cout << storedVals[p] << "\t\t";
cout << endl;
cout << "Target values: ";
for(int p=0; p<targetValues.size(); p++)
cout << targetValues[p] << "\t\t";
cout << endl;
cout << "Final Joint Values: ";
for(int p=0; p<jointValues.size(); p++)
cout << jointValues[p] << "\t\t";
cout << endl;
cout << "Norm: " << ( jointValues - targetValues ).norm() << endl;
cout << "Error: " << (parseTest.linkage(limb).tool().respectToRobot().translation()
- target.translation()).norm() << endl;
}
}
clock_t endTime;
endTime = clock();
cout << "Time: " << (endTime - time)/((double)CLOCKS_PER_SEC*tests) << " : " <<
(double)CLOCKS_PER_SEC*tests/(endTime-time) << endl;
cout << "Win: " << ((double)wins)/((double)tests)*100 << "%" << endl;
if(!totallyRandom)
cout << "Jump: " << ((double)jumps)/((double)tests)*100 << "%" << endl;
}
float Vector3d::DotProduct(const Vector3d &v1, const Vector3d &v2) {
return v1.DotProduct(v2);
}
int main(int argc, const char* argv[]){
if (argc<2)
{
cout << endl;
cout << "Please type: " << endl;
cout << "ba_demo [PIXEL_NOISE] [OUTLIER RATIO] [ROBUST_KERNEL] [STRUCTURE_ONLY] [DENSE]" << endl;
cout << endl;
cout << "PIXEL_NOISE: noise in image space (E.g.: 1)" << endl;
cout << "OUTLIER_RATIO: probability of spuroius observation (default: 0.0)" << endl;
cout << "ROBUST_KERNEL: use robust kernel (0 or 1; default: 0==false)" << endl;
cout << "STRUCTURE_ONLY: performe structure-only BA to get better point initializations (0 or 1; default: 0==false)" << endl;
cout << "DENSE: Use dense solver (0 or 1; default: 0==false)" << endl;
cout << endl;
cout << "Note, if OUTLIER_RATIO is above 0, ROBUST_KERNEL should be set to 1==true." << endl;
cout << endl;
exit(0);
}
double PIXEL_NOISE = atof(argv[1]);
double OUTLIER_RATIO = 0.0;
if (argc>2) {
OUTLIER_RATIO = atof(argv[2]);
}
bool ROBUST_KERNEL = false;
if (argc>3){
ROBUST_KERNEL = atoi(argv[3]) != 0;
}
bool STRUCTURE_ONLY = false;
if (argc>4){
STRUCTURE_ONLY = atoi(argv[4]) != 0;
}
bool DENSE = false;
if (argc>5){
DENSE = atoi(argv[5]) != 0;
}
cout << "PIXEL_NOISE: " << PIXEL_NOISE << endl;
cout << "OUTLIER_RATIO: " << OUTLIER_RATIO<< endl;
cout << "ROBUST_KERNEL: " << ROBUST_KERNEL << endl;
cout << "STRUCTURE_ONLY: " << STRUCTURE_ONLY<< endl;
cout << "DENSE: "<< DENSE << endl;
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(false);
g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
if (DENSE) {
linearSolver= new g2o::LinearSolverDense<g2o
::BlockSolver_6_3::PoseMatrixType>();
} else {
linearSolver
= new g2o::LinearSolverCholmod<g2o
::BlockSolver_6_3::PoseMatrixType>();
}
g2o::BlockSolver_6_3 * solver_ptr
= new g2o::BlockSolver_6_3(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
vector<Vector3d> true_points;
for (size_t i=0;i<500; ++i)
{
true_points.push_back(Vector3d((Sample::uniform()-0.5)*3,
Sample::uniform()-0.5,
Sample::uniform()+3));
}
double focal_length= 1000.;
Vector2d principal_point(320., 240.);
vector<g2o::SE3Quat,
aligned_allocator<g2o::SE3Quat> > true_poses;
g2o::CameraParameters * cam_params
= new g2o::CameraParameters (focal_length, principal_point, 0.);
cam_params->setId(0);
if (!optimizer.addParameter(cam_params)) {
assert(false);
}
int vertex_id = 0;
for (size_t i=0; i<15; ++i) {
Vector3d trans(i*0.04-1.,0,0);
Eigen:: Quaterniond q;
q.setIdentity();
g2o::SE3Quat pose(q,trans);
g2o::VertexSE3Expmap * v_se3
= new g2o::VertexSE3Expmap();
v_se3->setId(vertex_id);
if (i<2){
v_se3->setFixed(true);
}
v_se3->setEstimate(pose);
optimizer.addVertex(v_se3);
true_poses.push_back(pose);
vertex_id++;
}
int point_id=vertex_id;
int point_num = 0;
double sum_diff2 = 0;
cout << endl;
tr1::unordered_map<int,int> pointid_2_trueid;
tr1::unordered_set<int> inliers;
for (size_t i=0; i<true_points.size(); ++i){
g2o::VertexSBAPointXYZ * v_p
= new g2o::VertexSBAPointXYZ();
v_p->setId(point_id);
v_p->setMarginalized(true);
v_p->setEstimate(true_points.at(i)
+ Vector3d(Sample::gaussian(1),
Sample::gaussian(1),
Sample::gaussian(1)));
int num_obs = 0;
for (size_t j=0; j<true_poses.size(); ++j){
Vector2d z = cam_params->cam_map(true_poses.at(j).map(true_points.at(i)));
if (z[0]>=0 && z[1]>=0 && z[0]<640 && z[1]<480){
++num_obs;
}
}
if (num_obs>=2){
optimizer.addVertex(v_p);
bool inlier = true;
for (size_t j=0; j<true_poses.size(); ++j){
Vector2d z
= cam_params->cam_map(true_poses.at(j).map(true_points.at(i)));
if (z[0]>=0 && z[1]>=0 && z[0]<640 && z[1]<480){
double sam = Sample::uniform();
if (sam<OUTLIER_RATIO){
z = Vector2d(Sample::uniform(0,640),
Sample::uniform(0,480));
inlier= false;
}
z += Vector2d(Sample::gaussian(PIXEL_NOISE),
Sample::gaussian(PIXEL_NOISE));
g2o::EdgeProjectXYZ2UV * e
= new g2o::EdgeProjectXYZ2UV();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_p));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>
(optimizer.vertices().find(j)->second));
e->setMeasurement(z);
e->information() = Matrix2d::Identity();
if (ROBUST_KERNEL) {
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
}
e->setParameterId(0, 0);
optimizer.addEdge(e);
}
}
if (inlier){
inliers.insert(point_id);
Vector3d diff = v_p->estimate() - true_points[i];
sum_diff2 += diff.dot(diff);
}
pointid_2_trueid.insert(make_pair(point_id,i));
++point_id;
++point_num;
}
}
cout << endl;
optimizer.initializeOptimization();
optimizer.setVerbose(true);
if (STRUCTURE_ONLY){
g2o::StructureOnlySolver<3> structure_only_ba;
cout << "Performing structure-only BA:" << endl;
g2o::OptimizableGraph::VertexContainer points;
for (g2o::OptimizableGraph::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) {
g2o::OptimizableGraph::Vertex* v = static_cast<g2o::OptimizableGraph::Vertex*>(it->second);
if (v->dimension() == 3)
points.push_back(v);
}
structure_only_ba.calc(points, 10);
}
cout << endl;
cout << "Performing full BA:" << endl;
optimizer.optimize(10);
cout << endl;
cout << "Point error before optimisation (inliers only): " << sqrt(sum_diff2/point_num) << endl;
point_num = 0;
sum_diff2 = 0;
for (tr1::unordered_map<int,int>::iterator it=pointid_2_trueid.begin();
it!=pointid_2_trueid.end(); ++it){
g2o::HyperGraph::VertexIDMap::iterator v_it
= optimizer.vertices().find(it->first);
if (v_it==optimizer.vertices().end()){
cerr << "Vertex " << it->first << " not in graph!" << endl;
exit(-1);
}
g2o::VertexSBAPointXYZ * v_p
= dynamic_cast< g2o::VertexSBAPointXYZ * > (v_it->second);
if (v_p==0){
cerr << "Vertex " << it->first << "is not a PointXYZ!" << endl;
exit(-1);
}
Vector3d diff = v_p->estimate()-true_points[it->second];
if (inliers.find(it->first)==inliers.end())
continue;
sum_diff2 += diff.dot(diff);
++point_num;
}
cout << "Point error after optimisation (inliers only): " << sqrt(sum_diff2/point_num) << endl;
cout << endl;
}
void MyController::onInit(InitEvent &evt) {
// handle of target and tool
SimObj *target = getObj("box_001");
SimObj *toolName = getObj("TShapeTool_001");
toolName->setMass(10.0); // mass of all the tools should be set uniformly
target->getPosition(initTargetPos); // initial target position
target->getRotation(initTargetRotation); // initial target rotation in quaternion
toolName->getPosition(initToolPos); // initial tool position
toolName->getRotation(initToolRotation); // initial tool rotation in quaternion
isTargetAtRest = true; // target and tool both are at rest initially.
isToolAtRest = true;
insideTimer = true;
f_x = 0 ;
f_z = 0 ;
int xForceVariance = 4000;
int zForceVariance = 4000;
flag = true;
Colli = false;
counterOfCollision = 0;
counterOfAction = 0;
// Reset the forces applied
double r;
r = ((double) rand() / (RAND_MAX)) ;
f_x = r * int(xForceVariance);
f_z = r * int(zForceVariance);
forceOnTool_z = 8000 + f_z;
forceOnTool_x = 0;
forceOnTool.set(forceOnTool_x, 0 , forceOnTool_z);
toolName->addForce(forceOnTool.x(), forceOnTool.y(), forceOnTool.z());
toolName->getVelocity(appliedToolVel);
myfile.flush(); // I have uncommented the file, because I want to overwrite the file.
// if (myfile.is_open())
// {
// myfile << "toolVelAtHit_X" << " , " << "toolVelAtHit_Z" << " , ";
// myfile << "targetInitialVel_X" << " , " << "targetInitialVel_Z" << " , ";
// myfile << "Action" << " , " << "FunctionalFeature" << " , " ;
// myfile << "forceOnTool_X" << " , " << "forceOnTool_Z " << " , ";
// myfile << "InitialToolPos_X" << " , " << "InitialToolPos_Z" << " , ";
// myfile << "InitialTargetPos_X" << " , " << "InitialTargetPos_Z" << " , ";
// myfile << "targetFinalPos_X" << " , " << "targetFinalPos_Z" << " , ";
// myfile << "targetDisplacement_X" << " , " << "targetDisplacement_Z";
// myfile << "\n" ;
// }
}
