How to export robot URDF file from SolidWorks?

This post describes the detailed procedures for exporting URDF files of a robot from SolidWorks.

Test Environment:

• SolidWorks 2022, running on a PC with Windows 11 OS.
• The resulting URDFs are tested in a catkin_ws on a PC with Ubuntu 20.0.
• ROS1, noetic, Ubuntu 20.04.6 LTS

Main testbed:

• Panda urdf export.
• Kuka urdf export.

1. Install plugin tool (SW2URDF) in SolidWorks in Windows OS.

• Download “sw2urdfSetup.exe” from “SolidWorks to URDF Exporter (https://wiki.ros.org/sw_urdf_exporter )”
• Follow the instructions there, and install “sw2urdfSetup.exe”.
• If installation is successful, you should be able to find it in SolidWorks. When SolidWorks is open, under “Tools > Add-ins” you will see a SW2URDF item at the very bottom as shown below.

2. Create a new assembly file (.SLDASM) in SolidWorks and Setup/Assemble the robot from parts.

In SolidWorks 2022 or other versions, create a new filename.SLDASM file, and rename it with your own file name, e.g., I rename mine as “panda.SLDASM”.

Note: This name has nothing to do with the names later in URDF files, so you can name it whatever you want. Actually, for any names outside of the URDF configuration interface, they do not affect the names inside the URDF, because you will need to type in the link/joint names manually inside the URDF configuration anyway when you reach there.

Before proceeding, you should collect all the robot links’ parts (.SLDPRT files) into one dedicated folder, because later you will need to import them into the panda.SLDASM file one by one.

Now in the brand new panda.SLDASM file, import the first part which is the robot’s “base”, then put it anywhere by a click, and the “base” link will be fixed there. Again, the name and position of the links do not matter, you can keep the default for easy.

Same procedures, import all other link.SLDPRT files (e.g., link1, link2, link3,…, link7), put them one by one aside in the panda.SLDASM environment. If a specific part needs to be moved or rotated around itself, you can use the “Move Component” function. Or you can do this right before assembling it when needed.

Starting from the “base” link, create a mate at the joint for each two adjacent links. For revolute joint, it usually requires two mates, “Concentric” mate and “Coincident” mate. The “Concentric” mate is used to connect to the two links via a co-axis (they can still rotate around the co-axis and move along the co-axis), while the “Coincident” mate is used to make the two links touch each other exactly on the joint origin point, such that only rotation around the co-axis is allowed.

Before creating the mates, a good practice here is that, in each joint part (right-click the part and open it at the current position), create a new sketch at the plane of each link’s both ends (the plane is the joint plane where the two adjacent links touch each other), put an octagonal polygon (note, hexagon is not good enough.) on the plane for construction while taking the joint origin as the center of the octagonal polygon.

Then draw two lines (the horizontal and vertical lines for construction) by connecting the appropriate points on the octagon vertices. NOTE: These two lines and the octagons will be the critical references when you want to accurately (in term of visual observation) align the two adjacent links to be at 0 deg.

You may also want to put something extra (e.g., another smaller octagon polygon) as a marker to mark out which end of the link is n and which one is n+1 (sometimes, some links are too similar to discriminate (e.g., the Panda’s links), so with this marker you can quickly discriminate which one is which end.)

In each sketch, at the joint origin, it is better to put a sketch Point entity there (strongly suggested), it will later help you find this joint origin quickly and accurately via this sketch Point entity.

3. Add references frames and axis for each joint.

After creating all the mates for joints/links, the robot should be movable when you drag any of the link, it will be able to rotate around the joint axis (joint origin).

IMPORTANT NOTE HERE: Now it is better to rotate all joints to be at 0 degrees, and then lock the mates at that position which will disable the joint rotation. Aiming for a high level of accuracy for the link alignments (still on the visual observation level), you can take the horizontal & vertical lines inside the octagon as a reference when rotating the joint.

Now you need to add reference coordinates and reference axis on each joint in the panda.SLDASM file. Looking for top menu “Reference->Coordinate System” and “Reference->Axis” in the Assembly tab.

A good practice for adding “Coordinate System” and “Axis” here is to:

• add “joint0_frame” and “axis0” to the starting side of the “base”(link0),
• add “joint1_frame” and “axis1” to the starting side of the “link1”,
• add “joint2_frame” and “axis2” to the starting side of the “link2”,
• and so on.
• Good practice: set the previous link part as invisible when you work on the current link part, this will allow you to find the sketch Point and octagon correctly and quickly.
• NOTE: Here it is strongly suggested to rename them as “joint0_frame” and “axis0” for “base” link, “joint1_frame” and “axis1” for “link1” link, and so on. This will provide huge convenience when use them later in URDF configuration.

IMPORTANT NOTE: The Reference frames (Coordinate System) xyz directions must be exact same as the one you defined in the DH table and frame attachments, this will avoid you to do point/negative calibration for each joint later because you already correctly defined each frame here. IF you do not have a DH table and frame attachments, then you can obey the basic rules: (1) z-axis is always the joint rotation axis; (2) x-axis can be pointing to the direction of the link axis (assume the link is a cylinder here). To be precisely, the x-axis is pointing to the common perpendicular of the two adjacent joint axes.

The reference Coordinate System is attached to the joint origin Point, and use the earlier created horizontal & vertical lines in the octagon to align properly with the XYZ axis of the Coordinate System.

The reference Axis is created by going through the joint origin Point and normal to the joint rotating plane.

Make sure that all the created mates, reference frames and reference axis are all in the panda.SLDASM level, NOT inside the parts as shown below.

4. URDF configuration setup.

Now you can start to configure the URDF configuration.

Looking for the “tools/Export as URDF”, and click, it will start to open a URDF configuration (it will appear in the tree list, and save there).

NOTE: Remember to save the URDF configuration frequently during the configuration by clicking the green tick sign (check mark). This is because sometime when you click to select the link body, it may select a wrong component by mistake, you may end up with selected a curve which will automatically get you out of the URDF configuration UNSAVED. Then you need to repeat all the procedures again.

In a blank brand new URDF configuration interface, you can start to set up the link/joint connections. See the screenshot below for the details.

5. URDF export.

Once all this URDF configuration done, save it first by clicking the green tick sign, this will save all the configurations, otherwise you may need to go through all them again when need it again.

When all URDF configuration done and saved correctly, open it again, and check it though to the last link, make sure all correct, then click “Preview and Export”. This will open interface to export URDF files, see the screenshots below.

Now here you just need to check, ensure the joint types are correct as set earlier. Keep all other parameters as default if you do not know them (e.g., inertia, mass, color, etc), no matter whether they are blank, zeros, or auto-generated values, all ok, just use the default is ok.

Then select “Fine” in “Mesh Detail”, and click “Export URDF and Meshes”, re-name the export file name as “panda1”, or the name you needed in the URDF files. This will save all the URDF files in a folder named “panda1” (see screenshot below). NOTE, this name “panda1” is very important, it will be the name used everywhere inside the URDF file.

• “C:\Users\tengl\Desktop\20251205_CAD\panda_urdf\panda1\urdf\panda1.urdf”
• “C:\Users\tengl\Desktop\20251205_CAD\panda_urdf\panda1\meshes\link1.STL”
• “panda1\urdf\panda1.urdf”
• “panda1\meshes\link1.STL”

In order to test the URDF in RVIZ, In “panda1.urdf” file, you may need to rename the link names, joint names, and the paths. For example,

• “link1” -> “panda1/link1”
• “joint1” -> “panda1/joint1”
• filename=”package://panda1/meshes/base.STL” -> filename=”package://franka_description/panda1/meshes/base.STL” All done.

6. URDF export all done.

Now you have done all the procedures for exporting robot’s URDF, and you got the URDF files for a robot. Next you just need to do some tests and see if the URDF works correctly.

7. URDF Test Examples (Panda & Kuka)

URDF Test Example 1: Franka Panda (fr3) robot URDF test passed ok (Left: original URDF from the company; Right: self-exported URDF from SolidWorks.)

Test results: all joints works correctly and same as the official panda URDF. All positive/negative directions for each joint are correct, and no need to calibrate them.

URDF Test Example 2: Kuka (LBR iisy 11 R1300) robot URDF test passed ok (Left: original URDF from GitHub; Right: self-exported URDF from SolidWorks.)

Test results: all joints works and can rotate around the correct axis. The Reference Coordinate Systems is arbitrarily specified, so they are not correct yet, due to the fact that I do not have a DH table and frame attachment for it when do the URDF export and test.

Created on 2026-01-22.

How To convert [xacro] to [urdf] for Franka Emika Panda robot?

This post describes how you can create .urdf file from .xacro file for the Franka Emika Panda robot, based on the officially provided “franka_description (official)”.

Environment:

• ROS1
• rosdistro: noetic
• Ubuntu 20.04.6 LTS
• Source “franka_description (official)”
• Improved “franka_description (teng4)” with Panda URDF files inside.
• For application of the URDF file, please see here for a demo video.

STEP-0: Install Docker

Docker is required for the process of xacro -> URDF. The installation is quick and easy. With the Docker, you can also run Franka’s official code to visualize the Panda robot in rviz.

$ sudo apt update
$ sudo apt install docker.io
$ sudo systemctl enable docker
$ sudo systemctl start docker
$ newgrp docker
$ whoami #this will output the USERNAME
$ sudo usermod -aG docker USERNAME
$ groups #should have "docker" as one output
$ sudo reboot #reboot or logout-login the PC is required.
$ docker run hello-world

STEP-1: Create Panda URDF from .xacro, need to do:

(1) Put the original folder “franka_description (official)” into a valid catkin_ws, ensure the subfolder “scripts” exists, which include the code (create_urdf.sh etc) will be used to create URDF files later.

(1.1) Commands for generating URDF from Franka’s xacro.

NOTE-1: Docker is required.

NOTE-2: It is better to NOT use abs-path, it may have problems when using the URDF later.

  # Start the generation of the urdf model
$ ./scripts/create_urdf.sh <robot_id> <ee_id>
  # id of the robot model: `fr3`, `fp3`, `fer`, `multi_arm`, `none`
  # id of the robot end effector: `franka_hand` (default), `cobot_pump`
  # Run the following inside *"USERNAME@RT6958U-NESR:~/catkin_ws4/src/franka_description$"*
$ cd ~/catkin_ws4
$ source devel/setup.bash #required
$ cd ~/catkin_ws4/src/franka_description
  # NOTE: It is better to **NOT** use `abs-path`, it may have problems when using the `URDF` later.
$ ./scripts/create_urdf.sh fr3 --robot-ee franka_hand --with-sc --abs-path --no-prefix 
$ ./scripts/create_urdf.sh fr3 --robot-ee franka_hand --with-sc --no-prefix
$ ./scripts/create_urdf.sh fp3 --robot-ee franka_hand --with-sc --no-prefix
$ ./scripts/create_urdf.sh fer --robot-ee franka_hand --with-sc --no-prefix
$ ./scripts/create_urdf.sh multi_arm --robot-ee franka_hand --with-sc --no-prefix

(1.2) If cmd in (1.1) does not work, run the following inside “USERNAME@RT6958U-NESR:~/catkin_ws4/src/franka_description$”:

$ python3 ./scripts/create_urdf.py fr3 --robot-ee franka_hand --with-sc --no-prefix
$ python3 ./scripts/create_urdf.py fp3 --robot-ee franka_hand --with-sc --no-prefix
$ python3 ./scripts/create_urdf.py fer --robot-ee franka_hand --with-sc --no-prefix
$ python3 ./scripts/create_urdf.py multi_arm --robot-ee franka_hand --with-sc --no-prefix
$ python3 ./scripts/create_urdf.py fr3 --robot-ee cobot_pump --with-sc --no-prefix
$ python3 ./scripts/create_urdf.py fr3 --robot-ee cobot_pump --with-sc --no-prefix

(2) In the newly generated URDF file, need to do:

• Need to correct one conversion mistake for "fr3_link8" -> "link8".

• The last two fingers "prismatic" -> "fixed", otherwise it will have few related errors in rviz.

STEP-2: Panda in .rviz file, needs to do:

(1) Link names do not matter, and you can leave them alone.

(2) Robot Description: /panda1/robot_description (if your publish topic name is /panda1/joint_states)

(3) Fixed Frame: link0

STEP-3: Panda in .cpp file, need to do:

(1) Ensure the joint names are exactly the same as the ones in the URDF file.

(2) The publish topic name will be /panda1/joint_states.

STEP-4: if all ok, double check with:

$ rosrun rqt_tf_tree rqt_tf_tree

(optional) STEP-5: Visualize Panda via Franka’s official code:

It would be interesting to try the officially provided code for visualizing the Panda robot in rviz, it will have an independent small panel to control each joint angle via a slider.

  # Run the following inside "USERNAME@RT6958U-NESR:~/catkin_ws4/src/franka_description$"
$ ./scripts/visualize_franka.sh arm_id:=<robot_id> 
$ ./scripts/visualize_franka.sh arm_id:=fr3 #it will show a realistic white franka robot in rviz.
$ ./scripts/visualize_franka.sh arm_id:=fp3 #it will show a realistic black franka robot in rviz.
$ ./scripts/visualize_franka.sh arm_id:=fer #it will show a realistic white franka robot in rviz.

Created on 2025-04-16.

How To generate a triangular mesh plane in Blender for Unity?

In Unity, the default plane mesh is a 10x10 mesh (to be precisely 11x11 points). Sometimes you may need a plane mesh with more dense points, e.g., 50x50, 100x100, or even more, such that you can render a cloth plane with more details.

Here below are the steps for generating a 100x100 mesh plane in Blender and exporting it as an OBJ file which can be used in Unity.

Steps to generate OBJ file from blender.

1. Open Blender in Ubuntu: cd blender-3.0.0-linux-x64/ -> ./blender.
2. A to select all.
3. Shift+A to select Mesh -> Plane. Note, the default size of plane is 2m in Blender.
4. Rotate the plane if needed.
5. TAB to Edit Mode.
6. Select the plane, right-click, select Subdivide, then at the bottom left corner set the cut numbers as 99 (for obtaining a plane with 100x100 small squares). Note, limits to Max 100 cuts in Blender.
7. Select all vertices, then go to tab Face -> Triangulate Faces, this will generate triangular meshes on the plane. Note, Unity requires triangular mesh instead of quadrilateral mesh.
8. HOWEVER, the triangular mesh’s edges are not always in the same orientation. To obtain mesh edges all with the same orientation, DO NOT use the Face -> Triangulate Faces. Instead, select the plane object, go to the modifier (a wrench icon), add a Triangulate Modifier, Set Quad Method as Fixed, set N-gon Method as Beauty. Click Apply. TAB to check the result, and now all edges of the triangular mesh should be with the same orientation.
9. TAB to Object Mode, Ctrl+A to select and apply All Transforms.
10. Export to OBJ files: check SectionOnly, check Geometry/Triangulate Faces, (you can also try Keep Vertex Order).
11. Then click Export.
12. Done.

Import OBJ file into Unity:

• Remember to check Read/Write, and Apply. (Important! Although no Error msgs if not in Unity.).
• Import New Assets, then browse to find the OBJ file.
• Directly putting the OBJ file into the target folder in Unity seems also ok and has the same result as via Import New Assets.

Created on 2025-01-29.

How To display 2 or more robots in the same RViz environment?

Steps-1

Make sure two publishers are in separate namespace: "/robot1/joint_states", "/robot2/joint_states". Importantly, when constructing the msg, the joint names should be exactly the same as the ones used in the corresponding URDF files, robot2 joint names could be "robot2/waist", "robot2/shoulder", "robot2/elbow", etc.

Steps-2

In the URDF file of robot2, add namespace for all links and joints, e.g., "robot2/waist", "robot2/shoulder", "robot2/elbow", etc. Note that, for robot1, you can add and use namespace as well like what we did for robot2, but for simplicity, the robot1 URDF and its joint state publisher can remain unchanged which means it is not necessary to add namespace for robot1 URDF and its joint state publisher.

Steps-3

To move the base location of robot2 in RViz environment, use <node pkg="tf2_ros" type="static_transform_publisher" name="robot2_base_transform" args="0.1 0 0 0 0 0 world robot2/base" /> in the lauch file, which will move the robot2 base to x=0.1m. Note that, (a) the "robot2/base" is according to the name you defined in the URDF file. (b) this static base transform requires to set the "Fixed Frame" in rviz file to be "world" instead of "base" ("base" in the base name used in URDF file).

Steps-4

In the *.rviz file, add or modify as the following,

Displays:
    - Alpha: 0.5
      Cell Size: 1
      Class: rviz/Grid
      Color: 160; 160; 164
      Enabled: true
      Line Style:
        Line Width: 0.03
        Value: Lines
      Name: Grid
      Normal Cell Count: 0
      Offset:
        X: 0
        Y: 0
        Z: 0
      Plane: XY
      Plane Cell Count: 10
      Reference Frame: <Fixed Frame>
      Value: true
    - Alpha: 1
      Class: rviz/RobotModel
      Collision Enabled: false
      Enabled: true
      Links:
        All Links Enabled: true
        Expand Joint Details: false
        Expand Link Details: false
        Expand Tree: false
        Link Tree Style: Links in Alphabetic Order
        base:   #(Note: make sure these link names are exactly same as you used in the URDF file.)
          Alpha: 1
          Show Axes: true   #(Note: you can set it true to show the base frame axis in rviz.)
          Show Trail: false
          Value: true
        lower_arm:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        stylus:
          Alpha: 1
          Show Axes: true    #(Note: you can set it true to show the EE frame axis in rviz.)
          Show Trail: false
          Value: true
        tip:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        torso:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        upper_arm:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        wrist:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
      Name: RobotModel1  #(Note: set a unique name for this robot.)
      Robot Description: /robot1/robot_description  #(Note: modify here.)
      TF Prefix: ""  #(Note: DO NOT modify here.)
      Update Interval: 0
      Value: true
      Visual Enabled: true
    - Alpha: 1
      Class: rviz/RobotModel
      Collision Enabled: false
      Enabled: true
      Links:
        All Links Enabled: true
        Expand Joint Details: false
        Expand Link Details: false
        Expand Tree: false
        Link Tree Style: Links in Alphabetic Order
        base:   #(Note: make sure these link names are exactly same as you used in the URDF file.)
          Alpha: 1
          Show Axes: true    #(Note: you can set it true to show the base frame axis in rviz.)
          Show Trail: false
          Value: true
        lower_arm:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        stylus:
          Alpha: 1
          Show Axes: true    #(Note: you can set it true to show the EE frame axis in rviz.)
          Show Trail: false
          Value: true
        tip:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        torso:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        upper_arm:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        wrist:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
      Name: RobotModel2  #(Note: set a unique name for this robot.)
      Robot Description: /robot2/robot_description  #(Note: modify here.)
      TF Prefix: "" #(Note: DO NOT modify here.)
      Update Interval: 0
      Value: true
      Visual Enabled: true
  Enabled: true
  Global Options:
    Background Color: 48; 48; 48
    Fixed Frame: world  #(Note: modify here.)
    Frame Rate: 30

Steps-5

Adjust the pose of the two robots in rviz using a mouse, and save the rviz file (This will auto correct some minor mistakes you may have made when editing the rviz file, e.g., the robot2 links should be "robot2/base:", "robot2/lower_arm:", etc.).

Steps-6

Finally, in the launch file, add the following code to launch the two robots with different namespaces.

<launch>
  <!-- Launch Robot 1 -->
  <group ns="robot1">
    <param name="robot_description" command="cat $(find robot_description)/urdf/robot1.urdf" />
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="robot1_base_transform"
      args="-0.5 0 0 0 0 0 world base" /> 
      <!-- note, static base transform requires to set the "Fixed Frame" in rviz file to be "world" instead of "base". -->
  </group>

  <!-- Launch Robot 2 -->
  <group ns="robot2">
    <param name="robot_description" command="cat $(find robot_description)/urdf/robot2.urdf" />    
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="robot2_base_transform"
      args="0.1 0 0 0 0 0 world robot2/base" />
      <!-- note, static base transform requires to set the "Fixed Frame" in rviz file to be "world" instead of "base". -->
  </group>
  <node name="rviz2" pkg="rviz" type="rviz" args="-d $(find robot_common)/launch/robotx2.rviz" required="true" />
</launch>

Steps-7

Now you can run the launch file, if it can correctly display two robots in the same RViz, then you can command "$ rosrun rqt_tf_tree rqt_tf_tree" to check the tf tree.

Notes

• Note, it is better to NOT use <remap from="/tf" to="robot2/tf" /> and <remap from="/tf_static" to="robot2/tf_static" /> in the launch file. Because it will make the command "$ rosrun rqt_tf_tree rqt_tf_tree" be invalid when you want to check the tf tree. /tf itself can discriminate the joint states based on namespace, so no need to remap /tf.

Created on 2024-11-22.

How to use the variable List in Unity?

Steps for test a List

1. declare a List variable dh = new List<float[]>();.
2. define a row vector float[] row1 = { 11.0f, 12.0f, 13.0f, 14.0f };.
3. add the row vector to list dh.Add(row1);.
4. print out messages print("dh.Count = " + dh.Count).

Example c# code in Unity

Example c# code in Unity.

dh = new List<float[]>();
// Create rows as float arrays and add them to the list directly
float[] row1 = { 11.0f, 12.0f, 13.0f, 14.0f };
float[] row2 = { 21.0f, 22.0f, 23.0f, 24.0f };
float[] row3 = { 31.0f, 32.0f, 33.0f, 34.0f };
float[] row4 = { 41.0f, 42.0f, 43.0f, 44.0f };
// Use Add() method to add rows to the list
dh.Add(row1);
dh.Add(row2);
dh.Add(row3);
dh.Add(row4);
print("teng4test, mdh is defined as: dh.Count = " + dh.Count); //=4
print("teng4test, mdh is defined as: dh[1][1] = " + dh[1][1]); //=22
print("teng4test, mdh is defined as: dh[2][2] = " + dh[2][2]); //=33
print("teng4test, mdh is defined as: dh[3][3] = " + dh[3][3]); //=44
// Print the list values using Debug.Log
print("dh.GetType() = " + dh.GetType()); //= System.Collections.Generic.List`1[System.Single[]]
print("dh[1].GetType() = " + dh[1].GetType()); //= System.Single[].
print("dh[1][1].GetType() = " + dh[1][1].GetType()); //= System.Single
print("dh.Capacity = " + dh.Capacity); //= 4
//print("dh[1].Capacity = " + dh[1].Capacity); //will error: float no capacity.
//print("dh[1][1].Capacity = " + dh[1][1].Capacity); //will error: float no capacity.
foreach (float[] row in dh)
{
    string rowValues = string.Join(", ", row);
    Debug.Log("Row: " + rowValues);
}

//try to define another list2.
var list2 = new List<double> {2014.0, 2015.0, 2016.0, 2017.0};
foreach (double row in list2)
{
    string rowValues = string.Join(", ", row);
    Debug.Log("list2 Row: " + rowValues);
}

Created on 2024-10-04.

How To create a customized ROS msg in MATLAB?

MATLAB Help

step-1. Place your custom messages in a location and note the folder path.

It is recommended to put all your custom message definitions in a single packages folder.

% folderpath = 'c:\MATLAB\custom_msgs\packages';
folderpath = '/home/teng/MATLAB/custom_msgs/packages';

The above cmd no need to run, but you need to find your custom msg package folder in catkin_ws, and better to copy it to a single package folder in MATLAB. For example, my custom msg is located in /home/teng/catkin_ws2/src/phantom_omni/teng4pkg_msgs, then I copy the package folder teng4pkg_msgs into the MATLAB path /home/teng/MATLAB/custom_msgs/ where all the custom ROS msg definitions to be used in MATLAB will be collected here in the future.

step-2. In MATLAB, run the command to create custom msg for MATLAB,

% rosgenmsg('c:\MATLAB\custom_msgs')
rosgenmsg('/home/teng/MATLAB/custom_msgs/')

Matlab will run and show the following info if successful,

rosgenmsg(‘/home/teng/MATLAB/custom_msgs/’) Identifying message files in folder ‘/home/teng/MATLAB/custom_msgs’..Done. Validating message files in folder ‘/home/teng/MATLAB/custom_msgs’..Done. [1/1] Generating MATLAB interfaces for custom message packages… Done. Running catkin build in folder ‘/home/teng/MATLAB/custom_msgs/matlab_msg_gen_ros1/glnxa64’. Build in progress. This may take several minutes… Build succeeded.build_log.

To use the custom messages in Matlab, follow these steps:

1. Add the custom message folder to the MATLAB path by executing:

addpath('/home/teng/MATLAB/custom_msgs/matlab_msg_gen_ros1/glnxa64/install/m')
savepath

Note, the above cmd may need to run each time you restart MATLAB (not tested), But it is ok.

2. Refresh all message class definitions, which requires clearing the workspace, by executing:

clear classes
rehash toolboxcache

3. Verify that you can use the custom messages.

Enter “rosmsg list” and ensure that the output contains the generated custom message types.

rosmsg list

It will show a long list including your custom msg types. For example, mine is,

teng4pkg_msgs/Arm_State
teng4pkg_msgs/PoseCartesian6DOF
teng4pkg_msgs/Vector7DOF

4. Check the details of a specific msg type

% custommsg = rosmessage('B/Standalone')
custommsg = rosmessage('teng4pkg_msgs/PoseCartesian6DOF')

custommsg = ROS PoseCartesian6DOF message with properties: MessageType: ‘teng4pkg_msgs/PoseCartesian6DOF’ X: 0 Y: 0 Z: 0 A: 0 B: 0 G: 0 Use showdetails to show the contents of the message

Bingo. All done. Now you can use the custom msg in Matlab.

Debugging.

Matlab may need to configure ROS first. If so, first run this cmd in a Ubuntu terminal.

whereis python #Find the location of python in your pc.
sudo apt install python3.8-venv

Then, in MATLAB go to find Home->Preferences->ROS Toolbox, click button Open ROS Toolbox Preferences. put the path (e.g., mine is /usr/bin/python3.8) in the ‘browse’ line, and click Recreate Python Environment. Done.

Created on 2024-10-01.

How To convert ROBOT.urdf.xacro to ROBOT.urdf?

Terminal command to convert the ROBOT.urdf.xacro to ROBOT.urdf (open a terminal inside its folder).

rosrun xacro xacro --inorder -o ./niryo_one.urdf ./niryo_one.urdf.xacro

The above command is used to convert a .xacro file (niryo_one.urdf.xacro) into a URDF file (niryo_one.urdf), using the --inorder option.

Command Breakdown:

• rosrun xacro xacro: Invokes the xacro command using rosrun, which is part of the xacro package in ROS.

• --inorder: Ensures that the processing of macros follows the order in which they appear in the Xacro file. This is recommended for newer Xacro versions and is sometimes necessary to ensure macros are processed correctly, especially in more complex models.

• -o ./niryo_one.urdf: The -o flag specifies the output file (niryo_one.urdf), which will be generated by converting the .xacro file.

• ./niryo_one.urdf.xacro: The input .xacro file, which contains the robot description with macros.

How to generate urdf form xacro

(from dvrk code, dvrk_model/Readme.md, seems no difference with the above command.)

rosrun xacro xacro mtm_right_only.urdf.xacro > mtm_right_only.urdf
rosrun xacro xacro <input.xacro> -o <output.urdf> //(Syntax from ChatGPT)

Command Breakdown:

• rosrun xacro xacro: Runs the xacro command using rosrun to process the .xacro file.
• mtm_right_only.urdf.xacro: The input .xacro file that defines the robot description.
• >: Redirects the output of the command to a file.
• mtm_right_only.urdf: The output file where the URDF is stored.

Note

An example of robot URDF source files can be found here niryo_one_ros.

Created on 2024-09-23.

How to configure Unity to receive ROS data and import URDF robot model?

Suppose, you have already had a well-configured catkin_ws for publishing data to a ROS node (which will be subscribed by Unity later).

• Require a ROS-TCP-Endpoint package in your catkin_ws.
• Require a msg folder where you defined your own message type, it should be in a specific package in your catkin_ws.

Step-1: Unity add packages (ROS-TCP; Visualizations)

(Source)

Using Unity 2020.2 or later, open the Package Manager from Window –> Package Manager.

In the Package Manager window, find and click the + button in the upper lefthand corner of the window. Select Add package from git URL.... Enter the git URL for the desired package. Note: you can append a version tag to the end of the git url, like #v0.4.0 or #v0.5.0, to declare a specific package version, or exclude the tag to get the latest from the package’s main branch.

• For the ROS-TCP-Connector, enter https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector.

• For Visualizations, enter https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.visualizations. Note that visualization is for visualizing the ROS connection etc (e.g., an icon displays the ROS connection status).

• Click Add.

Step-2: Unity add packages (URDF-Importer)

(Source)

Enter the git URL for the URDF Importer with the latest version tag (currently v0.5.2) https://github.com/Unity-Technologies/URDF-Importer.git?path=/com.unity.robotics.urdf-importer#v0.5.2 in the text box and press Enter.

Step-3: Generate corresponding msg in Unity.

In Unity top menu, find Robotics –> Generate ROS Message.

• For ROS message path, click Browse to find the msg definitions folder in your specific catkin_ws, where the ROS-TCP-Endpoint package is also installed. Either use the msg folder or its package folder in catkin_ws both OK, it does not matter, will get the same results.

• For Built message path, click Browse to find the folder where you want to contain the build msg results. By default, it will be using ‘unityROSproject_test1/Assets/RosMessages/Mynewpkg/msg/Arm_StateMsg.cs’

Step-4: Unity add a script to receive ROS data.

• Add a Scripts folder inside folder Assets.
• Inside Scripts folder, create a new cs script (say, ROSDataReceiver.cs) to receive ROS data msg.
• Create an empty object in the scene, rename it as ROSDataReceiver, and attach the ROSDataReceiver.cs to it.
• Note, the received ROS data can be observed here when doing the testing later.

Copy-paste the following code into ROSDataReceiver.cs for receiving data from ROS. (Note, this code allows Unity to receive ROS data without importing any URDF robot model.)

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

using System;
using RosMessageTypes.Geometry;
using RosMessageTypes.Teng4pkg; //teng4note, Change to your own package folder name here! (find it in Assets/RosMessages/)
using Unity.Robotics;
using Unity.Robotics.ROSTCPConnector;
using Unity.Robotics.ROSTCPConnector.ROSGeometry;
using Unity.Robotics.UrdfImporter;
using Unity.Robotics.UrdfImporter.Control;

public class ROSDataReceiver : MonoBehaviour
{
    //Unity.Robotics.UrdfImporter.Control.Controller controller; //note, this "controller" is the "Controller.cs" attached to the robot object.
    
    // Required for ROS communication
    [SerializeField]
    string m_SubscriberName = "/arm_target1"; //note, '/arm_target1' is ROS topic message in customized 'Arm_State.msg' type. [joint_1_angle,.., joint_2_angle].
    GameObject myRobot; //teng4note, this will be defined by selecting the robot object as its 'value'.
    public float data_received_a1 = 0.0f; //to display the received ROS data.
    public float data_received_a2 = 0.0f; //to display the received ROS data.
    public float data_received_a3 = 0.0f; //to display the received ROS data.
    public float data_received_a4 = 0.0f; //to display the received ROS data.
    public float data_received_a5 = 0.0f; //to display the received ROS data.
    public float data_received_a6 = 0.0f; //to display the received ROS data.
   
    // ROS Connector
    ROSConnection m_Ros;


    // Start is called before the first frame update
    void Start()
    {
        //Get ROS connection static instance
        m_Ros = ROSConnection.GetOrCreateInstance();
        
        //Get the Controller.cs component from robot object
        //controller = myRobot.GetComponent<Controller>(); //this retrieves the Controller.cs attached to the robot object.        
        //print("test, controller.name=: " + controller.name);
    }

    // Update is called once per frame
    void Update()
    {
        //Receiving data from ROS node.
        m_Ros.Subscribe<Arm_StateMsg>(m_SubscriberName, MsgDataProcessing);
    }

    void MsgDataProcessing(Arm_StateMsg dataMsg){ 
        data_received_a1 = (float)dataMsg.joint_1_angle; //to display the received ROS data.
        data_received_a2 = (float)dataMsg.joint_2_angle; //to display the received ROS data.
        data_received_a3 = (float)dataMsg.joint_3_angle; //to display the received ROS data.
        data_received_a4 = (float)dataMsg.joint_4_angle; //to display the received ROS data.
        data_received_a5 = (float)dataMsg.joint_5_angle; //to display the received ROS data.
        data_received_a6 = (float)dataMsg.joint_6_angle; //to display the received ROS data.   
        
        //controller.joint_1_target = (float)dataMsg.joint_1_angle; //to display the received ROS data.
        //controller.joint_2_target = (float)dataMsg.joint_2_angle; //to display the received ROS data.
        //controller.joint_3_target = (float)dataMsg.joint_3_angle; //to display the received ROS data.
        //controller.joint_4_target = (float)dataMsg.joint_4_angle; //to display the received ROS data.
        //controller.joint_5_target = (float)dataMsg.joint_5_angle; //to display the received ROS data.
        //controller.joint_6_target = (float)dataMsg.joint_6_angle; //to display the received ROS data.           
    }
}

Test Unity receives ROS data.

In Terminal(s), do the following:

$ roscore

$ echo $ROS_PACKAGE_PATH
$ cd catkin_ws2
$ source devel/setup.bash
$ echo $ROS_PACKAGE_PATH
$ roslaunch ros_tcp_endpoint endpoint.launch tcp_ip:=127.0.0.1 tcp_port:=10000 //this will launch the bridge between ROS and Unity.

$ echo $ROS_PACKAGE_PATH
$ cd catkin_ws2
$ source devel/setup.bash
$ echo $ROS_PACKAGE_PATH
$ roslaunch omni_common omni_state4.launch //this will publish data to ROS. And now the received data in Unity can be observed in “ROSDataReceiver” object.

Error Debugging.

There may have the following warning message when you try to modify the Controller.cs.

The following asset(s) located in immutable packages were unexpectedly altered. Making changes to immutable packages isn’t recommended because the changes can be lost without any warning during Package Manager operations. Packages/com.unity.robotics.urdf-importer/Runtime/Controller/Controller.cs

Soulution

1. Copy-paste the com.unity.robotics.urdf-importer@90f353e435 from youUnityProj/Library/PackageCache/ to folder youUnityProj/Packages.
2. Or, Create a new script, say CustomController.cs to inherit the original Controller class in Controller.cs, then modify in the new script.

Example of the CustomController.cs (not tested).

using Unity.Robotics.UrdfImporter; // Import the original package namespace
using UnityEngine;

// Inherit from the existing Controller class
public class CustomController : Controller
{
    // Example of overriding a method from the original Controller class
    protected override void Start()
    {
        // Call the base Start method from Controller
        base.Start();
        
        // Add custom functionality here
        Debug.Log("Custom controller initialization");
    }

    // Add any new methods or override other methods from Controller.cs here
    public void CustomMethod()
    {
        Debug.Log("This is a custom method in the custom controller.");
    }
}

Notes.

• Originally, com.unity.robotics.urdf-importer@90f353e435 is located in path youUnityProj/Library/PackageCache/. You can copy-paste it from Library/PackageCache to folder Packages to avoid system warning message. (Note that, in it, a Controller folder with several controller scripts need to be modified for your own use.)
• When importing a URDF tool/robot, a “Controller.cs” will be attached to the robot object automatically.

Created on 2024-09-23.

How To get rotation matrix from RPY angles

Summary

• (1) The resulted matrix R_A_B is (new {B} expressed in old {A} frame), which means it may need transpose R_A_B first in order to calculate the position xyz in new {B} frame (e.g., P_B = R_B_A * P_A).
• (2) Roll-X, Pitch-Y, Yaw-Z, always true.
• (3) Positive/negative rotation angles are based on right-hand rule, correct.
• (4) ZYX-euler angles are first rotate around Z, then Y, then X. (Remember, ZYX each time always rotate the updated frame.)’)
• (5) ZYX-euler angles are SAME as the RPY where Z(yaw)Y(pitch)X(roll).

