[Jeff Xiang]

Hi there,

I have a question regarding using RoboDK (v5.7.4) with Python API (v5.6).

I found an example in the official document "Points to Program", chose the option 2 there and want to modify it a little bit, so that the orientation of the target points are also considered here.

The change is mainly the function MakePoints (especially line 33-55). By each point, the first three number refer to xyz coordinate, the rest three number is euler angle in radian. But somehow after executed the program, the translation information of the targets is correct, the orientation not. Here is the code:

Code:

from robodk import robolink
import sys
import os


# Move a robot along a line given a start and end point by steps
# This macro shows three different ways of programming a robot using a Python script and the RoboDK API

# More information about the RoboDK API here:
# https://robodk.com/doc/en/RoboDK-API.html
# For more information visit:
# https://robodk.com/doc/en/PythonAPI/robodk.html#robolink-py

# Default parameters:
P_START = [1765.0, 1.0409497792752507e-14, 1010.0, -3.1415926535897913, -1.4988010832439613e-15, -3.1415926535897927]  # Start point with respect to the robot base frame
P_END = [1765.0, 1.0409497792752507e-14, 1510.0, -3.1415926535897913, -1.4988010832439613e-15, -1]  # End point with respect to the robot base frame
NUM_POINTS = 10  # Number of points to interpolate


# Function definition to create a list of points (line)
def MakePoints(xStart, xEnd, numPoints):
    """Generates a list of points"""
    # if len(xStart) != 3 or len(xEnd) != 3:
    #     raise Exception("Start and end point must be 3-dimensional vectors")
    if numPoints < 2:
        raise Exception("At least two points are required")

    # Starting Points
    pt_list = []
    x = xStart[0]
    y = xStart[1]
    z = xStart[2]
    wx = xStart[3]
    wy = xStart[4]
    wz = xStart[5]

    # How much we add/subtract between each interpolated point
    x_steps = (xEnd[0] - xStart[0]) / (numPoints - 1)
    y_steps = (xEnd[1] - xStart[1]) / (numPoints - 1)
    z_steps = (xEnd[2] - xStart[2]) / (numPoints - 1)
    wx_steps = (xEnd[3] - xStart[3]) / (numPoints - 1)
    wy_steps = (xEnd[4] - xStart[4]) / (numPoints - 1)
    wz_steps = (xEnd[5] - xStart[5]) / (numPoints - 1)

    # Incrementally add to each point until the end point is reached
    for i in range(numPoints):
        point_i = [x, y, z, wx, wy, wz]  # create a point
        #append the point to the list
        pt_list.append(point_i)
        x = x + x_steps
        y = y + y_steps
        z = z + z_steps
        wx = wx + wx_steps
        wy = wy + wy_steps
        wz = wz + wz_steps
    return pt_list


#---------------------------------------------------
#--------------- PROGRAM START ---------------------
from robodk.robolink import *  # API to communicate with RoboDK
from robodk.robomath import *  # basic matrix operations

# Generate the points curve path
POINTS = MakePoints(P_START, P_END, NUM_POINTS)

# Initialize the RoboDK API
RDK = Robolink()

# turn off auto rendering (faster)
RDK.Render(False)

# Promt the user to select a robot (if only one robot is available it will select that robot automatically)
robot = RDK.ItemUserPick('Select a robot', ITEM_TYPE_ROBOT)

# Turn rendering ON before starting the simulation
RDK.Render(True)

# Abort if the user hits Cancel
if not robot.Valid():
    quit()

# Retrieve the robot reference frame
reference = robot.Parent()

# Use the robot base frame as the active reference
robot.setPoseFrame(reference)

# get the current orientation of the robot (with respect to the active reference frame and tool frame)
pose_ref = robot.Pose()
print(Pose_2_TxyzRxyz(pose_ref))
# a pose can also be defined as xyzwpr / xyzABC
#pose_ref = TxyzRxyz_2_Pose([100,200,300,0,0,pi])


#-------------------------------------------------------------
# Option 2: Create the program on the graphical user interface
# Turn off rendering
RDK.Render(False)
prog = RDK.AddProgram('AutoProgram')

# Iterate through all the points
for i in range(NUM_POINTS):
    # add a new target and keep the reference to it
    ti = RDK.AddTarget('Auto Target %i' % (i + 1))
    # use the reference pose and update the XYZ position
    pose_i = pose_ref
    pose_i.setPos(POINTS[i])
    ti.setPose(pose_i)
    # force to use the target as a Cartesian target
    ti.setAsCartesianTarget()

    # Optionally, add the target as a Linear/Joint move in the new program
    prog.MoveJ(ti)

# Turn rendering ON before starting the simulation
RDK.Render(True)

You can find the corresponding station in attachment, which actually contains only a robot.

Many thanks in advance for any suggestions here!

Best regards,
Jeff

[Albert]

If you want to split a movement between 2 poses you should properly account for the orientation change using an orientation vector.

You should use a function like the PoseSplit attached below:

Code:

def PoseSplit(pose1, pose2, delta_mm=1):
   """Split the move between 2 poses given delta_mm increments"""
   pose_delta = invH(pose1) * pose2
   distance = norm(pose_delta.Pos())   
   if distance <= delta_mm:
       return [pose2]
   pose_list = []
  
   x,y,z,w,p,r = Pose_2_UR(pose_delta)       
  
   steps = max(1,int(distance/delta_mm)) 
   xd = x/steps
   yd = y/steps
   zd = z/steps
   wd = w/steps
   pd = p/steps
   rd = r/steps
   for i in range(steps-1):
       factor = i+1
       pose_list.append( pose1 * UR_2_Pose([xd*factor,yd*factor,zd*factor,wd*factor,pd*factor,rd*factor]) )
      
   return pose_list

You can find more information here:
https://robodk.com/forum/Thread-Executin...ction-call

[Jeff Xiang]

If you want to split a movement between 2 poses you should properly account for the orientation change using an orientation vector.

You should use a function like the PoseSplit attached below:

Code:

def PoseSplit(pose1, pose2, delta_mm=1):
   """Split the move between 2 poses given delta_mm increments"""
   pose_delta = invH(pose1) * pose2
   distance = norm(pose_delta.Pos())   
   if distance <= delta_mm:
       return [pose2]
   pose_list = []
  
   x,y,z,w,p,r = Pose_2_UR(pose_delta)       
  
   steps = max(1,int(distance/delta_mm)) 
   xd = x/steps
   yd = y/steps
   zd = z/steps
   wd = w/steps
   pd = p/steps
   rd = r/steps
   for i in range(steps-1):
       factor = i+1
       pose_list.append( pose1 * UR_2_Pose([xd*factor,yd*factor,zd*factor,wd*factor,pd*factor,rd*factor]) )
      
   return pose_list

You can find more information here:
https://robodk.com/forum/Thread-Executin...ction-call

Hi Albert,

thank you for your quick answer. My problem is not really splitting a movement between 2 poses, but defining the generated poses as target points in the program. I already have generated a list of poses, then if I set target pose like the for-loop in my code and execute the program, RoboDK will define the target poses correspondingly, however, the translation part is correct, but the orientation part is somehow not correct defined. You can see in the picture in attachment that, the orientation of the poses does not change, though it should be different from begin pose to the end pose. Is there any better way to define target poses, or move the robot to a specific pose? Thanks a lot!

Best regards,
Jeff

[Albert]

The function I provided you will create a list of poses and properly interpolates both the position and the orientation. You should set the pose of your targets using setPose.

Can you share the complete project you tested in your last post?

[Jeff Xiang]

The function I provided you will create a list of poses and properly interpolates both the position and the orientation. You should set the pose of your targets using setPose.

Can you share the complete project you tested in your last post?

Yes, setPose is exactly the function that I have tried. You can find my code and the rdk file in attachment. I have the function from tutorial for splitting poses implemented from line 21 to 56. The function you provided me is implemented from line 60 to 81. From line 85 starts the connection to RoboDK and defining the target poses. You can find at line 89, I have tried the function from tutorial; line 90, the function you provided. But none of them worked with setPose, although the poses are already generated correctly.

[Albert]

You should set the full pose in your loop, not just the XYZ values of the targets. See attached example:

Code:

# Iterate through all the points
for i in range(NUM_POINTS):
    # add a new target and keep the reference to it
    ti = RDK.AddTarget('Auto Target %i' % (i + 1))
    # use the reference pose and update the XYZ position
    ti.setPose(POINTS[i])
    # force to use the target as a Cartesian target
    ti.setAsCartesianTarget()

    # Optionally, add the target as a Linear/Joint move in the new program
    prog.MoveJ(ti)

[Jeff Xiang]

Thanks, the problem is solved. I have wrongly given the joint as input of setPose()... Now everything works :)

[Albert]

Perfect, thank you for letting us know.