void Run(){
    ProcessQueuedCommands();

    // rotate hubble
    vctFixedSizeVector<double,3> u( 0.0, 0.0, 1.0 );
    vctAxisAngleRotation3<double> Rwh( u, theta );
    vctFrm3 Rtwh;
    Rtwh.Rotation().FromRaw( Rwh );
    Rtwh.Translation().Assign( vctFixedSizeVector<double,3>( 0.0, 0.0, 0.5 ) );

    Rt.Position() = Rtwh;
    Rt.SetValid( true );
    theta += 0.1;

  }
예제 #2
0
  void Run(){
    ProcessQueuedCommands();

    if( IsEnabled() ){

      if( !online ){

	online = true;

	for( size_t i=0; i<qready.size(); i++ ){

	  if( 0.015 < fabs( q[i] - qready[i] ) ){

	    online = false;

	    if( q[i] < qready[i] )
	      { q[i] += 0.0005; }
	    else if( qready[i] < q[i] )
	      { q[i] -= 0.0005; }

	  }

	}

	std::cout << q << std::endl;
	// this goes to the ikin
	mtsFrm4x4 mtsRt( manipulator->ForwardKinematics( q ) );
	SetPositionCartesian( mtsRt );

	// this goes to the teleop
	prmTelemetry.Position().FromRaw( mtsRt );
	prmTelemetry.SetValid( true );

	if( online )
	  { std::cout << "System is online. 'q' to quit" << std::endl; }

      }
      else{

	bool valid = false;
	prmCommand.GetValid( valid );

	if( valid ){

	  vctFrm3 frm3Rt;
	  prmCommand.GetPosition( frm3Rt );
	  
	  // this goes to the ikin
	  vctQuaternionRotation3<double> R( frm3Rt.Rotation(), VCT_NORMALIZE );
	  mtsFrm4x4 mtsRt( vctFrame4x4<double>( R, frm3Rt.Translation() ) );
	  SetPositionCartesian( mtsRt );
	  
	  // return the same value to the telop
	  prmTelemetry.Position().FromRaw( mtsRt );
	  prmTelemetry.SetValid( true );
	}

      }

    }

  }