Code (MATLAB)

The following MATLAB code help you calculate rotation matrix (3x3) from RPY angles.

%teng4, this file calculate robot rotation matrix (3x3) based on the
%ZYX-Euler angles.
% from ECE464 ppt 4.2

% ZYX Euler angles are defined as
% (1) First,rotate reference frame {A.old} 'alpha' around Z-axis;
% (2) then, rotate the rotated frame 'beta' around its Y-axis;
% (3) then, rotate the latest frame 'gamma' around its X-axis, results is new frame {B};
% frame{A} is old, and frame{B} is new.
%R_A_B = Rzprime(alpha) * Ryprime(beta) * Rxprime(gamma).
% = [ca, -sa, 0;
%    sa,  ca, 0;
%     0,   0, 1] *
% [cb, 0, sb;
%   0, 1,  0;
% -sb, 0, cb] *
% [1,  0,   0;
%  0, cg, -sg;
%  0, sg,  cg]
%= [ ca.cb, ca.sb.sg-sa.cg, ca.sb.cg+sa.sg;
%    sa.cb, sa.sb.sg+ca.cg, sa.sb.cg-ca.sg;
%      -sb,          cb.sg,          cb.cg]

% Note that, this ZYX-Euler transformation is exactly same as that obtained
% for the same three rotations in the opposite order about the fixed
% angles (i.e., XYZ fixed angles)!
% Rzprime*yprime*xprime(alpha,beta,gamma) = Rxyz(gamma, beta, alpha)
% = Rzprime(alpha) * Ryprime(beta) * Rxprime(gamma)
% = Rz(alpha) * Ry(beta) * Rx(gamma)

