Simulate and use Ned with MATLAB

ROS Logo Ned

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

What you will need

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:

Ned

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

showdetails(ned);
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

For this, we have two possibilities: we can use the getTransform() MATLAB function or add the transformation matrixs of Ned ourself.
We will first add Ned’s transformation matrixs. For this, add the following lines on your MATLAB script.
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:

Ned

Note

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

The first method works well but it is a bit long. If you want to use the second one you just need to add the following lines:
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:

Ned

Important

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

This short application shows you that you can use Ned with MATLAB to learn robotics.

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.

In the same script as the previous section, create an “inverseKinematics” object for Ned model. This object represents the solver of Ned’s inverse kinematics:
ik = inverseKinematics("RigidBodyTree", ned);
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:

Ned
This short application showed you that you can use Ned with MATLAB to learn robotics.

Go Further

If you want to go further with this tutorial you can use the joints values you got at the end and send it to Ned via the ROS Toolbox of MATLAB.

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: