[Bram van Asseldonk]

Hi there,

I'm trying to safely move an ABB robot arm to various targets using the python API. Before every movement, I test whether or not the target is in reach and does not cause collisions by running the Move*_Test method first. 

So far I've managed to find out that if there is a collision, the robot joints will be set to the position in which collision was detected. This was not necessarily what I had expected, but I can see why this would be useful.

However, if no collision is detected, and I send the Move* command for the same target, the movement of the robot seems to start from the robots Home Pose.

Of course I can simply save the robots joint positions before running the Move*_Test method and reset them afterwards, but I don't see anything like this happening in the examples. Which makes me wonder if I'm doing something wrong.

I'm doing the following:

Code:

   def move_to_pose(self, pose, frame="ROBOT_BASE_FRAME"):
       """
       Moves the robot arm to a specified pose (px, py, pz, rx, ry, rz).
       The move is only executed if it can be done without causing collisions.

       Arguments
       ---------
       pose : list(float,) or tuple(float,)
           List or tuple of floats representing the position and rotation of
           the target pose.
       frame : str, optional
           Reference frame with respect to which the pose should be placed.
           Default is 'ROBOT_BASE_FRAME'.

       Returns
       -------
       moved : bool
           Whether the arm was succesfuly moved to the specified pose.
       """
       # Convert pose from degrees to radians
       pose = [p if i < 3 else math.radians(p) for i, p in enumerate(pose)]
       # Add a new target to RoboDK relative to frame.
       target = self.RDK.AddTarget('TARGET', self.RDK.Item(frame, ITEM_TYPE_FRAME), self.robot)
       target.setAsCartesianTarget()
       target.setPose(TxyzRxyz_2_Pose(pose))
       # Ensure that target is stable with respect to robot base frame.
       if frame != 'ROBOT_BASE_FRAME':
           target.setParentStatic(self.RDK.Item('ROBOT_BASE_FRAME'))
       # Wait for user to validate target
       input('Target created, press enter to continue...')
       # Convert pose to joint instructions.
       joints = self.robot.SolveIK(target.Pose(),
                                   tool=self.robot.PoseTool(),
                                   reference=self.robot.PoseFrame()
                                  )
       # Remove target from RoboDK.
       target.Delete()
       # Execute movement.
       return self.move_to_joints(joints.list())

   def move_to_joints(self, joints):
       """
       Moves the robot arm to specified joint positions.
       The move is only executed if it can be done without causing collisions.

       Arguments
       ---------
       joints : list(float,)
           List of floats representing the target positions of the robots joints.

       Returns
       -------
       moved : bool
           Whether the arm was succesfully moved to the specified pose.
       """
       current = self.robot.Joints()
       # Check if movement causes any collisions.
       if len(joints) != len(current.list()) or self.robot.MoveJ_Test(current, joints, 1) > 0:
           print('Robot cannot reach target because it is out of reach or causes collision.')
           self.robot.Stop()
           self.robot.setJoints(current)
           return False
       # Execute movement
       self.robot.MoveJ(joints)
       return True

Is there a way to test the feasibility of movement without it changing the robots position?

Thanks in advance!

With kind regards,

Bram

[Ruud]

Very interested in this as well!

[Albert]

Hi Bram,

I understand what you mean and I understand that the behavior can be misleading when you simulate the movement. 

It is better if you do something like this to properly check the feasibility and simulate the move afterwards:

Code:

# Turn off rendering while doing the calculations (faster and it avoid the robot to be displayed in the final point):
RDK.Render(False)

# Check if movement causes any collisions.
if robot.MoveJ_Test(joints_from, joints_to, 1) > 0:
    print('The robot cannot reach target because it is out of reach or causes collision.')
    robot.Stop()
    robot.setJoints(joints_from)
    RDK.Render(True)
    return False

# Set the robot to the last known position:
robot.setJoints(joints_from)
RDK.Render(True)

# Execute movement
robot.MoveJ(joints)

return True

Some notes:

1. setJoints just sets the robot position in the simulation, not a real robot or a robot program if your goal is to generate a program.
   
2. Using Render is not required but it will prevent you from seing the robot jump unexpectedly.