Control Ned/Ned2 with a Keyboard or a Joystick through a ROS Node

V.1.1

Control Ned with keyboard or joystick

Difficulty: medium

Time: ~25 min

Note

This tutorial is working from:
The version v3.2.0 of the ned_ros_stack

The goal of this tutorial is to create a Python ROS node that will listen to the keyboard or to a Joystick Controller and move Ned/Ned2’s arm accordingly. What we’ll explain in this tutorial:

  • Setup: Quick reminder of ROS Multiple Machine setup

  • Introduction to the Jog Interface: How to implement a quick demonstration using the jog controller

  • Use the jog controller to control Ned with the keyboard

  • Use the jog controller to control Ned with a joystick controller

  • Control Ned/Ned2 directly through the Hardware Interface.

Requirements

In order to follow this tutorial, you will have to:

  • Use Ned/Ned2 and a computer.

  • Have a Joystick controller if you want to control Ned/Ned2 with it.

  • Set up the ROS system to work across Ned/Ned2 and your computer, in a multiple machines system (for more information, follow the Setup ROS multi-machines for Ned/Ned2 tutorial).

  • Be able to connect via ssh to Ned/Ned2 (for more information, follow the Connect to Ned/Ned2 via SSH tutorial).

The setup part will explain how to connect in MultiMachines and how to connect to Ned/Ned2 via SSH, but if you are facing issues, please refer to the mentioned tutorials.

Note

In this tutorial, we explain briefly the codes used to control Ned with a ROS node. Those codes are in a package ros_node_control, that you can find on our github repository: here.

Setup

SSH connection:

First, start Ned/Ned2 and make sure your computer can connect to it through ssh. In a terminal, you can connect through hotspot with:

ssh niryo@10.10.10.10

Or through a static IP address, for instance:

ssh niryo@192.168.1.52

Once you are connected, stop the current background execution of the ROS stack with:

sudo service niryo_robot_ros stop

Multi Machines setup:

Now, setup your ROS_MASTER_URI and ROS_IP environment variables on both your computer and Ned/Ned2. The master machine will be Ned/Ned2. Let’s assume you are connected to Ned/Ned2 with its static IP address, which is 192.168.1.52.

  • On your computer, you can check the current values of those variables with:

    printenv | grep ROS_MASTER_URI
    printenv | grep ROS_IP
    
  • You’ll have to define the ROS_MASTER_URI variable so that it refers to Ned IP address, as:

    export ROS_MASTER_URI=http://192.168.1.52:11311
    
  • You also have to define your computer IP address, (that you can find with the ifconfig command line) as:

    export ROS_IP=192.168.1.7
    
  • On Ned/Ned2 through SSH, you can also check the current values of those variables with:

    printenv | grep ROS_MASTER_URI
    printenv | grep ROS_IP
    
  • Define the ROS_MASTER_URI:

    export ROS_MASTER_URI=http://192.168.1.52:11311
    
  • Define the ROS_IP:

    export ROS_IP=192.168.1.52
    

You can then start a new roscore by launching the Ned/Ned2 ROS stack on the robot (in a terminal connected to Ned/Ned2 via ssh):

roslaunch niryo_robot_bringup niryo_ned_robot.launch

Now, you can subscribe and publish to topics running on Ned/Ned2 with your computer.

Note

You can check that the Multimachine setup is well working with the following command on your computer:

rostopic echo /niryo_robot/learning_mode/state

You’ll see the current learning mode state, being updated regularly.

Use basics functionalities of the Jog Interface with a ROS Node

The Jog Interface uses the jog_controller class (from the niryo_robot_commander package), that allows to move the arm given a desired shifting value, without using motion planning.

First, we create a node that simply uses the Jog Interface, and execute the same movements in a loop. On your computer, create a catkin workspace, put our package in the /src (find the package on our github) and do a

catkin_make
source devel/setup.bash

We’ll use here a python ROS Node named ‘control_node_jog.py’. To launch it, use:

rosrun ros_node_control control_node_jog.py

Code explanation :

Imports:

Here are the imports lines we need:

#!/usr/bin/env python

import rospy

from niryo_robot_msgs.msg import CommandStatus
from std_msgs.msg import Bool

from niryo_robot_commander.srv import JogShift, JogShiftRequest
from niryo_robot_msgs.srv import SetBool

Class definition:

We use a class named ‘JogClient’, that will, among other things, use the ‘/niryo_robot/jog_interface/jog_shift_commander’ service.

