Thread Rating:
  • 0 Vote(s) - 0 Average
  • 1
  • 2
  • 3
  • 4
  • 5

Calculation of Joint Value Using IK

#1
I want to use python robodk api to get the value of the 6 axis of my 6-axis robot from end-effector using the value of LIN {} which is the data in KUKA's *.src file. Also, I want to get a two-axis turntable while controlling it.
So I'm trying to get it using SolveIK_All(), but it's not going as well as I thought. Can you look at my code and let me know what the problem is?
For your information, if you look at my code, the value corresponds to LIN {} for the parameter of SolveIK_All(), the value of tool is the value of my 6-axis robot, and the value of my turntable for reference.
LIN {} that I will use Examples of data include: LIN {X 0.0001,Y -4.7575,Z40,A -2.9196,B0,C0,E1 0.1598,E2 90.075}
I want to know the robot's joint values when the end-effector of my robot arm is located in a position that is x, y, z away from the turntable, and rotates by a, b, and c, and my turntable is rotated by e1, e2.
P.S Additionally, I would like to know the joint values of the 6-axis robot when the end-effector moves to the corresponding coordinate when I receive a specific coordinate value with KR 70 R2100_2.4 station as the global coordinate. I would appreciate it if you could tell me these methods as well.

[Image: figure1.png]

Code:
from robodk.robomath import *
from robodk.robodialogs import *
from robodk.robofileio import *
from robodk.robolink import *
from robodk.robolinkutils import *

# Start the RoboDK API
RDK = Robolink()

# Ask the user to pick an SRC file:
rdk_file_path = RDK.getParam("C:/Users/JaeHongYoo/WRL Dropbox/Jaehong Yoo/RoboDK")
# src_file_path = getOpenFileName(rdk_file_path)
src_file_paths = getOpenFileNames(rdk_file_path, "Select SRC Files", "*.src")
if not src_file_paths:
    print("Nothing selected")
    quit()

# Process each selected SRC file
for src_file_path in src_file_paths:
    # Get the program name form the file path
    program_name = getFileName(src_file_path)
    program_name = program_name.replace('_', '').replace('(', '').replace(')', '')
    print("Loading program: " + program_name)

    # Check if the file extension is .src
    if not src_file_path.lower().endswith(".src"):
        print("Invalid file selected: " + src_file_path)
        continue

    def GetValues(line):
        "" "Get all the numeric values from a line"""
        line = line.replace(",", " ")
        line = line.replace("}", " ")
        values = line.split(" ")

        list_values = []
        for value in values:
            try:
                value = float(value)
            except:
                continue

            list_values.append(value)

        print("list_values: ")
        print(list_values[:6])
        return list_values

    # Open the file and iterate through each line
    with open(src_file_path) as f:
        count = 0
        for line in f:
            # Remove empty characters:
            line = line.strip()
            print("Loading line: " + line)

            # Get all the numeric values in order
            values = GetValues(line)
            values_str = str(values)

            # Increase the counter
            count = count + 1

            # Check operations that involve a pose
            if len(values) < 6:
                print("Warning! Invalid line: " + line)
                continue

            # Check what instruction we need to add:
            if line.startswith("LIN"):
                # Ask the user to select a robot (if more than a robot is available)
                robot = RDK.Item('KUKA KR 70 R2100', ITEM_TYPE_ROBOT)

                joints = robot.Joints()
                joints[6] = values[6]
                joints[7] = values[7]
                robot.setJoints(joints)
                RDK.ShowMessage('Turn-table has changed!', True)

                # Get the active tool frame
                tool = robot.Tool()
                tool_str = str(tool)
                RDK.ShowMessage('tool: ' + tool_str, True)

                target_position = values[:3]
                target_ori = values[3:6]
                target_ori_temp = target_ori[0]
                target_ori[0] = target_ori[2]
                target_ori[2] = target_ori_temp
                target_external = values[6:8]

                radian_x = target_ori[0] * (math.pi / 180)
                radian_y = target_ori[1] * (math.pi / 180)
                radian_z = target_ori[2] * (math.pi / 180)

                target_pos = transl(target_position) * rotx(radian_x) * roty(radian_y) * rotz(radian_z)

                baseline = RDK.Item('Baseline')
                baseline_pose = baseline.Pose()
                baseline_pose_abs = baseline.PoseAbs()

                KUKA_Base = RDK.Item('KUKA KR 70 R2100 Base')
                KUKA_Base_pose_abs = KUKA_Base.PoseAbs()
                baseline_rel_pose = invH(KUKA_Base_pose_abs) * baseline_pose_abs

                relative_pose = invH(baseline_rel_pose) * target_pos

                #all_solutions = robot.SolveIK_All(relative_pose, tool, baseline_pose_abs)
                all_solutions = robot.SolveIK_All(relative_pose, tool, baseline_rel_pose)
                joints_list = []
                # Iterate through each solution
                for j in all_solutions:
                    # Retrieve flags as a list for each solution
                    conf_RLF = robot.JointsConfig(j).list()

                    # Breakdown of flags:
                    rear = conf_RLF[0]  # 1 if Rear , 0 if Front
                    lower = conf_RLF[1]  # 1 if Lower, 0 if Upper (elbow)
                    flip = conf_RLF[2]  # 1 if Flip , 0 if Non flip (Flip is usually when Joint 5 is negative)

                    # Look for a solution with Front and Elbow up configuration
                    # if conf_RLF[0:2] == [0,0]:
                    if rear == 0 and lower == 0 and flip == 0:
                        print("Solution found!")
                        joints_sol = j
                        joints_sol_str = str(joints_sol)
                        robot.setJoints(joints_sol[:6] + values[6:8])
                        RDK.ShowMessage('SolveIK_All: ' + joints_sol_str, True)
                        break

RDK.ShowMessage("Done", False)
  




Users browsing this thread:
1 Guest(s)