Java 8.24. Inverse kinematics calculation (reference position)

/**
* @brief  Inverse kinematics calculation with reference joint position
* @param  [in] posMode 0-Absolute pose, 1-Relative pose-base frame, 2-Relative pose-tool frame
* @param  [in] desc_pos Cartesian pose
* @param  [in] joint_pos_ref Reference joint position
* @param  [out] joint_pos Joint position
* @return  Error code
*/
int GetInverseKinRef(int posMode, DescPose desc_pos, JointPos joint_pos_ref, JointPos joint_pos);

8.25. Inverse Kinematics Solution, Cartesian Space Includes Extended Axis Position

/**
* @brief Inverse kinematics solution, Cartesian space includes extended axis position
* @param  type 0-Absolute pose (base coordinate system), 1-Incremental pose (base coordinate system), 2-Incremental pose (tool coordinate system)
* @param  desc_pos Cartesian pose
* @param  exaxis Extended axis position
* @param  tool Tool number
* @param  workPiece Workpiece number
* @param  joint_pos Joint position
* @return Error code
*/
public int GetInverseKinExaxis(int type, DescPose desc_pos, ExaxisPos exaxis, int tool, int workPiece, JointPos joint_pos)

8.26. Inverse Kinematics Solution Including Extended Axis Position Code Example

public static void  TestInverseKinExaxis(Robot robot)
{
    DescPose desc=new DescPose(99.957, -0.002, 29.994, -176.569, -6.757, -167.462);
    ExaxisPos exaxis=new ExaxisPos(100.0, 0.0, 0.0, 0.0);
    JointPos jointPos =new JointPos();
    DescPose offsetPos =new DescPose();
    ROBOT_STATE_PKG pkg=robot.GetRobotRealTimeState();
    int toolnum = pkg.tool;
    int workPcsNum = pkg.user;
    robot.GetInverseKinExaxis(0, desc, exaxis, toolnum, workPcsNum, jointPos);
    System.out.printf("GetInverseKinExaxis joint is %f, %f, %f, %f, %f, %f\n", jointPos.J1, jointPos.J2, jointPos.J3, jointPos.J4, jointPos.J5, jointPos.J6);
    robot.ExtAxisMove(exaxis, 100, -1);
    robot.MoveJ(jointPos, desc, toolnum, workPcsNum, 100.0, 100.0, 100.0, exaxis, -1, 0, offsetPos);
    robot.CloseRPC();
    robot.Sleep(9999999);
}

8.27. Check if inverse kinematics has solution

/**
* @brief  Check if inverse kinematics has solution with reference joint position
* @param  [in] posMode 0-Absolute pose, 1-Relative pose-base frame, 2-Relative pose-tool frame
* @param  [in] desc_pos Cartesian pose
* @param  [in] joint_pos_ref Reference joint position
* @return  Error code  List[0]:Error code; List[1]: int hasResult 0-No solution, 1-Solution exists
*/
List<Integer> GetInverseKinHasSolution(int posMode, DescPose desc_pos, JointPos joint_pos_ref);

8.28. Forward kinematics calculation

/**
* @brief  Forward kinematics calculation
* @param  [in] joint_pos Joint position
* @param  [out] desc_pos Cartesian pose
* @return  Error code
*/
int GetForwardKin(JointPos joint_pos, DescPose desc_pos);

8.29. Robot forward/inverse kinematics calculation code example

public static int TestInverseKin(Robot robot)
{
    JointPos j1=new JointPos(-11.904, -99.669, 117.473, -108.616, -91.726, 74.256);
    DescPose desc_pos1=new DescPose(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);

    JointPos inverseRtn = new JointPos(){};

    robot.GetInverseKin(0, desc_pos1, -1, inverseRtn);
    robot.GetInverseKinRef(0, desc_pos1, j1, inverseRtn);

    int hasResut = 0;
    robot.GetInverseKinHasSolution(0, desc_pos1, j1);

    DescPose forwordResult = new DescPose(){};
    robot.GetForwardKin(j1, forwordResult);

    return 0;
}

8.30. Query robot teaching management point data

/**
* @brief Query robot teaching management point data
* @param [in]  name  Point name
* @return  List[0]:Error code; List[1] - List[20] : Point data double[20]{x,y,z,rx,ry,rz,j1,j2,j3,j4,j5,j6,tool,wobj,speed,acc,e1,e2,e3,e4}
*/
List<Number> GetRobotTeachingPoint(String name);

8.31. Get robot DH parameter compensation values