roll_x  = -pi/2; %angles around x-axis.
pitch_y = 0;     %angles around y-axis.
yaw_z   = 0;     %angles around z-axis.

%based on ZYX-Euler angles above, we have,
gamma = roll_x;
beta  = pitch_y;
alpha = yaw_z;

g = gamma;
b = beta;
a = alpha;

R_A_B = [ cos(a)*cos(b), cos(a)*sin(b)*sin(g)-sin(a)*cos(g), cos(a)*sin(b)*cos(g)+sin(a)*sin(g);
          sin(a)*cos(b), sin(a)*sin(b)*sin(g)+cos(a)*cos(g), sin(a)*sin(b)*cos(g)-cos(a)*sin(g);
                -sin(b),                      cos(b)*sin(g),                     cos(b)*cos(g)];
%

disp('ZYX-Euler angles -   yaw(z),pitch(y); roll(x)')
disp('ZYX-Euler angles - alpha(z), beta(y);gamma(x)')
disp([alpha, beta, gamma])
disp('R_A_B =: ')
disp(R_A_B)

disp('----------')
disp('teng4 important note: (2024-09-12)')
disp('(1) The resulted matrix R_A_B is (new {B} expressed in old {A} frame),')
disp('      means it may need transpose R_A_B to calculate xyz in new frame.')
disp('(2) Roll-X, Pitch-Y, Yaw-Z, always true.')
disp('(3) Positive/negative rotation angles are based on right-hand rule, correct.')
disp('(4) ZYX-euler angles are first rotate around Z, then Y, then X.')
disp('      (Remember, ZYX each time always rotate around the updated frame.)')
disp('(5) ZYX-euler angles are SAME as the RPY where Z(yaw)Y(pitch)X(roll).')