class JogClient:
    def __init__(self):
        rospy.init_node('jog_client_node', anonymous=True)

        self.__jog_enabled = False
        self.__jog_enabled_subscriber = rospy.Subscriber('/niryo_robot/jog_interface/is_enabled',
                                                        Bool, self.__callback_subscriber_jog_enabled,
                                                        queue_size=1)
        self.current_learning_mode = True
        rospy.Subscriber('/niryo_robot/learning_mode/state', Bool, self.update_learning_state)

        self.check_calibration()
        if self.current_learning_mode == True:
            self.set_learning_mode(False)

Note

In order to use the services and functions provided by the Jog Controller to move the arm, the learning mode must be deactivated, otherwise it won’t work.

Here, we first call the ‘check_calibration’ function, that:

  • Check if a motor calibration is needed, thanks to the ‘niryo_robot_hardware_interface’ package.

  • If a calibration is needed, use the service ‘/niryo_robot/joints_interface/calibrate_motors’ to launch an automatic calibration.

  • Check if calibration is done and succeeded.

Then, we deactivate the learning mode, using the ‘/niryo_robot/learning_mode/activate’ service with the ‘set_learning_mode’ function.

The JogClient class also implements other functions, that we’ll detail later. Here is the list of functions implemented in the JogClient Class:

  • __update_learning_state: update the current learning mode state.

  • __callback_subscriber_jog_enabled: update the current jog interface state (enabled or not).

  • set_learning_mode: change the current state of the learning mode.

  • set_jog: change the current state of the jog interface.

  • check_calibration: check if calibration is needed and launch it.

  • ask_for_jog_shift: use the ‘/niryo_robot/jog_interface/jog_shift_commander’ service to move the arm according to the command and the shifting value.

Main program:

In the main function of the python file, we create a JogClient object that we’ll use to move the arm. Here, the code allows us to:

  • Apply a positive and a negative shift on the 5th joint.

  • Apply a positive and a negative shift on pose (uses IK solver) (on y and z axis).

Note that the shifting values are small on purpose because you should not apply a huge differential when using the Jog Interface to move the arm, especially when it applies to pose.

When shifting pose, after each call to the jog_shift_commander service (through the ‘ask_for_jog_shift’ function), we check that the status of the response is not -231. Indeed, a -231 status mean that the IK solver was not able to find a solution for the desired pose. To avoid further movements (potentially undesirable positions), we then stop the current execution.

if __name__ == "__main__":
    # Creating Client Object
    jc = JogClient()
    jc.set_jog(True)

    for sign in [1, -1]:
        # this 'stop' boolean is used so that if one jog shift is not doable, instead of doing the
        # next one, it abord the loop. Indeed, doing some jog shifts (with this sign method we use) when some were skipped
        # can lead the robot to strange position.
        for i in range(10):
            print('joints jog', i)
            response = jc.ask_for_jog_shift(cmd=JogShiftRequest.JOINTS_SHIFT,
                                shift_values=[0.0, 0.0, 0.0, 0.0, sign * 0.05, 0.0])


    for sign in [1, -1]:
        stop = False
        for i in range(10):
            print('pose jog', i)
            response = jc.ask_for_jog_shift(cmd=JogShiftRequest.POSE_SHIFT,
                                shift_values=[0.0,sign * 0.01, sign * 0.01, 0.0, 0.0, 0.0])
            if response.status == -231:
                stop = True
                break
        if stop == True:
            break

    jc.set_learning_mode(True)

Control Ned/Ned2 with the Keyboard

We will now use the Jog Controller to control Ned/Ned2 with a keyboard, thanks to the control_with_keyboard.py node.

We use the python package pynput, that you can install with:

pip install pynput

Pynput allows us to monitor the keyboard and retrieve pressed key to process actions. To launch the node, launch:

rosrun ros_node_control control_with_keyboard.py

Code explanation:

We re-use the python class ‘JogClient’ of the previous code. In the main function, we use a keyboard listener with de Pynput library, that will execute in a thread so it’s non-blocking:

listener = keyboard.Listener(
on_press=on_press,
on_release=on_release)
listener.start()

And we define on_press and on_release functions, which will be called everytime a key is pressed and released, as:

def on_press(key):
    global current_key
    try:
            current_key=key.char
    except AttributeError:
        print('special key {0} pressed'.format(
            key))
        current_key=''


def on_release(key):
    global current_key
    current_key = ''
    if key == keyboard.Key.esc:
        return False
    if key == keyboard.Key.ctrl:
        return False
    else :
        current_key = ''

The current_key variable is initialized in the main function as “current_key = ‘’”, and we use it as a global variable in the functions of the keyboard thread so both threads can access and modify the same variable.

Moreover, when the ‘esc’ key or the ‘ctrl’ key are pressed and released, the program will end.

Finally, in the main function, we use a while loop, to do action according to the current pressed key:

