Tutorial 8: Setting Collision Detection Function

This part covers the following.

1. Perform preliminary work to use the collision detection function of RobotMotion.

2. Enable the collision detection function of RobotMotion.

3. Manage the parameters related to the collision detection function of RobotMotion as an xml file.

In this part, the simulation is limited, and the actual robot must be operated to proceed with the tutorial. Therefore, please be sure to pay attention to safety.

To proceed with this tutorial, the URDF of the target robot is required, and it must include inertial information. In addition, if a Tool is mounted, the mass and center of gravity position information of the Tool is essential, and if you have an Inertia Matrix, you can proceed more precisely.

1. Sample Code

Robot inertial information input and output

int SetToolInertial(wmx3Api::RobotMotion& robot, wmx3Api::RobotMotionParam& robotMotionParam, double mass, const double(&LinkOrigin)[3])
{
    int ret = 0;
    wmx3Api::coordinate::Point3d origin;
    origin.x = LinkOrigin[0];
    origin.y = LinkOrigin[1];
    origin.z = LinkOrigin[2];

    ret = robot.mKinematics.SetToolLinkParam(robotMotionParam.robotParam.robotId, mass, origin);
    if (ret != 0) { printf("SetToolLinkParam Failed, errorCode[%d]\n", ret); return ret; }

    return ret;
}

int SetToolInertial(wmx3Api::RobotMotion& robot, wmx3Api::RobotMotionParam& robotMotionParam, double mass, const double(&LinkOrigin)[3], const double(&LinkInertia)[6])
{
    wmx3Api::kinematics::LinkParam Tool1;
    wcscpy_s(Tool1.linkName, sizeof(Tool1.linkName) / sizeof(wchar_t), _T("ToolLink"));

    Tool1.linkInertia.mass = mass;

    Tool1.linkOrigin.x = LinkOrigin[0];
    Tool1.linkOrigin.y = LinkOrigin[1];
    Tool1.linkOrigin.z = LinkOrigin[2];

    Tool1.linkInertia.ixx = LinkOrigin[0];
    Tool1.linkInertia.ixy = LinkOrigin[1];
    Tool1.linkInertia.ixz = LinkOrigin[2];
    Tool1.linkInertia.iyy = LinkOrigin[3];
    Tool1.linkInertia.iyz = LinkOrigin[4];
    Tool1.linkInertia.izz = LinkOrigin[5];

    int ret = 0;
    ret = robot.mKinematics.SetToolLinkParam(robotMotionParam.robotParam.robotId, Tool1);
    if (ret != 0) { printf("SetToolLinkParam Failed, errorCode[%d]\n", ret); return ret; }

    return ret;
}

• The above code shows two ways to input the Inertial information of the Tool installed at the end of the Robot through the WMX Robot API, and the information required for each method is as follows.

  • Mass, position of center of gravity

  • Mass, position of center of gravity, inertia matrix, link name

• If the inertia matrix is not entered, it is calculated assuming that the Tool is a point mass.

• Without using the WMX Robot API, you can manage the Inertial of the Tool along with the Robot information in URDF and XML.

• After inputting the Inertial of the Tool through the WMX Robot API, the information of the Tool is saved together when exporting to XML, URDF.

Enable torque estimation fitting function

int SetRobotFittingOn(wmx3Api::RobotMotion& robot, wmx3Api::RobotMotionParam& robotMotionParam)
{
    int ret = robot.mKinematics.SetFittingOn(robotMotionParam.robotParam.robotId, true);
    if (ret != 0) { printf("SetFittingOn Failed, errorCode[%d]\n", ret); return ret; }

    return ret;
}

• The above code is a code that activates the fitting work for torque estimation in the robot’s joint.

• If the variable enable is true, the fitting work for torque estimation is activated.

• If the variable enable is false, the fitting work for torque estimation is deactivated.

Calculate torque estimation fitting data

int CalcRobotFittingData(wmx3Api::RobotMotion& robot, wmx3Api::RobotMotionParam& robotMotionParam)
{
    int ret = robot.mKinematics.CalcFittingData(robotMotionParam.robotParam.robotId);
    if (ret != 0) { printf("CalcFittingData Failed, errorCode[%d]\n", ret); return ret; }

    return ret;
}

• The above code is a code that calculates the fitting data for torque estimation in the robot’s joint.

  • The CalcFittingData api should only be executed after activating the torque estimation fitting function.

  • If the CalcFittingData api is executed normally, the torque estimation fitting function is automatically deactivated.

Enable collision detection function

