Collision detection in addon

Extensions and Python Add-ons
1

Hi!

When I try to run my add-on, even if I just use the vcHelpers.Robot2.getRobot()function, the graphical interface for simulation has no affect on the robot i used it on.

To use collision detection despite this, I tried to do the following.

from vcCommand import *
from vcHelpers.Application import *
from vcHelpers.Robot2 import *

app = getApplication()
cmd = getCommand()
simu = getSimulation()

DIALOG_NAME = 'Collision'

move_btn=None

def HandleCollision(args):
    print('collision')
    print(args)

def HandleOnPosition(args):
    print('on position')
    print(args)

def Collision(robot):
    robot = next((c for c in app.Components if c.Name == robot), None)
    robot_obj = getRobot(robot)
    controller = robot_obj.Controller
    executor = robot_obj.Executor

    controller.OnPosition = HandleOnPosition

    POINTS = []
    for statement in robot_obj.Program.MainRoutine.Statements:
        for point in statement.Positions:
           POINTS.append(
               {
                "name": point.Name,
                "x": point.PositionInWorld.P.X,
                "y": point.PositionInWorld.P.Y,
                "z": point.PositionInWorld.P.Z,
                "rx": point.PositionInWorld.WPR.X,
                "ry": point.PositionInWorld.WPR.Y,
                "rz": point.PositionInWorld.WPR.Z,
                "configuration": point.Configuration,
                "mode": statement.Type
                }
            )
           
    print(POINTS)
    if len(POINTS) == 0:
        print('Kérlek adj meg célhelyzeteket')
        return

    controller.clearTargets()
    controller.move()
    
    for point in POINTS:
        target = controller.createTarget()
        if point['mode'] == 'PtpMotion':
            target.MotionType =VC_MOTIONTARGET_MT_JOINT
        else:
            target.MotionType = VC_MOTIONTARGET_MT_LINEAR

        config_names = getConfigNames(executor, controller)
        wanted_config = point['configuration']
        
        config_index = config_names.index(wanted_config)
        target.RobotConfig = config_index
        robot_inverse = robot.WorldPositionMatrix
        robot_inverse.invert()
        target_matrix = vcMatrix.new()
        target_matrix.translateRel( point['x'], point['y'], point['z'])
        target_matrix.setWPR(point['rx'], point['ry'], point['rz'])            
        target.Target = robot_inverse * target_matrix
        #controller.moveTo(target)

    print('finished')
    #controller.move()
    print('collision stuff')
    detector = simu.newCollisionDetector()
    detector.NodeListA = [(robot, VC_NODELIST_INCLUDE, VC_NODELIST_TREE )]
    detector.NodeListB = [(simu.World, VC_NODELIST_INCLUDE, VC_NODELIST_TREE ),(robot,VC_NODELIST_EXCLUDE, VC_NODELIST_TREE )]
    detector.Active
    print('detector set')
    simu.OnCollision = HandleCollision
    print('handler set')

    #controller.move()
    simu.reset()
    simu.run()
    simu.update()
    app.render()

def OnStart():
    global move_btn 
    dlg = getDialog(DIALOG_NAME)

    robots = [c for c in app.Components if c.Category == 'Robots']

    dlg = getDialog(DIALOG_NAME)

    robot_name = dlg.addProperty(
        '::Select robot:', VC_STRING, None, VC_PROPERTY_STEP)
    robot_name.StepValues = [r.Name for r in robots]

    move_btn = dlg.addProperty('::Move', VC_BUTTON, Collision)
    move_btn.OnChanged = lambda _: Collision(robot_name.Value)

    dlg.showTabs()
    dlg.show()

When the execution gets to the commented out controller.move() or controller.moveTo() it throws a ReferenceError: Events cannot be registered in this scope. error, but earlier in this code, it works fine.

Is there a better way to do this?

Tom

2

Hey,
“Events cannot be registered in this scope” means that these methods can’t be called from that scope. That’s because the .move() and .moveTo() methods are time-consuming, so the valid place to call them would be in OnRun of a component script. So calling them from a command is not possible.

Do you want to run a continuous collision check for the program (so that collisions would be detected in all positions) or just validate that the target points in each statement are collision-free? If the latter is the case, please check the example script below on how to set robot joint values in immediate manner (without consuming time).

from vcScript import *

# This script assumes that the component has a robot controller and a matrix property Matrix_1 for the target 
comp = getComponent()
rc = comp.findBehavioursByType(VC_ROBOTCONTROLLER)[0]

def on_changed(mat):
  target = rc.createTarget(mat.Value)
  # You get configuration as a string from vcPositionStatement.Positions[index].Configuration.
  # Check vcHelpers.Robot2 script on how config's index is acquired from config's name.
  target.RobotConfig = 0
  jointvalues = target.JointValues
  for i, joint in enumerate(rc.Joints):
    jname = joint.Name
    comp.getProperty(jname).Value = jointvalues[i]

mat = comp.getProperty("Matrix_1")
mat.OnChanged = on_changed