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