예제 #1
0
 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;
 }
예제 #2
0
/*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 );
  }
}
예제 #3
0
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;
		}
	}

}  
예제 #6
0
  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]++;
    
  }
예제 #7
0
double vectorAngle(const Vector3d &r1,const Vector3d &r2){
  return(acos(r1.dot(r2)/r1.norm()/r2.norm()));
}
예제 #8
0
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()));
}
예제 #9
0
bool ContainedBySphere::Inside(const Vector3d& point) const
{
    Vector3d originToCenter = center - point;
    DBL ocSquared = originToCenter.lengthSqr();
    return (ocSquared <= Sqr(radius));
}
예제 #10
0
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();
	}
}
예제 #11
0
// 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

}
예제 #12
0
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();
}
예제 #13
0
파일: octree.cpp 프로젝트: Clodo76/povray
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
}
예제 #14
0
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]);
}
예제 #15
0
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);
} 
예제 #16
0
파일: main.cpp 프로젝트: obiou/fiducials
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;
}
예제 #17
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());
}
예제 #18
0
  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();    
  }
예제 #19
0
파일: layer.cpp 프로젝트: keesj/repsnapper
// 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;
}
예제 #20
0
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;

}
예제 #21
0
파일: glThread.cpp 프로젝트: bo-wu/surgical
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;
            
	}
}
예제 #27
0
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;



}
예제 #28
0
파일: vector3d.cpp 프로젝트: ngzaharias/Vox
float Vector3d::DotProduct(const Vector3d &v1, const Vector3d &v2) {
	return v1.DotProduct(v2);
}
예제 #29
0
파일: ba_demo.cpp 프로젝트: Aerobota/c2tam
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" ;

// }


}