Examples: Vision

This document shows how to use Ned robot’s Vision Set.

If you want see more about Ned robot’s vision package, go to Niryo robot vision

Prerequisites

In order to follow this example, you first need to create a workspace. To do so, you can follow this tutorial.

Simple Vision pick

This short example shows you how to perform your first vision pick with a vision kit shape (square or circle) placed on your workspace:

 1#!/usr/bin/env python3
 2
 3from niryo_robot_python_ros_wrapper import NiryoRosWrapper, Pose, ObjectShape, ObjectColor
 4
 5import math
 6
 7# - Constants
 8
 9# Robot's Workspace Name, replace by your own workspace name
10WORKSPACE_NAME = "workspace_1"
11# The observation pose
12OBSERVATION_POSE = Pose(0.18, 0.0, 0.35, math.pi, 0.0, 0.0)
13# The Place pose
14PLACE_POSE = Pose(0.0, -0.25, 0.1, math.pi, 0.0, -math.pi / 2)
15
16# Instantiate the ROS wrapper and initialize the ROS node
17robot = NiryoRosWrapper.init_with_node()
18
19# Calibrate if needed
20robot.calibrate_auto()  # Only for Ned2
21
22# Setup the tool
23robot.update_tool()
24
25# Move to observation pose
26robot.move(OBSERVATION_POSE)
27
28# Perform a vision pick
29result = robot.vision_pick(WORKSPACE_NAME, height_offset=0.0, shape=ObjectShape.ANY, color=ObjectColor.ANY)
30object_found, shape, color = result
31
32if object_found:
33    print("Object found, with shape: {}, and color: {}".format(shape, color))
34    robot.place(PLACE_POSE)
35else:
36    print("No object found")