Switch to C++

src/draw_svg/CMakeLists.txt

@@ -16,7 +16,7 @@ find_package(moveit_ros_planning_interface REQUIRED)
 # Install C++
 set(SRC_CPP_DIR src/cpp)
 # Example 0 - Follow target
-set(EXECUTABLE_0 log_position)
+set(EXECUTABLE_0 follow)
 add_executable(${EXECUTABLE_0} ${SRC_CPP_DIR}/${EXECUTABLE_0}.cpp)
 ament_target_dependencies(${EXECUTABLE_0}
     rclcpp

src/draw_svg/launch/lite6_pen.launch.py

@@ -7,7 +7,7 @@ from launch.launch_description_sources import load_python_launch_file_as_module
 from launch import LaunchDescription
 from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, RegisterEventHandler
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command, FindExecutable
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 from launch.event_handlers import OnProcessExit
@@ -109,14 +109,123 @@ def launch_setup(context, *args, **kwargs):
         }.items(),
     )
 
+    # URDF
+    _robot_description_xml = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [FindPackageShare('draw_svg'), 'urdf', 'xarm_pen.urdf.xacro']
+            ),
+            #PathJoinSubstitution(
+            #    [
+            #        FindPackageShare('xarm_description'),
+            #        "urdf",
+            #        "lite6",
+            #        #"lite6.urdf.xacro",
+            #        "lite6_robot_macro.xacro",
+            #    ]
+            #),
+            " ",
+            #"name:=", "lite6", " ",
+            "prefix:=", " ",
+            "hw_ns:=", hw_ns, " ",
+            "limited:=", limited, " ",
+            "effort_control:=", effort_control, " ",
+            "velocity_control:=", velocity_control, " ",
+            "add_gripper:=", add_gripper, " ",
+            "add_vacuum_gripper:=", add_vacuum_gripper, " ",
+            "dof:=", dof, " ",
+            "robot_type:=", robot_type, " ",
+            "ros2_control_plugin:=", ros2_control_plugin, " ",
+            #"ros2_control_params:=", ros2_control_params, " ",
+
+            "add_other_geometry:=", add_other_geometry, " ",
+            "geometry_type:=", geometry_type, " ",
+            "geometry_mass:=", geometry_mass, " ",
+            "geometry_height:=", geometry_height, " ",
+            "geometry_radius:=", geometry_radius, " ",
+            "geometry_length:=", geometry_length, " ",
+            "geometry_width:=", geometry_width, " ",
+            "geometry_mesh_filename:=", geometry_mesh_filename, " ",
+            "geometry_mesh_origin_xyz:=", geometry_mesh_origin_xyz, " ",
+            "geometry_mesh_origin_rpy:=", geometry_mesh_origin_rpy, " ",
+            "geometry_mesh_tcp_xyz:=", geometry_mesh_tcp_xyz, " ",
+            "geometry_mesh_tcp_rpy:=", geometry_mesh_tcp_rpy, " ",
+
+            #"robot_ip:=", robot_ip, " ",
+            #"report_type:=", report_type, " ",
+            #"baud_checkset:=", baud_checkset, " ",
+            #"default_gripper_baud:=", default_gripper_baud, " ",
+        ]
+    )
+    # TODO fix URDF loading
+    # xacro urdf/xarm_pen.urdf.xacro prefix:= hw_ns:=xarm dof:=6 limited:=false effort_control:=false velocity_control:=false add_gripper:=false add_vacuum_gripper:=false robot_type:=lite ros2_control_plugin:=gazebo_ros2_control/GazeboSystem add_other_geometry:=false geometry_type:=cylinder geometry_mass:=0.05 geometry_height:=0.1 geometry_radius:=0.005 geometry_length:=0.07 geometry_width:=0.1 geometry_mesh_filename:= geometry_mesh_origin_xyz:="0 0 0" geometry_mesh_origin_rpy:="0 0 0" geometry_mesh_tcp_xyz:="0 0 0" geometry_mesh_tcp_rpy:="0 0 0"
+    _robot_description_xml = Command(['cat ', PathJoinSubstitution([FindPackageShare('draw_svg'), 'urdf', 'lite6.tmp.urdf'])])
+    robot_description = {"robot_description": _robot_description_xml}
+    # SRDF
+    _robot_description_semantic_xml = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare('xarm_moveit_config'),
+                    "srdf",
+                    #"_lite6_macro.srdf.xacro",
+                    "xarm.srdf.xacro",
+                ]
+            ),
+            " ",
+            #"name:=", "lite6", " ",
+            "prefix:=", " ",
+            #"hw_ns:=", hw_ns, " ",
+            #"limited:=", limited, " ",
+            #"effort_control:=", effort_control, " ",
+            #"velocity_control:=", velocity_control, " ",
+            #"add_gripper:=", add_gripper, " ",
+            #"add_vacuum_gripper:=", add_vacuum_gripper, " ",
+            "dof:=", dof, " ",
+            "robot_type:=", robot_type, " ",
+            #"ros2_control_plugin:=", ros2_control_plugin, " ",
+            #"ros2_control_params:=", ros2_control_params, " ",
+
+            #"add_other_geometry:=", add_other_geometry, " ",
+            #"geometry_type:=", geometry_type, " ",
+            #"geometry_mass:=", geometry_mass, " ",
+            #"geometry_height:=", geometry_height, " ",
+            #"geometry_radius:=", geometry_radius, " ",
+            #"geometry_length:=", geometry_length, " ",
+            #"geometry_width:=", geometry_width, " ",
+            #"geometry_mesh_filename:=", geometry_mesh_filename, " ",
+            #"geometry_mesh_origin_xyz:=", geometry_mesh_origin_xyz, " ",
+            #"geometry_mesh_origin_rpy:=", geometry_mesh_origin_rpy, " ",
+            #"geometry_mesh_tcp_xyz:=", geometry_mesh_tcp_xyz, " ",
+            #"geometry_mesh_tcp_rpy:=", geometry_mesh_tcp_rpy, " ",
+
+            #"robot_ip:=", robot_ip, " ",
+            #"report_type:=", report_type, " ",
+            #"baud_checkset:=", baud_checkset, " ",
+            #"default_gripper_baud:=", default_gripper_baud, " ",
+        ]
+    )
+    robot_description_semantic = {
+        "robot_description_semantic": _robot_description_semantic_xml
+    }
+
+
 
     nodes = [
         Node(
             package="draw_svg",
-            executable="follow.py",
+            executable="follow",
             output="log",
             arguments=["--ros-args", "--log-level", log_level],
-            parameters=[{"use_sim_time": use_sim_time}],
+            parameters=[
+                robot_description,
+                robot_description_semantic,
+                {"use_sim_time": use_sim_time},
+            ],
         ),
         Node(
             package="draw_svg",
@@ -134,10 +243,10 @@ def launch_setup(context, *args, **kwargs):
         ),
     ]
 
