Data Sheet

57
else if(temp == 'r')
{
DesiredPose = CurrentPose;
matd DesiredRotation = Algebra::GetOrientationMatrix(delta_angle_rad, 0.0, 0.0) *
Algebra::GetOrientationMatrix(CurrentPose.Roll, CurrentPose.Pitch, CurrentPose.Yaw);
vecd DesiredRPY = Algebra::GetEulerRollPitchYaw(DesiredRotation);
DesiredPose.Roll = DesiredRPY(0);
DesiredPose.Pitch = DesiredRPY(1);
DesiredPose.Yaw = DesiredRPY(2);
ArmKinematics.ComputeIK(DesiredPose, &angle_rad, angle_rad, &ErrorStatus);
if(ErrorStatus == ARMSDK_NO_ERROR)
{
cout<<"Answer"<<endl;
cout<<angle_rad<<endl<<endl;
ArmComm.Arm_Set_JointPosition(RobotisArm.Rad2Value(angle_rad +
gvdAngleGapCalcandDynamixelRad));
}
else if(ErrorStatus & ARMSDK_ACCEPTABLE_ERROR)
{
cout<< "No IK solution"<<endl;
cout<< "But the caluation result is acceptable"<<endl;
char answer;
while(true)
{
cout<< "Do you want make the Robot move? (Y/N)"
cin >> answer;
if((answer == 'y') || (answer == 'n') || (answer == 'Y') || (answer == 'N'))
break;
else
cout<< "Invaild Answer"<<endl;
}
If((answer == 'y') || (answer == 'Y') )
ArmComm.Arm_Set_JointPosition(RobotisArm.Rad2Value(angle_rad +
gvdAngleGapCalcandDynamixelRad));
else
continue;
}
else {
cout<< "No IK Solution"<<endl;
continue;
}
ArmKinematics.Forward(angle_rad, &CurrentPose);
}