update aikit_encode.py and change rviz dae model

mycobot_ai/aikit_280_pi/config/ai_mycobot_pi.rviz

@@ -0,0 +1,226 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+        - /TF1
+        - /Marker1
+      Splitter Ratio: 0.5
+    Tree Height: 579
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        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
+        env:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        joint6_flange:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        env:
+          Value: true
+        joint1:
+          Value: true
+        joint2:
+          Value: true
+        joint3:
+          Value: true
+        joint4:
+          Value: true
+        joint5:
+          Value: true
+        joint6:
+          Value: true
+        joint6_flange:
+          Value: true
+      Marker Alpha: 1
+      Marker Scale: 0.30000001192092896
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        joint1:
+          env:
+            {}
+          joint2:
+            joint3:
+              joint4:
+                joint5:
+                  joint6:
+                    joint6_flange:
+                      {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: cube
+      Name: Marker
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: joint1
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 1.0433008670806885
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: -0.07064759731292725
+        Y: -0.0814988762140274
+        Z: 0.10758385062217712
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.3703983426094055
+      Target Frame: <Fixed Frame>
+      Yaw: 0.555390477180481
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 876
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002cefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ce000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001c2000002cefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002ce000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000400000002ce00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1848
+  X: 72
+  Y: 27

mycobot_ai/aikit_280_pi/launch/test.launch

@@ -0,0 +1,16 @@
+<launch>
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot_pi/mycobot_with_vision_v2.urdf"/>
+	<arg name="rvizconfig" default="$(find aikit_280_pi)/config/ai_mycobot_pi.rviz" />
+	<arg name="gui" default="true" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF -->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+		<param name="use_gui" value="$(arg gui)" />
+		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
+	</node>
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>

mycobot_ai/aikit_280_pi/launch/vision_m5.launch

@@ -1,27 +0,0 @@
-<launch>
-<arg name="port" default="/dev/ttyUSB0" />
-	<arg name="baud" default="115200" />
-
-	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_vision.urdf"/>
-	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
-	<!-- <arg name="gui" default="false" /> -->
-
-	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
-
-	<!-- Combinejoin values to TF -->
-	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" >
-		<rosparam param="source_list" subst_value="true">["joint_states"]</rosparam>
-	</node>
-
-	<!-- mycobot-topics -->
-    <include file="$(find mycobot_communication)/launch/communication_topic.launch">
-		<arg name="port" value="$(arg port)" />	
-		<arg name="baud" value="$(arg baud)" />	
-    </include>
-
-    <!-- listen and pub the real angles -->
-	<node name="real_listener" pkg="mycobot_280" type="listen_real_of_topic.py" />
-
-	<!-- Show in Rviz -->
-	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
-</launch>

mycobot_ai/aikit_280_pi/launch/vision_pi.launch

@@ -2,8 +2,9 @@
 <arg name="port" default="/dev/ttyAMA0" />
 	<arg name="baud" default="1000000" />
 
-	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_vision.urdf"/>
-	<arg name="rvizconfig" default="$(find mycobot_280)/config/ai_mycobot_pi.rviz" />
+	<!-- <arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_vision.urdf"/> -->
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot_pi/mycobot_with_vision_v2.urdf"/>
+	<arg name="rvizconfig" default="$(find aikit_280_pi)/config/ai_mycobot_pi.rviz" />
 	<!-- <arg name="gui" default="false" /> -->
 
 	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />

mycobot_ai/aikit_280_pi/launch/vision_wio.launch

@@ -1,27 +0,0 @@
-<launch>
-	<arg name="port" default="/dev/ttyACM0" />
-	<arg name="baud" default="115200" />
-
-	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_vision.urdf"/>
-	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
-	<!-- <arg name="gui" default="false" /> -->
-
-	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
-
-	<!-- Combinejoin values to TF -->
-	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" >
-		<rosparam param="source_list" subst_value="true">["joint_states"]</rosparam>
-	</node>
-
-	<!-- mycobot-topics -->
-    <include file="$(find mycobot_communication)/launch/communication_topic.launch">
-		<arg name="port" value="$(arg port)" />	
-		<arg name="baud" value="$(arg baud)" />	
-    </include>
-
-    <!-- listen and pub the real angles -->
-	<node name="real_listener" pkg="mycobot_280" type="listen_real_of_topic.py" />
-
-	<!-- Show in Rviz -->
-	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
-</launch>

mycobot_ai/aikit_280_pi/scripts/aikit_encode.py

@@ -32,8 +32,8 @@ class Detect_marker(Movement):
             # self.Pin = [20, 21]
             self.Pin = [2, 5]
 
-            for i in self.move_coords:
-                i[2] -= 20
+            # for i in self.move_coords:
+            #     i[2] -= 20
         elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
             import RPi.GPIO as GPIO
             GPIO.setwarnings(False)
@@ -53,6 +53,10 @@ class Detect_marker(Movement):
         self.cap = cv2.VideoCapture(cap_num)
         self.cap.set(3, 640)
         self.cap.set(4, 480)
+        
+        # Determine the placement point of the QR code
+        self.color = 0
+        
         # Get ArUco marker dict that can be detected.
         self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
         # Get ArUco marker params.
@@ -105,8 +109,10 @@ class Detect_marker(Movement):
             GPIO.output(21, 1)
 
     # Grasping motion
-    def move(self, x, y):
-    
+    def move(self, x, y, color):
+        
+        print(color)
+        
         angles = [
             [0.61, 45.87, -92.37, -41.3, 2.02, 9.58], # init to point
             [18.8, -7.91, -54.49, -23.02, -0.79, -14.76],
@@ -114,10 +120,10 @@ class Detect_marker(Movement):
 
         coords = [
             [145.6, -65.5, 285.1, 178.99, 7.67, 179.9], # 初始化点 init point
-            [132.2, -136.9, 200.8, -178.24, -3.72, -107.17],  # D分拣区 D sorting area
-            [238.8, -124.1, 204.3, -169.69, -5.52, -96.52], # C分拣区 C sorting area
             [115.8, 177.3, 210.6, 178.06, -0.92, -6.11], # A分拣区 A sorting area
             [-6.9, 173.2, 201.5, 179.93, 0.63, 33.83], # B分拣区  B sorting area
+            [238.8, -124.1, 204.3, -169.69, -5.52, -96.52], # C分拣区 C sorting area
+            [132.2, -136.9, 200.8, -178.24, -3.72, -107.17],  # D分拣区 D sorting area
         ]
         
         # publish marker
@@ -134,7 +140,10 @@ class Detect_marker(Movement):
         #time.sleep(2)
         self.mc.send_coords([coords[0][0]+x, coords[0][1]+y, 200, 178.99, -3.78, -62.9], 25, 0)
         time.sleep(2)
-        self.mc.send_coords([coords[0][0]+x, coords[0][1]+y, 105, 178.99, -3.78, -62.9], 25, 0)
+        # self.mc.send_coords([coords[0][0]+x, coords[0][1]+y, 105, 178.99, -3.78, -62.9], 25, 0)
+        # time.sleep(2)
+        self.mc.send_coords([coords[0][0]+x, coords[0][1]+y, 65.5, 178.99, -3.78, -62.9], 25, 0)
+        
         time.sleep(3.5)
         
         # open pump
@@ -173,7 +182,7 @@ class Detect_marker(Movement):
         time.sleep(2)
 
     # decide whether grab cube
-    def decide_move(self, x, y):
+    def decide_move(self, x, y, color):
 
         print(x,y)
         # detect the cube status move or run
@@ -183,7 +192,7 @@ class Detect_marker(Movement):
         else:
             self.cache_x = self.cache_y = 0
             # 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
-            self.move(x-5, y+145)
+            self.move(x, y+145, color)
 
     # init mycobot
     def init_mycobot(self):
@@ -212,7 +221,16 @@ class Detect_marker(Movement):
             corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(
                 gray, self.aruco_dict, parameters=self.aruco_params
             )
-        
+
+            # Determine the placement point of the QR code
+            if ids == np.array([[1]]):
+                self.color = 1
+            elif ids == np.array([[2]]):
+                self.color = 2
+            elif ids == np.array([[3]]):
+                self.color = 3
+            elif ids == np.array([[4]]):
+                self.color = 4
 
             if len(corners) > 0:
                 if ids is not None:
@@ -237,7 +255,7 @@ class Detect_marker(Movement):
                             sum_y += xyz[0]
                             num += 1
                         elif num ==40 :
-                            self.decide_move(sum_x/40.0, sum_y/40.0)
+                            self.decide_move(sum_x/40.0, sum_y/40.0, self.color)
                             num = sum_x = sum_y = 0
 
             cv2.imshow("encode_image", img)

mycobot_description/urdf/mycobot_pi/mycobot_with_vision_v2.urdf

@@ -0,0 +1,200 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="mycobot_ai">
+
+
+  <link name="env">
+    <visual>
+      <geometry>
+        <!--- 0.0 0 -0.04  1.5708 3.14159-->
+        <mesh filename="package://mycobot_description/urdf/mycobot_pi/suit_env_v2.dae"/>
+      </geometry>
+      <!-- <origin xyz = "0 0 -0.02 " rpy = "1.5708 0 -1.5708"/> -->
+      <origin xyz = "0.16 0 0.0 " rpy = "0 0 3.14159"/>
+    </visual>
+  </link>
+
+  <link name="joint1">
+    <visual>
+      <geometry>
+        <!--- 0.0 0 -0.04  1.5708 3.14159-->
+        <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint1_pi.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+        <!--- 0.0 0 -0.04  1.5708 3.14159-->
+        <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint1_pi.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+  <link name="joint2">
+    <visual>
+      <geometry>
+        <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint2.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.06096 " rpy = " 0 0 -1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint2.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.06096 " rpy = " 0 0 -1.5708"/>
+    </collision>
+  </link>
+
+
+  <link name="joint3">
+    <visual>
+      <geometry>
+        <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint3.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0.03256 " rpy = " 0 -1.5708 0"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint3.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0.03256 " rpy = " 0 -1.5708 0"/>
+    </collision>
+  </link>
+
+
+  <link name="joint4">
+    <visual>
+      <geometry>
+        <!--- 0.0 0 -0.04 -->
+        <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint4.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0.03056 " rpy = " 0 -1.5708 0"/>
+    </visual>
+    <collision>
+      <geometry>
+        <!--- 0.0 0 -0.04 -->
+        <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint4.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 0.03056 " rpy = " 0 -1.5708 0"/>
+    </collision>
+  </link>
+
+
+  <link name="joint5">
+    <visual>
+      <geometry>
+        <!--- 0.0 0 -0.04 -->
+        <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint5.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.03356 " rpy = "0 -1.5708 1.5708"/>
+    </visual>
+    <collision>
+      <geometry>
+        <!--- 0.0 0 -0.04 -->
+        <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint5.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.03356 " rpy = "0 -1.5708 1.5708"/>
+    </collision>
+  </link>
+
+
+  <link name="joint6">
+    <visual>
+      <geometry>
+        <!--- 0.0 0 -0.04 -->
+        <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint6.dae"/>
+      </geometry>
+      <origin xyz = "0 0.00 -0.038 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+        <!--- 0.0 0 -0.04 -->
+        <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint6.dae"/>
+      </geometry>
+      <origin xyz = "0 0.00 -0.038 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+
+  <link name="joint6_flange">
+    <visual>
+      <geometry>
+        <!--- 0.0 0 -0.04 -->
+        <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint7.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.012 " rpy = " 0 0 0"/>
+    </visual>
+    <collision>
+      <geometry>
+        <!--- 0.0 0 -0.04 -->
+        <mesh filename="package://mycobot_description/urdf/mycobot_pi/joint7.dae"/>
+      </geometry>
+      <origin xyz = "0.0 0 -0.012 " rpy = " 0 0 0"/>
+    </collision>
+  </link>
+
+
+
+
+
+  <joint name="joint2_to_joint1" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint1"/>
+    <child link="joint2"/>
+    <origin xyz= "0 0 0.13156" rpy = "0 0 0"/>
+  </joint>
+
+  <joint name="joint3_to_joint2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint2"/>
+    <child link="joint3"/>
+    <origin xyz= "0 0  0" rpy = "0 1.5708 -1.5708"/>
+  </joint>
+
+
+  <joint name="joint4_to_joint3" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint3"/>
+    <child link="joint4"/>
+    <origin xyz= "  -0.1104 0 0   " rpy = "0 0 0"/>
+  </joint>
+
+
+
+  <joint name="joint5_to_joint4" type="revolute">
+    <axis xyz=" 0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint4"/>
+    <child link="joint5"/>
+    <origin xyz= "-0.096 0 0.06462" rpy = "0 0 -1.5708"/>
+  </joint>
+
+  <joint name="joint6_to_joint5" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint5"/>
+    <child link="joint6"/>
+    <origin xyz= "0 -0.07318 0" rpy = "1.5708 -1.5708 0"/>
+  </joint>
+
+  <joint name="joint6output_to_joint6" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint6"/>
+    <child link="joint6_flange"/>
+    <origin xyz= "0 0.0456 0" rpy = "-1.5708 0 0"/>
+  </joint>
+
+  <joint name="vision_joint" type="fixed">
+    <axis xyz="0 0 0"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <parent link="joint1"/>
+    <child link="env"/>
+    <origin xyz= "0 0 0" rpy = "0 0 0"/>
+  </joint>
+
+</robot>
+