SolveIK_All for collisions based on each location AndrewLNguyen Junior Member Posts: 8 Threads: 4 Joined: Jun 2021 Reputation: 0   07-11-2022, 08:52 PM Hello. I am trying to create code that follows these steps: 1.     The robot reaches a spot in the pod area. 2.     It will check the feasibility to see if it can reach that spot by linear or joint move. 3.     If it can, it will solve for all possible configurations by using solveIK_all() to see if it will collide on the pod’s walls. 4.     End code if all spots tested do not collide. I am having trouble getting the results I need and understanding how to properly use SolveIK_All. So far my program is outputting a single joint and many other joints solved using SolveIK_All. I am not sure why some locations have more than 6 axis’ though. Attached is my code and file. Could I get help to correct my code? Code:```# This macro shows how you can create a program that moves the robot through a set of points # The points are automatically created as a cube grid around a reference target # If a linear movement can't be done from one point to the next one the robot will follow a joint movement from robodk.robolink import *  # API to communicate with RoboDK from robodk.robomath import *  # basic matrix operations from random import uniform  # to randomly calculate rz (rotation around the Z axis) # Name of the reference target REFERENCE_TARGET = 'RefTarget' # Check for collisions CHECK_COLLISIONS = False #Start the RoboDK API RDK = Robolink() # Set collision checking ON or OFF RDK.setCollisionActive(COLLISION_ON if CHECK_COLLISIONS else COLLISION_OFF) # Run on robot: Force the program to run on the connected robot (same behavior as right clicking the program, then, selecting "Run on Robot") # RDK.setRunMode(RUNMODE_RUN_ROBOT) # Get the main/only robot in the station robot = RDK.Item('', ITEM_TYPE_ROBOT) if not robot.Valid():    raise Exception("Robot not valid or not available") # Get the active reference frame frame = robot.getLink(ITEM_TYPE_FRAME) if not frame.Valid():    frame = robot.Parent()    robot.setPoseFrame(frame) # Get the reference pose with respect to the robot frame_pose = robot.PoseFrame() # Get the active tool tool = robot.getLink(ITEM_TYPE_TOOL) if not tool.Valid():    tool = robot.AddTool(transl(0, 0, 75), "Tool Grid")    robot.setPoseTool(tool) # Get the target reference RefTarget target_ref = RDK.Item(REFERENCE_TARGET, ITEM_TYPE_TARGET) if not target_ref.Valid():    target_ref = RDK.AddTarget(REFERENCE_TARGET, frame, robot) # Get the reference position (pose=4x4 matrix of the target with respect to the reference frame) pose_ref = target_ref.Pose() startpoint = target_ref.Joints() config_ref = robot.JointsConfig(startpoint) # Retrieve the tool pose tool_pose = tool.PoseTool() # Retrieve the degrees of freedom or axes (num_dofs = 6 for a 6 axis robot) num_dofs = len(robot.JointsHome().list()) # Get the reference frame of the target reference ref_frame = target_ref.Parent() # Function definition to check if 2 robot configurations are the same # Configurations are set as [Rear/Front,LowerArm/UpperArm,Flip/NonFlip] bits (int values) def config_equal(config1, config2):    if config1[0] != config2[0] or config1[1] != config2[1] or config1[2] != config2[2]:        return False    return True # Create a new program prog = RDK.AddProgram('AutoCreated') # This should make program generation slightly faster #prog.ShowInstructions(False) # Start creating the program or moving the robot: program_or_robot = prog program_or_robot.setPoseTool(tool_pose) program_or_robot.MoveJ(target_ref) lastjoints = startpoint rz = 0 ntargets = 0 for tz in range(-100, 101, 100):    for ty in range(0, 401, 200):        for tx in range(100, -5001, -250):            ntargets = ntargets + 1            # calculate a random rotation around the Z axis of the tool            #rz = uniform(-20*pi/180, 20*pi/180)            # Calculate the position of the new target: translate with respect to the robot base and rotate around the tool            newtarget_pose = transl(tx, ty, tz) * pose_ref * rotz(rz)            # First, make sure the target is reachable:            newtarget_joints = robot.SolveIK(newtarget_pose, lastjoints, tool_pose, frame_pose)            if len(newtarget_joints.list()) < num_dofs:                print('...target not reachable!! Skipping target')                continue            # Create a new target:            newtarget_name = 'Auto T%.0f,%.0f,%.0f Rz=%.1f' % (tx, ty, tz, rz)            print('Creating target %i: %s' % (ntargets, newtarget_name))            newtarget = RDK.AddTarget(newtarget_name, ref_frame, robot)            # At this point, the target is reachable.            # We have to check if we can do a linear move or not. We have 2 methods:            can_move_linear = True            # -------------------------------            # Validation method: use the robot.MoveL_Test option to check if the robot can make a linear movement            # This method is more robust and should provide a 100% accurate result but it may take more time            # robot.MoveL_Test can also take collisions into account if collision checking is activated            issues = robot.MoveL_Test(lastjoints, newtarget_pose)            can_move_linear = (issues == 0)            # We can retrieve the final joint position by retrieving the robot joints            if can_move_linear:                newtarget_joints = robot.Joints()            # ---------------------------------            if can_move_linear:                # All good, we don't need to modify the target.                # However, we could set the joints in the target as this may allow us to retrieve the robot configuration if we ever need it                newtarget.setAsCartesianTarget()  # default behavior                newtarget.setJoints(newtarget_joints)                # It is important to have setPose after setJoints as it may recalculate the joints to match the target                newtarget.setPose(newtarget_pose)                # Add the linear movement                program_or_robot.MoveL(newtarget)            else:                #print(newtarget_joints)                # Check if we can do a joint movement (check for collisions)                issues = robot.MoveJ_Test(lastjoints, newtarget_joints)                can_move_joints = (issues == 0)                                #Solve for all possible configurations                joints = robot.SolveIK_All(newtarget_pose)                if not can_move_joints:                    # Skip this point                    print("Skipping movement to: " + str(newtarget_joints))                    print(joints)                    continue                # Make sure we have a joint target and a joint movement                newtarget.setAsJointTarget()  # default behavior                # Setting the pose for a joint target is not important unless we want to retrieve the pose later                # or we want to use the Cartesian coordinates for post processing                newtarget.setPose(newtarget_pose)                # Make sure we set the joints after the pose for a joint taget as it may recalculate the pose                newtarget.setJoints(newtarget_joints)                # Add the joint movement                program_or_robot.MoveJ(newtarget)            # Remember the joint poisition of the last movement            lastjoints = newtarget_joints # Showing the instructions at the end is faster: prog.ShowInstructions(True) # Hiding the targets is cleaner and more difficult to accidentaly move a target #prog.ShowTargets(False) print('Program done with %i targets' % ntargets)``` Find Reply Albert Moderator Posts: 1,492 Threads: 1 Joined: Apr 2018 Reputation: 77 07-11-2022, 09:26 PM Your code is incorrect because SolveIK_All will return a list of lists. You are assuming that SolveIK_All returns only one solution with is very unlikely for most robots.  The return value of SolveIK_All is a 2D matrix with 6xN solutions, where N is the number of available solutions for 6-axis robots. Some versions of RoboDK return an array of 8xN solutions for a 6-axis robot. In this case, you can ignore the extra 2 values. A simple way to iterate over all possible solutions is to use the default iterator in Python and filter the extra values: Code:```joints_all = robot.SolveIK_All(pose_target) for joints in joints_all:    joints_6dof = list(joints)[0:6]    print(joints_6dof)``` Find Reply AndrewLNguyen Junior Member Posts: 8 Threads: 4 Joined: Jun 2021 Reputation: 0   07-11-2022, 09:35 PM (07-11-2022, 09:26 PM)Albert Wrote: Your code is incorrect because SolveIK_All will return a list of lists. You are assuming that SolveIK_All returns only one solution with is very unlikely for most robots.  The return value of SolveIK_All is a 2D matrix with 6xN solutions, where N is the number of available solutions for 6-axis robots. Some versions of RoboDK return an array of 8xN solutions for a 6-axis robot. In this case, you can ignore the extra 2 values. A simple way to iterate over all possible solutions is to use the default iterator in Python and filter the extra values: Code:```joints_all = robot.SolveIK_All(pose_target) for joints in joints_all:    joints_6dof = list(joints)[0:6]    print(joints_6dof)``` Hi Albert. Thank you for this! It is posting the results correctly now in the code below. But I am wondering if the different robot joint configurations it is solving for are actually feasible for the pod, which means the robot joints are not colliding with any of the pod walls.  Code:```# This macro shows how you can create a program that moves the robot through a set of points # The points are automatically created as a cube grid around a reference target # If a linear movement can't be done from one point to the next one the robot will follow a joint movement from robodk.robolink import *  # API to communicate with RoboDK from robodk.robomath import *  # basic matrix operations from random import uniform  # to randomly calculate rz (rotation around the Z axis) # Name of the reference target REFERENCE_TARGET = 'RefTarget' # Check for collisions CHECK_COLLISIONS = False #Start the RoboDK API RDK = Robolink() # Set collision checking ON or OFF RDK.setCollisionActive(COLLISION_ON if CHECK_COLLISIONS else COLLISION_OFF) # Run on robot: Force the program to run on the connected robot (same behavior as right clicking the program, then, selecting "Run on Robot") # RDK.setRunMode(RUNMODE_RUN_ROBOT) # Get the main/only robot in the station robot = RDK.Item('', ITEM_TYPE_ROBOT) if not robot.Valid():    raise Exception("Robot not valid or not available") # Get the active reference frame frame = robot.getLink(ITEM_TYPE_FRAME) if not frame.Valid():    frame = robot.Parent()    robot.setPoseFrame(frame) # Get the reference pose with respect to the robot frame_pose = robot.PoseFrame() # Get the active tool tool = robot.getLink(ITEM_TYPE_TOOL) if not tool.Valid():    tool = robot.AddTool(transl(0, 0, 75), "Tool Grid")    robot.setPoseTool(tool) # Get the target reference RefTarget target_ref = RDK.Item(REFERENCE_TARGET, ITEM_TYPE_TARGET) if not target_ref.Valid():    target_ref = RDK.AddTarget(REFERENCE_TARGET, frame, robot) # Get the reference position (pose=4x4 matrix of the target with respect to the reference frame) pose_ref = target_ref.Pose() startpoint = target_ref.Joints() config_ref = robot.JointsConfig(startpoint) # Retrieve the tool pose tool_pose = tool.PoseTool() # Retrieve the degrees of freedom or axes (num_dofs = 6 for a 6 axis robot) num_dofs = len(robot.JointsHome().list()) # Get the reference frame of the target reference ref_frame = target_ref.Parent() # Function definition to check if 2 robot configurations are the same # Configurations are set as [Rear/Front,LowerArm/UpperArm,Flip/NonFlip] bits (int values) def config_equal(config1, config2):    if config1[0] != config2[0] or config1[1] != config2[1] or config1[2] != config2[2]:        return False    return True # Create a new program prog = RDK.AddProgram('AutoCreated') # This should make program generation slightly faster #prog.ShowInstructions(False) # Start creating the program or moving the robot: program_or_robot = prog program_or_robot.setPoseTool(tool_pose) program_or_robot.MoveJ(target_ref) lastjoints = startpoint rz = 0 ntargets = 0 for tz in range(-100, 101, 100):    for ty in range(0, 401, 200):        for tx in range(100, -5001, -250):            ntargets = ntargets + 1            # calculate a random rotation around the Z axis of the tool            #rz = uniform(-20*pi/180, 20*pi/180)            # Calculate the position of the new target: translate with respect to the robot base and rotate around the tool            newtarget_pose = transl(tx, ty, tz) * pose_ref * rotz(rz)            # First, make sure the target is reachable:            newtarget_joints = robot.SolveIK(newtarget_pose, lastjoints, tool_pose, frame_pose)            if len(newtarget_joints.list()) < num_dofs:                print('...target not reachable!! Skipping target')                continue            # Create a new target:            newtarget_name = 'Auto T%.0f,%.0f,%.0f Rz=%.1f' % (tx, ty, tz, rz)            print('Creating target %i: %s' % (ntargets, newtarget_name))            newtarget = RDK.AddTarget(newtarget_name, ref_frame, robot)            # At this point, the target is reachable.            # We have to check if we can do a linear move or not. We have 2 methods:            can_move_linear = True            # -------------------------------            # Validation method: use the robot.MoveL_Test option to check if the robot can make a linear movement            # This method is more robust and should provide a 100% accurate result but it may take more time            # robot.MoveL_Test can also take collisions into account if collision checking is activated            issues = robot.MoveL_Test(lastjoints, newtarget_pose)            can_move_linear = (issues == 0)            # We can retrieve the final joint position by retrieving the robot joints            if can_move_linear:                newtarget_joints = robot.Joints()            # ---------------------------------            if can_move_linear:                # All good, we don't need to modify the target.                # However, we could set the joints in the target as this may allow us to retrieve the robot configuration if we ever need it                newtarget.setAsCartesianTarget()  # default behavior                newtarget.setJoints(newtarget_joints)                # It is important to have setPose after setJoints as it may recalculate the joints to match the target                newtarget.setPose(newtarget_pose)                # Add the linear movement                program_or_robot.MoveL(newtarget)            else:                #print(newtarget_joints)                # Check if we can do a joint movement (check for collisions)                issues = robot.MoveJ_Test(lastjoints, newtarget_joints)                can_move_joints = (issues == 0)                                #Solve for all possible configurations                joints_all = robot.SolveIK_All(newtarget_pose)                for joints in joints_all:                    joints_6dof = list(joints)[0:6]                    print(joints_6dof)                if not can_move_joints:                    # Skip this point                    print("Skipping movement to: " + str(newtarget_joints))                    continue                # Make sure we have a joint target and a joint movement                newtarget.setAsJointTarget()  # default behavior                # Setting the pose for a joint target is not important unless we want to retrieve the pose later                # or we want to use the Cartesian coordinates for post processing                newtarget.setPose(newtarget_pose)                # Make sure we set the joints after the pose for a joint taget as it may recalculate the pose                newtarget.setJoints(newtarget_joints)                # Add the joint movement                program_or_robot.MoveJ(newtarget)            # Remember the joint poisition of the last movement            lastjoints = newtarget_joints # Showing the instructions at the end is faster: prog.ShowInstructions(True) # Hiding the targets is cleaner and more difficult to accidentaly move a target #prog.ShowTargets(False) print('Program done with %i targets' % ntargets)``` Find Reply Albert Moderator Posts: 1,492 Threads: 1 Joined: Apr 2018 Reputation: 77 07-12-2022, 01:03 PM Your code does not look great because you are not properly iterating over all solutions. You should test your robot movements with each possible solution given by the inverse kinematics: Code:```# Look for a valid configurations to move to joints_valid = None joints_all = robot.SolveIK_All(newtarget_pose) for joints in joints_all:     print(joints)     # Check if we can do a joint movement (check for collisions)     issues = robot.MoveJ_Test(lastjoints, newtarget_joints)     can_move_joints = (issues == 0)     if can_move_joints:         joints_valid = joints         break if joints_valid is not None:     print("Valid joints found!")``` Find Reply AndrewLNguyen Junior Member Posts: 8 Threads: 4 Joined: Jun 2021 Reputation: 0   07-12-2022, 11:09 PM (This post was last modified: 07-12-2022, 11:14 PM by AndrewLNguyen.) (07-12-2022, 01:03 PM)Albert Wrote: Your code does not look great because you are not properly iterating over all solutions. You should test your robot movements with each possible solution given by the inverse kinematics: Code:```# Look for a valid configurations to move to joints_valid = None joints_all = robot.SolveIK_All(newtarget_pose) for joints in joints_all:     print(joints)     # Check if we can do a joint movement (check for collisions)     issues = robot.MoveJ_Test(lastjoints, newtarget_joints)     can_move_joints = (issues == 0)     if can_move_joints:         joints_valid = joints         break if joints_valid is not None:     print("Valid joints found!")```Hi Albert, when I tried this code, it did not work so I modified it, but I think I ran into solutions that I did not intend to find. The code is not printing out the valid joint configurations within the pod. What could be the issue? I modified the code below so that if there is no valid joints, it will just use the default one RoboDK generates and skip the point. I am new to coding so I would appreciate all the help.  Code:```               # Look for a valid configurations to move to                joints_valid = None                joints_all = robot.SolveIK_All(newtarget_pose)                for joints in joints_all:                    joints_6dof = list(joints)[0:6]                    print(joints_6dof)                    # Check if we can do a joint movement (check for collisions)                    issues = robot.MoveJ_Test(lastjoints, newtarget_joints)                    can_move_joints = (issues == 0)                    if can_move_joints:                        joints_valid = joints                        print("Valid joints found!" + str(joints_valid))                    else:                        # Skip this point                        print("Use" + str(newtarget_joints)+"instead")```Entire Code: Code:```# This macro shows how you can create a program that moves the robot through a set of points # The points are automatically created as a cube grid around a reference target # If a linear movement can't be done from one point to the next one the robot will follow a joint movement from robodk.robolink import *  # API to communicate with RoboDK from robodk.robomath import *  # basic matrix operations from random import uniform  # to randomly calculate rz (rotation around the Z axis) import numpy as np  # Name of the reference target REFERENCE_TARGET = 'RefTarget' # Check for collisions CHECK_COLLISIONS = False #Start the RoboDK API RDK = Robolink() # Set collision checking ON or OFF RDK.setCollisionActive(COLLISION_ON if CHECK_COLLISIONS else COLLISION_OFF) # Run on robot: Force the program to run on the connected robot (same behavior as right clicking the program, then, selecting "Run on Robot") # RDK.setRunMode(RUNMODE_RUN_ROBOT) # Get the main/only robot in the station robot = RDK.Item('', ITEM_TYPE_ROBOT) if not robot.Valid():     raise Exception("Robot not valid or not available") # Get the active reference frame frame = robot.getLink(ITEM_TYPE_FRAME) if not frame.Valid():     frame = robot.Parent()     robot.setPoseFrame(frame) # Get the reference pose with respect to the robot frame_pose = robot.PoseFrame() # Get the active tool tool = robot.getLink(ITEM_TYPE_TOOL) if not tool.Valid():     tool = robot.AddTool(transl(0, 0, 75), "Tool Grid")     robot.setPoseTool(tool) #Define the robot home position in terms of joint angles home_joints = [0,-90,90,-90,-90,0] robot.setJointsHome(home_joints) robot.setJoints(robot.JointsHome()) # Get the target reference RefTarget target_ref = RDK.Item(REFERENCE_TARGET, ITEM_TYPE_TARGET) if not target_ref.Valid():     target_ref = RDK.AddTarget(REFERENCE_TARGET, frame, robot) # Get the reference position (pose=4x4 matrix of the target with respect to the reference frame) pose_ref = target_ref.Pose() startpoint = target_ref.Joints() config_ref = robot.JointsConfig(startpoint) # Retrieve the tool pose tool_pose = tool.PoseTool() # Retrieve the degrees of freedom or axes (num_dofs = 6 for a 6 axis robot) num_dofs = len(robot.JointsHome().list()) # Get the reference frame of the target reference ref_frame = target_ref.Parent() # Function definition to check if 2 robot configurations are the same # Configurations are set as [Rear/Front,LowerArm/UpperArm,Flip/NonFlip] bits (int values) def config_equal(config1, config2):     if config1[0] != config2[0] or config1[1] != config2[1] or config1[2] != config2[2]:         return False     return True # Create a new program prog = RDK.AddProgram('AutoCreated') # This should make program generation slightly faster #prog.ShowInstructions(False) # Start creating the program or moving the robot: program_or_robot = prog program_or_robot.setPoseTool(tool_pose) program_or_robot.MoveJ(target_ref) lastjoints = startpoint rz = 0 ntargets = 0 for tz in range(0, 378, 100):     for ty in range(-378, 178, 100):         for tx in range(-700, -150, 100):             ntargets = ntargets + 1             # calculate a random rotation around the Z axis of the tool             #rz = uniform(-20*pi/180, 20*pi/180)             # Calculate the position of the new target: translate with respect to the robot base and rotate around the tool             newtarget_pose = transl(tx, ty, tz) * pose_ref * rotz(rz)             # First, make sure the target is reachable:             newtarget_joints = robot.SolveIK(newtarget_pose, lastjoints, tool_pose, frame_pose)             if len(newtarget_joints.list()) < num_dofs:                 print('...target not reachable!! Skipping target')                 continue             # Create a new target:             newtarget_name = 'Auto T%.0f,%.0f,%.0f Rz=%.1f' % (tx, ty, tz, rz)             print('Creating target %i: %s' % (ntargets, newtarget_name))             newtarget = RDK.AddTarget(newtarget_name, ref_frame, robot)             # At this point, the target is reachable.             # We have to check if we can do a linear move or not. We have 2 methods:             can_move_linear = True             # -------------------------------             # Validation method: use the robot.MoveL_Test option to check if the robot can make a linear movement             # This method is more robust and should provide a 100% accurate result but it may take more time             # robot.MoveL_Test can also take collisions into account if collision checking is activated             issues = robot.MoveL_Test(lastjoints, newtarget_pose)             can_move_linear = (issues == 0)             # We can retrieve the final joint position by retrieving the robot joints             if can_move_linear:                 newtarget_joints = robot.Joints()             # ---------------------------------             if can_move_linear:                 # All good, we don't need to modify the target.                 # However, we could set the joints in the target as this may allow us to retrieve the robot configuration if we ever need it                 newtarget.setAsCartesianTarget()  # default behavior                 newtarget.setJoints(newtarget_joints)                 # It is important to have setPose after setJoints as it may recalculate the joints to match the target                 newtarget.setPose(newtarget_pose)                 # Add the linear movement                 program_or_robot.MoveL(newtarget)             else:                 #print(newtarget_joints)                 # Check if we can do a joint movement (check for collisions)                 #issues = robot.MoveJ_Test(lastjoints, newtarget_joints)                 #can_move_joints = (issues == 0)                 # Look for a valid configurations to move to                 joints_valid = None                 joints_all = robot.SolveIK_All(newtarget_pose)                 for joints in joints_all:                     joints_6dof = list(joints)[0:6]                     print(joints_6dof)                     # Check if we can do a joint movement (check for collisions)                     issues = robot.MoveJ_Test(lastjoints, newtarget_joints)                     can_move_joints = (issues == 0)                     if can_move_joints:                         joints_valid = joints                         print("Valid joints found!" + str(joints_valid))                     else:                         # Skip this point                         print("Use" + str(newtarget_joints)+"instead")                          # Make sure we have a joint target and a joint movement                 newtarget.setAsJointTarget()  # default behavior                 # Setting the pose for a joint target is not important unless we want to retrieve the pose later                 # or we want to use the Cartesian coordinates for post processing                 newtarget.setPose(newtarget_pose)                 # Make sure we set the joints after the pose for a joint taget as it may recalculate the pose                 newtarget.setJoints(newtarget_joints)                 # Add the joint movement                 program_or_robot.MoveJ(newtarget)             # Remember the joint poisition of the last movement             lastjoints = newtarget_joints # Showing the instructions at the end is faster: prog.ShowInstructions(True) # Hiding the targets is cleaner and more difficult to accidentaly move a target #prog.ShowTargets(False) print('Program done with %i targets' % ntargets)``` Attached Files   Robot Collision Test.rdk (Size: 1.03 MB / Downloads: 21) Find Reply