Created on 2024-09-12.

How To create a customized ROS msg in a catkin_ws?

In ROS, custom message types are defined using .msg files, which are stored in a package’s msg folder. These message types allow you to define custom data structures that can be used for inter-node communication. Below is a guide on how to create and use custom message types in a catkin_ws workspace.

Steps to Customize a msg Folder and Files in a catkin_ws.

1. Create a ROS Package

First, create a package in your catkin_ws workspace. This package will hold your custom messages.

cd ~/catkin_ws/src
catkin_create_pkg teng4pkg_msgs std_msgs rospy roscpp

Here, teng4pkg_msgs is the package name, and it depends on std_msgs, rospy, and roscpp.

2. Create a msg Directory

Inside your package, create a msg folder where your custom message types will be defined.

cd ~/catkin_ws/src/teng4pkg_msgs
mkdir msg

3. Create a ‘.msg’ File

Inside the msg folder, create a file with a .msg extension. This file defines your custom message type.

Example: PoseCartesian6DOF.msg

gedit msg/PoseCartesian6DOF.msg

Inside PoseCartesian6DOF.msg, define the structure of the message. For example, for a 6D cartesian pose:

float64 x
float64 y
float64 z
float64 a
float64 b
float64 g

Each line defines a field in the message, with the type followed by the field name.

