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

Bound the movement of the robot and control the orientation of the end effector

#1
Hi guys,

I am using the offline programming using keyboard to control the robot. This is just a base for a bigger project I am working on. I am trying to move the robot to a a set position in-case it reaches singularity. The code I tried is:
Code:
# This macro allows moving a robot using the keyboard
# More information about the RoboDK API here:
# //www.sinclairbody.com/doc/en/RoboDK-API.html
# Type help("robolink") or help("robodk") for more information
# Press F5 to run the script
# Note: you do not need to keep a copy of this file, your python script is saved with the station
from robolink import * # API to communicate with RoboDK
from robodk import * # basic matrix operations
RDK = Robolink()

# Arrow keys program example

# get a robot
robot = RDK.Item('', ITEM_TYPE_ROBOT)
if not robot.Valid():
print("No robot in the station. Load a robot first, then run this program.")
pause(5)
raise Exception("No robot in the station!")

print('Using robot: %s' % robot.Name())
print('Use the arrows (left, right, up, down), Q and A keys to move the robot')
print('Note: This works on console mode only, you must run the PY file separately')

# define the move increment
move_speed = 10
robot_joints1= [ 8.805 , -13.848 , 0.000, -97.620, 0.849, 90.965, 145.752 ]
print(robot_joints1)

from msvcrt import getch
while True:
RDK.Render(True)
key = (ord(getch()))
move_direction = [0,0,0]
# print(key)
if key == 75:
print('arrow left (Y-)')
move_direction = [0,-1,0]
elif key == 77:
print('arrow right (Y+)')
move_direction = [0,1,0]
elif key == 72:
print('arrow up (X-)')
move_direction = [-1,0,0]
elif key == 80:
print('arrow down (X+)')
move_direction = [1,0,0]
elif key == 113:
print('Q (Z+)')
move_direction = [0,0,1]
elif key == 97:
print('A (Z-)')
move_direction = [0,0,-1]

#确保运动方向是specified
if norm(move_direction) <= 0:
continue

# calculate the movement in mm according to the movement speed
xyz_move = mult3(move_direction, move_speed)

# get the robot joints
robot_joints = robot.Joints()

# get the robot position from the joints (calculate forward kinematics)
robot_position = robot.SolveFK(robot_joints)

# get the robot configuration (robot joint state)
robot_config = robot.JointsConfig(robot_joints)

#计算新的robot position
new_robot_position = transl(xyz_move)*robot_position
print(new_robot_position)

#计算新的robot joints
new_robot_joints = robot.SolveIK(new_robot_position)

if len(new_robot_joints.tolist()) < 6:
#print("No robot solution!! The new position is too far, out of reach or close to a singularity")
new_robot_config = robot.JointsConfig(robot_joints1)
# robot.MoveJ (robot_joints1)
#continue
else:
# calculate the robot configuration for the new joints
new_robot_config = robot.JointsConfig(new_robot_joints)



if robot_config[0] != new_robot_config[0] or robot_config[1] != new_robot_config[1] or robot_config[2] != new_robot_config[2]:
print("Warning!! Robot configuration changed!! This will lead to unextected movements!")
#print(robot_config)
#print(new_robot_config)
else:

# move the robot joints to the new position
robot.MoveJ(new_robot_joints)
#robot.MoveL(new_robot_joints)


I got the values forrobot_joints1,by printing the value of therobot.joints of mytarget position. In the code I tried inputting these joint value to the robot, to go incase it reaches singularity. I am getting no errors but the robot is not moving to the set joint values( the robot is not moving at all). I am trying to make the robot move to a target position if it encounters singularity.

Also,I understand the code and how it moves the robot end-effector on the x,y,z axis but I also wanted to know if its possible to control the rotation of the end effector or the orientation of the end effector using the keyboard ? I printed out the new_robot_position and saw that only the x,y,z, changed while the angles were constant at 0,90,0. I wanted to to if I can manipulate the angles as well using keyboard keys.

I want the starting pose to be - Pose (490.000, 0.000, 780.000, 0.000, 90.000, 0.000) and instead of moving the xyz as shown in the example, is there a way to manipulate the pose ,like adding or subracting from the above Pose value? Something like the below code (not working and wrong syntax but just show you what I mean):


