################################################# Simulate Ned/Ned2 with Matlab ################################################# **V.2.1** .. image:: /images/simu_matlab/simu_use_ned_matlab.png :alt: simulate and use ned with matlab :width: 100% :align: center Difficulty: ``easy`` Time: ``~15 min`` .. note:: This tutorial is working from: | The version v4.1.0 of the ned_ros_stack Objectives ^^^^^^^^^^^^^^^^^^^^^^^^ - Being able to create content with Ned/Ned2 and Matlab - Visualize Ned/Ned2 model on Matlab and control it. Requirements ^^^^^^^^^^^^^^^^^^^^^^^^ - Basic knowledge of Matlab - Basic knowledge in robotics and kinematics .. note:: | Documentation of Ned: https://docs.niryo.com/product/ned/index.html | Documentation of Ned2: https://docs.niryo.com/product/ned2/index.html | Matlab documentation: https://fr.mathworks.com/help/matlab/ What you will need ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - Matlab - Robotics System Toolbox: https://fr.mathworks.com/products/robotics.html - 3D model of Ned (URDF, STL, DAE) .. note:: Although the robots are different, the 3D models are similar, so you can download the Ned model to simulate a Ned2. | You can find the source code of the project and the 3D model of Ned on `this package `_, download and extract it. | In the folder: "tutorial/matlab_simulation", copy all the content of the folder and paste it into the same folder as the matlab file (.m). Visualize Ned/Ned2 simulation ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ In this section you will be able to have a first application of Ned/Ned2 using Matlab. You will learn how to import the rigid body of Ned/Ned2 on Matlab. It's simple to import Ned/Ned2 on Matlab. For this, you just need to create a script, add the following lines to import the Ned's URDF and put the STL/URDF/DAE files on the same folder as the script (as explained above). .. code-block:: matlab clear All clc ned = importrobot("ned.urdf"); show(ned); Result: .. figure:: /images/simu_matlab/NED_First_View.png :alt: Ned simulation first view :width: 600px :align: center Ned/Ned2 Simulation first view Then, you can change the view of the simulation by using some functions: .. code-block:: matlab ned = importrobot("ned.urdf"); axes = show(ned); axes.XLim=[-0.5,0.5]; axes.YLim=[-0.5,0.5]; axes.ZLim=[0,0.5]; campos([3,3,1]); grid off; Result: .. figure:: /images/simu_matlab/Better_View.png :alt: Ned simulation better view :width: 600px :align: center Ned/Ned2 Simulation improved view .. note:: | You can find the mechanical interface of Ned here: https://docs.niryo.com/product/ned/source/hardware/mechanical_interface.html | And the one from Ned2 here: https://docs.niryo.com/product/ned2/source/hardware/mechanical_interface.html You can check the structure of Ned/Ned2 by using the following command: .. code-block:: matlab showdetails(ned); .. figure:: /images/simu_matlab/matlab_urdf.png :alt: Ned details :width: 800px :align: center Details of Ned/Ned2 structure Get the model of Ned/Ned2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Get the initial configuration of Ned/Ned2 --------------------------------------------- Go back to the Matlab script you created and add the following line. .. code-block:: matlab inititial_configuration = ned.homeConfiguration Get the direct geometric model of Ned/Ned2 ----------------------------------------------- | For this, we have two possibilities: we can use the getTransform() Matlab function or add the transformation matrices of Ned ourselves. | We will first add Ned's transformation matrices. To do so, add the following lines on your Matlab script. .. code-block:: matlab 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: .. figure:: /images/simu_matlab/T.png :alt: T Matrix :width: 400px :align: center Matrix T .. note:: You can find more information about transformation matrices `here `_. | 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: .. code-block:: matlab 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_M: .. figure:: /images/simu_matlab/T_M.png :alt: T_M Matrix :width: 400px :align: center Matrix T_M .. note:: The matrices T and T_M should be the same when comparing. Get the inverse geometric model -------------------------------------------- Unlike the direct geometric model which purpose is to express the cartesian coordinates of the end effector considering the rotation angles of the joints, the inverse geometric model expresses the angular displace of the joints considering the cartesian coordinates of the end effector in a Galilean coordinate system. | In the same script as the previous section, create an “inverseKinematics” object for Ned/Ned2 model. This object represents the solver of Ned’s inverse kinematics: .. code-block:: matlab ik = inverseKinematics("RigidBodyTree", ned); .. figure:: /images/simu_matlab/ik_ned.png :alt: ik inverse kinematics :width: 400px :align: center Inverse kinematics object 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 so, 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: .. code-block:: matlab weight = [0.1 0.1 0 1 1 1]; initialguess = ned.homeConfiguration pose_M = [0.25 0 0.3]; tform = trvec2tform(pose_M); configSoln = ik("end_effector", tform, weight,initialguess); cell = struct2cell(configSoln); Joint = cell(2,:,:); matrixJoints = cell2mat(Joint); With this code and because of the inverse geometric model of Ned/Ned2, you can see which movement it should do to reach the initial position [0.25,0,0.3]. .. note:: | We have this movement of joint to do: (0, 0.0786, -0.6605, 0, 0.5819, 0). | You can also compare these values with those in Niryo Studio, they are identical. .. figure:: /images/simu_matlab/Joint_NStudio.png :alt: Joints values in NiryoStudio :width: 400px :align: center Joints values in Niryo Studio .. figure:: /images/simu_matlab/ConfigSoln.png :alt: ConfigSoln :width: 400px :align: center Joints values in Matlab These short applications show you that you can use Ned/Ned2 with Matlab to learn robotics. Make the simulation move ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ After compiling all the previous lines of code, type the following lines in order to see the new position of Ned/Ned2: .. code-block:: matlab for i=1:6 ned.Bodies{1,i+1}.Joint.HomePosition = Joint{1,1,i}; end show(ned); Now, you can make the simulation move by changing the coordinates of point M. .. figure:: /images/simu_matlab/Other_Pose.png :alt: NED simulation other pose :width: 600px :align: center NED/Ned2 simulation in an other position Now, if you want to see the simulation in motion, type the following code: .. code-block:: matlab figure(1) y=-0.10:0.02:0.10; for j=1:length(y) pose_M=[0.25 y(j) 0.3]; tform=trvec2tform(pose_M); configSoln=ik("end_effector",tform,weight,initialguess); cell=struct2cell(configSoln); Joint=cell(2,:,:); for i = 1:6 ned.Bodies{1,i+1}.Joint.HomePosition = Joint{1,1,i}; end axes=show(ned); axes.XLim=[-0.5,0.5]; axes.YLim=[-0.5,0.5]; axes.ZLim=[0,0.5]; campos([4,0,0.5]) grid off pause(0.01); end Result: .. figure:: /images/simu_matlab/NED_Simulation.gif :alt: Simulation in motion :width: 600px :align: center Simulation in motion .. note:: Attention: the simulation does not work with all positions, especially those that are too close to the center (0,0,0). .. note:: You can find all the information concerning the use of Ned/Ned2 with Matlab and the ROS Toolbox by following this tutorial below: - :doc:`/source/tutorials/setup_ned_matlab_ros_toolbox` Or this one to control Ned/Ned2 with Matlab: - :doc:`/source/tutorials/Use_ned_with_matlab`