4. Modify CMakeLists.txt

You need to modify your package’s CMakeLists.txt file to include the message generation.

• Find and uncomment (or add) message_generation as a required component in find_package:

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
)

• Add the custom message files under add_message_files:

add_message_files(
  FILES
  PoseCartesian6DOF.msg
)

• Add the following after add_message_files() to actually generate the messages. Add generate_messages() and ensure std_msgs and other dependencies are listed:

generate_messages(
  DEPENDENCIES
  std_msgs
)

• Ensure the catkin_package includes message_runtime:

catkin_package(
  CATKIN_DEPENDS message_runtime std_msgs
)

5. Modify package.xml

In your package’s package.xml, ensure you have the following dependencies for message generation:

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

6. Build the Package

After modifying the files, go back to the root of your catkin_ws workspace and build the workspace.

cd ~/catkin_ws
catkin_make

If everything is set up correctly, the custom message types will be generated and ready for use.

7. Using the Custom Message

Now, you can use your custom message type in your ROS nodes.

C++ Example:

// teng4note, the Position.h may not be easy to find or even not exist. But it does not matter.
#include "teng4pkg_msgs/PoseCartesian6DOF.h"

// Publishing a custom message
teng4pkg_msgs::PoseCartesian6DOF test1_msg;
test1_msg.x = 1.11;
test1_msg.y = 1.11;
test1_msg.z = 1.11;
test1_msg.a = 1.11;
test1_msg.b = 1.11;
test1_msg.g = 1.11;
pub_test1_pose6dof.publish(test1_msg);