int SetCollisionOn(wmx3Api::RobotMotion& robot, wmx3Api::RobotMotionParam& robotMotionParam)
{
    robotMotionParam.collisionParam.enable = true;
    robotMotionParam.collisionParam.thresHold = 6;

    int ret = robot.mKinematics.SetCollisionParam(robotMotionParam.robotParam.robotId, robotMotionParam.collisionParam);
    if (ret != 0) { printf("SetCollisionParam Failed, errorCode[%d]\n", ret); return ret; }

    return ret;
}

• The above code is a code that performs Fitting of robot characteristics to WMX to use collision detection and a code that activates the collision detection function using the Fitting result.

  • Perform arbitrary motion between the SetRobotFitting function and the CalcRobotFittingData function to fit the parameter changes during motion.

  • If the variable enable is true, the collision detection function is activated.

  • If the variable enable is false, the collision detection function is deactivated.

Initialize and deactivate collision detection parameters

int ClearCollisionParam(wmx3Api::RobotMotion& robot, wmx3Api::RobotMotionParam& robotMotionParam)
{
    int ret = 0;

    ret = robot.mKinematics.ClearFittingData(robotMotionParam.robotParam.robotId);
    if (ret != 0) { printf("CalcFittingData Failed, errorCode[%d]\n", ret); return ret; }

    wmx3Api::kinematics::CollisionParam collisionParam;
    collisionParam.enable = false;

    ret = robot.mKinematics.SetCollisionParam(robotMotionParam.robotParam.robotId, collisionParam);
    if (ret != 0) { printf("SetCollisionParam Failed, errorCode[%d]\n", ret); return ret; }
}

• The above code represents the work of initializing the parameters required for collision detection and deactivating the collision detection function.

Import and export collision detection parameters

int ExportCollisionParam(wmx3Api::RobotMotion& robot, wmx3Api::RobotMotionParam& robotMotionParam) {

    TCHAR robotParameterXMLFile[] = _T("robotParamXML_CR12A_export.xml");

    int ret = 0;

    for (int i = 0; i < 6; ++i) {
        robot.mKinematics.GetFittingData(robotMotionParam.robotParam.robotId, i, robotMotionParam.fittingData[i]);
    }
    ret = robot.mKinematics.GetCollisionParam(robotMotionParam.robotParam.robotId, robotMotionParam.collisionParam);


    ret = robot.mKinematics.GetRobotParam(robotMotionParam.robotParam.robotId, robotMotionParam.robotParam);
    if (ret != 0) { printf("GetRobotParam Failed, errorCode[%d]\n", ret); return ret; }

    ret = robot.mRobotConfig.ExportParamXML(robotParameterXMLFile, robotMotionParam);
    if (ret != 0) { printf("ExportParamXML Failed, errorCode[%d]\n", ret); return ret; }
}

int ImportCollisionParam(wmx3Api::RobotMotion& robot, wmx3Api::RobotMotionParam& robotMotionParam)
{
    int ret = 0;
    TCHAR wmxParameterFile[] = _T("wmx_parameter_CR12A.xml");
    TCHAR robotParameterXMLFile[] = _T("robotParamXML_CR12A_export.xml");

    ret = robot.mRobotConfig.ImportParamXML(robotParameterXMLFile, robotMotionParam);
    if (ret != 0) { printf("ImportParamXML Failed, errorCode[%d]\n", ret); return ret; }

    ret = robot.mKinematics.SetRobotParam(robotMotionParam.robotParam);
    if (ret != 0) { printf("SetRobotParam Failed, errorCode[%d]\n", ret); return ret; }


    for (int i = 0; i < 6; ++i) {
        robot.mKinematics.SetFittingData(robotMotionParam.robotParam.robotId, i, robotMotionParam.fittingData[i]);
    }

    robotMotionParam.collisionParam.enable = true;
    ret = robot.mKinematics.SetCollisionParam(robotMotionParam.robotParam.robotId, robotMotionParam.collisionParam);
    if (ret != 0) { printf("SetCollisionParam Failed, errorCode[%d]\n", ret); return ret; }

    return ret;
}

• The above code represents the work of saving and loading the parameters used for collision detection.

• When exporting to an XML file, the collision detection parameters are also saved, but when exporting to a URDF file, the corresponding parameters are not saved, so if the parameters are lost, the Fitting work is required again.

• After inputting the Inertial of the Tool through the WMX Robot API, the information of the Tool is saved together when exporting to XML, URDF.

main