while(listener.is_alive()):

    while(current_key != ''):

        # SHIFT POSE
        if current_key == 'i': #i : z axis (+)
            response = jc.ask_for_jog_shift(cmd=JogShiftRequest.POSE_SHIFT,
                    shift_values=[0.0,0.0, 0.01, 0.0, 0.0, 0.0])

        elif current_key == 'k': #k : z axis (-)
            response = jc.ask_for_jog_shift(cmd=JogShiftRequest.POSE_SHIFT,
                    shift_values=[0.0,0.0, -0.01, 0.0, 0.0, 0.0])

        elif current_key == 'l': #l : y axis (+)
            response = jc.ask_for_jog_shift(cmd=JogShiftRequest.POSE_SHIFT,
                    shift_values=[0.0,0.01, 0.0, 0.0, 0.0, 0.0])

        elif current_key == 'j': #j : y axis (-)
            response = jc.ask_for_jog_shift(cmd=JogShiftRequest.POSE_SHIFT,
                    shift_values=[0.0, -0.01, 0.0, 0.0, 0.0, 0.0])

         # The rest of the python code is not included there, but explained right after

The arm can be controlled with the keyboard with:

Pose of the end effector:

  • i / k : shift of 0.01 (goes up) / - 0.01 (goes down) along the z axis

  • l / j : shift of 0.01 (goes right) / - 0.01 (goes left) along the y axis

  • p / m : shift of 0.01 (goes frontward) / - 0.01 (goes backward) along the x axis

Note

In this code, we didn’t implement the shift over Roll, Pitch and Yaw, because the Jog Controller used to shift a Pose uses the IK processing to reach a certain pose. Because of that, movements can be really sudden when the arm tries to go from its current Pitch pose to the next one. However, this feature is implemented in the section ‘Control Ned through the Hardware Interface’, as an example.

Joints:

  • a / q : shift the 1st joint of 0.05 / - 0.05

  • z / s : shift the 2nd joint of 0.05 / - 0.05

  • e / d : shift the 3rd joint of 0.05 / - 0.05

  • r / f : shift the 4th joint of 0.05 / - 0.05

  • t / g : shift the 5th joint of 0.05 / - 0.05

  • y / h : shift the 6th joint of 0.05 / - 0.05

Now you can control Ned/Ned2 through simulation and in reality with your keyboard!

Control Ned/Ned2 with a joystick controller

Setup joystick

For this part, you’ll need to have a joystick controller that you can plug to your computer, through USB for instance. Also, we use the joystick drivers ROS package, that you can install with:

sudo apt install ros-melodic-joystick-drivers

First, plug your joystick to your computer. Then, you can use the joystick driver by launching in a terminal:

rosrun joy joy_node

If your joystick input is not found, you can check which inputs are used with:

ls /dev/input/

And you can then change the input with:

rosparam set joy_node/dev "/dev/input/js0"

Note

Each terminal you work in on your computer should have the same ROS_MASTER_URI (referencing the robot IP address) and ROS_IP (referencing your computer’s IP address). To check their value, do a:

printenv | grep ROS

You can now check that you can read the joystick output with:

rostopic echo /joy

When you press or unpress a button, a message of type “sensor_msgs/Joy” will be published here.

Now, you should have:

  • The roscore running on Ned/Ned2.

  • A terminal, linked in MultiMachines to Ned/Ned2, which runs the joy_node node.

In a third terminal, also linked in MultiMachines with Ned/Ned2, launch the node that will gather information from the /joy topic, process them and control Ned/Ned2 :

rosrun ros_node_control control_with_joystick.py

You should now be able to control Ned/Ned2 with the joystick:

Note

For this tutorial, we used a Xbox joystick. If you want to use another joystick, you may have to change the ROS node code to adapt it to your joystick, according to what you want Ned/Ned2 to do with each button!

How to use:

  • A button: activate Learning Mode.

  • B button: deactivate Learning Mode.

Left joystick:

  • Up/Down: Positive / Negative translation of effector along the X axis.

Directional arrow keys:

  • Right / Left: Positive / Negative translation of effector along the Y axis.

  • Up / Down: Positive / Negative translation of effector along the Z axis.

Right Joystick:

  • Right / Left : Positive / Negative shift of the 5th joint.

  • Up / Down : Positive / Negative shift of the 6th joint.

Code explanation - Joystick

We use again the class JogClient, but we modified it in order to subscribe to the /joy topic and process its messages. According to what was received on this topic, we update the class attribute self.shift_values, that is used by a thread launched at the initialization of the node, working with the move_arm function:

