vcPythonKinematics

vcPythonKinematics allows you to create custom kinematics, which can be used by robot controllers to drive and manage robot movements.

Inherits: vcBehaviour

Contents HideContents Show
  1. Properties
  2. Methods
  3. Events
  4. Examples

Properties

Name Type Access Description
Name String RW Defines the name of the kinematics object.
Application vcApplication R Gets the application object.
Simulation vcSimulation R Gets the simulation object.
Script String RW Defines the executable Python code of kinematics object, thereby the custom kinematics of a vcKinObject object.

See Python Kinematics for more information.

Methods

Name Return Type Parameters Description

forwardKin

vcMatrix

List of Real values

Returns a forward kinematics matrix calculated for given joint values.
getApplication vcApplication None Returns the application object.
getComponent vcComponent None Returns the component of the kinematics object.
getNode vcNode None Returns the node containing the kinematics object.
getSimulation vcSimulation None Returns the simulation object.
sasa Real Real side1, Real angle, Real side2 Returns the angle (in radians) of a triangle based on two given sides and the angle between them.

In this case, the third side of a triangle is calculated, and then used to calculate the angle between that side and the second side argument.
sass Real Real side1, Real angle, Real side2 Returns the side of a triangle based on two given sides and the angle between them.
sssa Real Real side1, Real side2, Real side3 Returns the angle (in radians) of a triangle based on three given sides.

In this case, returned angle is the angle opposite the first side argument.

Events

The following event handlers are functions that can be defined and used for customizing the kinematics of a vcKinObject type object. In some cases, event handlers need to output values which are automatically passed as arguments in other related functions.

Name Parameters Description
OnCalculateTarget vcMatrix fullTarget, vcMatrix baseFrame, vcMatrix toolFrame, Boolean targetInverted Triggered when a motion target is being calculated, thereby allowing you to edit the motion target that will be passed to OnInverse event.

The fullTarget argument is the target matrix calculated from either World origin or Robot World Frame to TCP.

The baseFrame and toolFrame arguments define the base and tool frames used for motion target.

The targetInverted argument can be used to reverse the target's calculation, for example from TCP to base frame.

Generally, you should use OnCalculateTarget for robots that have less than six degrees of freedom to remove potential effects caused by a tool offset.
OnConstrainParams vcKinObject kin_object Triggered when the joint configuration of a robot or motion target is being interpolated and requires joint limits to determine valid kinematic solutions.

Generally, you use OnConstrainParams to avoid potential joint limit errors and narrow possible solutions for a joint motion target. For example, you can specify the limits of a joint for certain types of movements.

The return value should be a Boolean.
OnFinalize None Triggered when layout or component is being loaded in 3D world.

The event occurs, for example, when copying a component, or loading a layout of components. The event is triggered before the needed components are attached to the 3D world.

Generally, you use OnFinalize to set the joint initial position of a robot or any necessary implementation that uses kinematics.
OnForward vcKinObject kin_object Triggered when a motion target is calculated using joint values.

Return a Boolean value to enable or disable forward kinematics.
OnGetConfigurationCount None Triggered when the configuration count of a robot is requested by the kinematics solver.

If configuration properties are dynamically changing, this event must be implemented and return a constant integer value, which overrides the default count defined in the OnInitKinObject event.

Important: If OnGetConfigurationCount is used, you need to implement the OnGetCurrentConfiguration and OnSetCurrentConfiguration events.
OnGetCurrentConfiguration None Triggered when the current configuration of a robot is requested by its kinematics solver.

If this event is implemented, you should read the robot's configurations property list, and then return the currently selected index position (integer). That value must be greater than or equal to 0 and less than the value returned by the OnGetConfigurationCount event.

Important: If OnGetCurrentConfiguration is used, you need to implement the OnGetConfigurationCount and OnSetCurrentConfiguration events.
OnGetJointCount None Triggered when the joint count of a robot is requested by the kinematics solver.

In most cases, you should the total joint count as an integer.
OnGetJointName Integer index Triggered when the name of a joint at a given index is requested by kinematics solver.

The return value should be a string.
OnGetKinematicJointCount None Triggered when the actual number of kinematic joints used in forward kinematics is requested by kinematics solver.

Generally, additional helper joints are needed for interpolating redundant solutions.

The return value should be an integer.
OnGetSecondaryJointCount None Triggered when the external joint count of a robot is requested by kinematics solver.

