Пример #1
0
void ui3SlaveArm::GetCartesianPosition(prmPositionCartesianGet & position)
{
    vctFrm3 temporary;
    this->GetCartesianPositionFunction(position);
    this->Transformation.ApplyTo(position.Position(), temporary);
    position.Position().Assign(temporary);
    position.Position().Translation().Multiply(this->Scale);
}
void mtsIntuitiveResearchKitArm::SetBaseFrame(const prmPositionCartesianGet & newBaseFrame)
{
    if (newBaseFrame.Valid()) {
        this->BaseFrame.FromNormalized(newBaseFrame.Position());
        this->BaseFrameValid = true;
    } else {
        this->BaseFrameValid = false;
    }
}
  CameraMotion() : mtsTaskPeriodic( "CameraMotion", 0.01, true ){
    Rt.Position().Translation()[2] = 1;

    StateTable.AddData( Rt, "PositionOrientation" );

    // provide the camera position
    mtsInterfaceProvided* output = AddInterfaceProvided( "Output" );
    output->AddCommandReadState( StateTable, Rt, "GetPositionCartesian" );

  }
void mtsIntuitiveResearchKitConsole::SUJECMBaseFrameHandler(const prmPositionCartesianGet & baseFrameParam)
{
    // get position from ECM and convert to useful type
    prmPositionCartesianGet positionECMLocalParam;
    mGetPositionCartesianLocalFromECM(positionECMLocalParam);
    vctFrm3 positionECM = baseFrameParam.Position() * positionECMLocalParam.Position();

    // compute and send new base frame for all SUJs (SUJ will handle ECM differently)
    prmPositionCartesianGet baseFrameSUJParam;
    baseFrameSUJParam.Position().From(positionECM.Inverse());
    baseFrameSUJParam.SetValid(baseFrameParam.Valid() && positionECMLocalParam.Valid());
    baseFrameSUJParam.SetTimestamp(positionECMLocalParam.Timestamp());
    mECMBaseFrameEvent(baseFrameSUJParam);
}
  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;
    theta += 0.001;

  }
 void Run(){
   ProcessQueuedCommands();
   Rt.Position().Translation()[2] += 0.001;
 }
Пример #7
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 );
	}

      }

    }

  }
Пример #8
0
void mtsROSToCISST(const geometry_msgs::Pose & rosData, prmPositionCartesianGet & cisstData)
{
    mtsROSPoseToCISST(rosData, cisstData.Position());
}
Пример #9
0
void mtsROSToCISST(const geometry_msgs::Transform & rosData, prmPositionCartesianGet & cisstData)
{
    mtsROSTransformToCISST(rosData, cisstData.Position());
}