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;
}
}
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;
Rt.SetValid( true );
theta += 0.1;
}
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 Run(){
ProcessQueuedCommands();
Rt.Position().Translation()[2] += 0.001;
}
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 );
}
}
}
}
void mtsROSToCISST(const geometry_msgs::Pose & rosData, prmPositionCartesianGet & cisstData)
{
mtsROSPoseToCISST(rosData, cisstData.Position());
}
void mtsROSToCISST(const geometry_msgs::Transform & rosData, prmPositionCartesianGet & cisstData)
{
mtsROSTransformToCISST(rosData, cisstData.Position());
}
