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