top of page
6.54. Obtain joint torque sensor sensitivity calibration results
/**
* @brief Get joint torque sensor sensitivity calibration results
* @param [out] calibResult j1~j6 joint sensitivity [0-1]
* @param [out] linearity j1~j6 joint linearity [0-1]
* @return Error code
*/
errno_t JointSensitivityCalibration(double calibResult[6], double linearity[6]);
6.55. Get Joint Torque Sensor Hysteresis Error
/**
* @brief Get joint torque sensor hysteresis error
* @param [out] hysteresisError j1~j6 joint hysteresis error
* @return Error code
*/
errno_t JointHysteresisError(double hysteresisError[6]);
6.56. Get Joint Torque Sensor Repeatability
/**
* @brief Get joint torque sensor repeatability
* @param [out] repeatability j1~j6 joint torque sensor repeatability
* @return Error code
*/
errno_t JointRepeatability(double repeatability[6]);
6.57. Set Joint Force Sensor Parameters
/**
* @brief Set joint force sensor parameters
* @param [in] M J1-J6 mass coefficient [0.001 ~ 10]
* @param [in] B J1-J6 damping coefficient [0.001 ~ 10]
* @param [in] K J1-J6 stiffness coefficient [0.001 ~ 10]
* @param [in] threshold Force control threshold, Nm
* @param [in] sensitivity Sensitivity, Nm/V, [0 ~ 10]
* @param [in] setZeroFlag Function enable flag; 0-Off; 1-On; 2-Record zero point at position 1; 3-Record zero point at position 2
* @return Error code
*/
errno_t SetAdmittanceParams(double M[6], double B[6], double K[6], double threshold[6], double sensitivity[6], int setZeroFlag);
6.58. An example of joint torque sensor sensitivity auto-calibration code
int TestSensitivityCalib()
{
ROBOT_STATE_PKG pkg = {};
FRRobot robot;
robot.LoggerInit();
robot.SetLoggerLevel(1);
robot.SetReConnectParam(true, 30000, 500);
int rtn = robot.RPC("192.168.58.2");
if (rtn != 0)
{
return 0;
}
rtn = robot.JointSensitivityEnable(0);
rtn = robot.JointSensitivityEnable(1);
printf("JointSensitivityEnable rtn is %d\n", rtn);
JointPos curJPos = {};
robot.GetActualJointPosDegree(0, &curJPos);
ExaxisPos epos = { 0,0,0,0 };
DescPose offset_pos = { 0,0,0,0,0,0 };
JointPos jointPos1 = { curJPos.jPos[0], 0, 0, -90, 0.02, curJPos.jPos[5] };
DescPose descPos1 = {};
robot.GetForwardKin(&jointPos1, &descPos1);
robot.MoveJ(&jointPos1, &descPos1, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
robot.Sleep(200);
rtn = robot.JointSensitivityCollect();
printf("JointSensitivityCollect 1 rtn is %d\n", rtn);
robot.Sleep(100);
JointPos jointPos2 = { curJPos.jPos[0], -30, 0, -90, 0.02, curJPos.jPos[5] };
DescPose descPos2 = {};
robot.GetForwardKin(&jointPos2, &descPos2);
robot.MoveJ(&jointPos2, &descPos2, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
robot.Sleep(100);
rtn = robot.JointSensitivityCollect();
printf("JointSensitivityCollect 2 rtn is %d\n", rtn);
robot.Sleep(100);
JointPos jointPos3 = { curJPos.jPos[0], -60, 0, -90, 0.02, curJPos.jPos[5] };
DescPose descPos3 = {};
robot.GetForwardKin(&jointPos3, &descPos3);
robot.MoveJ(&jointPos3, &descPos3, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
robot.Sleep(100);
rtn = robot.JointSensitivityCollect();
printf("JointSensitivityCollect 3 rtn is %d\n", rtn);
robot.Sleep(100);
JointPos jointPos4 = { curJPos.jPos[0], -90, 0, -90, 0.02, curJPos.jPos[5] };
DescPose descPos4 = {};
robot.GetForwardKin(&jointPos4, &descPos4);
robot.MoveJ(&jointPos4, &descPos4, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
robot.Sleep(100);
rtn = robot.JointSensitivityCollect();
printf("JointSensitivityCollect 4 rtn is %d\n", rtn);
robot.Sleep(100);
JointPos jointPos5 = { curJPos.jPos[0], -120, 0, -90, 0.02, curJPos.jPos[5] };
DescPose descPos5 = {};
robot.GetForwardKin(&jointPos5, &descPos5);
robot.MoveJ(&jointPos5, &descPos5, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
robot.Sleep(100);
rtn = robot.JointSensitivityCollect();
printf("JointSensitivityCollect 5 rtn is %d\n", rtn);
robot.Sleep(100);
JointPos jointPos6 = { curJPos.jPos[0], -150, 0, -90, 0.02, curJPos.jPos[5] };
DescPose descPos6 = {};
robot.GetForwardKin(&jointPos6, &descPos6);
robot.MoveJ(&jointPos6, &descPos6, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
robot.Sleep(100);
rtn = robot.JointSensitivityCollect();
printf("JointSensitivityCollect 6 rtn is %d\n", rtn);
robot.Sleep(100);
JointPos jointPos7 = { curJPos.jPos[0], -180, 0, -90, 0.02, curJPos.jPos[5] };
DescPose descPos7 = {};
robot.GetForwardKin(&jointPos7, &descPos7);
robot.MoveJ(&jointPos7, &descPos7, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
robot.Sleep(100);
rtn = robot.JointSensitivityCollect();
printf("JointSensitivityCollect 7 rtn is %d\n", rtn);
robot.Sleep(100);
//-------------------
robot.MoveJ(&jointPos6, &descPos6, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
robot.Sleep(100);
rtn = robot.JointSensitivityCollect();
printf("JointSensitivityCollect 8 rtn is %d\n", rtn);
robot.Sleep(100);
robot.MoveJ(&jointPos5, &descPos5, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
robot.Sleep(100);
rtn = robot.JointSensitivityCollect();
printf("JointSensitivityCollect 9 rtn is %d\n", rtn);
robot.Sleep(100);
robot.MoveJ(&jointPos4, &descPos4, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
robot.Sleep(100);
rtn = robot.JointSensitivityCollect();
printf("JointSensitivityCollect 10 rtn is %d\n", rtn);
robot.Sleep(100);
robot.MoveJ(&jointPos3, &descPos3, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
robot.Sleep(100);
rtn = robot.JointSensitivityCollect();
printf("JointSensitivityCollect 11 rtn is %d\n", rtn);
robot.Sleep(100);
robot.MoveJ(&jointPos2, &descPos2, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
robot.Sleep(100);
rtn = robot.JointSensitivityCollect();
printf("JointSensitivityCollect 12 rtn is %d\n", rtn);
robot.Sleep(100);
robot.MoveJ(&jointPos1, &descPos1, 0, 0, 100, 100, 100, &epos, -1, 0, &offset_pos);
robot.Sleep(200);
rtn = robot.JointSensitivityCollect();
printf("JointSensitivityCollect 13 rtn is %d\n", rtn);
robot.Sleep(100);
double calibResult[6] = { 0.0 };
double linearity[6] = { 0.0 };
rtn = robot.JointSensitivityCalibration(calibResult, linearity);
printf("JointSensitivityCalibration rtn is %d\n", rtn);
rtn = robot.JointSensitivityEnable(0);
printf("JointSensitivityEnable rtn is %d\n", rtn);
printf("jointSensor Calib result is %f %f %f %f %f %f\njointSensor linearity is %f %f %f %f %f %f\n",
calibResult[0], calibResult[1], calibResult[2],
calibResult[3], calibResult[4], calibResult[5],
linearity[0], linearity[1], linearity[2],
linearity[3], linearity[4], linearity[5]);
double hysteresisError[6] = { 0.0 };
rtn = robot.JointHysteresisError(hysteresisError);
printf("JointHysteresisError result is %f %f %f %f %f %f\n",
hysteresisError[0], hysteresisError[1], hysteresisError[2],
hysteresisError[3], hysteresisError[4], hysteresisError[5]);
double repeatability[6] = { 0.0 };
rtn = robot.JointRepeatability(repeatability);
printf("JointRepeatability result is %f %f %f %f %f %f\n",
repeatability[0], repeatability[1], repeatability[2],
repeatability[3], repeatability[4], repeatability[5]);
double M[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
double B[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
double K[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
double threshold[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
int setZeroFlag = 1;
rtn = robot.SetAdmittanceParams(M, B, K, threshold, calibResult, setZeroFlag);
printf("SetAdmittanceParams rtn is %d\n", rtn);
robot.CloseRPC();
}
6.59. Gets 8 slave error frames
/**
* @brief gets 8 slave error frames
* @param [ out ] in recverr
* @param [ out -RSB- inCRCErr enCRC CRC error frames
* @param [ out ] in forwarding error frames
* @param [ out ] inlinker R enter link error frames
* @param [ out ] out recverr output received error frames
* @param [ out ] outCRCErr output CRC error frames
* @param [ out ] outtransmitterr output forwarding error frames
* @param [ out ] outLinkErr output link error frames
* @return error code
*/
errno_t GetSlavePortErrCounter(int inRecvErr[8], int inCRCErr[8], int inTransmitErr[8], int inLinkErr[8], int outRecvErr[8], int outCRCErr[8], int outTransmitErr[8], int outLinkErr[8]);
6.60. Error frame zeroing at slave port
/**
* @brief Error frame zeroing at slave port
* @param [in] slaveID Slave station number 0 ~ 7
* @return error code
*/
errno_t SlavePortErrCounterClear(int slaveID);
6.61. Gets an example of a slave port error frame code
int TestSlavePortErr()
{
ROBOT_STATE_PKG pkg = {};
FRRobot robot;
robot.LoggerInit();
robot.SetLoggerLevel(1);
int rtn = robot.RPC("192.168.58.2");
if (rtn != 0)
{
return 0;
}
robot.SetReConnectParam(true, 30000, 500);
int inRecvErr[8] = {0.0};
int inCRCErr[8] = { 0.0 };
int inTransmitErr[8] = { 0.0 };
int inLinkErr[8] = { 0.0 };
int outRecvErr[8] = { 0.0 };
int outCRCErr[8] = { 0.0 };
int outTransmitErr[8] = { 0.0 };
int outLinkErr[8] = { 0.0 };
robot.GetSlavePortErrCounter(inRecvErr, inCRCErr, inTransmitErr, inLinkErr,
outRecvErr, outCRCErr, outTransmitErr, outLinkErr);
for (int i = 0; i < 8; i++)
{
if (inRecvErr[i] != 0)
{
printf("inRecvErr %d is %d\n", i, inRecvErr[i]);
}
if (inCRCErr[i] != 0)
{
printf("inRecvErr %d is %d\n", i, inCRCErr[i]);
}
if (inTransmitErr[i] != 0)
{
printf("inRecvErr %d is %d\n", i, inTransmitErr[i]);
}
if (inLinkErr[i] != 0)
{
printf("inRecvErr %d is %d\n", i, inLinkErr[i]);
}
if (outRecvErr[i] != 0)
{
printf("outRecvErr %d is %d\n", i, outRecvErr[i]);
}
if (outCRCErr[i] != 0)
{
printf("outCRCErr %d is %d\n", i, outCRCErr[i]);
}
if (outTransmitErr[i] != 0)
{
printf("outTransmitErr %d is %d\n", i, outTransmitErr[i]);
}
if (outLinkErr[i] != 0)
{
printf("outLinkErr %d is %d\n", i, outLinkErr[i]);
}
}
printf("others has no err!\n");
for (int i = 0; i < 8; i++)
{
robot.SlavePortErrCounterClear(i);
}
robot.CloseRPC();
return 0;
}
6.62. Set feed-forward coefficients for each axis
/**
* @brief Set feed-forward coefficients for each axis
* @param [in] radio Feed-forward coefficient of velocity of each axis
* @return error code
*/
errno_t SetVelFeedForwardRatio(double radio[6]);
6.63. Obtain the feed-forward coefficients of each axis
/**
* @brief Obtain the feed-forward coefficients of each axis
* @param [out] radio Feed-forward coefficient of velocity of each axis
* @return error code
*/
errno_t GetVelFeedForwardRatio(double radio[6]);
6.64. Example of robot velocity feed-forward coefficient code
int TestVelFeedForwardRatio()
{
ROBOT_STATE_PKG pkg = {};
FRRobot robot;
robot.LoggerInit();
robot.SetLoggerLevel(1);
int rtn = robot.RPC("192.168.58.2");
if (rtn != 0)
{
return 0;
}
robot.SetReConnectParam(true, 30000, 500);
double setRadio[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
robot.SetVelFeedForwardRatio(setRadio);
double getRadio[6] = { 0.0 };
robot.GetVelFeedForwardRatio(getRadio);
printf(" %f %f %f %f %f %f\n", getRadio[0], getRadio[1], getRadio[2], getRadio[3], getRadio[4], getRadio[5]);
robot.CloseRPC();
return 0;
}
6.65. Photoelectric Sensor TCP Calibration - Compute Tool RPY
/**
* @brief Photoelectric Sensor TCP Calibration - Compute Tool RPY
* @param [in] Btool Robot Cartesian position
* @param [in] Etool Current tool coordinate values
* @param [in] sensor Current sensor coordinate values (not yet available)
* @param [in] radius Circular motion radius in mm (not yet available)
* @param [in] dz Movement distance along the negative Z-axis of the base coordinate system; when dz = 10000, the function directly returns tool RPY
* @param [out] TCPRPY Tool RPY values
* @return Error code
*/
errno_t TCPComputeRPY(DescPose Btool, DescPose Etool, DescPose sensor, double radius, double dz, Rpy& TCPRPY);
6.66. Photoelectric Sensor TCP Calibration - Compute Tool XYZ
/**
* @brief Photoelectric Sensor TCP Calibration - Compute Tool XYZ
* @param [in] select 0-Compute tool TCP; 1-Compute sensor origin; 2-Compute sensor orientation; 3-Directly return tool TCP; 4-Record current workpiece coordinate system and tool coordinate system
* @param [in] originDirection 0-X direction; 1-Y direction; 2-Z direction
* @param [in] pos1 Robot Cartesian position 1
* @param [in] pos2 Robot Cartesian position 2
* @param [in] pos3 Robot Cartesian position 3
* @param [in] pos4 Robot Cartesian position 4
* @param [out] TCP Tool XYZ values
* @return Error code
*/
errno_t TCPComputeXYZ(int select, double originDirection, DescTran pos1, DescTran pos2, DescTran pos3, DescTran pos4, DescTran& TCP);
6.67. Photoelectric Sensor TCP Calibration - Start Recording Flange Center Position
/**
* @brief Photoelectric Sensor TCP Calibration - Start Recording Flange Center Position
* @return Error code
*/
errno_t TCPRecordFlangePosStart();
6.68. Photoelectric Sensor TCP Calibration - Stop Recording Flange Center Position
/**
* @brief Photoelectric Sensor TCP Calibration - Stop Recording Flange Center Position
* @return Error code
*/
errno_t TCPRecordFlangePosEnd();
6.69. Photoelectric Sensor TCP Calibration - Get Tool Center Point Position
/**
* @brief Photoelectric Sensor TCP Calibration - Get Tool Center Point Position
* @param [out] TCP Tool center point position (x, y, z)
* @return Error code
*/
errno_t TCPGetRecordFlangePos(DescTran& TCP);
6.70. Photoelectric Sensor TCP Calibration
/**
* @brief Photoelectric Sensor TCP Calibration
* @param [in] luaPath Automatic calibration Lua program path: For QX version robots - "/fruser/FR_CalibrateTheToolTcp.lua"; For LA version robots - "/usr/local/etc/controller/lua/FR_CalibrateTheToolTcp.lua"
* @param [in] offsetX Teaching point offset (x, y, z) in mm
* @param [out] TCP Calibrated tool coordinate system (x, y, z, rx, ry, rz)
* @return Error code
*/
errno_t PhotoelectricSensorTCPCalibration(std::string luaPath, DescTran offset, DescPose& TCP);
6.71. Photoelectric Sensor TCP Calibration Code Example
int TestPhotoelectricSensorTCPCalib(void)
{
ROBOT_STATE_PKG pkg = {};
FRRobot robot;
robot.LoggerInit();
robot.SetLoggerLevel(1);
int rtn = robot.RPC("192.168.58.2");
if (rtn != 0)
{
return 0;
}
robot.SetReConnectParam(true, 30000, 500);
DescTran offset = { 10.0, 10.0, 3.0 };
DescPose TCP = {};
rtn = robot.PhotoelectricSensorTCPCalibration("/fruser/FR_CalibrateTheToolTcp.lua", offset, TCP);
printf("PhotoelectricSensorTCPCalibration rtn is %d %f %f %f %f %f %f \n", rtn, TCP.tran.x, TCP.tran.y, TCP.tran.z, TCP.rpy.rx, TCP.rpy.ry, TCP.rpy.rz);
robot.CloseRPC();
robot.Sleep(9999999);
return 0;
}bottom of page