-    return nodes + [
+    return [
         robot_moveit_common_launch,
         robot_gazebo_launch,
-    ]
+    ] + nodes
 
 
 def generate_launch_description():

src/draw_svg/src/cpp/follow.cpp

@@ -0,0 +1,71 @@
+//#include "follow.h"
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <rclcpp/qos.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+const std::string MOVE_GROUP = "lite6";
+
+class MoveItFollowTarget : public rclcpp::Node
+{
+public:
+  /// Constructor
+  MoveItFollowTarget();
+
+  /// Move group interface for the robot
+  moveit::planning_interface::MoveGroupInterface move_group_;
+  /// Subscriber for target pose
+  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr target_pose_sub_;
+  /// Target pose that is used to detect changes
+  geometry_msgs::msg::Pose previous_target_pose_;
+
+private:
+  /// Callback that plans and executes trajectory each time the target pose is changed
+  void target_pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg);
+};
+
+MoveItFollowTarget::MoveItFollowTarget() : Node("follow_target"),
+                                           move_group_(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP)
+{
+  // Use upper joint velocity and acceleration limits
+  this->move_group_.setMaxAccelerationScalingFactor(1.0);
+  this->move_group_.setMaxVelocityScalingFactor(1.0);
+
+  // Subscribe to target pose
+  target_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1));
+
+  RCLCPP_INFO(this->get_logger(), "Initialization successful.");
+}
+
+void MoveItFollowTarget::target_pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg)
+{
+  // Return if target pose is unchanged
+  if (msg->pose == previous_target_pose_)
+  {
+    return;
+  }
+
+  RCLCPP_INFO(this->get_logger(), "Target pose has changed. Planning and executing...");
+
+  // Plan and execute motion
+  this->move_group_.setPoseTarget(msg->pose);
+  this->move_group_.move();
+
+  // Update for next callback
+  previous_target_pose_ = msg->pose;
+}
+
+int main(int argc, char *argv[])
+{
+  rclcpp::init(argc, argv);
+
+  auto target_follower = std::make_shared<MoveItFollowTarget>();
+
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(target_follower);
+  executor.spin();
+
+  rclcpp::shutdown();
+  return EXIT_SUCCESS;
+}

src/draw_svg/urdf/lite6.tmp.urdf

