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:
# https://robodk.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]

   # make sure that a movement direction is 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)

   # calculate the new robot position
   new_robot_position = transl(xyz_move)*robot_position  
   print(new_robot_position)

   # calculate the new 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 for robot_joints1, by printing the value of the robot.joints of my target 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

   # make sure that a movement direction is 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)

   # calculate the new 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)

   # calculate the new 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 @Albert and @Jeremy,

Any suggestions or tips?
  




Users browsing this thread:
1 Guest(s)