[AndrewLNguyen]

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

# 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)
如果不是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)

[Albert]

代码是我ncorrect 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)

[AndrewLNguyen]

代码是我ncorrect 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)

嗨,艾伯特。谢谢你!这是发布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

# 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)
如果不是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)

[Albert]

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!")

[AndrewLNguyen]

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

# 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)
如果不是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)