def move_arm(self):
    while(1):
        try :
            i=0
            if self.zAxis != 0 or self.yAxis != 0 or abs(self.xAxis) == 1.0 or abs(self.joint5) == 1.0 or abs(self.joint6) == 1.0:
                if self.current_mode == "Pose" :
                    if i<3: #just to make sure we don't ask for too many jog shift at a time
                        response = jc.ask_for_jog_shift(cmd=JogShiftRequest.POSE_SHIFT,shift_values=self.shift_values)
                        i+=1
                elif self.current_mode == "Joint":
                    if i<3: #just to make sure we don't ask for too many jog shift at a time
                        response = jc.ask_for_jog_shift(cmd=JogShiftRequest.JOINTS_SHIFT,shift_values=self.shift_values)
                        i+=1

        except KeyboardInterrupt:
            print('quit')
            self.thread_move.join()
            break

This function sends a comand to the jog_controller of Ned/Ned2 according to the current desired shift_value.

Customize the node:

If you want to change what Ned/Ned2 does when buttons or joystick are used, refer to what you see when you launch:

rostopic echo /joy

You can use different buttons and see which value is changed in buttons and axes field of the message. As instance, the A button is the first element of the buttons list. You can retrieve its state with:

def joy_callback(self,data):

    listbutton = data.buttons
    self.buttonA = listbutton[0]

You can then check for the state of this button: if it is pressed, it will be equal to 1.0, and if it is not pressed, it will be 0.0. According to that, you can choose what command you’ll send to Ned/Ned2!

Control Ned/Ned2 through the hardware interface

In the previous parts, we used the Robot Commander package. Here, we’ll control Ned/Ned2 directly through the Hardware Interface Package, with the keyboard. We use the Joint Trajectory Controller through the topic /niryo_robot_follow_joint_trajectory_controller/command.

Note

You still need to be connected to Ned/Ned2 in MultiMachines. See the Setup section for more information.

The node used to control Ned/Ned2 here is implemented in the file control_with_joint_traj_controller.py in the ros_node_control package.

We use the python package pynput, that you can install with:

pip install pynput

How to use - Control hardware

Once connected to Ned/Ned2 in MultiMachines, launch the node on your computer with:

rosrun ros_node_control control_with_joint_traj_controller.py

You can now control the pose of Ned/Ned2’s end effector with:

  • i / k : shift of 0.01 (goes up) / - 0.01 (goes down) along the z axis

  • l / j : shift of 0.01 (goes right) / - 0.01 (goes left) along the y axis

  • p / m : shift of 0.01 (goes frontward) / - 0.01 (goes backward) along the x axis

  • a / q : shift roll of 0.05 / -0.05

  • z / s : shift pitch of 0.05 / -0.05

  • e / d : shift yaw of 0.05 / -0.05

Code explanation - Control hardware

Node initialization

We listen to the keyboard input in a thread launched in the initialization of the node. We also check if a calibration is needed, and the learning mode is deactivated.

Main loop

If a key is pressed, the target pose of the end effector is updated according to the key and the current effector pose.

Note

In order to get the current end effector pose, we use the lookupTransform from the tf ROS package.

# main loop
    while(listener.is_alive()):

         # When a key is pressed
        if(self.current_key != ''):

            target_pose = self.get_current_pose()

            if self.current_key in self.key_list:
                execute = True

            if self.current_key == 'i':
                target_pose.pose.position.z +=0.01

            elif self.current_key == 'k':
                target_pose.pose.position.z -=0.01

            elif self.current_key == 'l':
                target_pose.pose.position.y +=0.01

            elif self.current_key == 'j':
                target_pose.pose.position.y -=0.01

            # [...]

The list key_list gathers all the above-mentioned keys.

Once the target_pose is updated, we pass it to the get_position_ik function. This function compute the inverse kinematics used to go from the current pose to the target pose, using the /compute_ik service, provided by MoveIt!. This function return the joint trajectory position solution we’ll have to apply to Ned/Ned2’s joints in order to reach the target pose.

If the target pose is reachable (the IK calculations succeeded), we then send the solution found to the send_joint_trajectory function. From the solution, this function create a JointTrajectory message and send it to the /niryo_robot_follow_joint_trajectory_controller/command topic.

# [In the main loop]

        if execute==True:
            solution, result = self.get_position_ik(target_pose)

            if result == True :
                self.send_joint_trajectory(solution)
                rospy.sleep(0.05) # To avoid an issue regarding trajectory time

            elif result == False :
                print "IK calculation impossible"
                rospy.sleep(0.05)

            execute=False

The hardware interface subscribe to this topic and apply the command to the joints controller.

You can now control Ned/Ned2’s end effector directly through its hardware interface!