############################################################# Pick and place with Matlab ############################################################# **V.1.1** .. image:: /images/Matlab/Bannière_Matlab_Ned2.png :alt: Pick and place Matlab Banner :width: 100% :align: center 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. .. raw:: html
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: :doc:`/source/tutorials/setup_ned_matlab_ros_toolbox`. .. 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: .. figure:: /images/setup_matlab/Rosinit.png :alt: Rosinit Setup :width: 900px :align: center Rosinit setup Otherwise, do :doc:`this tutorial ` first. .. hint:: We advise you to do :doc:`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: :doc:`setup ` and :doc:`use ` Ned/Ned2 with Matlab). .. code-block:: 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. .. figure:: /images/setup_matlab/Rosinit.png :alt: Rosinit Setup :width: 900px :align: center Rosinit setup 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: .. admonition:: Open and close the gripper :class: hint, dropdown .. code-block:: matlab 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): .. admonition:: Create Matlab functions :class: hint, dropdown .. warning:: For the functions to work, make sure you save the function files (.m) with the same name as the function. Pick function: .. code-block:: matlab 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: .. code-block:: matlab 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: .. code-block:: matlab 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. .. 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"); 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: .. admonition:: Make a loop :class: hint, dropdown .. 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"); 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: - :doc:`/source/tutorials/setup_ned_matlab_ros_toolbox` - :doc:`/source/tutorials/Use_ned_with_matlab`