int InitWMX(wmx3Api::WMX3Api& dev)
{
    int ret = 0;
    ret = dev.CreateDevice("C:\\Program Files\\SoftServo\\WMX3\\", wmx3Api::DeviceType::DeviceTypeNormal, INFINITE);
    if (ret != 0) { printf("CreateDevice Failed", ret);  return ret; }

    ret = dev.SetDeviceName("Robot");
    if (ret != 0) { printf("SetDeviceName Failed, errorCode[%d]\n", ret);  return ret; }

    ret = dev.SetWatchdog(60 * 60 * 1000);
    if (ret != 0) { printf("SetWatchdog Failed, errorCode[%d]\n", ret);  return ret; }

    ret = dev.StartCommunication(INFINITE);
    if (ret != 0) { printf("StartCommunication Failed, errorCode[%d]\n", ret);  return ret; }

    return ret;
}

int InitRobotSystem(wmx3Api::RobotMotion& robot, wmx3Api::RobotMotionParam& robotMotionParam)
{
    int ret = 0;
    TCHAR wmxParameterFile[] = _T("wmx_parameter_CR12A.xml");
    TCHAR robotParameterURDFFile[] = _T("robotParamURDF_CR12A.urdf");

    ret = robot.IsDeviceValid();
    if (ret == 0) { printf("IsDeviceValid Failed, errorCode[%d]\n", ret);  return ret; }


    ret = robot.mCoreMotion.config->ImportAndSetAll(wmxParameterFile);
    if (ret != 0) { printf("ImportAndSetAll Failed, errorCode[%d]\n", ret); return ret; }


    ret = robot.mRobotConfig.ImportParamURDF(robotParameterURDFFile, 0, robotMotionParam);
    if (ret != 0) { printf("ImportParamURDF Failed, errorCode[%d]\n", ret); return ret; }



    for (int index = 0; index < robotMotionParam.robotParam.numJoints; index++) {
        robotMotionParam.robotParam.jointParams[index].axis = index;
        robotMotionParam.robotParam.jointParams[index].velocityLimit = 180.0;
        robotMotionParam.robotParam.jointParams[index].accelerationLimit = 720.0;
    }
    robotMotionParam.robotParam.robotType = wmx3Api::kinematics::constants::Articulate6Axes;



    ret = robot.mKinematics.SetRobotParam(robotMotionParam.robotParam);
    if (ret != 0) { printf("SetRobotParam Failed, errorCode[%d]\n", ret); return ret; }


    return ret;
}

int InitRobotPose(wmx3Api::RobotMotion& robot, wmx3Api::RobotMotionParam& robotMotionParam)
{
    int ret = 0;
    const double initialJointDegree[] = { 0, 0, 90, 0, -90, 0 };
    double JointVelocity = 10;
    double JointAcceleration = 20;

    wmx3Api::RobotStatus robotStatus;

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

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

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


    for (int index = 0; index < robotMotionParam.robotParam.numJoints; index++) {
        robotMotionParam.profile.jointVelocity[index] = JointVelocity;
        robotMotionParam.profile.jointAcceleration[index] = JointAcceleration;
    }

    robotMotionParam.profile.profileType = wmx3Api::ProfileType::T::Trapezoidal;
    robotMotionParam.profile.velOverride = 1.0;
    robotMotionParam.profile.accOverride = 1.0;

    wmx3Api::PtpMotionParam::PtpPosParam ptpParam_init = wmx3Api::PtpMotionParam::PtpPosParam(robotMotionParam.robotParam, robotMotionParam.profile, initialJointDegree);


    // Start PTP motion
    ret = robot.mKinematics.StartPTPPos(ptpParam_init);
    if (ret != 0) { printf("StartPTPPos Failed, errorCode[%d]\n", ret); return ret; }

    // Wait until motion finished
    ret = robot.mKinematics.Wait(robotMotionParam.robotParam.robotId);
    if (ret != 0) { printf("Wait Failed, errorCode[%d]\n", ret); return ret; }

    // Update Status
    ret = robot.UpdateRobotStatus(robotMotionParam.robotParam.robotId, robotStatus);
    if (ret != 0) { printf("UpdateRobotStatus Failed, errorCode[%d]\n", ret); return ret; }


    return ret;
}