/**
* @brief Get robot DH parameter compensation values
* @param dhCompensation Robot DH parameter compensation values (mm) [cmpstD1,cmpstA2,cmpstA3,cmpstD4,cmpstD5,cmpstD6]
* @return Error code
*/
public int GetDHCompensation(Object[] dhCompensation)

8.32. Get controller SN code

New in version Java: SDK-v1.0.4-3.8.1

/**
* @brief Get controller SN code
* @param [out] SNCode Controller SN code
* @return Error code
*/
int GetRobotSN(String[] SNCode);

8.33. Query robot teaching management point data code example

public static int TestGetTeachPoint(Robot robot)
{
    String name = "P1";
    List<Number> data=new ArrayList<>();
    data = robot.GetRobotTeachingPoint(name);
    System.out.println(name+" name is: "+data.get(0));
    for (int i = 0; i < 20; i++)
    {
        System.out.println("data is: "+ data.get(i+1));
    }

    int[] que_len = {0};
    int rtn = robot.GetMotionQueueLength(que_len);
    System.out.println("GetMotionQueueLength rtn is:"+rtn+", queue length is:"+ que_len[0]);

    Object[] dh = new Object[]{ 0,0,0,0,0,0 };
    int retval = 0;
    retval = robot.GetDHCompensation(dh);
    System.out.println("retval is: "+retval);

    String[] SN = new String[]{""};
    robot.GetRobotSN(SN);
    System.out.println("robot SN is "+SN[0]);
    return 0;
}

8.34. Get Tool Coordinate System by ID

New in version Java: SDK-v1.0.9-3.8.6

/**
* @brief Get Tool Coordinate System by ID
* @param [in] id Tool coordinate system ID
* @param [out] coord Coordinate system value
* @return Error code
*/
int GetToolCoordWithID(int id, DescPose coord)

8.35. Get Work Object Coordinate System by ID

New in version Java: SDK-v1.0.9-3.8.6

/**
* @brief Get Work Object Coordinate System by ID
* @param [in]  id Work object coordinate system ID
* @param [out] coord Coordinate system value
* @return Error code
*/
public int GetWObjCoordWithID(int id, DescPose coord)

8.36. Get External Tool Coordinate System by ID

New in version Java: SDK-v1.0.9-3.8.6

/**
* @brief Get External Tool Coordinate System by ID
* @param [in]  id External tool coordinate system ID
* @param [out] coord Coordinate system value
* @return Error code
*/
public int GetExToolCoordWithID(int id, DescPose coord)

8.37. Get Extended Axis Coordinate System by ID

New in version Java: SDK-v1.0.9-3.8.6

/**
* @brief Get Extended Axis Coordinate System by ID
* @param [in]  id Extended axis coordinate system ID
* @param [out] coord Coordinate system value
* @return Error code
*/
public int GetExAxisCoordWithID(int id, DescPose coord)

8.38. Get Current Tool Coordinate System

New in version Java: SDK-v1.0.9-3.8.6

/**
 * @brief Get Current Tool Coordinate System
 * @param [out] coord Coordinate system value
 * @return Error code
 */
public int GetCurToolCoord(DescPose coord)

8.39. Get Current Work Object Coordinate System

New in version Java: SDK-v1.0.9-3.8.6

/**
 * @brief Get Current Work Object Coordinate System
 * @param [out] coord Coordinate system value
 * @return Error code
 */
public int GetCurWObjCoord(DescPose coord)

8.40. Get Current External Tool Coordinate System

New in version Java: SDK-v1.0.9-3.8.6

/**
 * @brief Get Current External Tool Coordinate System
 * @param  [out] coord Coordinate system value
 * @return Error code
 */
public int GetCurExToolCoord(DescPose coord)

8.41. Get Current Extended Axis Coordinate System

New in version Java: SDK-v1.0.9-3.8.6

/**
 * @brief Get Current Extended Axis Coordinate System
 * @param [out] coord Coordinate system value
 * @return Error code
 */
public int GetCurExAxisCoord(DescPose coord)

8.42. Get Robot Coordinate System and Payload Code Example