Python Example:

from teng4pkg_msgs.msg import PoseCartesian6DOF

# Publishing a custom message
test1_msg = Position()
test1_msg.x = 1.11;
test1_msg.y = 1.11;
test1_msg.z = 1.11;
test1_msg.a = 1.11;
test1_msg.b = 1.11;
test1_msg.g = 1.11;

8. Verify the Custom Message

Now, we can verify the custom message in a terminal. First you need to run your file.

echo $ROS_PACKAGE_PATH

// May need to source the Workspace Again
source devel/setup.bash

// note, the Terminal needs `cd` to the correct catkin_ws, in order to be able to echo the customized msg.
cd ~/catkin_ws
rostopic list
rostopic list -v

// Check Message Availability, suppose msg route is /teng4pkg_msgs/PoseCartesian6DOF
rosmsg show teng4pkg_msgs/PoseCartesian6DOF

// echo the rostopic, suppose the publisher is /teng4/test1_pose6dof
rostopic echo /teng4/test1_pose6dof

// some other cmd for retrieving the ROS param.
rosparam list
rosparam get /PSM1/rate
cd catkin_ws
echo $ROS_PACKAGE_PATH
source devel/setup.bash

9. Error Debugging.

You may encounter some errors when rostopic echo /your_topic the customized ROS msg. For example,

$ rostopic echo /teng4/test1_pose6dof

ERROR: Cannot load message class for [teng4pkg_msgs/PoseCartesian6DOF]. Are your messages built?

SOLUTION: This error is caused by wrong catkin_ws in terminal. You need to get into the correct catkin_ws2 ($cd catkin_ws2; $cd ~/catkin_ws2), and then “rostopic echo /teng4/test1_pose6dof “ will work.

$ cd ~/catkin_ws2

$ source devel/setup.bash

$ echo $ROS_PACKAGE_PATH

$ rostopic list

$ rostopic echo /teng4/test1_pose6dof

Summary

• Create a ROS package in catkin_ws/src.
• Create a msg directory and define your custom .msg file.
• Update the CMakeLists.txt and package.xml to include message generation.
• Build your workspace with catkin_make.
• Use the custom message in your ROS nodes for inter-node communication.

This setup allows you to define custom data structures for your ROS applications.

Created on 2024-09-11.