int SamplePtpMotionPosJoint_Repeat2Time(wmx3Api::RobotMotion& robot, wmx3Api::RobotMotionParam& robotMotionParam)
{
    int ret = 0;

    const double initAngle[] = { 0, 0, 90, 0, -90, 0 };
    const double TargetAngle[] = { -40, 20, 100, -20, -70, 40 };


    double JointVelocity = 30;
    double JointAcceleration = 60;


    for (int index = 0; index < robotMotionParam.robotParam.numJoints; index++) {
        robotMotionParam.profile.jointVelocity[index] = JointVelocity;
        robotMotionParam.profile.jointAcceleration[index] = JointAcceleration;
    }

    robotMotionParam.profile.profileType = wmx3Api::ProfileType::T::Trapezoidal;
    robotMotionParam.profile.velOverride = 1.0;
    robotMotionParam.profile.accOverride = 1.0;

    wmx3Api::PtpMotionParam::PtpPosParam ptpParam_init = wmx3Api::PtpMotionParam::PtpPosParam(robotMotionParam.robotParam, robotMotionParam.profile, initAngle);
    wmx3Api::PtpMotionParam::PtpPosParam ptpParam_target = wmx3Api::PtpMotionParam::PtpPosParam(robotMotionParam.robotParam, robotMotionParam.profile, TargetAngle);


    for (int i = 0; i < 2; ++i) {

        ret = robot.mKinematics.StartPTPPos(ptpParam_target);
        if (ret != 0) { printf("StartPTPPos Failed, errorCode[%d]\n", ret); return ret; }

        ret = robot.mKinematics.Wait(robotMotionParam.robotParam.robotId);
        if (ret != 0) { printf("Wait Failed, errorCode[%d]\n", ret); return ret; }

        Sleep(100);


        ret = robot.mKinematics.StartPTPPos(ptpParam_init);
        if (ret != 0) { printf("StartPTPPos Failed, errorCode[%d]\n", ret); return ret; }

        ret = robot.mKinematics.Wait(robotMotionParam.robotParam.robotId);
        if (ret != 0) { printf("Wait Failed, errorCode[%d]\n", ret); return ret; }

        Sleep(100);

    }

    return ret;
}

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

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

    // Be sure to enter the URDF of the robot you are actually testing.
    // Be sure to enter the URDF that includes Inertial information. 
    ret = InitRobotSystem(robot, robotMotionParam);
    if (ret != 0) { return ret; }


    // Select one of the two methods and enter the actual Tool information.
    const double LinkOrigin[] = { 0.1, 0.1, 0.1 };
    const double LinkInertia[] = { 0.01, 0, 0, 0.01, 0, 0.01 };
    const double Mass = 1;
    ret = SetToolInertial(robot, robotMotionParam, Mass, LinkOrigin);
    ret = SetToolInertial(robot, robotMotionParam, Mass, LinkOrigin, LinkInertia);
    if (ret != 0) { return ret; }

    ret = InitRobotPose(robot, robotMotionParam);
    if (ret != 0) { return ret; }

    ret = SetRobotFittingOn(robot, robotMotionParam);
    if (ret != 0) { return ret; }

    // Collision detection not possible
    ret = SamplePtpMotionPosJoint_Repeat2Time(robot, robotMotionParam);
    if (ret != 0) { return ret; }

    ret = CalcRobotFittingData(robot, robotMotionParam);
    if (ret != 0) { return ret; }

    ret = SetCollisionOn(robot, robotMotionParam);
    if (ret != 0) { return ret; }

    // Collision detection possible
    ret = SamplePtpMotionPosJoint_Repeat2Time(robot, robotMotionParam);
    if (ret != 0) { return ret; }

    ret = ExportCollisionParam(robot, robotMotionParam);
    if (ret != 0) { return ret; }

    ret = ClearCollisionParam(robot, robotMotionParam);
    if (ret != 0) { return ret; }

    ret = ImportCollisionParam(robot, robotMotionParam);
    if (ret != 0) { return ret; }

    // Collision detection possible
    ret = SamplePtpMotionPosJoint_Repeat2Time(robot, robotMotionParam);
    if (ret != 0) { return ret; }

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

    return 0;
}

• Call the collision detection function in order using the previously declared/defined function.

• robotParamURDF_CR12A.urdf is the robot’s settings file. The settings file (urdf) is parsed into RobotMotionParam and stored in the third argument of the ImportParamURDF function.

• After importing with URDF and exporting with XML, you can easily input Inertial information into XML, and if you manage robot parameters with an XML file, you can also manage Collision information together, which is more convenient.

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

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

2. Program Execution Result

• The collision detection function can be used in the motion that operates with the SamplePtpMotionPosJoint_Repeat2Time function marked with // Collision detection possible.

• The robotParamXML_CR12A_export.xml file with the collision detection parameters is created.