Code:
x=0
y=0
z=0
a=0
b=0
c=0
robot_position= (490.000, 0.000, 780.000, 0.000, 90.000, 0.000)

from msvcrt import getch
while True:
RDK.Render(True)
key = (ord(getch()))
#move_direction = [0,0,0]
# print(key)
if key == 75:
print('arrow left (Y-)')
y=y-5
elif key == 77:
print('arrow right (Y+)')
y = y + 5
elif key == 72:
print('arrow up (X-)')
x=x-5
elif key == 80:
print('arrow down (X+)')
x=x+5
elif key == 113:
print('Q (Z+)')
z=z+5
elif key == 97:
print('A (Z-)')
z=z-5

#确保运动方向是specified
#if norm(move_direction) <= 0:
#continue

# calculate the movement in mm according to the movement speed
#xyz_move = mult3(move_direction, move_speed)
#print (xyz_move)

# get the robot joints
robot_joints = robot.Joints()

# get the robot position from the joints (calculate forward kinematics)
robot_position = robot.SolveFK(robot_joints)
#print(robot_position)

# get the robot configuration (robot joint state)
robot_config = robot.JointsConfig(robot_joints)

#计算新的robot position
new_robot_position.pose = (490.000+x, 0.000+y, 780.000+z, 0.000, 90.000, 0.000)#transl(xyz_move)*robot_position
#print(new_robot_position)

#计算新的robot joints
new_robot_joints = robot.SolveIK(new_robot_position)

if len(new_robot_joints.tolist()) < 6:
#print("No robot solution!! The new position is too far, out of reach or close to a singularity")
new_robot_config = robot.JointsConfig(robot_joints)
# robot.MoveJ (robot_joints1)
continue
else:
# calculate the robot configuration for the new joints
new_robot_config = robot.JointsConfig(new_robot_joints)



if robot_config[0] != new_robot_config[0] or robot_config[1] != new_robot_config[1] or robot_config[2] != new_robot_config[2]:
print("Warning!! Robot configuration changed!! This will lead to unextected movements!")
#print(robot_config)
#print(new_robot_config)
else:

# move the robot joints to the new position
robot.MoveJ(new_robot_joints)
#robot.MoveL(new_robot_joints)
#2
Hi @Albertand @Jeremy,

Any suggestions or tips?
#3
Hi avpai1992,

Sorry for the delay, this one slipped through our fingers.
Unfortunately I'm not the guy to answer that question, my experience with the API is limited at the moment and Albert is out of the office for a week with limited access to email and the forum.
If he doesn't answer while he is away, I'll bring this thread to his attention as soon as he is back.

I really hope you understand.
Jeremy
#4
Thank you and No Problem. Please let him know of this thread. I will really appreciate all the help.
#5
I'm not sure I understand what you are trying to do. It looks like both sections of code are equivalent. The first version uses the movement as a vector [x,y,z] (I assume based onthisexample), the second version of the code just adds a shift along X,Y,Z coordinates.

Before running a joint or a linear movement you can test if the movement is feasible by calling:
MoveJ_Test or MoveL_Test
If the movement is not feasible, you can then bring the robot back to a location free of singularities.

More information here:
//www.sinclairbody.com/doc/en/PythonAPI/robo...MoveL_Test
#6
Hi @Albert,

Thank you for your reply. The keyboard example given controls the direction of the robot. What I am trying to do is similar to it. I would want to control the cartesian pose of the robot using keyboard. I am trying to assign a key to control each 6 elements of the cartesian pose - X,Y,Z and the orientation angles A,B,C. For example:
if the robot pose is :

robot_position= (490.000, 0.000, 780.000, 0.000, 90.000, 0.000) # (x,y,z,a,b,c);

I want to change each of them separately using Keyboard. I hope I am clear now. Sorry for the confusion
#7
In that case I recommend you to do something like this:

pose = robot.Pose() # or: pose = KUKA_2_Pose(xyzabc)
pose = pose * transl(dX,dY,dZ)*rotz(dA)*roty(dB)*rotx(dC)
robot.MoveL(pose)

Where dX,dY,dZ,dA,dB,dC are the shifts to their corresponding values (X,Y,Z,A,B,C repsectively). I assume you'll modify them one at a time.




Users browsing this thread:
1 Guest(s)