Simulink/ROS: Publishing JointTrajectory Messages for end effector positions

Hi,

I have a human model in Gazebo ROS.The controller is joint trajectory that works fine in ROS environment. Now I have to add a controller but that in Simulink ROS toolbox. Instead of sending Joint Positions I want to send End Effector positions. In other words I am working in Task Space and not in Joint Space. I am unable to access positions from joint trajectory blank message and could not find a solution for quite a while now.

I request if anyone can help me in this regard. I have gone through a question posted in 2017 with title

Simulink/ROS: Publishing JointTrajectory Messages

but it plans in joint space and inputs joint positions. I have to send position commands to my arm group to perform point to point trajectroy planing in Task space(cartesian space). Any help would be appreciated.

Update

As an initial step, I have broken down my problem to first publish one joint position that is to my gripper finger. For this purpose I have used the idea mentioned in the above post highlighted in bold for my gripper joint movement. I do not get any erros in Matlab but I am getting the following error repeatedly in my ROS terminal and hence my joint is not actuated.

Dropping all 1 trajectory point(s), as they occur before the current time. Last point is 0s in the past.

I dont understand why this is happening. Anyhelp would be appreciated please. As if this gets published to the joint and it finally gets actuated, I can eventually take it up further to reach the bigger goal of planning in Cartesian Space.

Thanks

Best

Hasan

Answer 1

[Remo Pillat]

Hi Ehtisham,

Please take a look at the "Control PR2 Arm Movements" example in Robotics System Toolbox. It shows how to create a JointTrajectory message and assign the relevant data (see the rGoalMsg.Trajectory variable in the example). You should be able to extrapolate from that for your own robot, e.g. assign the right joint names.

The example also shows how to use Inverse Kinematics in MATLAB to find the right joint positions for your desired end effector pose.

Once you have it working in MATLAB, you should be able to port this to Simulink pretty easily (there are existing blocks for all the functionality you need).

Hope this help.

[Ehtisham ul Hasan]

Hi Remo,

Thank you for the response. I went through the PR2 example but I think I just overlooked it so I will try to understand it and implement it. There is one thing I need to mention in the mean time. I have a ROS enabled human arm model running in Gazebo and I have already developed its moveit package which does the inverse kinematics for me once I send it the position and orientation for the end effector. So just wanted to know, is it possible to send positions from Robotics System Toolbox and not perform inverse kinematics in Matlab Simulink ?

Thank you

Best

Hasan

[Ehtisham ul Hasan]

Hi,

As per your suggestion, I went through the link you shared for controlling arm joints. I did manage to use them (in an m-file) for my arm model and the commands were sent and joints of the ROS enabled model in Gazebo were actuated but I got the following error in my terminal window

Client [/matlab_global_node_51294] wants topic /mhuman/arm_controller/follow_joint_trajectory/result to have datatype/md5sum [control_msgs/FollowJointTrajectoryActionResult/bce83d50f7bb28226801436caf0e2043], but our version has [control_msgs/FollowJointTrajectoryActionResult/c4fb3b000dc9da4fd99699380efcc5d9]. Dropping connection.

However, this does not concern me much at the moment because I have to actually do this from Simulink ROS Toolbox blocks and not from an m-file. So I tried using the code in m-file in Matlab Function block, but I could not succeed in publishing joint angles to Gazebo or actuate the joints.

In my Simulink Model, as per my knowledge so far, I kept both input and output message types as trajectory_msgs/JointTrajectory, I get the following error in Simulink (There are other errors as well which can be reproduced from the attached simulink model file).

Function 'rosactionclient' not supported for code generation.

