Use Ned’s Services with MATLAB ROS Toolbox - Inverse Kinematics

control ned matlab inverse kinematics

Note

This tutorial is working from:
The version v3.0.0 of the ned_ros_stack
The version v3.0.0 of Niryo Studio

Objectives

  • Be able to use Ned with MATLAB

  • Be able to create content with Ned and MATLAB using ROS

  • Have access to the different topics of Ned with MATLAB using ROS

  • Use the different services of Ned with MATLAB using ROS

Requirements

  • Basic knowledge of MATLAB

  • Basic knowledge in robotics and ROS (know how a service is working in ROS)

  • Know how Ned is working

  • Having done the “How to setup Ned for MATLAB and the ROS Toolbox” tutorial

  • Having looked at the “Publish and subscribe on Ned with MATLAB ROS Toolbox” tutorial

Note

Documentation of Ned: https://archive-docs.niryo.com/product/ned/index.html
MATLAB documentation: https://fr.mathworks.com/help/matlab/
ROS documentation: http://wiki.ros.org/Documentation
Tutorial: Setup Matlab and the ROS Toolbox for Ned/Ned2
Tutorial: Publish and Subscribe on Ned with MATLAB ROS Toolbox

What you will need

Warning

Make sure that you already followed the “How to setup Ned for MATLAB and the ROS Toolbox” tutorial before doing this one.
You should now be able to communicate with Ned from MATLAB via ROS Toolbox and the ‘rosinit’ function.
In this tutorial you will have to use the code you created on the “How to setup Ned for MATLAB and the ROS Toolbox” tutorial or use the code on the github at this link: control ned inverse kinematics

Start a new roscore on Ned to make sure to have access to the topics

It is important to start a new roscore and launch the ‘niryo_robot_bringup niryo_ned_robot.launch’ launcher. Indeed, when Ned starts, it automatically launches a roscore and all the packages it needs to run. Unfortunately, MATLAB doesn’t manage to have access to this roscore and needs it to have it launched an other time.

To do that you need to connect via ssh to Ned:

ssh niryo@IP_OF_NED

Then you have to stop all the services of Ned and launch them again.

sudo service niryo_robot_ros stop
roslaunch niryo_robot_bringup niryo_ned_robot.launch

Warning

If you have the error below, you will have to generate again Ned’s messages and services from the ‘ned_ros’ package.

Ned

To do that you have to uncomment the two following lines:

folderpath = "/YOUR_PATH/ned_ros"
rosgenmsg(folderpath)

See on the “How to setup Ned for MATLAB and the ROS Toolbox” tutorial if you don’t remember all the steps to follow.

You are now able to use Ned’s services and call it on MATLAB using the ROS Toolbox.

Use the MATLAB ROS Toolbox: Call a service of Ned send a message

You should now be able to publish and subscribe on Ned using MATLAB and the ROS Toolbox but you want to go further and use the ROS services of Ned. Indeed, Ned has a lot of different services. You can have a look to them using this command on the command window of matlab:

rosservice list

/niryo_robot/conveyor/control_conveyor
/niryo_robot/conveyor/ping_and_set_conveyor
/niryo_robot/kinematics/forward
/niryo_robot/kinematics/inverse
            (...)
/niryo_robot_commander/get_loggers
/niryo_robot_commander/is_active
/niryo_robot_commander/motor_debug_start
/niryo_robot_commander/motor_debug_stop
/niryo_robot_commander/set_logger_level
/niryo_robot_commander/set_max_velocity_scaling_factor
/niryo_robot_commander/stop_command
/niryo_robot_user_interface/get_loggers
/niryo_robot_user_interface/set_logger_level
For this tutorial we will be interested in two services:

  • /niryo_robot/kinematics/inverse which is the service called when Ned needs to have the Inverse Kinematic to reach a joint position

  • /niryo_robot/kinematics/forward which is the service called when Ned needs to have the Direct Kinematic to reach a coordonate position

To call those services with the MATLAB ROS Toolbox you will always need to follow the next three steps.

