# Control Ned/Ned2 with Matlab

V.2.1 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

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

Otherwise, do this tutorial first.

## What you will need

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.

## 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:
```ned=importrobot(ned.urdf);
eeoffset = 0
eeBody = robotics.RigidBody("end_effector")
setFixedTransform(eeBody.Joint, trvec2tform([eeoffset,0,0]))
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.
```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:

## 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:

```%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_IP','IP_HOST_COMPUTER') %IP of the computer

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.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.
```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);