In most cases, you should return the total joint count as an integer value.
OnGetSecondaryJointName Integer index Triggered when the name of an external joint at a given index is requested by kinematics solver.

The return value should be a string.
OnInitKinObject vcKinObject kin_object Triggered when a new instance of vcKinObject is created, thereby allowing you to access and edit the kinematics object of a component.

Generally, you use OnInitKinObject to add new properties and define initial values for a kinematics object.
OnInverse vcKinObject kin_object Triggered when the joint values of a motion target need to be calculated and given as a solution for the kinematics object.

Return a Boolean value to enable or disable inverse kinematics.
OnRebuild None Triggered when the kinematics object is rebuilt, thereby updating changes made to its properties.
OnRelaxParams vcKinObject kin_object Triggered when possible solutions for a motion target are being calculated by kinematics solver.

Generally, you use OnRelaxParams to allow for all possible solutions, even those which exceed joint limits. For example, you might want to force a robot motion with potential risks such as singularity or Gimbal lock.
OnSetCurrentConfiguration Integer configurationIndex Triggered when the configuration of a robot is needed for a kinematic solution.

If this event is implemented, you should set the robot's configurations property list based on the given configurationIndex.

Important: If OnSetCurrentConfiguration is used, you need to implement the OnGetConfigurationCount and OnGetCurrentConfiguration events.
OnUpdateSecondaryJoints vcKinObject kin_object Triggered when external joint values need to be recalculated for motion target updates.

The return value should be a Boolean.

Examples

Example. Define custom Cartesian kinematics

from vcPythonKinematics import *
import vcMatrix
import vcVector
 
# Define the amount of joints and their names what this kinematics supports
JOINT_COUNT = 6
JOINT_NAMES = [ "Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6" ]
 
# Base offset is the distance from robot component origin to flange nodes toolpoint
BASE_OFFSET = vcVector.new(234.542, 203.301, 1134.46)
 
# Returns the amount of joints this kinematics handles
def OnGetJointCount():   
  return JOINT_COUNT
 
# Returns the indexed joint names
def OnGetJointName(index):   
  return JOINT_NAMES[index]
 
# Returns Kinematic chain target (matrix) value based on joint values
def OnForward(kinobj):   
  m = vcMatrix.new()   
  p = vcVector.new()   
  p.X = kinobj.JointValues[0]   
  p.Y = kinobj.JointValues[1]   
  p.Z = kinobj.JointValues[2]   
  p += BASE_OFFSET   
  m.P = p   
  e = vcVector.new()   
  e.X = kinobj.JointValues[3]   
  e.Y = kinobj.JointValues[4]   
  e.Z = kinobj.JointValues[5]   
  m.WPR = e   
  kinobj.Target = m   
  return True
 
# Returns Kinematic chain joint values based on the target (matrix)
def OnInverse(kinobj):   
  m = kinobj.Target   
  p = m.P   
  e = m.WPR   
  kinobj.JointValues = [ p.X-BASE_OFFSET.X, p.Y-BASE_OFFSET.Y, p.Z-BASE_OFFSET.Z, e.X, e.Y, e.Z ]
  return True

Example. Define custom Scara kinematics

from vcPythonKinematics import *
import vcMatrix
import vcVector
from math import *
 
# Define some helpers
DEBUG = False
EPSILON = 0.000001
MAX_DOUBLE = 1e39
PI = atan2(0,-1)
RAD_TO_DEG = 180.0/PI
DEG_TO_RAD = PI/180.0
 
JOINT_COUNT = 4
JOINT_NAMES = ["J1","J2","J3","J4"]
 
CONFIG_COUNT = 2
CONFIG_NAMES = ["LEFTY","RIGHTY"]
 
JS = [1,1,1,1]
JZO = [0.0,0.0,0.0,0.0]
 
LL1 = 0.0
LL2 = 0.0
ZeroHeight = 0.0
 
FirstCall = True
 
def OnRebuild():
  OnFinalize()
 