Function 'MATLAB Function5' (#60.50.138), line 3, column 1:

"[rGrip,rGoalMsg] = rosactionclient('/mhuman/gripper_controller/follow_joint_traj"

Launch diagnostic report.

In addition to this error, the error in linux terminal stays the same (as mentioned above)

Since message types have to be same so I stick to keeping the message type as trajectory_msgs/JointTrajectory but as per m-file structure I also tried keeping input message type as trajectory_msgs/JointTrajectoryPoint (of the blankMsg) and message type for output as control_msgs/FollowJointTrajectoryActionalGoal (of the publish block) but heirarchy of the messages did not match. I think there is some issue with message definitions may be as I am using ROS Kinetic with Matlab 2020a. But I am unable to understand from discussions related to this like what package to download or from where to update the message definitions in my linux machine or if it is a custom message case or how to define and use a custom message. So I seek help in this regard.

(NOTE: For your reference please, I have my Matlab installed in /usr/local/Matlab/R2020a/bin and I launch it from the same folder.)

I request kind advice and guidance on subject matter. Thank you.

Best Regards

Hasan

[Cam Salzberger]

Hello Hasan,

For R2020a and earlier versions, please see these instructions for updating built-in message types. You'll need the ROS Kinetic message definition packages to do so.

-Cam

[Ehtisham ul Hasan]

Hi,

I will look for ROS KInetic message definition package and will ty to see if I can do as per the intructions in the link on my linux machine. However, I wanted to know if you also think it is because of the message definitions that the simulink block does not communicate ? there is no other issue in the methodology I am following using the simulink blocks ?

Thanks

Best

Hasan

[Cam Salzberger]

Well, if MATLAB cannot communicate with an external ROS system for a particular message type, Simulink will certainly have the same issue. So this issue would come up as well.

The specific error message you mentioned indicated you were trying to generate code with a rosactionclient call in it (I assume in a MATLAB Function block). Ensure that your Simulink model is not using Accelerator or Rapid Accelerator mode, as those try to generate code before running, if you need to use actions. Or keep the model just using publishers and subscribers if you need to generate code.

-Cam

[Ehtisham ul Hasan]

Hi

I have tried to follow instruction on the link

https://it.mathworks.com/matlabcentral/answers/283695-updating-existing-custom-message-types-with-rosgenmsg

I could not find ros message defintions folder on my Ubuntu 16.04 or even the javaclasspath.txt. Please guide so that I can update message defintions. Thank you

[Ehtisham ul Hasan]

With regard to your previous comment. Just a refresher, as I think I have not been able to explain correctly. The rosaction in the from of m-file (as per PR2 example) works fine for my model. I get the error what i shared but the execution takes place from m-file as desired for said joint positions.

Hope it gets easier to understand now.

(My concern at the moment is if something works from an m-file regardless of the error, then it should eventually work from simulink through matlab function blocks.)

Best

Hasan

Answer 2

[Ehtisham ul Hasan]

Hi,

Following through number of posts and documentation I am currently at the following stage. I found from a mathwork post and at the following link https://it.mathworks.com/help/ros/ref/rosgenmsg.html that though /opt/ros/kinetic/share although holds scattered msg packages but can be given as folderpath in rosgenmsg(foderpath).

I am using ROS Kinetic and Matlab 2020a on Ubuntu 16.04 OS and I did the following in matlab

>> rosgenmsg('/opt/ros/kinetic/share')

Unrecognized function or variable 'rosgenmsg'.

Did you mean:

>> ros2genmsg('/opt/ros/kinetic/share')

Identifying message files in folder '/opt/ros/kinetic/share'..Done.

Error using ros.ros2.internal.createOrGetLocalPython (line 55)

Could not find Python version 3.7 or higher on your system. If you have multiple versions of python, use

pyversion to set the path to the executable for the version of Python to use.

Error in ros2genmsg (line 53)

ros.ros2.internal.createOrGetLocalPython(); %ensure python is available

>> pyenv

ans =

PythonEnvironment with properties:

Version: "2.7"

Executable: "/usr/bin/python"

Library: "libpython2.7.so.1.0"

Home: "/usr"

Status: NotLoaded

ExecutionMode: InProcess

Please note I am using ROS1 and not ROS2 but matlab did not read rosgenmsg command and instead asked for ros2genmsg command and even then did not work.

Please help sort out this issue.

Thanks

Best

Hasan

[Cam Salzberger]

In R2020a, ROS 1 use of rosgenmsg requires the custom message support package installed. See the linked pages from here for some general guidance.

R2020b+ includes rosgenmsg in-product, and does not require a support package. Ensure that you are looking at the documentation for your MATLAB release.

ROS 2 is very different from ROS 1. See here and here for an overview.

-Cam

[Ehtisham ul Hasan]

Hi,

I installed the custom message support package. Then I ran rosgenmsg ('/opt/ros/kinetic/share') and it gave me the following error

>> rosgenmsg('/opt/ros/kinetic/share')

Cannot create folder /opt/ros/kinetic/share/matlab_gen. It is likely that the location is not writable. Error

message: Permission denied

I also tried running the rosgenmsg with the command

rosgenmsg('~/Documents/MATLAB/SupportPackages/R2020a/toolbox/ros/supportpackages/roscustommsg')

and it showed me the following errors

Checking subfolder "+matlabshared" for custom messages.

Warning: The folder

/home/pal/Documents/MATLAB/SupportPackages/R2020a/toolbox/ros/supportpackages/roscustommsg/+matlabshared does

not contain a valid ROS package, because the 'package.xml' file is missing. Create the 'package.xml' file in

this folder.

> In ros.custommsg.internal/CustomMessageJAR/buildFolders (line 93)

In rosgenmsg (line 43)

Checking subfolder "+ros" for custom messages.

Warning: The folder

/home/pal/Documents/MATLAB/SupportPackages/R2020a/toolbox/ros/supportpackages/roscustommsg/+ros does not

contain a valid ROS package, because the 'package.xml' file is missing. Create the 'package.xml' file in this

folder.

> In ros.custommsg.internal/CustomMessageJAR/buildFolders (line 93)

In rosgenmsg (line 43)

Checking subfolder "examples" for custom messages.

Warning: The folder

/home/pal/Documents/MATLAB/SupportPackages/R2020a/toolbox/ros/supportpackages/roscustommsg/examples does not

contain a valid ROS package, because the 'package.xml' file is missing. Create the 'package.xml' file in this

folder.

> In ros.custommsg.internal/CustomMessageJAR/buildFolders (line 93)

In rosgenmsg (line 43)

Checking subfolder "help" for custom messages.

Warning: The folder

/home/pal/Documents/MATLAB/SupportPackages/R2020a/toolbox/ros/supportpackages/roscustommsg/help does not

contain a valid ROS package, because the 'package.xml' file is missing. Create the 'package.xml' file in this

folder.

> In ros.custommsg.internal/CustomMessageJAR/buildFolders (line 93)

In rosgenmsg (line 43)

Checking subfolder "internal" for custom messages.

Warning: The folder

/home/pal/Documents/MATLAB/SupportPackages/R2020a/toolbox/ros/supportpackages/roscustommsg/internal does not

contain a valid ROS package, because the 'package.xml' file is missing. Create the 'package.xml' file in this

folder.

> In ros.custommsg.internal/CustomMessageJAR/buildFolders (line 93)

In rosgenmsg (line 43)

Checking subfolder "registry" for custom messages.

Warning: The folder

/home/pal/Documents/MATLAB/SupportPackages/R2020a/toolbox/ros/supportpackages/roscustommsg/registry does not

contain a valid ROS package, because the 'package.xml' file is missing. Create the 'package.xml' file in this

folder.

> In ros.custommsg.internal/CustomMessageJAR/buildFolders (line 93)

In rosgenmsg (line 43)

No message packages were found in folder

/home/pal/Documents/MATLAB/SupportPackages/R2020a/toolbox/ros/supportpackages/roscustommsg. Check to make

sure the path to your folder is correct.

Just because the message definitions do not match why I have to develop any folder on my own. Why a simple update cannot do the Job by running rosgem message on /opt/ros/kinetic/share fodler that has all the messages available (scattered but yes) ?

Kind help is requested please

Best

Hasan

[Ehtisham ul Hasan]

Client [/matlab_global_node_72273] wants topic /mhuman/gripper_controller/follow_joint_trajectory/result to have datatype/md5sum [control_msgs/FollowJointTrajectoryActionResult/bce83d50f7bb28226801436caf0e2043], but our version has [control_msgs/FollowJointTrajectoryActionResult/c4fb3b000dc9da4fd99699380efcc5d9]. Dropping connection.

I am taking the message definition from ROS network and not really defining my own.

Best

Hasan

[Ehtisham ul Hasan]

I would be really glad to know if there is any documentation on using the rosactionclient developed in the mfile (based on PR2 example) in the form of simulink blocks of robotics system toolbox. As when I run my m-file, the arm does respond despite of this error but any documentation on arranging the same code in the form of simulink blocks can be of great help in parallel to fixing the topic mismatch.

Kind help requested please

Best

Hasan

[Ehtisham ul Hasan]

As per m-file, I tried alot of different settings and now this is the latest error I am getting (screenshot attached).

I have also attached the m-file (working) and the simulink model from where you can open the Matlab function block and compare its code with m-file and suggest possible amendments.

Best

Hasan

[Ehtisham ul Hasan]

Hi,

I kindly request the Mathworks team to kindly share some documentation on how can I integrate a rosaction client made in m-file into simulink using ROS toolbox and publish it to my ros enabled arm.

Best

Hasan

[charles guo]

hi, I come across the same problem. Have you ever gotten a solution?

[Ehtisham ul Hasan]

Hi,

Unfortunatley I did not find any solution and there were no more support options available. I was not even using any custom message. In my case ROS kinetic was giving me errors for even the available messages so I thought of simply updating the message definitions that should do the job but it did not ever get updated. So I had to take a decision on my own. Since more than using a custom message my aim was using trajectroy_msgs/JointTrajectory messages so I switched to ROS melodic and Matlab 2020b (fortunately it was possible for me to switch to it) and it did the job for me.

This might not be the soolution in your case as I dont know whether you can switch between ROS and Matlab Versions or not but if you are planning to use custom ros message in Matlab I would suggest using ROS Melodic and Matlab 2020b. I hope it helps in someway.

Best

Hasan

[charles guo]

Thank you for your quick reply. Now, I'm working on ur10 robot , and have tried the method mentioned in "Control PR2 Arm Movements" example , but it didn't work. Would you mind sharing the simulink file as a reference. Thank you again.

[charles guo]

I also find that action is not supported in simulink as descrbed in ROS Simulink Support and Limitations. There is no action block in ROS Toolbox.

[Ehtisham ul Hasan]

Hi,

Yes same conclusion, you can only use rosactions from m-file and not from simulink, it does not support that. So, Its better not to waste time on that, I would suggest you see if you can just use rostopics to publish to your robot.

This can be done by understanding this Mathworks thread that helped me. Just go through the thread and directly download the testrosmsgs.zip provided by one of the Mathworks Staff

https://it.mathworks.com/matlabcentral/answers/344938-simulink-ros-publishing-jointtrajectory-messages

It rather not easy to understand it as the message definitions in Matlab compared to ROS are too complex the way we need to assign the values. Ammend the code to your joint names and joint positions. But it will only send data to ROS and not to Gazebo and for that you just need ROS time used properly. For which I attach my model for an idea. Also you might need to do a few more coloumn rows switching in the code as I did which you can see by simply comparing the two files. Hope it helps.

NOTE: All this is still in joint space and not in cartesian space unfortunately.

Best Regards

[charles guo]

Well done! Great appreciation for your help.