top of page
private static void TestUDPWireSearch(Robot robot)
{
UDPComParam param = new UDPComParam("192.168.58.88", 2021, 2, 100, 3, 100, 1, 100, 10,0);
robot.ExtDevSetUDPComParam(param);//UDP extended axis communication
robot.SetWireSearchExtDIONum(0, 0);
int rtn0, rtn1, rtn2 = 0;
ExaxisPos exaxisPos = new ExaxisPos(0.0, 0.0, 0.0, 0.0);
DescPose offdese = new DescPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
DescPose descStart = new DescPose(-158.767, -510.596, 271.709, -179.427, -0.745, -137.349);
JointPos jointStart = new JointPos(61.667, -79.848, 108.639, -119.682, -89.700, -70.985);
DescPose descEnd = new DescPose(0.332, -516.427, 270.688, 178.165, 0.017, -119.989);
JointPos jointEnd = new JointPos(79.021, -81.839, 110.752, -118.298, -91.729, -70.981);
robot.MoveL(jointStart, descStart, 1, 0, 100, 100, 100, -1, exaxisPos, 0, 0, offdese, 0, 100);
robot.MoveL(jointEnd, descEnd, 1, 0, 100, 100, 100, -1, exaxisPos, 0, 0, offdese, 0, 100);
DescPose descREF0A = new DescPose(-66.106, -560.746, 270.381, 176.479, -0.126, -126.745);
JointPos jointREF0A = new JointPos(73.531, -75.588, 102.941, -116.250, -93.347, -69.689);
DescPose descREF0B = new DescPose(-66.109, -528.440, 270.407, 176.479, -0.129, -126.744);
JointPos jointREF0B = new JointPos(72.534, -79.625, 108.046, -117.379, -93.366, -70.687);
DescPose descREF1A = new DescPose(72.975, -473.242, 270.399, 176.479, -0.129, -126.744);
JointPos jointREF1A = new JointPos(87.169, -86.509, 115.710, -117.341, -92.993, -56.034);
DescPose descREF1B = new DescPose(31.355, -473.238, 270.405, 176.480, -0.130, -126.745);
JointPos jointREF1B = new JointPos(82.117, -87.146, 116.470, -117.737, -93.145, -61.090);
rtn0 = robot.WireSearchStart(0, 10, 100, 0, 10, 100, 0);
robot.MoveL(jointREF0A, descREF0A, 1, 0, 100, 100, 100, -1, exaxisPos, 0, 0, offdese, 0, 100); //Starting point
robot.MoveL(jointREF0B, descREF0B, 1, 0, 10, 100, 100, -1, exaxisPos, 1, 0, offdese, 0, 100); //Direction point
rtn1 = robot.WireSearchWait("REF0");
rtn2 = robot.WireSearchEnd(0, 10, 100, 0, 10, 100, 0);
rtn0 = robot.WireSearchStart(0, 10, 100, 0, 10, 100, 0);
robot.MoveL(jointREF1A, descREF1A, 1, 0, 100, 100, 100, -1, exaxisPos, 0, 0, offdese, 0, 100); //Starting point
robot.MoveL(jointREF1B, descREF1B, 1, 0, 10, 100, 100, -1, exaxisPos, 1, 0, offdese, 0, 100); //Direction point
rtn1 = robot.WireSearchWait("REF1");
rtn2 = robot.WireSearchEnd(0, 10, 100, 0, 10, 100, 0);
rtn0 = robot.WireSearchStart(0, 10, 100, 0, 10, 100, 0);
robot.MoveL(jointREF0A, descREF0A, 1, 0, 100, 100, 100, -1, exaxisPos, 0, 0, offdese, 0, 100); //Starting point
robot.MoveL(jointREF0B, descREF0B, 1, 0, 10, 100, 100, -1, exaxisPos, 1, 0, offdese, 0, 100); //Direction point
rtn1 = robot.WireSearchWait("RES0");
rtn2 = robot.WireSearchEnd(0, 10, 100, 0, 10, 100, 0);
rtn0 = robot.WireSearchStart(0, 10, 100, 0, 10, 100, 0);
robot.MoveL(jointREF1A, descREF1A, 1, 0, 100, 100, 100, -1, exaxisPos, 0, 0, offdese, 0, 100); //Starting point
robot.MoveL(jointREF1B, descREF1B, 1, 0, 10, 100, 100, -1, exaxisPos, 1, 0, offdese, 0, 100); //Direction point
rtn1 = robot.WireSearchWait("RES1");
rtn2 = robot.WireSearchEnd(0, 10, 100, 0, 10, 100, 0);
String[] varNameRef = {"REF0", "REF1", "#", "#", "#", "#"};
String[] varNameRes = {"RES0", "RES1", "#", "#", "#", "#"};
int offectFlag = 0;
//DescPose offectPos = new DescPose(0, 0, 0, 0, 0, 0);
DescOffset offset = new DescOffset();
rtn0 = robot.GetWireSearchOffset(0, 0, varNameRef, varNameRes, offset);
robot.PointsOffsetEnable(0, offset.offset);
robot.MoveL(jointStart, descStart, 1, 0, 100, 100, 100, -1, exaxisPos, 0, 0, offdese, 0, 100);
robot.MoveL(jointEnd, descEnd, 1, 0, 100, 100, 100, -1, exaxisPos, 1, 0, offdese, 0, 100);
robot.PointsOffsetDisable();
}
12.42. Impedance Control Start/Stop
/**
* @brief Impedance Control Start/Stop
* @param [in] status 0: disable; 1-enable
* @param [in] workSpace 0-joint space; 1-Cartesian space
* @param [in] forceThreshold Trigger force threshold (N)
* @param [in] m Mass parameter
* @param [in] b Damping parameter
* @param [in] k Stiffness parameter
* @param [in] maxV Max linear velocity (mm/s)
* @param [in] maxVA Max linear acceleration (mm/s2)
* @param [in] maxW Max angular velocity (°/s)
* @param [in] maxWA Max angular acceleration (°/s2)
* @return Error code
*/
public int ImpedanceControlStartStop(int status, int workSpace, double[] forceThreshold, double[] m, double[] b, double[] k, double maxV, double maxVA, double maxW, double maxWA)
12.43. Robot Impedance Control Start/Stop Code Example
public static int TestImpedanceControl(Robot robot)
{
JointPos j1=new JointPos(102.622, -135.990, 120.769, -73.950, -90.848, 35.507);
JointPos j2=new JointPos(93.674, -80.062, 82.947, -92.199, -90.967, 26.559);
DescPose desc_pos1=new DescPose(136.552, -149.799, 449.532, 179.817, -1.172, 157.123);
DescPose desc_pos2=new DescPose(136.540, -561.048, 449.542, 179.819, -1.172, 157.122);
DescPose offset_pos=new DescPose(0, 0, 0, 0, 0, 0);
ExaxisPos epos=new ExaxisPos(0, 0, 0, 0);
int tool = 0;
int user = 0;
double vel = 100.0;
double acc = 200.0;
double ovl = 100.0;
double blendT = -1.0;
double blendR = -1.0;
int flag = 0;
int search = 0;
robot.SetSpeed(20);
int company = 17;
int device = 0;
int softversion = 0;
int bus = 1;
DeviceConfig con=new DeviceConfig(company, device, softversion, bus);
robot.FT_SetConfig(con);
robot.Sleep(1000);
robot.FT_GetConfig(con);
System.out.println("FT config:"+con.company+","+con.device+","+con.softwareVersion+"con"+ con.bus);
robot.Sleep(1000);
robot.FT_Activate(0);
robot.Sleep(1000);
robot.FT_Activate(1);
robot.Sleep(1000);
robot.Sleep(1000);
robot.FT_SetZero(0);
robot.Sleep(1000);
robot.FT_SetZero(1);
robot.Sleep(1000);
double[] forceThreshold = { 30,30,30,5,5,5 };
double[] m= { 0.1,0.1,0.1,0.02,0.02,0.02 };
double[] b = { 1,1,1,0.08,0.08,0.08 };
double[] k = { 0,0,0,0,0,0 };
int rtn = robot.ImpedanceControlStartStop(1, 1, forceThreshold, m, b, k, 1000, 500, 100, 100);
System.out.println("ImpedanceControlStartStop errcode:"+ rtn);
rtn = robot.MoveL(desc_pos1, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1,0,-1, 1);
rtn = robot.MoveL(desc_pos2, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1,0,-1, 1);
rtn = robot.MoveL(desc_pos1, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1,0,-1, 1);
rtn = robot.MoveL(desc_pos2, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1,0,-1, 1);
rtn = robot.MoveL(desc_pos1, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1,0,-1, 1);
rtn = robot.MoveL(desc_pos2, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1,0,-1, 1);
System.out.println("movel errcode:"+ rtn);
robot.ImpedanceControlStartStop(0, 1, forceThreshold, m, b, k, 1000, 500, 100, 100);
robot.CloseRPC();
return 0;
}
12.44. Enable Torque Compensation Function and Compensation Coefficients
/**
* @brief Enable Torque Compensation Function and Compensation Coefficients
* @param status Switch, 0-Disable; 1-Enable
* @param torqueCoeff J1-J6 Torque compensation coefficients [0-1]
* @return Error code
*/
public int SerCoderCompenParams(int status, double[] torqueCoeff)
bottom of page