Arm

This file presents the different Arm - Command functions, Arm - Enums, Arm - Niryo Topics & Arm - Objects available with the Arm API

Arm - Command functions

This section reference all existing functions to control your robot arm, which include

  • Getting the robot state

  • Moving the arm

  • Getting inverse and forward kinematics

  • Calibrating the robot

All functions to control the robot are accessible via an instance of the class NiryoRobot

robot = NiryoRobot(<robot_ip_address>)

robot.arm.calibrate_auto()
robot.arm.move_joints([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
...

See examples on Examples Section

List of functions subsections:

class Arm(client)[source]

Arm robot functions

Example:

ros_instance = NiryoRos("10.10.10.10") # Hotspot
arm_interface = Arm(ros_instance)
Parameters

client (NiryoRos) – Niryo ROS client

Calibration functions

Arm.calibrate(calibrate_mode, callback=None, errback=None, timeout=None)[source]

Calibrates (manually or automatically) motors. Automatic calibration will do nothing if motors are already calibrated

Examples:

# Synchronous use
arm.calibrate(CalibrateMode.MANUAL)
arm.calibrate(CalibrateMode.AUTO)

# Asynchronous use
def calibration_callback(result):
    if result["status"] < RobotErrors.SUCCESS.value:
        print("Calibration failed")
    else:
        print("Calibration completed with success")

arm.calibrate(CalibrateMode.AUTO, calibration_callback)
Parameters
  • calibrate_mode (CalibrateMode) – Auto or Manual

  • callback (function) – Callback invoked on successful execution.

  • errback (function) – Callback invoked on error.

  • timeout (float) – Timeout for the operation, in seconds. Only used if blocking.

Return type

None

Arm.calibrate_auto(callback=None, errback=None, timeout=None)[source]

Starts a automatic motors calibration if motors are not calibrated yet.

Examples:

# Synchronous use
arm.calibrate_auto()

# Asynchronous use
def calibration_callback(result):
    if result["status"] < RobotErrors.SUCCESS.value:
        print("Calibration failed")
    else:
        print("Calibration completed with success")

arm.calibrate_auto(calibration_callback)
Parameters
  • callback (function) – Callback invoked on successful execution.

  • errback (function) – Callback invoked on error.

  • timeout (float) – Timeout for the operation, in seconds. Only used if blocking.

Return type

None

Arm.request_new_calibration(callback=None, errback=None, timeout=None)[source]

Starts a automatic motors calibration even if motors are calibrated yet.

Examples:

# Synchronous use
arm.request_new_calibration()

# Asynchronous use
def calibration_callback(result):
    if result["status"] < RobotErrors.SUCCESS.value:
        print("Calibration failed")
    else:
        print("Calibration completed with success")

arm.request_new_calibration(calibration_callback)
Parameters
  • callback (function) – Callback invoked on successful execution.

  • errback (function) – Callback invoked on error.

  • timeout (float) – Timeout for the operation, in seconds. Only used if blocking.

Return type

None

Arm.reset_calibration(timeout=2)[source]

Resets current calibration status. A new calibration is then necessary.

Parameters

timeout (float) – Timeout for the operation, in seconds.

Return type

None

Arm.need_calibration()[source]

Returns a bool indicating whereas the robot motors need to be calibrate.

Returns

True if calibration is needed, False otherwise.

Return type

bool

Robot move functions

Arm.set_arm_max_velocity(percentage_speed)[source]

Limit arm max velocity to a percentage of its maximum velocity

Parameters

percentage_speed (int) – Should be between 1 & 100

Return type

None

Arm.go_to_sleep()[source]

Go to home pose and activate learning mode

Return type

None

Arm.stop_move(callback=None, errback=None, timeout=None)[source]

Stop a current execution of move_pose, move_joint or move_linear_pose. The robot will stop at its current position . If a callback function is not passed in parameter, the function will be blocking. Otherwise, the callback will be called when the execution of the function is finished.

Examples:

# Synchronous use
arm.stop_move()

# Asynchronous use
def stop_callback(result):
    if result["status"] < RobotErrors.SUCCESS.value:
        print("Succeeded")
    else:
        print("Failed")

arm.stop_move(stop_callback)
Parameters
  • callback (function) – Callback invoked on successful execution.

  • errback (function) – Callback invoked on error.

  • timeout (float) – Timeout for the operation, in seconds. Only used if blocking.

Return type

None

Arm.move_to_home_pose(callback=None)[source]

Move to a position where the forearm lays on shoulder If a callback function is not passed in parameter, the function will be blocking. Otherwise, the callback will be called when the execution of the function is finished.

Parameters

callback (function) – Callback invoked on successful execution.

Return type

None

Arm.move_joints(joints, callback=None)[source]

Move robot joints. Joints are expressed in radians. If a callback function is not passed in parameter, the function will be blocking. Otherwise, the callback will be called when the execution of the function is finished.

All lines of the next example realize the same operation:

arm.joints = [0.2, 0.1, 0.3, 0.0, 0.5, 0.0]
arm.move_joints([0.2, 0.1, 0.3, 0.0, 0.5, 0.0])

def move_callback(_):
    print("Move completed")

arm.move_joints([0chronous use
arm.calibrate(CalibrateMode.MANUAL)
arm.calibrate(CalibrateMode.AUTO)

# Asynchronous use
def calibration_callback(result):
    if result["status"] < RobotErrors.SUCCESS.value:
        print("Calibration failed")
    else:
        print("Calibration completed with success")

arm.calibrate(CalibrateMode.AUTO, calibration_callback)
Parameters
  • callback (function) – Callback invoked on successful execution.

  • joints (Union[list[float], tuple[float]]) – a list of 6 joints

Return type

None

Arm.move_pose(pose, frame='', callback=None)[source]

Move robot end effector pose to a (x, y, z, roll, pitch, yaw) pose in the frame (frame_name) if defined. x, y & z are expressed in meters / roll, pitch & yaw are expressed in radians If a callback function is not passed in parameter, the function will be blocking. Otherwise, the callback will be called when the execution of the function is finished.

All lines of the next example realize the same operation:

arm.pose = [0.2, 0.1, 0.3, 0.0, 0.5, 0.0]
arm.move_pose([0.2, 0.1, 0.3, 0.0, 0.5, 0.0])
arm.move_pose(PoseObject(0.2, 0.1, 0.3, 0.0, 0.5, 0.0))
arm.move_pose([0.2, 0.1, 0.3, 0.0, 0.5, 0.0], "default_frame")
arm.move_pose(PoseObject(0.2, 0.1, 0.3, 0.0, 0.5, 0.0), "default_frame")

def move_callback(_):
    print("Move completed")

arm.move_joints([0.2, 0.1, 0.3, 0.0, 0.5, 0.0], callback=move_callback)
Parameters
  • callback (function) – Callback invoked on successful execution.

  • pose (Union[tuple[float], list[float], PoseObject]) – either a list of 6 coordinates or a PoseObject

  • frame – name of the frame

Return type

None

Arm.move_linear_pose(pose, frame='', callback=None)[source]

Move robot end effector pose to a (x, y, z, roll, pitch, yaw) pose in a linear way, in the frame (frame_name) if defined. If a callback function is not passed in parameter, the function will be blocking. Otherwise, the callback will be called when the execution of the function is finished.

All lines of the next example realize the same operation:

arm.move_linear_pose([0.2, 0.1, 0.3, 0.0, 0.5, 0.0])
arm.move_linear_pose(PoseObject(0.2, 0.1, 0.3, 0.0, 0.5, 0.0))
arm.move_linear_pose([0.2, 0.1, 0.3, 0.0, 0.5, 0.0], "default_frame")
arm.move_linear_pose(PoseObject(0.2, 0.1, 0.3, 0.0, 0.5, 0.0), "default_frame")

def move_callback(_):
    print("Move completed")

arm.move_linear_pose([0.2, 0.1, 0.3, 0.0, 0.5, 0.0], callback=move_callback)
Parameters
  • callback (function) – Callback invoked on successful execution.

  • pose (Union[tuple[float], list[float], PoseObject]) – either or a list of 6 coordinates or a PoseObject

  • frame – name of the frame

Return type

None

Arm.shift_pose(axis, shift_value, callback=None)[source]

Shift robot end effector pose along one axis If a callback function is not passed in parameter, the function will be blocking. Otherwise, the callback will be called when the execution of the function is finished.

Examples:

self.arm.shift_pose(RobotAxis.X, 0.05)
self.arm.shift_pose(RobotAxis.Y, -0.05)
self.arm.shift_pose(RobotAxis.Z, 0.1)
self.arm.shift_pose(RobotAxis.ROLL, 1.57)
self.arm.shift_pose(RobotAxis.PITCH, -1.57)
self.arm.shift_pose(RobotAxis.YAW, 0.78)

def move_callback(_):
    print("Move completed")

self.arm.shift_pose(RobotAxis.X, 0.1, move_callback)
Parameters
  • axis (RobotAxis) – Axis along which the robot is shifted

  • shift_value (float) – In meter for X/Y/Z and radians for roll/pitch/yaw

  • callback (function) – Callback invoked on successful execution.

Return type

None

Arm.move_relative(offset, frame='world')[source]

Move robot end of a offset in a frame

Example:

arm.move_relative([0.05, 0.05, 0.05, 0.3, 0, 0], "default_frame")
Parameters
  • offset (list[float]) – list which contains offset of x, y, z, roll, pitch, yaw

  • frame (str) – name of local frame

Returns

status, message

Return type

(int, str)

Arm.move_linear_relative(offset, frame='world')[source]

Move robot end of a offset by a linear movement in a frame

Example:

arm.move_linear_relative([0.05, 0.05, 0.05, 0.3, 0, 0], "default_frame")
Parameters
  • offset (list[float]) – list which contains offset of x, y, z, roll, pitch, yaw

  • frame (str) – name of local frame

Returns

status, message

Return type

(int, str)

Arm.set_jog_control(enabled)[source]

Set jog control mode if param is True, else turn it off

Parameters

enabled (Bool) – True or False

Return type

None

Arm.jog_joints(joints_offset, callback=None, errback=None, timeout=None)[source]

Jog robot joints’. Jog corresponds to a shift without motion planning. Values are expressed in radians. If a callback function is not passed in parameter, the function will be blocking. Otherwise, the callback will be called when the execution of the function is finished.

Examples:

arm.jog_joints([0.1, 0.0, 0.5, 0.0, 0.0, -1.57])

def jog_callback(_):
    print("Jog completed")
    arm.set_jog_control(False) # Disable Jog interface

arm.jog_joints([0.1, 0.0, 0.5, 0.0, 0.0, -1.57], jog_callback)
Parameters
  • joints_offset (Union[list[float], tuple[float]]) – a list of 6 joints offset

  • callback (function) – Callback invoked on successful execution.

  • errback (function) – Callback invoked on error.

  • timeout (float) – Timeout for the operation, in seconds. Only used if blocking.

Return type

None

Arm.jog_pose(pose_offset, callback=None, errback=None, timeout=None)[source]

Jog robot end effector pose Jog corresponds to a shift without motion planning Arguments are [dx, dy, dz, d_roll, d_pitch, d_yaw] dx, dy & dz are expressed in meters / d_roll, d_pitch & d_yaw are expressed in radians If a callback function is not passed in parameter, the function will be blocking. Otherwise, the callback will be called when the execution of the function is finished.

Examples:

arm.jog_pose([0.01, 0.0, 0.05, 0.0, 0.0, -1.57])

def jog_callback(_):
    print("Jog completed")
    arm.set_jog_control(False) # Disable Jog interface

arm.jog_pose([0.1, 0.0, 0.5, 0.0, 0.0, -1.57], jog_callback)
Parameters
  • pose_offset (Union[list[float], tuple[float]]) – a list of 6 offset

  • callback (function) – Callback invoked on successful execution.

  • errback (function) – Callback invoked on error.

  • timeout (float) – Timeout for the operation, in seconds. Only used if blocking.

Return type

None

Robot status functions

property Arm.hardware_status

Returns the hardware state client which can be used synchronously or asynchronously to obtain the hardware state.

Examples:

# Get last value
arm.hardware_status()
arm.hardware_status.value

# Subscribe a callback
def hs_callback(msg):
    print msg.voltage

arm.hardware_status.subscribe(hs_callback)
arm.hardware_status.unsubscribe()
Returns

hardware state topic instance

Return type

NiryoTopic

property Arm.joints_state

Get the joints state topic which can be used synchronously or asynchronously to obtain the joints state. The joints state topic returns a JointStateObject.

It can be used as follows::

# Get last joint state
joint_state = arm.joints_state()
joint_state = arm.joints_state.value

joint_names = arm.joints_state().name
joint_positions = arm.joints_state().position
joint_velocities = arm.joints_state.value.velocity

# Raise a callback at each new value
from __future__ import print_function

arm.joints_state.subscribe(lambda message: print(message.position))
arm.joints_state.unsubscribe()
Returns

Joint states topic.

Return type

NiryoTopic

Arm.get_joints()[source]

Get joints value in radians You can also use a getter

joints = arm.get_joints()
joints = arm.joints
Returns

List of joints value

Return type

list[float]

property Arm.joints

Get joints value in radians

Returns

List of joints value

Return type

list[float]

property Arm.pose

Get end effector link pose as [x, y, z, roll, pitch, yaw]. x, y & z are expressed in meters / roll, pitch & yaw are expressed in radians

You can also use a getter

pose = arm.pose
pose_list = arm.pose.to_list()
x, y, z, roll, pitch, yaw = arm.pose.to_list()
Returns

end effector link pose

Return type

PoseObject

property Arm.get_pose

Get the end effector link pose topic which can be used synchronously or asynchronously to obtain the end effector link pose. The joints state topic returns a PoseObject. x, y & z are expressed in meters / roll, pitch & yaw are expressed in radians

See below some usage

pose = arm.get_pose()
pose = arm.get_pose.value
pose_list = arm.get_pose().to_list()
x, y, z, roll, pitch, yaw = arm.get_pose().to_list()

arm.get_pose.subscribe(callback)
arm.get_pose.unsubscribe()
Returns

end effector link pose topic

Return type

NiryoTopic

Arm.get_pose_quat()[source]

Get end effector link pose in Quaternion coordinates

Returns

Position and quaternion coordinates concatenated in a list : [x, y, z, qx, qy, qz, qw]

Return type

list[float]

Learning mode functions

property Arm.learning_mode

Returns the learning mode client which can be used synchronously or asynchronously to obtain the learning mode state. The learning mode client returns a boolean value.

Examples:

# Get last value
arm.learning_mode()
if arm.learning_mode.value:
    print("Learning mode enabled"))

# Subscribe a callback
def lm_callback(is_learning_mode_enabled):
    print is_learning_mode_enabled

arm.learning_mode.subscribe(lm_callback)
arm.learning_mode.unsubscribe()
Returns

learning mode state topic instance

Return type

NiryoTopic

Arm.get_learning_mode()[source]

Get learning mode state.

Returns

True if learning mode is on

Return type

bool

Arm.set_learning_mode(enabled)[source]

Set learning mode if param is True, else turn it off

Parameters

enabled (bool) – True or False

Return type

None

Kinematics functions

Arm.forward_kinematics(*args)[source]

Compute forward kinematics of a given joints configuration and give the associated spatial pose

Examples:

pose_obj = arm.forward_kinematics(1.57, 0.0, 0.0, 0.78, 0.0, -1.57)
pose_obj = arm.forward_kinematics([1.57, 0.0, 0.0, 0.78, 0.0, -1.57])
Parameters

args (Union[list[float], tuple[float]]) – either 6 args (1 for each joints) or a list of 6 joints

Return type

PoseObject

Arm.inverse_kinematics(*args)[source]

Compute inverse kinematics

Examples:

joint_list = arm.inverse_kinematics(0.2, 0.0, 0.3, 0.0, 1.57, 0.0)
joint_list = arm.inverse_kinematics([0.2, 0.0, 0.3, 0.0, 1.57, 0.0])
joint_list = arm.inverse_kinematics(PoseObject(0.2, 0.0, 0.3, 0.0, 1.57, 0.0))
Parameters

args (Union[tuple[float], list[float], PoseObject]) – either 6 args (1 for each coordinates) or a list of 6 coordinates or a PoseObject

Returns

List of joints value

Return type

list[float]

Arm - Niryo Topics

The use of these functions is explained in the NiryoTopic section. They allow the acquisition of data in real time by callbacks or by direct call.

Arm’s Niryo Topics

Name

Function

Return type

/joint_states

joints_state

JointStateObject

/niryo_robot/robot_state

get_pose

PoseObject

/niryo_robot_hardware_interface/hardware_status

hardware_status

HardwareStatusObject

/niryo_robot/learning_mode/state

learning_mode

bool

/niryo_robot/max_velocity_scaling_factor

get_arm_max_velocity

float

Arm - Enums

List of enums:

  • CalibrateMode

  • RobotAxis

  • JogShift

class CalibrateMode(value)[source]

Enumeration of Calibration Modes

AUTO = 1
MANUAL = 2
class RobotAxis(value)[source]

Enumeration of Robot Axis : it used for Shift command

X = 0
Y = 1
Z = 2
ROLL = 3
PITCH = 4
YAW = 5
class JogShift(value)[source]

Enumeration of Jog Shift : it used for Jog commands

JOINTS_SHIFT = 1
POSE_SHIFT = 2

Arm - Objects

  • HardwareStatusObject

class HardwareStatusObject[source]

Object used to store every hardware information

Variables
  • rpi_temperature (float) – Number representing the rpi temperature

  • hardware_version (str) – Number representing the hardware version

  • connection_up (bool) – Boolean indicating if the connection with the robot is up

  • error_message (str) – Error message status on error

  • calibration_needed (bool) – Boolean indicating if a calibration is needed

  • calibration_in_progress (bool) – Boolean indicating if calibration is in progress

  • motor_names (list[str]) – List of motor names

  • motor_types (list[str]) – List of motor types

  • motors_temperature (list[float]) – List of motors_temperature

  • motors_voltage (list[float]) – List of motors_voltage

  • hardware_errors (list[int]) – List of hardware errors

  • hardware_error_messages (list[str]) – List of hardware error messages

  • JointStateObject

class JointStateObject[source]

Object used to store every joint state information

Variables
  • name (list[str]) – List of joint names

  • position (list[float]) – List of joint positions

  • velocity (list[float]) – List of joint velocities

  • effort (list[float]) – List of joint efforts

class PoseObject(x, y, z, roll, pitch, yaw)[source]

Pose object which stores x, y, z, roll, pitch & yaw parameters

Variables
  • x (float) – X (meter)

  • y (float) – Y (meter)

  • z (float) – Z (meter)

  • roll (float) – Roll (radian)

  • pitch (float) – Pitch (radian)

  • yaw (float) – Yaw (radian)