def OnFinalize():
  global JS, JZO, CONFIG_NAMES, LL1, LL2, ZeroHeight, FirstCall
  FirstCall = False  
  comp = getComponent()
  JS[0] = comp.JointSign1
  JS[1] = comp.JointSign2
  JS[2] = comp.JointSign3
  JS[3] = comp.JointSign4
  JZO[0] = comp.JointZeroOffset1
  JZO[1] = comp.JointZeroOffset2
  JZO[2] = comp.JointZeroOffset3
  JZO[3] = comp.JointZeroOffset4
  CONFIG_NAMES[0] = comp.ConfigName1
  CONFIG_NAMES[1] = comp.ConfigName2
  LL1 = comp.L12.X
  LL2 = comp.L23.X
  ZeroHeight = comp.L01.Z+comp.L12.Z+comp.L23.Z+comp.L34.Z
 
# Returns the indexed configuration names
def GetConfigName(index):
  return CONFIG_NAMES[index]
 
def ANGLE(a):
  remainder = fmod((a)+180, 360)
  if remainder < 0:
    remainder += 360
  return remainder - 180
 
def GetNominalJoints(mappedjoints):
  nominal = [0.0,0.0,0.0,0.0]
  for i in range(JOINT_COUNT):
    if i == 2: # translational joint
      nominal[i] = JS[i]*mappedjoints[i]-JZO[i]
    else: # rotational joint
      nominal[i] = ANGLE(JS[i]*mappedjoints[i]-JZO[i])      
  return nominal
 
def GetMappedJoints(nominaljoints):
  mapped = [0.0,0.0,0.0,0.0]
  for i in range(JOINT_COUNT):
    if i == 2: # translational joint
      mapped[i] = JS[i]*(nominaljoints[i] + JZO[i])
    else: # rotational joint
      mapped[i] = ANGLE(JS[i]*(nominaljoints[i] + JZO[i]))
  return mapped
 
def OnInitKinObject( kinobj ):
  prop = kinobj.createProperty(VC_STRING, "Configuration", VC_PROPERTY_STEP)
  prop.Value = CONFIG_NAMES[0]
  prop.StepValues = CONFIG_NAMES
  
def OnGetJointCount():
  return JOINT_COUNT
 
def OnGetJointName(index):
  return JOINT_NAMES[index]
 
def OnInverse(kinobj):
  if FirstCall:
    OnFinalize()
  solutions = []
  warning = VC_SOLUTION_REACHABLE
  px = kinobj.Target.P.X
  py = kinobj.Target.P.Y
  pz = kinobj.Target.P.Z
  ox = kinobj.Target.O.X   
  oy = kinobj.Target.O.Y   
  # First we calculate the direction to the wrist center
  a13= atan2( py, px )*RAD_TO_DEG
  d13 = sqrt( px*px + py*py )
  if( d13 > LL1 + LL2 ):
    t1 = 0.0
    t2 = 180.0
    warning = VC_SOLUTION_UNREACHABLE
  else:
    if( d13 < abs(LL1 - LL2) ):
      t1 = 180.0
      t2 =   0.0
      warning = VC_SOLUTION_UNREACHABLE
    else:
      t1 = sssa( LL2, LL1,  d13 )*RAD_TO_DEG
      t2 = sssa(  d13, LL1, LL2 )*RAD_TO_DEG
    #end if
  #endif
  j3 = pz - ZeroHeight
  t4 = atan2(-ox, oy)*RAD_TO_DEG
  for iconf in range(CONFIG_COUNT):  
    if iconf == 1:
      sgn1 = -1
    else:
      sgn1 = 1
    #endif
    j1 = a13 + sgn1*t1
    j2 = sgn1*(t2-180.0)
    j4 = t4 - j1 - j2
    solutions.append( [ warning, GetMappedJoints( [j1,j2,j3,j4] ) ] )
  # end for NUM_CONFIGS
  kinobj.Solutions = solutions
  return True
 
def OnForward(kinobj):
  if FirstCall:
    OnFinalize()
  nominal = GetNominalJoints( kinobj.JointValues )
  m04 = vcMatrix.new()
  m04.rotateRelZ( nominal[0] )
  m04.translateRel( LL1, 0.0, 0.0 )
  m04.rotateRelZ( nominal[1] )
  m04.translateRel( LL2, 0.0, nominal[2]+ZeroHeight )
  m04.rotateRelZ( nominal[3] )
  #m04.rotateRelY( 180.0 )
  kinobj.Target = m04
  if kinobj.JointValues[1] <= 0.0:
    kinobj.Configuration = GetConfigName(0)
  else:
    kinobj.Configuration = GetConfigName(1)
  return True