##################################################################################### Use Ned's Services with MATLAB ROS Toolbox - Inverse Kinematics ##################################################################################### .. image:: /images/services_matlab/ned_services_matlab_kinematics.png :alt: control ned matlab inverse kinematics :width: 100% :align: center .. note:: This tutorial is working from: | The version v3.0.0 of the ned_ros_stack | The version v3.0.0 of Niryo Studio Objectives ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - Be able to use Ned with MATLAB - Be able to create content with Ned and MATLAB using ROS - Have access to the different topics of Ned with MATLAB using ROS - Use the different services of Ned with MATLAB using ROS Requirements ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - Basic knowledge of MATLAB - Basic knowledge in robotics and ROS (know how a service is working in ROS) - Know how Ned is working - Having done the "How to setup Ned for MATLAB and the ROS Toolbox" tutorial - Having looked at the "Publish and subscribe on Ned with MATLAB ROS Toolbox" tutorial .. note:: | Documentation of Ned: https://docs.niryo.com/product/ned/index.html | MATLAB documentation: https://fr.mathworks.com/help/matlab/ | ROS documentation: http://wiki.ros.org/Documentation | Tutorial: :doc:`/source/tutorials/setup_ned_matlab_ros_toolbox` | Tutorial: :doc:`/source/tutorials/control_ned_matlab_ros_publish_subscribe` What you will need ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - A Ned - MATLAB - MATLAB ROS Toolbox https://fr.mathworks.com/products/ros.html - A wifi communication .. warning:: | Make sure that you already followed the "How to setup Ned for MATLAB and the ROS Toolbox" tutorial before doing this one. | You should now be able to communicate with Ned from MATLAB via ROS Toolbox and the 'rosinit' function. | In this tutorial you will have to use the code you created on the "How to setup Ned for MATLAB and the ROS Toolbox" tutorial or use the code on the github at this link: `control ned inverse kinematics `_ Start a new roscore on Ned to make sure to have access to the topics ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ It is important to start a new roscore and launch the 'niryo_robot_bringup niryo_ned_robot.launch' launcher. Indeed, when Ned starts, it automatically launches a roscore and all the packages it needs to run. Unfortunately, MATLAB doesn't manage to have access to this roscore and needs it to have it launched an other time. To do that you need to connect via ssh to Ned: .. code-block:: matlab ssh niryo@IP_OF_NED Then you have to stop all the services of Ned and launch them again. .. code-block:: matlab sudo service niryo_robot_ros stop roslaunch niryo_robot_bringup niryo_ned_robot.launch .. warning:: If you have the error below, you will have to generate again Ned's messages and services from the 'ned_ros' package. .. image:: /images/services_matlab/error.png :alt: Ned :width: 600px :align: center To do that you have to uncomment the two following lines: .. code-block:: matlab folderpath = "/YOUR_PATH/ned_ros" rosgenmsg(folderpath) See on the "How to setup Ned for MATLAB and the ROS Toolbox" tutorial if you don't remember all the steps to follow. You are now able to use Ned's services and call it on MATLAB using the ROS Toolbox. Use the MATLAB ROS Toolbox: Call a service of Ned send a message ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ You should now be able to publish and subscribe on Ned using MATLAB and the ROS Toolbox but you want to go further and use the ROS services of Ned. Indeed, Ned has a lot of different services. You can have a look to them using this command on the command window of matlab: .. code-block:: matlab rosservice list .. code-block:: matlab /niryo_robot/conveyor/control_conveyor /niryo_robot/conveyor/ping_and_set_conveyor /niryo_robot/kinematics/forward /niryo_robot/kinematics/inverse (...) /niryo_robot_commander/get_loggers /niryo_robot_commander/is_active /niryo_robot_commander/motor_debug_start /niryo_robot_commander/motor_debug_stop /niryo_robot_commander/set_logger_level /niryo_robot_commander/set_max_velocity_scaling_factor /niryo_robot_commander/stop_command /niryo_robot_user_interface/get_loggers /niryo_robot_user_interface/set_logger_level | For this tutorial we will be interested in two services: - /niryo_robot/kinematics/inverse which is the service called when Ned needs to have the Inverse Kinematic to reach a joint position - /niryo_robot/kinematics/forward which is the service called when Ned needs to have the Direct Kinematic to reach a coordonate position To call those services with the MATLAB ROS Toolbox you will always need to follow the next three steps. .. note:: You can have more information on how to use ROS services with MATLAB at this link: https://fr.mathworks.com/help/ros/services-and-actions.html?s_tid=CRUX_lftnav Step 1: Create a Client ----------------------- To do that, you have to use a MATLAB ROS Toolbox function called 'rossvcclient'. Add the line below on your code: .. code-block:: matlab client = rossvcclient('/niryo_robot/kinematics/inverse'); This function creates a ROS service client object with the service you gave as parameter. In our case we will use '/niryo_robot/kinematics/inverse' service. | If you have a look to this ROS service client you should have this: .. image:: /images/services_matlab/client.png :alt: Ned :width: 600px :align: center You have the name of the service and the type of the messages used by it. In our case the type of the messages are 'niryo_robot_commander/GetIK'. The next step will consist in creating this message. Step 2: Create a rosmessage which will be used by the service -------------------------------------------------------------- | Services always use messages to communicate. In this part we will create a message with the adapted type. So we need to create the rosmessage. To do that add this line on your code: .. code-block:: matlab ReqMsg = rosmessage(client); When you use the 'rosmessage' function with the client service we created just before as parameter you will directly have a rosmessage with the adapted type. You should have this message: .. image:: /images/services_matlab/ReqMsg.png :alt: Ned :width: 600px :align: center Now that your rosmessage is created, you have to give him the parameters you want. Step 3: Put the parameters you want on the ReqMes -------------------------------------------------- In this part we will give parameters to the ReqMes rosmessage we created earlier. As we are using the Inverse Kinematic service, we need to give this message coordonate to reach and the service will calculate the position of the Joints Ned should reach. As an example, you can use this code: .. code-block:: matlab ReqMsg.Pose.Position.X = 0.156000000000000; ReqMsg.Pose.Position.Y = 0.243000000000000; ReqMsg.Pose.Position.Z = 0.434000000000000; ReqMsg.Pose.Rpy.Roll = -4.689665592537737e-12; ReqMsg.Pose.Rpy.Pitch = 1.110222783151426e-16; ReqMsg.Pose.Rpy.Yaw = 1.000000000015311; ReqMsg.Pose.Orientation.X = 0; ReqMsg.Pose.Orientation.Y = 0; ReqMsg.Pose.Orientation.Z = 0.479000000000000; ReqMsg.Pose.Orientation.W = 0.878000000000000; Step 4: Call the service ------------------------- We can now call the service using the MATLAB ROS Toolbox function 'call'. Add the line below on your script: .. code-block:: matlab IK = call(client, ReqMsg); If you put the same parameters as the parameters above on your ReqMes rosmessage, you should have the same Joints state on the IK variable. .. image:: /images/services_matlab/IK.png :alt: Ned :width: 600px :align: center Use the MATLAB ROS Toolbox: make Ned move with the Inverse Kinematic service ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ We will now use the Joints state that we got from the /niryo_robot/kinematics/inverse service and publish it to Ned to make it move. To do that, add the next lines on your code: .. code-block:: matlab 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 = IK.Joints; %We get the Joints goal from the IK service 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); If you have followed the previous tutorial concerning how to publish on Ned via MATLAB ROS Toolbox, you should understand this code. We create a rospublisher and give it as Position parameter the Joints state we got from the Inverse Kinematix service. .. warning:: Be sure that on Niryo Studio, Ned is not on learning mode, unless it won't move. .. note:: If everything went ok you should see Ned going to the [1.002344965934753;-0.002415388822556;0.001709167379886;0.070674084126949;-5.491633055498824e-05;-0.070349559187889] joint position.