@@ -0,0 +1,436 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from urdf/xarm_pen.urdf.xacro       | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="xarm6">
+  <gazebo>
+    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
+      <robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
+      <parameters>/root/ws/install/share/xarm_controller/config/xarm6_controllers.yaml</parameters>
+    </plugin>
+  </gazebo>
+  <link name="world"/>
+  <joint name="world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="link_base"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  <ros2_control name="UFRobotSystem" type="system">
+    <hardware>
+      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+      <param name="hw_ns">xarm</param>
+      <param name="velocity_control">False</param>
+      <param name="prefix">P</param>
+      <param name="robot_ip">R</param>
+      <param name="report_type">normal</param>
+      <param name="dof">6</param>
+      <param name="baud_checkset">True</param>
+      <param name="default_gripper_baud">2000000</param>
+      <param name="robot_type">lite</param>
+      <param name="add_gripper">False</param>
+    </hardware>
+    <joint name="joint1">
+      <command_interface name="position">
+        <param name="min">-6.283185307179586</param>
+        <param name="max">6.283185307179586</param>
+      </command_interface>
+      <command_interface name="velocity">
+        <param name="min">-3.14</param>
+        <param name="max">3.14</param>
+      </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <!-- <state_interface name="effort"/> -->
+    </joint>
+    <joint name="joint2">
+      <command_interface name="position">
+        <param name="min">-2.61799</param>
+        <param name="max">2.61799</param>
+      </command_interface>
+      <command_interface name="velocity">
+        <param name="min">-3.14</param>
+        <param name="max">3.14</param>
+      </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <!-- <state_interface name="effort"/> -->
+    </joint>
+    <joint name="joint3">
+      <command_interface name="position">
+        <param name="min">-0.061087</param>
+        <param name="max">5.235988</param>
+      </command_interface>
+      <command_interface name="velocity">
+        <param name="min">-3.14</param>
+        <param name="max">3.14</param>
+      </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <!-- <state_interface name="effort"/> -->
+    </joint>
+    <joint name="joint4">
+      <command_interface name="position">
+        <param name="min">-6.283185307179586</param>
+        <param name="max">6.283185307179586</param>
+      </command_interface>
+      <command_interface name="velocity">
+        <param name="min">-3.14</param>
+        <param name="max">3.14</param>
+      </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <!-- <state_interface name="effort"/> -->
+    </joint>
+    <joint name="joint5">
+      <command_interface name="position">
+        <param name="min">-2.1642</param>
+        <param name="max">2.1642</param>
+      </command_interface>
+      <command_interface name="velocity">
+        <param name="min">-3.14</param>
+        <param name="max">3.14</param>
+      </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <!-- <state_interface name="effort"/> -->
+    </joint>
+    <joint name="joint6">
+      <command_interface name="position">
+        <param name="min">-6.283185307179586</param>
+        <param name="max">6.283185307179586</param>
+      </command_interface>
+      <command_interface name="velocity">
+        <param name="min">-3.14</param>
+        <param name="max">3.14</param>
+      </command_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <!-- <state_interface name="effort"/> -->
+    </joint>
+  </ros2_control>
+  <material name="White">
+    <color rgba="1.0 1.0 1.0 1.0"/>
+  </material>
+  <material name="Silver">
+    <color rgba="0.753 0.753 0.753 1.0"/>
+  </material>
+  <link name="link_base">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.00829544579053192 3.26357432323433E-05 0.0631194584987089"/>
+      <mass value="1.65393501783165"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/base.stl"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/base.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link1">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.00036 0.03788 -0.0027"/>
+      <mass value="1.169"/>
+      <inertia ixx="1.45164E-03" ixy="1.24E-05" ixz="-6.7E-06" iyy="8.873E-04" iyz="1.255E-04" izz="1.31993E-03"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link1.stl"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint1" type="revolute">
+    <origin rpy="0 0 0" xyz="0 0 0.2435"/>
+    <parent link="link_base"/>
+    <child link="link1"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="50.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.14"/>
+    <dynamics damping="1.0" friction="1.0"/>
+  </joint>
+  <link name="link2">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.178 0.0 0.0576"/>
+      <mass value="1.192"/>
+      <inertia ixx="1.5854E-03" ixy="-6.766E-06" ixz="-1.15136E-03" iyy="5.6097E-03" iyz="1.14E-06" izz="4.85E-03"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link2.stl"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link2.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint2" type="revolute">
+    <origin rpy="1.5708 -1.5708 3.1416" xyz="0 0 0"/>
+    <parent link="link1"/>
+    <child link="link2"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="50.0" lower="-2.61799" upper="2.61799" velocity="3.14"/>
+    <dynamics damping="1.0" friction="1.0"/>
+  </joint>
+  <link name="link3">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.07285 -0.030 -0.0009"/>
+      <mass value="0.930"/>
+      <inertia ixx="8.861E-04" ixy="-3.9287E-04" ixz="7.066E-05" iyy="1.5785E-03" iyz="-2.445E-05" izz="1.84677E-03"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link3.stl"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link3.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint3" type="revolute">
+    <origin rpy="-3.1416 0 1.5708" xyz="0.2002 0 0"/>
+    <parent link="link2"/>
+    <child link="link3"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="32.0" lower="-0.061087" upper="5.235988" velocity="3.14"/>
+    <dynamics damping="1.0" friction="1.0"/>
+  </joint>
+  <link name="link4">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.0004 -0.0275 -0.0817"/>
+      <mass value="1.31"/>
+      <inertia ixx="3.705E-03" ixy="-2.0E-06" ixz="7.17E-06" iyy="3.0455E-03" iyz="-9.3188E-04" izz="1.5413E-03"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link4.stl"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link4.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint4" type="revolute">
+    <origin rpy="1.5708 0 0" xyz="0.087 -0.22761 0"/>
+    <parent link="link3"/>
+    <child link="link4"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="32.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.14"/>
+    <dynamics damping="1.0" friction="1.0"/>
+  </joint>
+  <link name="link5">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 0.010 0.0019"/>
+      <mass value="0.784"/>
+      <inertia ixx="5.668E-04" ixy="6E-07" ixz="-5.3E-06" iyy="5.077E-04" iyz="-4.8E-07" izz="5.3E-04"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link5.stl"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link5.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint5" type="revolute">
+    <origin rpy="1.5708 0 0" xyz="0 0 0"/>
+    <parent link="link4"/>
+    <child link="link5"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="32.0" lower="-2.1642" upper="2.1642" velocity="3.14"/>
+    <dynamics damping="1.0" friction="1.0"/>
+  </joint>
+  <link name="link6">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0 -0.00194 -0.0102"/>
+      <mass value="0.180"/>
+      <inertia ixx="7.726E-05" ixy="1E-06" ixz="4E-07" iyy="8.5665E-05" iyz="-6E-07" izz="1.4814E-04"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link6.stl"/>
+      </geometry>
+      <material name="Silver"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link6.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint6" type="revolute">
+    <origin rpy="-1.5708 0 0" xyz="0 0.0625 0"/>
+    <parent link="link5"/>
+    <child link="link6"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="20.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.14"/>
+    <dynamics damping="1.0" friction="1.0"/>
+  </joint>
+  <link name="link_eef"/>
+  <joint name="joint_eef" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="link6"/>
+    <child link="link_eef"/>
+  </joint>
+  <transmission name="tran1">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="joint1">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor1">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>100</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="tran2">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="joint2">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor2">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>100</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="tran3">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="joint3">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor3">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>100</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="tran4">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="joint4">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor3">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>100</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="tran5">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="joint5">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor5">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>100</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="tran6">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="joint6">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor6">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>100</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <gazebo reference="link_base">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="link1">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="link2">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="link3">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="link4">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="link5">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <gazebo reference="link6">
+    <selfCollide>true</selfCollide>
+  </gazebo>
+  <joint name="eef_joint" type="fixed">
+    <parent link="link_eef"/>
+    <child link="pen_link"/>
+  </joint>
+  <link name="pen_link">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.2" radius="0.005"/>
+      </geometry>
+      <material name="Cyan">
+        <color rgba="0.0 1.0 1.0 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0 0 0"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/src/gazebo_ros_p3d.cpp -->
+  <gazebo>
+    <plugin filename="libgazebo_ros_p3d.so" name="pen_position">
+      <!-- <alwaysOn>true</alwaysOn> -->
+      <ros>
+        <remapping>odom:=pen_position</remapping>
+      </ros>
+      <frame_name>world</frame_name>
+      <!-- <body_name>pen_link</body_name> -->
+      <body_name>link6</body_name>
+      <!--       topic name is always /odom -->
+      <!-- <topic_name>pen_position</topic_name> -->
+      <!--       Update rate in Hz -->
+      <update_rate>10</update_rate>
+      <!-- <xyzOffsets>0 0 0</xyzOffsets> -->
+      <!-- <rpyOffsets>0 0 0</rpyOffsets> -->
+    </plugin>
+  </gazebo>
+</robot>
+<!-- <robot name="lite6_robot" xmlns:xacro="http://www.ros.org/wiki/xacro" xmlns:xi="http://www.w3.org/2001/XInclude">
+     <xacro:include filename="$(find xarm_description)/urdf/lite6/lite6.urdf.xacro"/>
+     <xacro:lite6_urdf prefix=''/> -->
+<!--
+     </robot> -->