Tutorial 5: Initializing Robot Pose

This part covers the following.

1. Initializing the posture of the control target using the WMX3 Robot module.

1. Sample Code

InitPose

InitRobotPose is sample code for initializing the posture of the control target (6-axis robot) using the WMX3 Robot class.

int InitRobotPose(wmx3Api::RobotMotion& robot, wmx3Api::RobotMotionParam& robotMotionParam, const double(&initialJointDegree)[6])
{
    int ret = 0;
    wmx3Api::RobotStatus robotStatus;

    robot.mCoreMotion.ReleaseEStop();
    Sleep(DELAY_MILLISECOND);

    // Set servo on
    wmx3Api::AxisSelection axis = wmx3Api::RobotConfig::GetAxisSelection(robotMotionParam.robotParam);
    robot.mCoreMotion.axisControl->SetServoOn(&axis, 1);
    Sleep(DELAY_MILLISECOND);

    // Clear motion error(if the case)
    ret = robot.UpdateRobotStatus(robotMotionParam.robotParam.robotId, robotStatus);
    if (ret != 0) {
        ret = robot.mKinematics.ClearMotionError(robotMotionParam.robotParam.robotId);
        Sleep(1000);
    }

    // Init robot joint position(degree)
    wmx3Api::PtpMotionParam::PtpPosParam ptpParam(robotMotionParam.robotParam,
        robotMotionParam.profile,
        initialJointDegree);

    // Start PTP motion
    ret = robot.mKinematics.StartPTPPos(ptpParam);

    // Wait until motion finished
    ret = robot.mKinematics.Wait(robotMotionParam.robotParam.robotId);

    // Update Status
    ret = robot.UpdateRobotStatus(robotMotionParam.robotParam.robotId, robotStatus);
    printf("End Init TestCase\n");

    return ret;
}

• The sample code above performs the following tasks.

  • If the Robot is in an emergency stop state, it is released.

  • Applies power to the Servo.

  • Updates the status of the Robot. If a motion error exists, it is cleared.

  • Creates a PtpParam (Point To Point Parameter) class. The robot’s parameters, profile information, and the target position of each Joint (initialJointDegree) are passed as arguments.

  • The StartPTPPos function performs PTP motion.

  • Waits until the motion is complete.

  • After the motion is finished, the UpdateRobotStatus function is called to acquire the current status of the robot.

• The argument initialJointDegree is the target position (degree) of each joint of the control target.

※Caution

• It is recommended to add error code handling in case a problem occurs during the sequence of the Robot module.

• For example, you can add the following code for the return value (ret) of the above sequence.

if (ret != 0) { return ret; }

main

int main(int argc, const char* argv[])
{
    int ret = 0;
    wmx3Api::WMX3Api dev;
    wmx3Api::RobotMotion robot(&dev);
    wmx3Api::RobotMotionParam robotMotionParam;
    wmx3Api::RobotStatus robotStatus;

    // Initialization
    ret = InitWMX(dev);
    ret = InitRobotSystem(robot, robotMotionParam);
    ret = InitRobotPose(robot, robotMotionParam, {0,0,90,0,0,0});

    printf("End of program.\n");

    return 0;
}

2. Program Execution Result

• As a result of executing the main function, each joint of the robot in the simulation moved to the specified initial position.