Examples: Tool Action

This page shows how to control Ned’s tools

If you want see more, you can look at PyNiryo - Tools

Important

In this section, it is supposed that you are already connected to a calibrated robot. The robot instance is save in the variable robot

Danger

If you are using the real robot, make sure the environment around it is clear

Tool control

Equip Tool

In order to use a tool, it should be plugged mechanically to the robot but also connected software wise.

To do that, we should use the function update_tool() which take no argument. It will scan motor connections and set the new tool !

The line to equip a new tool is

robot.tool.update_tool()

Note

For the Grasping and Releasing sections, this command should be added in your codes ! If you wan to use a specific tool, you need to store the ToolID you are using in a variable named tool_used

Grasping

To grasp with any tool, you can use the function grasp_with_tool(). This action correspond to :

  • Close gripper for Grippers

  • Pull Air for Vacuum pump

  • Activate for Electromagnet

The line to grasp is

robot.tool.grasp_with_tool()

To grasp an object by specifying the tool

if tool_used in [ToolID.GRIPPER_1, ToolID.GRIPPER_2, ToolID.GRIPPER_3]:
    robot.tool.close_gripper(speed=500)
elif tool_used == ToolID.ELECTROMAGNET_1:
    pin_electromagnet = PinID.XXX
    robot.tool.setup_electromagnet(pin_electromagnet)
    robot.tool.activate_electromagnet(pin_electromagnet)
elif tool_used == ToolID.VACUUM_PUMP_1:
    robot.tool.pull_air_vacuum_pump()

Releasing

To release with any tool, you can use the function release_with_tool(). This action correspond to:

  • Open gripper for Grippers

  • Push Air for Vacuum pump

  • Deactivate for Electromagnet

To release an object by specifying parameters

if tool_used in [ToolID.GRIPPER_1, ToolID.GRIPPER_2, ToolID.GRIPPER_3]:
    robot.tool.open_gripper(speed=500)
elif tool_used == ToolID.ELECTROMAGNET_1:
    pin_electromagnet = PinID.XXX
    robot.tool.setup_electromagnet(pin_electromagnet)
    robot.tool.deactivate_electromagnet(pin_electromagnet)
elif tool_used == ToolID.VACUUM_PUMP_1:
    robot.tool.push_air_vacuum_pump()

Pick & Place with tools

A Pick & Place is a action which consists in going to a certain pose in order to pick an object and then, going to another pose to place it.

This operation can be proceed as follows :

  1. Going over the object with a certain offset to avoid collision

  2. Going down until object’s height

  3. Grasping with tool

  4. Going back to step 1’s pose.

  5. Going over the place pose with a certain offset to avoid collision

  6. Going down until place’s height

  7. Releasing the object with tool

  8. Going back to step 5’s pose.

There is a plenty of ways to perform a pick and place with PyNiryo. Methods will be presented from the lowest to highest level

Code Baseline

For the sake of brevity, every piece of code beside the Pick & Place function won’t be rewritten for every method. So that, you will need to use the code and implement the Pick & Place function to it

# Imports
from pyniryo2 import *

tool_used = ToolID.XXX  # Tool used for picking
robot_ip_address = "x.x.x.x" # Robot address

# The pick pose
pick_pose = PoseObject(
    x=0.25, y=0., z=0.15,
    roll=-0.0, pitch=1.57, yaw=0.0,
)
# The Place pose
place_pose = PoseObject(
    x=0.0, y=-0.25, z=0.1,
    roll=0.0, pitch=1.57, yaw=-1.57)

def pick_n_place_version_x(robot):
    # -- -------------- -- #
    # -- CODE GOES HERE -- #
    # -- -------------- -- #

if __name__ == '__main__':
    # Connect to robot
    client = NiryoRobot(robot_ip_address)
    # Calibrate robot if robot needs calibration
    client.arm.calibrate_auto()
    # Changing tool
    client.arm.update_tool()

    pick_n_place_version_x(client)

    # Releasing connection
    client.end()

First Solution : the heaviest

For this first function, everything steps is done by hand, as well as poses computing

Note

In this example, the tool used is a Gripper. If you want to use another tool than a gripper, do not forget to adapt grasp & release functions !

def pick_n_place_version_1(robot):
    height_offset = 0.05  # Offset according to Z-Axis to go over pick & place poses
    gripper_speed = 400

    # Going Over Object
    robot.arm.move_pose(pick_pose.x, pick_pose.y, pick_pose.z + height_offset,
                               pick_pose.roll, pick_pose.pitch, pick_pose.yaw)
    # Opening Gripper
    robot.tool.open_gripper(gripper_speed)
    # Going to picking place and closing gripper
    robot.arm.move_pose(pick_pose)
    robot.tool.close_gripper(gripper_speed)

    # Raising
    robot.arm.move_pose(pick_pose.x, pick_pose.y, pick_pose.z + height_offset,
                               pick_pose.roll, pick_pose.pitch, pick_pose.yaw)

    # Going Over Place pose
    robot.arm.move_pose(place_pose.x, place_pose.y, place_pose.z + height_offset,
                               place_pose.roll, place_pose.pitch, place_pose.yaw)
    # Going to Place pose
    robot.arm.move_pose(place_pose)
    # Opening Gripper
    robot.tool.open_gripper(gripper_speed)
    # Raising
    robot.arm.move_pose(place_pose.x, place_pose.y, place_pose.z + height_offset,
                               place_pose.roll, place_pose.pitch, place_pose.yaw)

Second Solution : Use of PoseObject

For the second solution, we use a PoseObject in order to calculate approach poses more easily

Note

To see more about PoseObject, go look at PoseObject dedicated section

def pick_n_place_version_2(robot):
    height_offset = 0.05  # Offset according to Z-Axis to go over pick & place poses

    pick_pose_high = pick_pose.copy_with_offsets(z_offset=height_offset)
    place_pose_high = place_pose.copy_with_offsets(z_offset=height_offset)

    # Going Over Object
    robot.arm.move_pose(pick_pose_high)
    # Opening Gripper
    robot.tool.release_with_tool()
    # Going to picking place and closing gripper
    robot.arm.move_pose(pick_pose)
    robot.tool.grasp_with_tool()
    # Raising
    robot.arm.move_pose(pick_pose_high)

    # Going Over Place pose
    robot.arm.move_pose(place_pose_high)
    # Going to Place pose
    robot.arm.move_pose(place_pose)
    # Opening Gripper
    robot.tool.release_with_tool(gripper_speed)
    # Raising
    robot.arm.move_pose(place_pose_high)

Third Solution : Pick from pose & Place from pose functions

For those who already look at the API Documentation, you may have see pick & place dedicated functions !

In this example, we use pick_from_pose() and place_from_pose() in order to split our function in only 2 commands !

def pick_n_place_version_3(robot):
    # Pick
    robot.pick_place.pick_from_pose(pick_pose)
    # Place
    robot.pick_place.place_from_pose(place_pose)

Fourth Solution : All in one

The example exposed in the previous section could be useful if you want to do an action between the pick & the place phases.

For those who want to do everything in one command, you can use the pick_and_place() function !

def pick_n_place_version_4(robot):
    # Pick & Place
    robot.pick_place.pick_and_place(pick_pose, place_pose, dist_smoothing=0.01)