Note

You can have more information on how to use ROS services with MATLAB at this link: https://fr.mathworks.com/help/ros/services-and-actions.html?s_tid=CRUX_lftnav

Step 1: Create a Client

To do that, you have to use a MATLAB ROS Toolbox function called ‘rossvcclient’. Add the line below on your code:

client = rossvcclient('/niryo_robot/kinematics/inverse');

This function creates a ROS service client object with the service you gave as parameter. In our case we will use ‘/niryo_robot/kinematics/inverse’ service.

If you have a look to this ROS service client you should have this:
Ned

You have the name of the service and the type of the messages used by it. In our case the type of the messages are ‘niryo_robot_commander/GetIK’. The next step will consist in creating this message.

Step 2: Create a rosmessage which will be used by the service

Services always use messages to communicate. In this part we will create a message with the adapted type.

So we need to create the rosmessage. To do that add this line on your code:

ReqMsg = rosmessage(client);

When you use the ‘rosmessage’ function with the client service we created just before as parameter you will directly have a rosmessage with the adapted type. You should have this message:

Ned

Now that your rosmessage is created, you have to give him the parameters you want.

Step 3: Put the parameters you want on the ReqMes

In this part we will give parameters to the ReqMes rosmessage we created earlier. As we are using the Inverse Kinematic service, we need to give this message coordonate to reach and the service will calculate the position of the Joints Ned should reach.

As an example, you can use this code:

ReqMsg.Pose.Position.X = 0.156000000000000;
ReqMsg.Pose.Position.Y = 0.243000000000000;
ReqMsg.Pose.Position.Z = 0.434000000000000;

ReqMsg.Pose.Rpy.Roll = -4.689665592537737e-12;
ReqMsg.Pose.Rpy.Pitch = 1.110222783151426e-16;
ReqMsg.Pose.Rpy.Yaw = 1.000000000015311;


ReqMsg.Pose.Orientation.X = 0;
ReqMsg.Pose.Orientation.Y = 0;
ReqMsg.Pose.Orientation.Z = 0.479000000000000;
ReqMsg.Pose.Orientation.W = 0.878000000000000;

Step 4: Call the service

We can now call the service using the MATLAB ROS Toolbox function ‘call’. Add the line below on your script:

IK = call(client, ReqMsg);

If you put the same parameters as the parameters above on your ReqMes rosmessage, you should have the same Joints state on the IK variable.

Ned

Use the MATLAB ROS Toolbox: make Ned move with the Inverse Kinematic service

We will now use the Joints state that we got from the /niryo_robot/kinematics/inverse service and publish it to Ned to make it move.

To do that, add the next lines on your code:

NedState = rossubscriber("/niryo_robot_follow_joint_trajectory_controller/state");

NedCmd = rospublisher("/niryo_robot_follow_joint_trajectory_controller/command");
CmdMsg = rosmessage(NedCmd);

CmdPoint = rosmessage('trajectory_msgs/JointTrajectoryPoint');
CmdPoint.Positions = IK.Joints; %We get the Joints goal from the IK service
CmdPoint.Velocities = zeros(1,6);
CmdPoint.Accelerations = zeros(1,6);
CmdPoint.TimeFromStart = ros.msg.Duration(3);

CmdMsg.Header.Stamp = rostime("now") + rosduration(0.05);
CmdMsg.JointNames = {'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'};
CmdMsg.Points = CmdPoint;

send(NedCmd,CmdMsg);

If you have followed the previous tutorial concerning how to publish on Ned via MATLAB ROS Toolbox, you should understand this code. We create a rospublisher and give it as Position parameter the Joints state we got from the Inverse Kinematix service.

Warning

Be sure that on Niryo Studio, Ned is not on learning mode, unless it won’t move.

Note

If everything went ok you should see Ned going to the [1.002344965934753;-0.002415388822556;0.001709167379886;0.070674084126949;-5.491633055498824e-05;-0.070349559187889] joint position.