#################################################
Control Ned/Ned2 with Matlab
#################################################

**V.2.1**

.. image:: /images/simu_matlab/Sub_Pub_Matlab.png
                     :alt: simulate and use ned with matlab
                     :width: 100%
                     :align: center

Difficulty: ``medium``

Time: ``~30 min``

.. note::
    This tutorial is working from:
        | The version v4.1.0 of the ned_ros_stack 

Objectives
^^^^^^^^^^^^^^^^^^^^^^^^

- Being able to control and create content with Ned/Ned2 and Matlab

Requirements
^^^^^^^^^^^^^^^^^^^^^^^^

- Basic knowledge of Matlab
- Basic knowledge in robotics and kinematics
- Having looked at the tutorial: :doc:`/source/tutorials/setup_ned_matlab_ros_toolbox`

.. note::
    | Documentation of Ned : https://docs.niryo.com/product/ned/index.html
    | Documentation of Ned2: https://docs.niryo.com/product/ned2/index.html
    | Matlab documentation : https://fr.mathworks.com/help/matlab/

You have to be in this situation to start this tutorial.

.. figure:: /images/setup_matlab/Rosinit.png
            :alt: Rosinit Setup
            :width: 900px
            :align: center

            Rosinit setup

Otherwise, do :doc:`this tutorial </source/tutorials/setup_ned_matlab_ros_toolbox>` first.

What you will need 
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
- A Ned or a Ned2
- Matlab (2014 version or more)
- Robotics System Toolbox : https://fr.mathworks.com/products/robotics.html
- A Wi-Fi communication

.. note:: 
    To do this tutorial we used Windows10, Matlab2022a and a Ned2.


Connection between Ned/Ned2 and the computer
-----------------------------------------------

| First of all, you have to connect the robot with a Wi-Fi communication (not hotspot).
| To do so, check our previous tutorial `just here <https://docs.niryo.com/product/niryo-studio/vx.x.x/en/source/connection.html#using-ned-on-your-wi-fi-network>`_.


Get the direct geometry model
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

.. note::
   The direct geometric model of a robot provides the location of the end-effector in terms of joint coordinates. 

In this section you will be able to have a first application of Ned/Ned2 using Matlab. You will learn how to import the rigid body of Ned/Ned2
on Matlab and work on its direct geometric model.


Get the initial configuration of Ned/Ned2 and prepare the matrix
-------------------------------------------------------------------

| In order to send a command to the robot, you need to know its configuration.
| First you just need to add the following lines:

.. code-block:: matlab

    ned=importrobot(ned.urdf);
    eeoffset = 0
    eeBody = robotics.RigidBody("end_effector")
    setFixedTransform(eeBody.Joint, trvec2tform([eeoffset,0,0]))
    addBody(niryo_one, eeBody, "tool_link");
    ik = inverseKinematics("RigidBodyTree", ned);
    weight = [0.1 0.1 0 1 1 1];
    initialguess = ned.homeConfiguration;

| You don't need to recompile these lines every time.
| 
| Now, you have to prepare the matrix you will send to the robot. 

.. code-block:: matlab

    pose_M = [0.25 0 0.3];
    tform = trvec2tform(pose_M);
    configSoln = ik("end_effector", tform, weight, initialguess);
    cell = struct2cell(configSoln); 
    Joint = cell(2,:,:);
    matrixJoints = cell2mat(Joint);

| Your matrix is ready, you can check the values of joints (0, 0.0786, -0.6605, 0, 0.5819, 0).
| You can also compare these values with those in Niryo Studio, they are identical.

You should have these values in Matlab:

.. figure:: /images/simu_matlab/ConfigSoln.png
            :alt: ConfigSoln
            :width: 400px
            :align: center

            Joints values in Matlab

.. figure:: /images/simu_matlab/Joint_NStudio.png
         :alt: Joints values in NiryoStudio
         :width: 400px
         :align: center

         Joints values in Niryo Studio

Prepare and send a ROS message
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

.. important:: 
    If you are using a Ned, before sending the message to the robot, make sure to disable the learning mode in Niryo Studio (you should hear the motors).

Now, you will setup a ROS message, with your matrix previously prepared.

To do that you just have to type this:

.. code-block:: matlab
   
    %These lines remain commented if you have already initialized ROS, otherwise, make sure you have done the matlab setup tutorial and uncomment the lines.

    %ipaddress = "http://IP_OF_NED:11311"; %IP of the Ned
    %rosshutdown; %to be sure that an other ROS network is not actually working
    %setenv('ROS_MASTER_URI',ipaddress) %IP of the Ned
    %setenv('ROS_IP','IP_HOST_COMPUTER') %IP of the computer
    %rosinit(ipaddress) 

    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');

    for i=1:6
        CmdPoint.Positions(i) = matrixJoints(i);
    end
    
    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);

.. note:: 
    It is possible that the robot does not move instantly, wait about 15 seconds maximum.

.. hint:: 
    If you are using a Ned2 and the robot don't move after sending the message, use Niryo Studio to do a movement (by changing the values of joints for exemple) and the relaunch the script.


Go further
^^^^^^^^^^^^^^^^^
| You can now control Ned/Ned2 as you want with Matlab.
| As an exemple, we made a loop, which makes the robot move permanently, to reproduce some repetitive tasks.

.. code-block:: matlab
    
    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');

    y=0.10;

    while 1
        y=y*(-1);
        pose_M = [0.25 y 0.3];
        tform = trvec2tform(pose_M);
        configSoln = ik("end_effector", tform, weight, initialguess);
        cell = struct2cell(configSoln); 
        Joint = cell(2,:,:);
        matrixJoints = cell2mat(Joint);

        for i=1:6
            CmdPoint.Positions(i) = matrixJoints(i);
        end

        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);
        pause(3);
    end

.. note::
   You can find all the information concerning the use of Ned/Ned2 with Matlab
   and the ROS Toolbox by following the two tutorials below:
   
   - :doc:`/source/tutorials/setup_ned_matlab_ros_toolbox`
   - :doc:`/source/tutorials/SImulate_ned_with_matlab`