Pick and place with Matlab

V.1.1

Difficulty: hard

Time: ~45 min

Note

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

The version v4.1.0 of Niryo Studio

Introduction

This process consists in taking any type of object at a specific location and placing it at another location in a more orderly fashion.

Objectives

Show one of the examples of applications you can do with Matlab and Ned/Ned2
Have a first approach with industrial processes which can be realized with Ned/Ned2
Learn the basics of Matlab programming while using the Ned/Ned2.

Requirements

Basic knowledge of Matlab
Being able to use Ned/Ned2
Having looked at the tutorial: Setup Matlab and the ROS Toolbox for Ned/Ned2.

Note

Documentation of Ned: https://archive-docs.niryo.com/product/ned/index.html
Documentation of Ned2: https://archive-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:

Otherwise, do this tutorial first.

Hint

We advise you to do this tutorial before starting and to practice moving the robot.

What you will need

A Ned or a Ned2
A gripper (the one you want)
A conveyor belt (optional)
Matlab (2014 version or more)
Robotics System Toolbox : https://fr.mathworks.com/products/robotics.html
A Wi-Fi communication

Note

To do this example we used Windows10, Matlab2022a and a Ned2.

Details of the script

First of all, you have to connect the robot in Wi-Fi mode and connect it with ROS.

(Normally this is already done if you have followed the previous tutorials: setup and use Ned/Ned2 with Matlab).

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)

You have to be in this situation.

First, try to open and close the gripper using the Ros toolbox. In the same way as for moving the robot, you must create and send a Ros message. This time the subject of the message is not a Point, but a ToolGoal.

Try to write the code yourself. If you struggle too much, you can find a code sample in the drop-down window below:

Open and close the gripper

NedState2 = rossubscriber("/niryo_robot_tools_commander/action_server/goal");
NedCmd2 = rospublisher("/niryo_robot_tools_commander/action_server/goal");
CmdMsg2 = rosmessage(NedCmd2);
CmdGoal = rosmessage('niryo_robot_tools_commander/ToolGoal');

CmdGoal.Cmd.ToolId=11;
CmdGoal.Cmd.CmdType=1; %Or 2 if you want to close the gripper
CmdGoal.Cmd.MaxTorquePercentage=100;
CmdGoal.Cmd.HoldTorquePercentage=100;
CmdGoal.Cmd.Speed=500;
CmdGoal.Cmd.Activate=1;

CmdMsg2.Header.Stamp = rostime("now") + rosduration(0.05);
CmdMsg2.GoalId.Stamp = rostime("now") + rosduration(0.05);
CmdMsg2.GoalId.Id="OPENGRIPPER"; %Or CLOSEGRIPPER
CmdMsg2.Goal=CmdGoal;

send(NedCmd2,CmdMsg2);

Warning

The ToolID depends on the gripper you are using, be sure to enter the correct ID.

Custom Gripper=11, Large Gripper=12, Adaptative Gripper=13.

Note

If the gripper doesn’t open (or close), make sure that you have scan the tool in Niryo Studio and try to open and close it, then retry the matlab script.

To make the program as clear as possible, make Matlab functions to call them several times easily. Here, the best way is to make one function per action (move, pick and place) in order to separate the steps (but you can also do everything together):

Create Matlab functions

Warning

For the functions to work, make sure you save the function files (.m) with the same name as the function.

Pick function:

function pick(toolID)
    NedState2 = rossubscriber("/niryo_robot_tools_commander/action_server/goal");
    NedCmd2 = rospublisher("/niryo_robot_tools_commander/action_server/goal");
    CmdMsg2 = rosmessage(NedCmd2);
    CmdGoal = rosmessage('niryo_robot_tools_commander/ToolGoal');

    CmdGoal.Cmd.ToolId=toolID;
    CmdGoal.Cmd.CmdType=2;
    CmdGoal.Cmd.MaxTorquePercentage=100;
    CmdGoal.Cmd.HoldTorquePercentage=100;
    CmdGoal.Cmd.Speed=500;
    CmdGoal.Cmd.Activate=1;

    CmdMsg2.Header.Stamp = rostime("now") + rosduration(0.05);
    CmdMsg2.GoalId.Stamp = rostime("now") + rosduration(0.05);
    CmdMsg2.GoalId.Id="CLOSEGRIPPER";
    CmdMsg2.Goal=CmdGoal;

    send(NedCmd2,CmdMsg2);

Place function:

function pick(toolID)
    NedState2 = rossubscriber("/niryo_robot_tools_commander/action_server/goal");
    NedCmd2 = rospublisher("/niryo_robot_tools_commander/action_server/goal");
    CmdMsg2 = rosmessage(NedCmd2);
    CmdGoal = rosmessage('niryo_robot_tools_commander/ToolGoal');

    CmdGoal.Cmd.ToolId=toolID;
    CmdGoal.Cmd.CmdType=2;
    CmdGoal.Cmd.MaxTorquePercentage=100;
    CmdGoal.Cmd.HoldTorquePercentage=100;
    CmdGoal.Cmd.Speed=500;
    CmdGoal.Cmd.Activate=1;

    CmdMsg2.Header.Stamp = rostime("now") + rosduration(0.05);
    CmdMsg2.GoalId.Stamp = rostime("now") + rosduration(0.05);
    CmdMsg2.GoalId.Id="CLOSEGRIPPER";
    CmdMsg2.Goal=CmdGoal;

    send(NedCmd2,CmdMsg2);

Move function:

function move(ned,vect,eul)
    ik = inverseKinematics("RigidBodyTree", ned);
    weight = [0.1 0.1 0 1 1 1];
    initialguess = ned.homeConfiguration;

    XYZ=vect;
    RPY=eul;

    postform=trvec2tform(XYZ);
    eultform=eul2tform(RPY,'ZYX');

    tform=postform*eultform;
    configSoln = ik("end_effector", tform, weight, initialguess);
    cell = struct2cell(configSoln);
    Joint = cell(2,:,:);
    matrixJoints = cell2mat(Joint);

    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

For the move function, you must first write the following lines in your main file, before calling the function.

ned=importrobot(ned.urdf);
eeoffset = 0
eeBody = robotics.RigidBody("end_effector")
setFixedTransform(eeBody.Joint, trvec2tform([eeoffset,0,0]))
addBody(niryo_one, eeBody, "tool_link");

To complete this example, try to make a loop to repeat the movement. So you can reproduce the movement as in a production line. Here is the code:

Make a loop

ned=importrobot(ned.urdf);
eeoffset = 0
eeBody = robotics.RigidBody("end_effector")
setFixedTransform(eeBody.Joint, trvec2tform([eeoffset,0,0]))
addBody(niryo_one, eeBody, "tool_link");

while 1

    %Pick
    move(ned,[0.25 -0.2 0.3],[0 0 0]);
    pause(4);

    pick(11);
    pause(0.5);

    %Intermediate position
    move(ned,[0.35 0 0.3],[0 0 0]);
    pause(0.5);

    %Place
    move(ned,[0.25 0.2 0.3],[0 0 0]);
    pause(4);

    place(11);
    pause(0.5);

end

Now all you have to do is change the pick and place positions, choose the correct ToolID, and you can do a pick and place with Matlab.

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: