Data Sheet

54
ⅱ) SimpleIK Source Description
if(temp == 'q') {
DesiredPose = CurrentPose;
DesiredPose.x += delta;
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 calcuation 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);
}