public static void TestCoord(Robot robot)
{
    int id = 1;
    int rtn = 0;
    DescPose toolCoord = new DescPose();
    DescPose extoolCoord = new DescPose();
    DescPose wobjCoord = new DescPose();
    DescPose exAxisCoord = new DescPose();


    robot.GetCurToolCoord(toolCoord);// Tool
    System.out.println("GetToolCoord:"+id+","+
            toolCoord.tran.x+","+ toolCoord.tran.y+","+ toolCoord.tran.z+","+
            toolCoord.rpy.rx+","+ toolCoord.rpy.ry+","+ toolCoord.rpy.rz);


    robot.GetCurWObjCoord(toolCoord);// Work object
    System.out.println("GetCurWObjCoord:"+id+","+
            toolCoord.tran.x+","+ toolCoord.tran.y+","+ toolCoord.tran.z+","+
            toolCoord.rpy.rx+","+ toolCoord.rpy.ry+","+ toolCoord.rpy.rz);

    robot.GetCurExToolCoord(toolCoord);// External tool
    System.out.println("GetCurExToolCoord:"+id+","+
            toolCoord.tran.x+","+ toolCoord.tran.y+","+ toolCoord.tran.z+","+
            toolCoord.rpy.rx+","+ toolCoord.rpy.ry+","+ toolCoord.rpy.rz);


    robot.GetCurExAxisCoord(toolCoord);// Extended axis
    System.out.println("GetCurExAxisCoord:"+id+","+ // Corrected print string for consistency
            toolCoord.tran.x+","+ toolCoord.tran.y+","+ toolCoord.tran.z+","+
            toolCoord.rpy.rx+","+ toolCoord.rpy.ry+","+ toolCoord.rpy.rz);


    List<Number> weightT = new ArrayList<>();// Center of gravity
    DescTran cogT=new DescTran();
    weightT=robot.GetTargetPayload(0);
    robot.GetTargetPayloadCog(0,cogT);
    System.out.println("GetTargetPayload :"+weightT.get(1).doubleValue()+", "+
            cogT.x+", "+cogT.y+", "+cogT.z);


    robot.GetToolCoordWithID(id, toolCoord);
    System.out.println("GetToolCoordWithID:"+id+","+
            toolCoord.tran.x+","+ toolCoord.tran.y+","+ toolCoord.tran.z+","+
            toolCoord.rpy.rx+","+ toolCoord.rpy.ry+","+ toolCoord.rpy.rz);

    robot.GetWObjCoordWithID(id, wobjCoord);
    System.out.println("GetWObjCoordWithID "+id+", "+
            wobjCoord.tran.x+","+ wobjCoord.tran.y+","+ wobjCoord.tran.z+","+
            wobjCoord.rpy.rx+","+ wobjCoord.rpy.ry+","+ wobjCoord.rpy.rz);


    robot.GetExToolCoordWithID(id, extoolCoord);// External tool
    System.out.println("GetExToolCoordWithID :"+ id+","+
            extoolCoord.tran.x+","+ extoolCoord.tran.y+","+ extoolCoord.tran.z+","+
            extoolCoord.rpy.rx+","+ extoolCoord.rpy.ry+","+ extoolCoord.rpy.rz);

    robot.GetExAxisCoordWithID(id, exAxisCoord);// Extended axis
    System.out.println("GetExAxisCoordWithID "+id+","+
            exAxisCoord.tran.x+","+ exAxisCoord.tran.y+","+ exAxisCoord.tran.z+","+
            exAxisCoord.rpy.rx+","+ exAxisCoord.rpy.ry+","+ exAxisCoord.rpy.rz);


    double[] weight = new double[1];// Payload center of gravity
    DescTran getCog = new DescTran();
    robot.GetTargetPayloadWithID(id, weight, getCog);
    System.out.println("GetTargetPayloadWithID :"+ id+","+ weight[0]+","+
            getCog.x+","+ getCog.y+","+ getCog.z);

    DescPose coordSet0 = new DescPose(0, 0, 0, 0, 0, 0);
    DescPose coordSet = new DescPose(1, 2, 3, 4, 5, 6);
    DescPose etcp = new DescPose(10, 20, 30, 40, 50, 60);
    DescPose etool = new DescPose(0.1, 0.2, 0.3, 0.4, 0.5, 0.6);
    DescTran cog = new DescTran(1, 2, 3);

    robot.SetToolCoord(id, coordSet, 0, 0, 1, 0);
    robot.Sleep(100);
    robot.SetWObjCoord(id, coordSet, 0);
    robot.Sleep(100);
    robot.ExtAxisActiveECoordSys(id, 1, coordSet, 1); // Apply calibration result to extended axis coordinate system
    robot.Sleep(100);
    rtn = robot.SetExToolCoord(id, etcp, etool);
    robot.Sleep(100);
    rtn = robot.SetLoadWeight(id, 1.5);
    robot.Sleep(500);
    rtn = robot.SetLoadCoord(id, cog);
    robot.Sleep(100);
}