# Simulate and use Ned with MATLAB¶

## Objectives¶

Being able to create content with Ned and MATLAB

Visualize Ned model on MATLAB and control it.

See different example of robotics applications using MATLAB and Ned

## Requirements¶

Basic knowledge of MATLAB

Basic knowledge in robotics and kinematics

Being able to use Ned

Having looked at the tutorial on how to set Ned with MATLAB

Note

## What you will need¶

MATLAB

Robotics System Toolbox https://fr.mathworks.com/products/robotics.html

3D model of Ned (URDF, STL, DAE)

You can find the source code of the project and the 3D model of Ned on this package: here

Warning

You will need to change the path of the .stl and .dae files on the ned.urdf file.

## Application 1: Direct Geometry Model¶

Note

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

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

### Import the rigid body tree model of Ned with the URDF file on MATLAB¶

On MATLAB, it’s really easy to import Ned. For this, you just need to create a script, add the following lines to import the URDF of Ned and put the STL/URDF/DAE files on the same folder as the script.

```
clear All
clc
ned = importrobot("ned.urdf");
axes = show(ned);
axes.CameraPositionMode = 'auto';
```

Note

You can find the mechanical interface of Ned here: https://docs.niryo.com/product/ned/en/source/hardware/mechanical_interface.html

If you run your code, you should have Ned simulated like below on MATLAB:

You can check the structure of Ned by using the following command:

```
showdetails(ned);
```

### Get the initial configuration of Ned¶

Go back to the MATLAB script you created and add the following line.

```
inititial_configuration = ned.homeConfiguration
```

### Get the direct geometric model of Ned¶

```
T1_2 = ned.Bodies{1,2}.Joint.JointToParentTransform;
T2_3 = ned.Bodies{1,3}.Joint.JointToParentTransform;
T3_4 = ned.Bodies{1,4}.Joint.JointToParentTransform;
T4_5 = ned.Bodies{1,5}.Joint.JointToParentTransform;
T5_6 = ned.Bodies{1,6}.Joint.JointToParentTransform;
T6_7 = ned.Bodies{1,7}.Joint.JointToParentTransform;
T7_8 = ned.Bodies{1,8}.Joint.JointToParentTransform;
T = T1_2*T2_3*T3_4*T4_5*T5_6*T6_7*T7_8;
```

You should have this matrix T:

Note

You can find more information about transformation matrixs here: https://fr.mathworks.com/help/robotics/ref/rigidbodyjoint.html#bvam96c-1-JointToParentTransform

```
eeoffset = 0
eeBody = robotics.RigidBody("end_effector")
setFixedTransform(eeBody.Joint, trvec2tform([eeoffset,0,0]))
addBody(niryo_one, eeBody, "tool_link");
T_M = getTransform(ned, ned.homeConfiguration,"end_effector","base_link")
```

You should have this matrix T:

Important

If you compare the matrix T and T_M they should be the same.

## Application 2: Inverse Geometry Model¶

Unlike the direct geometry model whose purpose is to express the cartesian coordinates of the end effector considering the rotation angles of the joints, the inverse geometric model express the angular displace of the joints considering the cartesian coordinates of the end effector in a Galilean coordinate system.

Warning

We advise you to have a look at the previous application or at least just to get the code because we will modify it.

```
ik = inverseKinematics("RigidBodyTree", ned);
```

You can find more information about MATLAB function above here: https://fr.mathworks.com/help/robotics/ref/inversekinematics-system-object.html

### From a random position, find the required joint angles to reach the initial position¶

To do that, we will have to define:

Pose: the pose of the end effector, specified as an homogenous 4x4 transform matrix.

Weight: the weights on the error of the desired pose [r p y x y z].

InitialGuess: initial estimate of the robot configuration, specified as an array or a vector.

Add the following lines to your script:

```
weight = [0.1 0.1 0 1 1 1];
initialguess = ned.homeConfiguration
pose_M = [0 0 0];
tform = trvec2tform(pose_M);
configSoln = ik("end_effector", tform, weight,initialguess);
```

With this code and because of the inverse geometry model of Ned, you can see which movement it should do to reach the initial position [0,0,0].

Important

We have this movement of joint to do: (-2.9247, -1.5803, -1.0173, 0.7300, -0.6767, 2.5307).

You should have this matrix T:

## Go Further¶

To do that you just have to uncomment the last part of the code to have it like this:

```
%Convert configSoln into Joint values readable by the ROS Toolbox
cell = struct2cell(configSoln);
Joint = cell(2,:,:);
matrixJoints = cell2mat(Joint);
% Publish the Inverse Geometry Model on a real Ned via the ROS Toolbox of MATLAB
% Use this part when you launch the script for the first time to have
% access to all the messages of the Ned
% folderpath = "/home/niryodev1/Bureau/Nicolas/Niryo_Tool/ned_ros_stack";
% rosgenmsg(folderpath)
rosshutdown; %to be sure that an other ROS network is not actually working
setenv('ROS_MASTER_URI','http://IP_OF_NED:11311') %IP of the Ned
setenv('ROS_IP','IP_HOST_COMPUTER') %IP of the computer
ipaddress = "http://IP_OF_NED:11311"; %IP of the Ned
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');
CmdPoint.Positions(1) = matrixJoints(1);
CmdPoint.Positions(2) = matrixJoints(2);
CmdPoint.Positions(3) = matrixJoints(3);
CmdPoint.Positions(4) = matrixJoints(4);
CmdPoint.Positions(5) = matrixJoints(5);
CmdPoint.Positions(6) = matrixJoints(6);
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

You can find all the information concerning the use of Ned with MATLAB and the ROS Toolbox by following the two tutorials below:

Setup MATLAB and the ROS Toolbox for Ned: Setup MATLAB and the ROS Toolbox for Ned

Publish and subscribe on Ned with MATLAB ROS Toolbox: Publish and Subscribe on Ned with MATLAB ROS Toolbox