diff --git a/src/rl_sar/config.yaml b/src/rl_sar/config.yaml
index 136c78a..79bcc2d 100644
--- a/src/rl_sar/config.yaml
+++ b/src/rl_sar/config.yaml
@@ -1,9 +1,4 @@
 a1:
-# order:
-# FL
-# FR
-# RL
-# RR
   model_name: "model_0522.pt"
   num_observations: 45
   clip_obs: 100.0
@@ -48,7 +43,39 @@ a1:
                     -0.1000, 0.8000, -1.5000, 
                      0.1000, 1.0000, -1.5000, 
                     -0.1000, 1.0000, -1.5000] 
-  joint_names: ["FL_hip_joint", "FL_thigh_joint", "FL_calf_joint",
-                "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint",
-                "RL_hip_joint", "RL_thigh_joint", "RL_calf_joint",
-                "RR_hip_joint", "RR_thigh_joint", "RR_calf_joint"]
+  joint_controller_names: ["FL_hip_controller", "FL_thigh_controller", "FL_calf_controller",
+                           "FR_hip_controller", "FR_thigh_controller", "FR_calf_controller",
+                           "RL_hip_controller", "RL_thigh_controller", "RL_calf_controller",
+                           "RR_hip_controller", "RR_thigh_controller", "RR_calf_controller"]
+
+gr1t1:
+  model_name: "model_4000_jit.pt"
+  num_observations: 39
+  clip_obs: 100.0
+  clip_actions_upper: [1.1391, 1.0491, 1.0491, 2.2691, 0.8691,
+                       0.4391, 1.0491, 1.0491, 2.2691, 0.8691]
+  clip_actions_lower: [-0.4391, -1.0491, -2.0991, -0.4391, -1.3991,
+                       -1.1391, -1.0491, -2.0991, -0.4391, -1.3991]
+  rl_kp: [57.0, 43.0, 114.0, 114.0, 15.3, 
+          57.0, 43.0, 114.0, 114.0, 15.3]
+  rl_kd: [5.7, 4.3, 11.4, 11.4, 1.5, 
+          5.7, 4.3, 11.4, 11.4, 1.5]
+  fixed_kp: [57.0, 43.0, 114.0, 114.0, 15.3, 
+             57.0, 43.0, 114.0, 114.0, 15.3]
+  fixed_kd: [5.7, 4.3, 11.4, 11.4, 1.5, 
+             5.7, 4.3, 11.4, 11.4, 1.5]
+  hip_scale_reduction: 0.5
+  hip_scale_reduction_indices: [0, 3, 6, 9]
+  num_of_dofs: 10
+  action_scale: 1.0
+  lin_vel_scale: 1.0
+  ang_vel_scale: 1.0
+  dof_pos_scale: 1.0
+  dof_vel_scale: 1.0
+  commands_scale: [1.0, 1.0, 1.0]
+  torque_limits: [60.0, 45.0, 130.0, 130.0, 16.0,
+                  60.0, 45.0, 130.0, 130.0, 16.0]
+  default_dof_pos: [0.0, 0.0, -0.2618, 0.5236, -0.2618, 
+                    0.0, 0.0, -0.2618, 0.5236, -0.2618]
+  joint_controller_names: ["l_hip_roll_controller", "l_hip_yaw_controller", "l_hip_pitch_controller", "l_knee_pitch_controller", "l_ankle_pitch_controller", 
+                           "r_hip_roll_controller", "r_hip_yaw_controller", "r_hip_pitch_controller", "r_knee_pitch_controller", "r_ankle_pitch_controller"]
\ No newline at end of file
diff --git a/src/rl_sar/launch/gazebo_gr1t1.launch b/src/rl_sar/launch/gazebo_gr1t1.launch
new file mode 100644
index 0000000..46a76dc
--- /dev/null
+++ b/src/rl_sar/launch/gazebo_gr1t1.launch
@@ -0,0 +1,52 @@
+<launch>
+    <arg name="wname" default="stairs"/>
+    <arg name="rname" default="gr1t1"/>
+    <param name="robot_name" type="str" value="gr1t1"/>
+    <param name="use_history" type="bool" value="false"/>
+    <param name="ros_namespace" type="str" value="/$(arg rname)_gazebo/"/>
+    <arg name="robot_path" value="(find $(arg rname)_description)"/>
+    <arg name="dollar" value="$"/>
+
+    <arg name="paused" default="true"/>
+    <arg name="use_sim_time" default="true"/>
+    <arg name="gui" default="true"/>
+    <arg name="headless" default="false"/>
+    <arg name="debug" default="false"/>
+    
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="world_name" value="$(find rl_sar)/worlds/$(arg wname).world"/>
+        <arg name="debug" value="$(arg debug)"/>
+        <arg name="gui" value="$(arg gui)"/>
+        <arg name="paused" value="$(arg paused)"/>
+        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+        <arg name="headless" value="$(arg headless)"/>
+    </include>
+
+    <!-- Load the URDF into the ROS Parameter Server -->
+    <param name="robot_description" textfile="$(find gr1t1_description)/urdf/GR1T1_lower_limb.urdf"/>
+
+    <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
+    <!-- Set trunk and joint positions at startup -->
+    <node pkg="gazebo_ros" type="spawn_model" name="urdf_spawner" respawn="false" output="screen"
+          args="-urdf -z 1.0 -model $(arg rname)_gazebo -param robot_description -unpause"/>
+
+    <!-- Load joint controller configurations from YAML file to parameter server -->
+    <rosparam file="$(arg dollar)$(arg robot_path)/config/robot_control.yaml" command="load"/>
+
+    <!-- <rosparam param="/gr1t1_gazebo/joint_state_controller/publish_rate">5000</rosparam> -->
+
+    <!-- load the controllers -->
+    <node pkg="controller_manager" type="spawner" name="controller_spawner" respawn="false"
+          output="screen" ns="/$(arg rname)_gazebo" args="joint_state_controller
+          l_hip_roll_controller l_hip_yaw_controller l_hip_pitch_controller l_knee_pitch_controller l_ankle_pitch_controller
+          r_hip_roll_controller r_hip_yaw_controller r_hip_pitch_controller r_knee_pitch_controller r_ankle_pitch_controller "/>
+
+    <!-- convert joint states to TF transforms for rviz, etc -->
+    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"
+          respawn="false" output="screen">
+        <remap from="/joint_states" to="/$(arg rname)_gazebo/joint_states"/>
+    </node>
+
+    <node pkg="rl_sar" type="rl_sim" name="rl_sim" output="screen"/>
+
+</launch>
diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.cpp b/src/rl_sar/library/rl_sdk/rl_sdk.cpp
index 418b8e1..34fffbb 100644
--- a/src/rl_sar/library/rl_sdk/rl_sdk.cpp
+++ b/src/rl_sar/library/rl_sdk/rl_sdk.cpp
@@ -293,7 +293,7 @@ void RL::ReadYaml(std::string robot_name)
     this->params.fixed_kd = torch::tensor(ReadVectorFromYaml<double>(config["fixed_kd"])).view({1, -1});
     this->params.torque_limits = torch::tensor(ReadVectorFromYaml<double>(config["torque_limits"])).view({1, -1});
     this->params.default_dof_pos = torch::tensor(ReadVectorFromYaml<double>(config["default_dof_pos"])).view({1, -1});
-    this->params.joint_names = ReadVectorFromYaml<std::string>(config["joint_names"]);
+    this->params.joint_controller_names = ReadVectorFromYaml<std::string>(config["joint_controller_names"]);
 }
 
 void RL::CSVInit(std::string robot_name)
diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.hpp b/src/rl_sar/library/rl_sdk/rl_sdk.hpp
index d90c4f3..f997d84 100644
--- a/src/rl_sar/library/rl_sdk/rl_sdk.hpp
+++ b/src/rl_sar/library/rl_sdk/rl_sdk.hpp
@@ -83,7 +83,7 @@ struct ModelParams
     torch::Tensor fixed_kd;
     torch::Tensor commands_scale;
     torch::Tensor default_dof_pos;
-    std::vector<std::string> joint_names;
+    std::vector<std::string> joint_controller_names;
 };
 
 struct Observations
diff --git a/src/rl_sar/models/gr1t1/model_4000_jit.pt b/src/rl_sar/models/gr1t1/model_4000_jit.pt
new file mode 100644
index 0000000..a0e73a5
Binary files /dev/null and b/src/rl_sar/models/gr1t1/model_4000_jit.pt differ
diff --git a/src/rl_sar/src/rl_sim.cpp b/src/rl_sar/src/rl_sim.cpp
index 1c70c28..2e09e22 100644
--- a/src/rl_sar/src/rl_sim.cpp
+++ b/src/rl_sar/src/rl_sim.cpp
@@ -20,11 +20,11 @@ RL_Sim::RL_Sim()
 
     // Due to the fact that the robot_state_publisher sorts the joint names alphabetically,
     // the mapping table is established according to the order defined in the YAML file
-    std::vector<std::string> sorted_joint_names = params.joint_names;
-    std::sort(sorted_joint_names.begin(), sorted_joint_names.end());
-    for(size_t i = 0; i < params.joint_names.size(); ++i)
+    std::vector<std::string> sorted_joint_controller_names = params.joint_controller_names;
+    std::sort(sorted_joint_controller_names.begin(), sorted_joint_controller_names.end());
+    for(size_t i = 0; i < params.joint_controller_names.size(); ++i)
     {
-        sorted_to_original_index[sorted_joint_names[i]] = i;
+        sorted_to_original_index[sorted_joint_controller_names[i]] = i;
     }
     mapped_joint_positions = std::vector<double>(params.num_of_dofs, 0.0);
     mapped_joint_velocities = std::vector<double>(params.num_of_dofs, 0.0);
@@ -48,8 +48,8 @@ RL_Sim::RL_Sim()
     for (int i = 0; i < params.num_of_dofs; ++i)
     {
         // joint need to rename as xxx_joint
-        torque_publishers[params.joint_names[i]] = nh.advertise<robot_msgs::MotorCommand>(
-            ros_namespace + params.joint_names[i].substr(0, params.joint_names[i].size() - 6) + "_controller/command", 10);
+        torque_publishers[params.joint_controller_names[i]] = nh.advertise<robot_msgs::MotorCommand>(
+            ros_namespace + params.joint_controller_names[i] + "/command", 10);
     }
 
     // subscriber
@@ -127,7 +127,7 @@ void RL_Sim::SetCommand(const RobotCommand<double> *command)
     
     for(int i = 0; i < params.num_of_dofs; ++i)
     {
-        torque_publishers[params.joint_names[i]].publish(motor_commands[i]);
+        torque_publishers[params.joint_controller_names[i]].publish(motor_commands[i]);
     }
 }
 
@@ -166,7 +166,7 @@ void RL_Sim::MapData(const std::vector<double>& source_data, std::vector<double>
 {
     for(size_t i = 0; i < source_data.size(); ++i)
     {
-        target_data[i] = source_data[sorted_to_original_index[params.joint_names[i]]];
+        target_data[i] = source_data[sorted_to_original_index[params.joint_controller_names[i]]];
     }
 }
 
diff --git a/src/robots/gr1t1_description/CMakeLists.txt b/src/robots/gr1t1_description/CMakeLists.txt
new file mode 100644
index 0000000..d2927b2
--- /dev/null
+++ b/src/robots/gr1t1_description/CMakeLists.txt
@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 3.0)
+
+project(gr1t1_description)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+find_package(roslaunch)
+
+foreach(dir config launch meshes urdf)
+	install(DIRECTORY ${dir}/
+		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
+endforeach(dir)
diff --git a/src/robots/gr1t1_description/config/robot_control.yaml b/src/robots/gr1t1_description/config/robot_control.yaml
new file mode 100644
index 0000000..d622661
--- /dev/null
+++ b/src/robots/gr1t1_description/config/robot_control.yaml
@@ -0,0 +1,57 @@
+gr1t1_gazebo:
+    # Publish all joint states -----------------------------------
+    joint_state_controller:
+        type: joint_state_controller/JointStateController
+        publish_rate: 1000  
+
+    # left leg Controllers ---------------------------------------
+    l_hip_roll_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: l_hip_roll
+        pid: {p: 57.0, i: 0.0, d: 5.7}
+    
+    l_hip_yaw_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: l_hip_yaw
+        pid: {p: 43.0, i: 0.0, d: 4.3}
+
+    l_hip_pitch_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: l_hip_pitch
+        pid: {p: 114.0, i: 0.0, d: 11.4}
+
+    l_knee_pitch_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: l_knee_pitch
+        pid: {p: 114.0, i: 0.0, d: 11.4}
+
+    l_ankle_pitch_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: l_ankle_pitch
+        pid: {p: 15.3, i: 0.0, d: 1.5}
+
+    # right leg Controllers ---------------------------------------
+    r_hip_roll_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: r_hip_roll
+        pid: {p: 57.0, i: 0.0, d: 5.7}
+    
+    r_hip_yaw_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: r_hip_yaw
+        pid: {p: 43.0, i: 0.0, d: 4.3}
+
+    r_hip_pitch_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: r_hip_pitch
+        pid: {p: 114.0, i: 0.0, d: 11.4}
+
+    r_knee_pitch_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: r_knee_pitch
+        pid: {p: 114.0, i: 0.0, d: 11.4}
+
+    r_ankle_pitch_controller:
+        type: robot_joint_controller/RobotJointController
+        joint: r_ankle_pitch
+        pid: {p: 15.3, i: 0.0, d: 1.5}
diff --git a/src/robots/gr1t1_description/launch/display.launch b/src/robots/gr1t1_description/launch/display.launch
new file mode 100644
index 0000000..d4cc4fb
--- /dev/null
+++ b/src/robots/gr1t1_description/launch/display.launch
@@ -0,0 +1,20 @@
+<launch>
+  <arg
+    name="model" />
+  <param
+    name="robot_description"
+    textfile="$(find gr1t1_description)/urdf/GR1T1_lower_limb.urdf" />
+  <node
+    name="joint_state_publisher_gui"
+    pkg="joint_state_publisher_gui"
+    type="joint_state_publisher_gui" />
+  <node
+    name="robot_state_publisher"
+    pkg="robot_state_publisher"
+    type="robot_state_publisher" />
+  <node
+    name="rviz"
+    pkg="rviz"
+    type="rviz"
+    args="-d $(find gr1t1_description)/urdf.rviz" />
+</launch>
\ No newline at end of file
diff --git a/src/robots/gr1t1_description/launch/gazebo.launch b/src/robots/gr1t1_description/launch/gazebo.launch
new file mode 100644
index 0000000..ba67d26
--- /dev/null
+++ b/src/robots/gr1t1_description/launch/gazebo.launch
@@ -0,0 +1,20 @@
+<launch>
+  <include
+    file="$(find gazebo_ros)/launch/empty_world.launch" />
+  <node
+    name="tf_footprint_base"
+    pkg="tf"
+    type="static_transform_publisher"
+    args="0 0 0 0 0 0 base_link base_footprint 40" />
+  <node
+    name="spawn_model"
+    pkg="gazebo_ros"
+    type="spawn_model"
+    args="-file $(find gr1t1_description)/urdf/GR1T1_lower_limb.urdf -urdf -model gr1t1_description"
+    output="screen" />
+  <node
+    name="fake_joint_calibration"
+    pkg="rostopic"
+    type="rostopic"
+    args="pub /calibrated std_msgs/Bool true" />
+</launch>
\ No newline at end of file
diff --git a/src/robots/gr1t1_description/meshes/base.STL b/src/robots/gr1t1_description/meshes/base.STL
new file mode 100644
index 0000000..fd2f20a
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/base.STL differ
diff --git a/src/robots/gr1t1_description/meshes/head_pitch.STL b/src/robots/gr1t1_description/meshes/head_pitch.STL
new file mode 100644
index 0000000..c17c1a4
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/head_pitch.STL differ
diff --git a/src/robots/gr1t1_description/meshes/head_roll.STL b/src/robots/gr1t1_description/meshes/head_roll.STL
new file mode 100644
index 0000000..b4ff40b
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/head_roll.STL differ
diff --git a/src/robots/gr1t1_description/meshes/head_yaw.STL b/src/robots/gr1t1_description/meshes/head_yaw.STL
new file mode 100644
index 0000000..2b152b0
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/head_yaw.STL differ
diff --git a/src/robots/gr1t1_description/meshes/l_foot_pitch.STL b/src/robots/gr1t1_description/meshes/l_foot_pitch.STL
new file mode 100644
index 0000000..b8d4b4f
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/l_foot_pitch.STL differ
diff --git a/src/robots/gr1t1_description/meshes/l_foot_roll.STL b/src/robots/gr1t1_description/meshes/l_foot_roll.STL
new file mode 100644
index 0000000..89ae40c
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/l_foot_roll.STL differ
diff --git a/src/robots/gr1t1_description/meshes/l_hand_pitch.STL b/src/robots/gr1t1_description/meshes/l_hand_pitch.STL
new file mode 100644
index 0000000..40f8d33
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/l_hand_pitch.STL differ
diff --git a/src/robots/gr1t1_description/meshes/l_hand_roll.STL b/src/robots/gr1t1_description/meshes/l_hand_roll.STL
new file mode 100644
index 0000000..af3f154
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/l_hand_roll.STL differ
diff --git a/src/robots/gr1t1_description/meshes/l_hand_yaw.STL b/src/robots/gr1t1_description/meshes/l_hand_yaw.STL
new file mode 100644
index 0000000..279f5e6
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/l_hand_yaw.STL differ
diff --git a/src/robots/gr1t1_description/meshes/l_lower_arm_pitch.STL b/src/robots/gr1t1_description/meshes/l_lower_arm_pitch.STL
new file mode 100644
index 0000000..9422de6
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/l_lower_arm_pitch.STL differ
diff --git a/src/robots/gr1t1_description/meshes/l_shank_pitch.STL b/src/robots/gr1t1_description/meshes/l_shank_pitch.STL
new file mode 100644
index 0000000..301152b
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/l_shank_pitch.STL differ
diff --git a/src/robots/gr1t1_description/meshes/l_thigh_pitch.STL b/src/robots/gr1t1_description/meshes/l_thigh_pitch.STL
new file mode 100644
index 0000000..0e8d690
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/l_thigh_pitch.STL differ
diff --git a/src/robots/gr1t1_description/meshes/l_thigh_roll.STL b/src/robots/gr1t1_description/meshes/l_thigh_roll.STL
new file mode 100644
index 0000000..a4d7654
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/l_thigh_roll.STL differ
diff --git a/src/robots/gr1t1_description/meshes/l_thigh_yaw.STL b/src/robots/gr1t1_description/meshes/l_thigh_yaw.STL
new file mode 100644
index 0000000..f4ea275
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/l_thigh_yaw.STL differ
diff --git a/src/robots/gr1t1_description/meshes/l_upper_arm_pitch.STL b/src/robots/gr1t1_description/meshes/l_upper_arm_pitch.STL
new file mode 100644
index 0000000..c634251
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/l_upper_arm_pitch.STL differ
diff --git a/src/robots/gr1t1_description/meshes/l_upper_arm_roll.STL b/src/robots/gr1t1_description/meshes/l_upper_arm_roll.STL
new file mode 100644
index 0000000..0e92ff2
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/l_upper_arm_roll.STL differ
diff --git a/src/robots/gr1t1_description/meshes/l_upper_arm_yaw.STL b/src/robots/gr1t1_description/meshes/l_upper_arm_yaw.STL
new file mode 100644
index 0000000..7af2b75
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/l_upper_arm_yaw.STL differ
diff --git a/src/robots/gr1t1_description/meshes/r_foot_pitch.STL b/src/robots/gr1t1_description/meshes/r_foot_pitch.STL
new file mode 100644
index 0000000..2a053cd
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/r_foot_pitch.STL differ
diff --git a/src/robots/gr1t1_description/meshes/r_foot_roll.STL b/src/robots/gr1t1_description/meshes/r_foot_roll.STL
new file mode 100644
index 0000000..3fd1295
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/r_foot_roll.STL differ
diff --git a/src/robots/gr1t1_description/meshes/r_hand_pitch.STL b/src/robots/gr1t1_description/meshes/r_hand_pitch.STL
new file mode 100644
index 0000000..9b7399b
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/r_hand_pitch.STL differ
diff --git a/src/robots/gr1t1_description/meshes/r_hand_roll.STL b/src/robots/gr1t1_description/meshes/r_hand_roll.STL
new file mode 100644
index 0000000..9535cb7
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/r_hand_roll.STL differ
diff --git a/src/robots/gr1t1_description/meshes/r_hand_yaw.STL b/src/robots/gr1t1_description/meshes/r_hand_yaw.STL
new file mode 100644
index 0000000..dd2de0f
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/r_hand_yaw.STL differ
diff --git a/src/robots/gr1t1_description/meshes/r_lower_arm_pitch.STL b/src/robots/gr1t1_description/meshes/r_lower_arm_pitch.STL
new file mode 100644
index 0000000..7f95ee5
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/r_lower_arm_pitch.STL differ
diff --git a/src/robots/gr1t1_description/meshes/r_shank_pitch.STL b/src/robots/gr1t1_description/meshes/r_shank_pitch.STL
new file mode 100644
index 0000000..9cb7e59
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/r_shank_pitch.STL differ
diff --git a/src/robots/gr1t1_description/meshes/r_thigh_pitch.STL b/src/robots/gr1t1_description/meshes/r_thigh_pitch.STL
new file mode 100644
index 0000000..04712d7
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/r_thigh_pitch.STL differ
diff --git a/src/robots/gr1t1_description/meshes/r_thigh_roll.STL b/src/robots/gr1t1_description/meshes/r_thigh_roll.STL
new file mode 100644
index 0000000..365a4a6
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/r_thigh_roll.STL differ
diff --git a/src/robots/gr1t1_description/meshes/r_thigh_yaw.STL b/src/robots/gr1t1_description/meshes/r_thigh_yaw.STL
new file mode 100644
index 0000000..49a68fc
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/r_thigh_yaw.STL differ
diff --git a/src/robots/gr1t1_description/meshes/r_upper_arm_pitch.STL b/src/robots/gr1t1_description/meshes/r_upper_arm_pitch.STL
new file mode 100644
index 0000000..aff0133
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/r_upper_arm_pitch.STL differ
diff --git a/src/robots/gr1t1_description/meshes/r_upper_arm_roll.STL b/src/robots/gr1t1_description/meshes/r_upper_arm_roll.STL
new file mode 100644
index 0000000..661e9eb
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/r_upper_arm_roll.STL differ
diff --git a/src/robots/gr1t1_description/meshes/r_upper_arm_yaw.STL b/src/robots/gr1t1_description/meshes/r_upper_arm_yaw.STL
new file mode 100644
index 0000000..58d2471
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/r_upper_arm_yaw.STL differ
diff --git a/src/robots/gr1t1_description/meshes/waist_pitch.STL b/src/robots/gr1t1_description/meshes/waist_pitch.STL
new file mode 100644
index 0000000..d759e9c
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/waist_pitch.STL differ
diff --git a/src/robots/gr1t1_description/meshes/waist_roll.STL b/src/robots/gr1t1_description/meshes/waist_roll.STL
new file mode 100644
index 0000000..b06c0cb
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/waist_roll.STL differ
diff --git a/src/robots/gr1t1_description/meshes/waist_yaw.STL b/src/robots/gr1t1_description/meshes/waist_yaw.STL
new file mode 100644
index 0000000..d6b96e5
Binary files /dev/null and b/src/robots/gr1t1_description/meshes/waist_yaw.STL differ
diff --git a/src/robots/gr1t1_description/package.xml b/src/robots/gr1t1_description/package.xml
new file mode 100644
index 0000000..889a69d
--- /dev/null
+++ b/src/robots/gr1t1_description/package.xml
@@ -0,0 +1,20 @@
+<package format="2">
+  <name>gr1t1_description</name>
+  <version>1.0.0</version>
+  <description>
+    <p>URDF Description package for gr1t1_description</p>
+    <p>This package contains configuration data, 3D models and launch files for fldlar_description robot</p>
+  </description>
+  <author>Ziqi Fan</author>
+  <maintainer email="fanziqi614@gmail.com" />
+  <license>Apache</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>roslaunch</depend>
+  <depend>robot_state_publisher</depend>
+  <depend>rviz</depend>
+  <depend>joint_state_publisher_gui</depend>
+  <depend>gazebo</depend>
+  <export>
+    <architecture_independent />
+  </export>
+</package>
\ No newline at end of file
diff --git a/src/robots/gr1t1_description/urdf.rviz b/src/robots/gr1t1_description/urdf.rviz
new file mode 100644
index 0000000..d469e8f
--- /dev/null
+++ b/src/robots/gr1t1_description/urdf.rviz
@@ -0,0 +1,225 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 549
+  - 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:
+        10:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        11:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        12:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        13:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        20:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        21:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        22:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        23:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        30:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        31:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        32:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        33:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        40:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        41:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        42:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        43:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link:
+          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
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: base
+    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: 2.634476661682129
+      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.0988716408610344
+        Y: 0.08353396505117416
+        Z: -0.19447802007198334
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5753985047340393
+      Target Frame: <Fixed Frame>
+      Yaw: 0.47539806365966797
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1200
+  X: 278
+  Y: 96
diff --git a/src/robots/gr1t1_description/urdf/GR1T1.urdf b/src/robots/gr1t1_description/urdf/GR1T1.urdf
new file mode 100644
index 0000000..a3faf8a
--- /dev/null
+++ b/src/robots/gr1t1_description/urdf/GR1T1.urdf
@@ -0,0 +1,2023 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot
+        name="GR1T1">
+    <link
+            name="base">
+        <inertial>
+            <origin
+                    xyz="-0.0508888422032162 0.000109183466089114 -0.0455794100581443"
+                    rpy="0 0 0"/>
+            <mass
+                    value="6.45116957899581"/>
+            <inertia
+                    ixx="0.0149278332076571"
+                    ixy="1.72607763235119E-05"
+                    ixz="-7.0218313084794E-05"
+                    iyy="0.00640926461574383"
+                    iyz="-3.16385131035405E-06"
+                    izz="0.0127995550738519"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/base.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+            </material>
+        </visual>
+<!--        <collision>-->
+<!--            <origin-->
+<!--                    xyz="0 0 0.0"-->
+<!--                    rpy="0 0 0"/>-->
+<!--            <geometry>-->
+<!--                <cylinder length="0.1" radius="0.1"/>-->
+<!--            </geometry>-->
+<!--        </collision>-->
+        <!-- <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="../meshes/base.STL" />
+        </geometry> -->
+    </link>
+    <link
+            name="l_thigh_roll">
+        <inertial>
+            <origin
+                    xyz="0.04163 3.4568E-05 -0.0021382"
+                    rpy="0 0 0"/>
+            <mass
+                    value="1.1437"/>
+            <inertia
+                    ixx="0.0010239"
+                    ixy="9.9505E-08"
+                    ixz="5.4748E-05"
+                    iyy="0.0009439"
+                    iyz="-1.1272E-06"
+                    izz="0.0011557"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/l_thigh_roll.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.75294 0.75294 0.75294 1"/>
+            </material>
+        </visual>
+        <!-- <collision>
+          <origin
+            xyz="0 0 0"
+            rpy="0 0 0" />
+          <geometry>
+            <mesh
+              filename="../meshes/l_thigh_roll.STL" />
+          </geometry>
+        </collision> -->
+    </link>
+    <joint
+            name="l_hip_roll"
+            type="revolute">
+        <origin
+                xyz="-0.047999 0.105 -0.057493"
+                rpy="0 0 0"/>
+        <parent
+                link="base"/>
+        <child
+                link="l_thigh_roll"/>
+        <axis
+                xyz="1 0 1.221E-05"/>
+        <limit
+                lower="-0.09"
+                upper="0.79"
+                effort="100"
+                velocity="12.15"/>
+    </joint>
+    <link
+            name="l_thigh_yaw">
+        <inertial>
+            <origin
+                    xyz="0.00018348 0.046462 -0.10389"
+                    rpy="0 0 0"/>
+            <mass
+                    value="3.9888"/>
+            <inertia
+                    ixx="0.010009"
+                    ixy="-4.6866E-05"
+                    ixz="7.2342E-05"
+                    iyy="0.016331"
+                    iyz="0.00022608"
+                    izz="0.0092577"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/l_thigh_yaw.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.75294 0.75294 0.75294 1"/>
+            </material>
+        </visual>
+        <!-- <collision>
+          <origin
+            xyz="0 0 0"
+            rpy="0 0 0" />
+          <geometry>
+            <mesh
+              filename="../meshes/l_thigh_yaw.STL" />
+          </geometry>
+        </collision> -->
+    </link>
+    <joint
+            name="l_hip_yaw"
+            type="revolute">
+        <origin
+                xyz="0.048 0 -0.030499"
+                rpy="0 0 0"/>
+        <parent
+                link="l_thigh_roll"/>
+        <child
+                link="l_thigh_yaw"/>
+        <axis
+                xyz="1.2209E-05 6.8349E-05 1"/>
+        <limit
+                lower="-0.7"
+                upper="0.7"
+                effort="82.5"
+                velocity="16.76"/>
+    </joint>
+    <link
+            name="l_thigh_pitch">
+        <inertial>
+            <origin
+                    xyz="0.0028493 -0.021688 -0.090886"
+                    rpy="0 0 0"/>
+            <mass
+                    value="6.369"/>
+            <inertia
+                    ixx="0.042477"
+                    ixy="1.4561E-05"
+                    ixz="0.00097249"
+                    iyy="0.053235"
+                    iyz="-8.0062E-05"
+                    izz="0.013469"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/l_thigh_pitch.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.75294 0.75294 0.75294 1"/>
+            </material>
+        </visual>
+        <collision>
+            <origin
+                    xyz="0 0 -0.15"
+                    rpy="0 0 0"/>
+            <geometry>
+                <cylinder length="0.25" radius="0.05"/>
+            </geometry>
+        </collision>
+        <!-- <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="../meshes/l_thigh_pitch.STL" />
+        </geometry> -->
+    </link>
+    <joint
+            name="l_hip_pitch"
+            type="revolute">
+        <origin
+                xyz="0 0 -0.11"
+                rpy="0 0 0"/>
+        <parent
+                link="l_thigh_yaw"/>
+        <child
+                link="l_thigh_pitch"/>
+        <axis
+                xyz="0 1 -6.8349E-05"/>
+        <limit
+                lower="-1.75"
+                upper="0.7"
+                effort="160"
+                velocity="37.38"/>
+    </joint>
+    <link
+            name="l_shank_pitch">
+        <inertial>
+            <origin
+                    xyz="0.0024312 -2.9945E-05 -0.11554"
+                    rpy="0 0 0"/>
+            <mass
+                    value="2.1895"/>
+            <inertia
+                    ixx="0.016117"
+                    ixy="-6.426E-06"
+                    ixz="-5.8179E-05"
+                    iyy="0.01627"
+                    iyz="-2.4143E-06"
+                    izz="0.0011365"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/l_shank_pitch.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.75294 0.75294 0.75294 1"/>
+            </material>
+        </visual>
+        <collision>
+            <origin
+                    xyz="0 0 -0.15"
+                    rpy="0 0 0"/>
+            <geometry>
+                <cylinder length="0.2" radius="0.05"/>
+            </geometry>
+        </collision>
+        <!-- <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="../meshes/l_shank_pitch.STL" />
+        </geometry> -->
+    </link>
+    <joint
+            name="l_knee_pitch"
+            type="revolute">
+        <origin
+                xyz="0 2.4606E-05 -0.36"
+                rpy="0 0 0"/>
+        <parent
+                link="l_thigh_pitch"/>
+        <child
+                link="l_shank_pitch"/>
+        <axis
+                xyz="0 1 6.8349E-05"/>
+        <limit
+                lower="-0.09"
+                upper="1.92"
+                effort="160"
+                velocity="37.38"/>
+    </joint>
+    <link
+            name="l_foot_pitch">
+        <inertial>
+            <origin
+                    xyz="1.748E-11 4.3804E-11 -1.1768E-14"
+                    rpy="0 0 0"/>
+            <mass
+                    value="0.080335"/>
+            <inertia
+                    ixx="5.1022E-06"
+                    ixy="-7.7589E-16"
+                    ixz="1.1105E-11"
+                    iyy="4.3976E-06"
+                    iyz="1.4349E-11"
+                    izz="4.188E-06"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/l_foot_pitch.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.75294 0.75294 0.75294 1"/>
+            </material>
+        </visual>
+        <!-- <collision>
+          <origin
+            xyz="0 0 0"
+            rpy="0 0 0" />
+          <geometry>
+            <mesh
+              filename="../meshes/l_foot_pitch.STL" />
+          </geometry>
+        </collision> -->
+    </link>
+    <joint
+            name="l_ankle_pitch"
+            type="revolute">
+        <origin
+                xyz="0 2.3239E-05 -0.34"
+                rpy="0 0 0"/>
+        <parent
+                link="l_shank_pitch"/>
+        <child
+                link="l_foot_pitch"/>
+        <axis
+                xyz="0 1 6.8349E-05"/>
+        <limit
+                lower="-1.05"
+                upper="0.52"
+                effort="8"
+                velocity="20.32"/>
+    </joint>
+    <link
+            name="l_foot_roll">
+        <inertial>
+            <origin
+                    xyz="0.039446 0.00035757 -0.038101"
+                    rpy="0 0 0"/>
+            <mass
+                    value="0.584"/>
+            <inertia
+                    ixx="0.00036926"
+                    ixy="2.5654E-06"
+                    ixz="1.6145E-05"
+                    iyy="0.0024027"
+                    iyz="-1.171E-06"
+                    izz="0.0027102"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/l_foot_roll.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.75294 0.75294 0.75294 1"/>
+            </material>
+        </visual>
+        <!--        <collision>-->
+        <!--            <origin-->
+        <!--                    xyz="0 0 0"-->
+        <!--                    rpy="0 0 0"/>-->
+        <!--            <geometry>-->
+        <!--                <mesh-->
+        <!--                        filename="../meshes/l_foot_roll.STL"/>-->
+        <!--            </geometry>-->
+        <!--        </collision>-->
+        <collision name="l_foot_1">
+            <origin rpy="0 1.5708 0" xyz="0.05 0.02 -0.035"/>
+            <geometry>
+                <cylinder length="0.24" radius="0.02"/>
+            </geometry>
+        </collision>
+        <collision name="l_foot_2">
+            <origin rpy="0 1.5708 0" xyz="0.05 -0.02 -0.035"/>
+            <geometry>
+                <cylinder length="0.24" radius="0.02"/>
+            </geometry>
+        </collision>
+    </link>
+    <joint
+            name="l_ankle_roll"
+            type="revolute">
+        <origin
+                xyz="0 0 0"
+                rpy="0 0 0"/>
+        <parent
+                link="l_foot_pitch"/>
+        <child
+                link="l_foot_roll"/>
+        <axis
+                xyz="1 0 1.221E-05"/>
+        <limit
+                lower="-0.44"
+                upper="0.44"
+                effort="3"
+                velocity="20.32"/>
+    </joint>
+    <link
+            name="r_thigh_roll">
+        <inertial>
+            <origin
+                    xyz="0.0416295155706895 3.45677738990829E-05 -0.00213823706160741"
+                    rpy="0 0 0"/>
+            <mass
+                    value="1.14366511831048"/>
+            <inertia
+                    ixx="0.00102394921532563"
+                    ixy="9.95085325681151E-08"
+                    ixz="5.47475125260362E-05"
+                    iyy="0.000943899068816994"
+                    iyz="-1.12724702713386E-06"
+                    izz="0.00115569508392434"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/r_thigh_roll.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+            </material>
+        </visual>
+        <!-- <collision>
+          <origin
+            xyz="0 0 0"
+            rpy="0 0 0" />
+          <geometry>
+            <mesh
+              filename="../meshes/r_thigh_roll.STL" />
+          </geometry>
+        </collision> -->
+    </link>
+    <joint
+            name="r_hip_roll"
+            type="revolute">
+        <origin
+                xyz="-0.047999 -0.105 -0.057508"
+                rpy="0 0 0"/>
+        <parent
+                link="base"/>
+        <child
+                link="r_thigh_roll"/>
+        <axis
+                xyz="1 0 1.221E-05"/>
+        <limit
+                lower="-0.79"
+                upper="0.09"
+                effort="100"
+                velocity="12.15"/>
+    </joint>
+    <link
+            name="r_thigh_yaw">
+        <inertial>
+            <origin
+                    xyz="-0.00159762056384161 -0.0464481886102113 -0.103898553073221"
+                    rpy="0 0 0"/>
+            <mass
+                    value="3.98881919610613"/>
+            <inertia
+                    ixx="0.0100089332385704"
+                    ixy="6.17707630241845E-05"
+                    ixz="6.45577121456205E-05"
+                    iyy="0.0163310979398362"
+                    iyz="-0.000225113633002584"
+                    izz="0.00925763732044222"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/r_thigh_yaw.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+            </material>
+        </visual>
+        <!-- <collision>
+          <origin
+            xyz="0 0 0"
+            rpy="0 0 0" />
+          <geometry>
+            <mesh
+              filename="../meshes/r_thigh_yaw.STL" />
+          </geometry>
+        </collision> -->
+    </link>
+    <joint
+            name="r_hip_yaw"
+            type="revolute">
+        <origin
+                xyz="0.048 0 -0.030499"
+                rpy="0 0 0"/>
+        <parent
+                link="r_thigh_roll"/>
+        <child
+                link="r_thigh_yaw"/>
+        <axis
+                xyz="1.2209E-05 6.8349E-05 1"/>
+        <limit
+                lower="-0.7"
+                upper="0.7"
+                effort="82.5"
+                velocity="16.76"/>
+    </joint>
+    <link
+            name="r_thigh_pitch">
+        <inertial>
+            <origin
+                    xyz="0.00284199593611564 0.021700884823371 -0.0919987122372821"
+                    rpy="0 0 0"/>
+            <mass
+                    value="6.36895320063918"/>
+            <inertia
+                    ixx="0.0424483438482445"
+                    ixy="-1.47927561603623E-05"
+                    ixz="0.000984281537641053"
+                    iyy="0.0532347537596881"
+                    iyz="7.05431175910573E-05"
+                    izz="0.013497669133255"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/r_thigh_pitch.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+            </material>
+        </visual>
+        <collision>
+            <origin
+                    xyz="0 0 -0.15"
+                    rpy="0 0 0"/>
+            <geometry>
+                <cylinder length="0.25" radius="0.05"/>
+            </geometry>
+        </collision>
+        <!-- <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="../meshes/r_thigh_pitch.STL" />
+        </geometry> -->
+    </link>
+    <joint
+            name="r_hip_pitch"
+            type="revolute">
+        <origin
+                xyz="0 0 -0.11"
+                rpy="0 0 0"/>
+        <parent
+                link="r_thigh_yaw"/>
+        <child
+                link="r_thigh_pitch"/>
+        <axis
+                xyz="0 1 6.8349E-05"/>
+        <limit
+                lower="-1.75"
+                upper="0.7"
+                effort="160"
+                velocity="37.38"/>
+    </joint>
+    <link
+            name="r_shank_pitch">
+        <inertial>
+            <origin
+                    xyz="0.00236658249661569 4.56188310302558E-05 -0.115531948381968"
+                    rpy="0 0 0"/>
+            <mass
+                    value="2.1894662603365"/>
+            <inertia
+                    ixx="0.0161181947280801"
+                    ixy="5.07999055973116E-06"
+                    ixz="-5.80047175383452E-05"
+                    iyy="0.0162705809299741"
+                    iyz="8.20054646838796E-06"
+                    izz="0.001135584923079"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/r_shank_pitch.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+            </material>
+        </visual>
+        <collision>
+            <origin
+                    xyz="0 0 -0.15"
+                    rpy="0 0 0"/>
+            <geometry>
+                <cylinder length="0.2" radius="0.05"/>
+            </geometry>
+        </collision>
+        <!-- <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="../meshes/r_shank_pitch.STL" />
+        </geometry> -->
+    </link>
+    <joint
+            name="r_knee_pitch"
+            type="revolute">
+        <origin
+                xyz="0 2.4606E-05 -0.36"
+                rpy="0 0 0"/>
+        <parent
+                link="r_thigh_pitch"/>
+        <child
+                link="r_shank_pitch"/>
+        <axis
+                xyz="0 1 -6.8349E-05"/>
+        <limit
+                lower="-0.09"
+                upper="1.92"
+                effort="160"
+                velocity="37.38"/>
+    </joint>
+    <link
+            name="r_foot_pitch">
+        <inertial>
+            <origin
+                    xyz="1.75705006100202E-11 4.40342345919831E-11 -1.0991207943789E-14"
+                    rpy="0 0 0"/>
+            <mass
+                    value="0.0799218634469264"/>
+            <inertia
+                    ixx="5.11122380590824E-06"
+                    ixy="-7.90463256918838E-16"
+                    ixz="1.13183213108103E-11"
+                    iyy="4.40003788346816E-06"
+                    iyz="1.50971653307747E-11"
+                    izz="4.17953587325444E-06"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/r_foot_pitch.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+            </material>
+        </visual>
+        <!-- <collision>
+          <origin
+            xyz="0 0 0"
+            rpy="0 0 0" />
+          <geometry>
+            <mesh
+              filename="../meshes/r_foot_pitch.STL" />
+          </geometry>
+        </collision> -->
+    </link>
+    <joint
+            name="r_ankle_pitch"
+            type="revolute">
+        <origin
+                xyz="0 2.3239E-05 -0.34"
+                rpy="0 0 0"/>
+        <parent
+                link="r_shank_pitch"/>
+        <child
+                link="r_foot_pitch"/>
+        <axis
+                xyz="0 1 6.8349E-05"/>
+        <limit
+                lower="-1.05"
+                upper="0.52"
+                effort="8"
+                velocity="20.32"/>
+    </joint>
+    <link
+            name="r_foot_roll">
+        <inertial>
+            <origin
+                    xyz="0.043909 -0.00032475 -0.039419"
+                    rpy="0 0 0"/>
+            <mass
+                    value="0.58089"/>
+            <inertia
+                    ixx="0.00036866"
+                    ixy="-2.4583E-06"
+                    ixz="2.061E-05"
+                    iyy="0.0024886"
+                    iyz="1.0326E-06"
+                    izz="0.0028008"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/r_foot_roll.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.75294 0.75294 0.75294 1"/>
+            </material>
+        </visual>
+        <!--        <collision>-->
+        <!--            <origin-->
+        <!--                    xyz="0 0 0"-->
+        <!--                    rpy="0 0 0"/>-->
+        <!--            <geometry>-->
+        <!--                <mesh-->
+        <!--                        filename="../meshes/r_foot_roll.STL"/>-->
+        <!--            </geometry>-->
+        <!--        </collision>-->
+        <collision name="r_foot_1">
+            <origin rpy="0 1.5708 0" xyz="0.05 0.02 -0.035"/>
+            <geometry>
+                <cylinder length="0.24" radius="0.02"/>
+            </geometry>
+        </collision>
+        <collision name="r_foot_2">
+            <origin rpy="0 1.5708 0" xyz="0.05 -0.02 -0.035"/>
+            <geometry>
+                <cylinder length="0.24" radius="0.02"/>
+            </geometry>
+        </collision>
+    </link>
+    <joint
+            name="r_ankle_roll"
+            type="revolute">
+        <origin
+                xyz="0 0 0"
+                rpy="0 0 0"/>
+        <parent
+                link="r_foot_pitch"/>
+        <child
+                link="r_foot_roll"/>
+        <axis
+                xyz="1 0 1.221E-05"/>
+        <limit
+                lower="-0.44"
+                upper="0.44"
+                effort="3"
+                velocity="20.32"/>
+    </joint>
+    <link
+            name="waist_yaw">
+        <inertial>
+            <origin
+                    xyz="-0.01038801904175 -0.0016963578434683 0.0224708834468219"
+                    rpy="0 0 0"/>
+            <mass
+                    value="0.378859790655159"/>
+            <inertia
+                    ixx="0.000196919706249791"
+                    ixy="2.12382402842255E-08"
+                    ixz="2.56981032600088E-05"
+                    iyy="0.000354992150456574"
+                    iyz="2.2415775111576E-10"
+                    izz="0.00038367592149874"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/waist_yaw.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+            </material>
+        </visual>
+        <!--        <collision>-->
+        <!--            <origin-->
+        <!--                    xyz="0 0 -0.06"-->
+        <!--                    rpy="1.57 0 0"/>-->
+        <!--            <geometry>-->
+        <!--                <cylinder length="0.29" radius="0.039"/>-->
+        <!--            </geometry>-->
+        <!--        </collision>-->
+        <!--      &lt;!&ndash; <origin-->
+        <!--        xyz="0 0 0"-->
+        <!--        rpy="0 0 0" />-->
+        <!--      <geometry>-->
+        <!--        <mesh-->
+        <!--          filename="../meshes/waist_yaw.STL" />-->
+        <!--      </geometry> &ndash;&gt;-->
+    </link>
+    <joint
+            name="waist_yaw"
+            type="revolute">
+        <origin
+                xyz="0 0 0.0065"
+                rpy="0 0 0"/>
+        <parent
+                link="base"/>
+        <child
+                link="waist_yaw"/>
+        <axis
+                xyz="-1.221E-05 -6.8349E-05 1"/>
+        <limit
+                lower="-1.05"
+                upper="1.05"
+                effort="82.5"
+                velocity="16.76"/>
+    </joint>
+    <link
+            name="waist_pitch">
+        <inertial>
+            <origin
+                    xyz="0.00229172837325653 -0.00229363862540611 0.0425378199809092"
+                    rpy="0 0 0"/>
+            <mass
+                    value="4.40328023801883"/>
+            <inertia
+                    ixx="0.00487323232531471"
+                    ixy="-1.33065018698405E-06"
+                    ixz="-4.0266051775557E-05"
+                    iyy="0.00486933742500086"
+                    iyz="-5.32468490704019E-05"
+                    izz="0.00355689827572313"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/waist_pitch.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+            </material>
+        </visual>
+        <!-- <collision>
+          <origin
+            xyz="0 0 0"
+            rpy="0 0 0" />
+          <geometry>
+            <mesh
+              filename="../meshes/waist_pitch.STL" />
+          </geometry>
+        </collision> -->
+    </link>
+    <joint
+            name="waist_pitch"
+            type="revolute">
+        <origin
+                xyz="0 0 0.055"
+                rpy="0 0 0"/>
+        <parent
+                link="waist_yaw"/>
+        <child
+                link="waist_pitch"/>
+        <axis
+                xyz="0 1 -6.8349E-05"/>
+        <limit
+                lower="-0.52"
+                upper="1.22"
+                effort="82.5"
+                velocity="16.76"/>
+    </joint>
+    <link
+            name="waist_roll">
+        <inertial>
+            <origin
+                    xyz="-0.00982804024116587 -1.29539615708683E-05 0.160653030053409"
+                    rpy="0 0 0"/>
+            <mass
+                    value="7.34392576050251"/>
+            <inertia
+                    ixx="0.03052609429886"
+                    ixy="-7.04406839473537E-07"
+                    ixz="8.78092229296906E-05"
+                    iyy="0.0217567392965276"
+                    iyz="2.92528051867509E-07"
+                    izz="0.013678748857541"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/waist_roll.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+            </material>
+        </visual>
+        <collision>
+            <origin
+                    xyz="0 0 0.2"
+                    rpy="0 0 0"/>
+            <geometry>
+                <cylinder length="0.25" radius="0.1"/>
+            </geometry>
+        </collision>
+        <!-- <collision>
+          <origin
+            xyz="0 0 0"
+            rpy="0 0 0" />
+          <geometry>
+            <mesh
+              filename="../meshes/waist_roll.STL" />
+          </geometry>
+        </collision> -->
+    </link>
+    <joint
+            name="waist_roll"
+            type="revolute">
+        <origin
+                xyz="0 0 0.085"
+                rpy="0 0 0"/>
+        <parent
+                link="waist_pitch"/>
+        <child
+                link="waist_roll"/>
+        <axis
+                xyz="1 0 1.221E-05"/>
+        <limit
+                lower="-0.7"
+                upper="0.7"
+                effort="82.5"
+                velocity="16.76"/>
+    </joint>
+    <link
+            name="head_yaw">
+        <inertial>
+            <origin
+                    xyz="0.000255272678900229 -1.43142991538659E-05 0.0571736596753861"
+                    rpy="0 0 0"/>
+            <mass
+                    value="0.168424767391324"/>
+            <inertia
+                    ixx="0.000120637037320682"
+                    ixy="-5.93229450399767E-09"
+                    ixz="-1.70979295447655E-05"
+                    iyy="0.00013240987030897"
+                    iyz="-4.69267427280031E-08"
+                    izz="7.43274136318794E-05"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/head_yaw.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+            </material>
+        </visual>
+        <!-- <collision>
+          <origin
+            xyz="0 0 0"
+            rpy="0 0 0" />
+          <geometry>
+            <mesh
+              filename="../meshes/head_yaw.STL" />
+          </geometry>
+        </collision> -->
+    </link>
+    <joint
+            name="head_yaw"
+            type="revolute">
+        <origin
+                xyz="0 -2.3548E-05 0.34453"
+                rpy="0 0 0"/>
+        <parent
+                link="waist_roll"/>
+        <child
+                link="head_yaw"/>
+        <axis
+                xyz="-1.221E-05 -6.8349E-05 1"/>
+        <limit
+                lower="-2.71"
+                upper="2.71"
+                effort="10.2"
+                velocity="24.4"/>
+    </joint>
+    <link
+            name="head_roll">
+        <inertial>
+            <origin
+                    xyz="-0.00053971727334301 0.00299722575980531 -0.00194710854925029"
+                    rpy="0 0 0"/>
+            <mass
+                    value="0.0189573887284304"/>
+            <inertia
+                    ixx="3.07261028096843E-06"
+                    ixy="4.10292725305652E-08"
+                    ixz="1.78312300579722E-08"
+                    iyy="5.66077116289085E-06"
+                    iyz="-1.51708218419401E-08"
+                    izz="3.71878356025897E-06"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/head_roll.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+            </material>
+        </visual>
+        <!-- <collision>
+          <origin
+            xyz="0 0 0"
+            rpy="0 0 0" />
+          <geometry>
+            <mesh
+              filename="../meshes/head_roll.STL" />
+          </geometry>
+        </collision> -->
+    </link>
+    <joint
+            name="head_roll"
+            type="revolute">
+        <origin
+                xyz="0 0 0.1225"
+                rpy="0 0 0"/>
+        <parent
+                link="head_yaw"/>
+        <child
+                link="head_roll"/>
+        <axis
+                xyz="1 0 1.221E-05"/>
+        <limit
+                lower="-0.35"
+                upper="0.35"
+                effort="3.95"
+                velocity="27.96"/>
+    </joint>
+    <link
+            name="head_pitch">
+        <inertial>
+            <origin
+                    xyz="0.01799372239635 0.000157202623736513 -0.0107883907241233"
+                    rpy="0 0 0"/>
+            <mass
+                    value="0.315067351746468"/>
+            <inertia
+                    ixx="0.00100471588146565"
+                    ixy="-8.2389801869501E-07"
+                    ixz="0.000209876082162464"
+                    iyy="0.00105272220107397"
+                    iyz="2.28002016869807E-06"
+                    izz="0.00114154911943759"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/head_pitch.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+            </material>
+        </visual>
+<!--        <collision>-->
+<!--            <origin-->
+<!--                    xyz="0 0 0"-->
+<!--                    rpy="0 0 0"/>-->
+<!--            <geometry>-->
+<!--                <sphere radius="0.07"/>-->
+<!--            </geometry>-->
+<!--        </collision>-->
+        <!-- <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="../meshes/head_pitch.STL" />
+        </geometry> -->
+    </link>
+    <joint
+            name="head_pitch"
+            type="revolute">
+        <origin
+                xyz="0 0 0"
+                rpy="0 0 0"/>
+        <parent
+                link="head_roll"/>
+        <child
+                link="head_pitch"/>
+        <axis
+                xyz="0 1 6.8349E-05"/>
+        <limit
+                lower="-0.52"
+                upper="0.35"
+                effort="3.95"
+                velocity="27.96"/>
+    </joint>
+    <link
+            name="l_upper_arm_pitch">
+        <inertial>
+            <origin
+                    xyz="0.00515733406641455 0.0588336893604175 0.000122943787672525"
+                    rpy="0 0 0"/>
+            <mass
+                    value="0.777393531978708"/>
+            <inertia
+                    ixx="0.000546409518236063"
+                    ixy="4.5500097592642E-07"
+                    ixz="6.15521787285397E-08"
+                    iyy="0.000340546617645802"
+                    iyz="8.62383243316686E-07"
+                    izz="0.000319394142648864"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/l_upper_arm_pitch.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+            </material>
+        </visual>
+        <!-- <collision>
+          <origin
+            xyz="0 0 0"
+            rpy="0 0 0" />
+          <geometry>
+            <mesh
+              filename="../meshes/l_upper_arm_pitch.STL" />
+          </geometry>
+        </collision> -->
+    </link>
+    <joint
+            name="l_shoulder_pitch"
+            type="revolute">
+        <origin
+                xyz="0 0.12661 0.21727"
+                rpy="0.4364 0 0"/>
+        <parent
+                link="waist_roll"/>
+        <child
+                link="l_upper_arm_pitch"/>
+        <axis
+                xyz="0 1 0"/>
+        <limit
+                lower="-2.79"
+                upper="1.92"
+                effort="38"
+                velocity="9.11"/>
+    </joint>
+    <link
+            name="l_upper_arm_roll">
+        <inertial>
+            <origin
+                    xyz="0.0260054889695985 0.0238304608304643 -0.0272078144143461"
+                    rpy="0 0 0"/>
+            <mass
+                    value="0.0841110150281994"/>
+            <inertia
+                    ixx="8.86545692234474E-05"
+                    ixy="1.10261615048222E-05"
+                    ixz="-1.68388747194297E-05"
+                    iyy="6.47384851947623E-05"
+                    iyz="2.98032549399858E-05"
+                    izz="5.86690192674296E-05"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/l_upper_arm_roll.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+            </material>
+        </visual>
+<!--        <collision>-->
+<!--            <origin-->
+<!--                    xyz="0 0 0"-->
+<!--                    rpy="0 0 0"/>-->
+<!--            <geometry>-->
+<!--                <sphere radius="0.03"/>-->
+<!--            </geometry>-->
+<!--        </collision>-->
+        <!-- <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="../meshes/l_upper_arm_roll.STL" />
+        </geometry> -->
+    </link>
+    <joint
+            name="l_shoulder_roll"
+            type="revolute">
+        <origin
+                xyz="0 0.067 0"
+                rpy="-0.4364 0 0"/>
+        <parent
+                link="l_upper_arm_pitch"/>
+        <child
+                link="l_upper_arm_roll"/>
+        <axis
+                xyz="1 0 -1.221E-05"/>
+        <limit
+                lower="-0.57"
+                upper="3.27"
+                effort="38"
+                velocity="9.11"/>
+    </joint>
+    <link
+            name="l_upper_arm_yaw">
+        <inertial>
+            <origin
+                    xyz="-4.12427484584449E-05 0.0013503058680803 -0.101007372712731"
+                    rpy="0 0 0"/>
+            <mass
+                    value="0.789929607890225"/>
+            <inertia
+                    ixx="0.000420362349750673"
+                    ixy="-4.29935210110168E-08"
+                    ixz="2.9021678251778E-07"
+                    iyy="0.000479981540492924"
+                    iyz="1.20389492536702E-05"
+                    izz="0.00031649091251987"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/l_upper_arm_yaw.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+            </material>
+        </visual>
+        <collision>
+            <origin
+                    xyz="0 0 -0.05"
+                    rpy="0 0 0"/>
+            <geometry>
+                <cylinder length="0.11" radius="0.03"/>
+            </geometry>
+        </collision>
+        <!-- <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="../meshes/l_upper_arm_yaw.STL" />
+        </geometry> -->
+    </link>
+    <joint
+            name="l_shoulder_yaw"
+            type="revolute">
+        <origin
+                xyz="0 0.040004 -0.057502"
+                rpy="0 0 0"/>
+        <parent
+                link="l_upper_arm_roll"/>
+        <child
+                link="l_upper_arm_yaw"/>
+        <axis
+                xyz="1.221E-05 6.8349E-05 1"/>
+        <limit
+                lower="-2.97"
+                upper="2.97"
+                effort="30"
+                velocity="7.33"/>
+    </joint>
+    <link
+            name="l_lower_arm_pitch">
+        <inertial>
+            <origin
+                    xyz="2.6051859526588E-07 0.0196936824971259 -0.0210902088376421"
+                    rpy="0 0 0"/>
+            <mass
+                    value="0.0553958425180931"/>
+            <inertia
+                    ixx="2.63266982648187E-05"
+                    ixy="8.99558077509379E-11"
+                    ixz="1.02219606504308E-10"
+                    iyy="2.41073388509647E-05"
+                    iyz="-7.43975788176561E-06"
+                    izz="1.84112656900213E-05"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/l_lower_arm_pitch.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+            </material>
+        </visual>
+        <!-- <collision> -->
+        <!-- <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="../meshes/l_lower_arm_pitch.STL" />
+        </geometry> -->
+        <!-- </collision> -->
+    </link>
+    <joint
+            name="l_elbow_pitch"
+            type="revolute">
+        <origin
+                xyz="0 1.28869477361571E-05 -0.188546025627068"
+                rpy="0 0 0"/>
+        <parent
+                link="l_upper_arm_yaw"/>
+        <child
+                link="l_lower_arm_pitch"/>
+        <axis
+                xyz="0 0.999999997664201 6.83490816804566E-05"/>
+        <limit
+                lower="-2.27"
+                upper="2.27"
+                effort="30"
+                velocity="7.33"/>
+    </joint>
+    <link
+            name="l_hand_yaw">
+        <inertial>
+            <origin
+                    xyz="-4.20995524185452E-05 0.000336116177448609 -0.0559829380105823"
+                    rpy="0 0 0"/>
+            <mass
+                    value="0.606076158384213"/>
+            <inertia
+                    ixx="0.000747174769416669"
+                    ixy="6.19684554111363E-07"
+                    ixz="6.37825833529036E-06"
+                    iyy="0.000708647092597255"
+                    iyz="-1.62066266681277E-05"
+                    izz="0.000281470921643104"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/l_hand_yaw.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+            </material>
+        </visual>
+        <collision>
+            <origin
+                    xyz="0 0 -0.08"
+                    rpy="0 0 0"/>
+            <geometry>
+                <cylinder length="0.17" radius="0.035"/>
+            </geometry>
+        </collision>
+        <!-- <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="../meshes/l_hand_yaw.STL" />
+        </geometry> -->
+    </link>
+    <joint
+            name="l_wrist_yaw"
+            type="revolute">
+        <origin
+                xyz="0 0 -0.0405043040990825"
+                rpy="0 0 0"/>
+        <parent
+                link="l_lower_arm_pitch"/>
+        <child
+                link="l_hand_yaw"/>
+        <axis
+                xyz="1.22100045815953E-05 6.83490816738029E-05 0.999999997589659"/>
+        <limit
+                lower="-2.97"
+                upper="2.97"
+                effort="10.2"
+                velocity="24.4"/>
+    </joint>
+    <link
+            name="l_hand_roll">
+        <inertial>
+            <origin
+                    xyz="-1.5483E-08 -0.00044846 -1.9394E-08"
+                    rpy="0 0 0"/>
+            <mass
+                    value="0.0054473"/>
+            <inertia
+                    ixx="7.344E-08"
+                    ixy="-5.3228E-13"
+                    ixz="-5.6785E-12"
+                    iyy="3.5527E-07"
+                    iyz="-1.577E-13"
+                    izz="3.6036E-07"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/l_hand_roll.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.75294 0.75294 0.75294 1"/>
+            </material>
+        </visual>
+        <!-- <collision>
+          <origin
+            xyz="0 0 0"
+            rpy="0 0 0" />
+          <geometry>
+            <mesh
+              filename="../meshes/l_hand_roll.STL" />
+          </geometry>
+        </collision> -->
+    </link>
+    <joint
+            name="l_wrist_roll"
+            type="revolute">
+        <origin
+                xyz="0 1.3775E-05 -0.20155"
+                rpy="0 0 0"/>
+        <parent
+                link="l_hand_yaw"/>
+        <child
+                link="l_hand_roll"/>
+        <axis
+                xyz="1 0 -1.2211E-05"/>
+        <limit
+                lower="-0.61"
+                upper="0.61"
+                effort="3.95"
+                velocity="27.96"/>
+    </joint>
+    <link
+            name="l_hand_pitch">
+        <inertial>
+            <origin
+                    xyz="0.0071289 -0.0026192 -0.078567"
+                    rpy="0 0 0"/>
+            <mass
+                    value="0.5239"/>
+            <inertia
+                    ixx="0.00020226"
+                    ixy="1.8145E-06"
+                    ixz="7.7506E-06"
+                    iyy="0.00034574"
+                    iyz="2.8432E-06"
+                    izz="0.00020545"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/l_hand_pitch.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.75294 0.75294 0.75294 1"/>
+            </material>
+        </visual>
+        <collision>
+            <origin
+                    xyz="0 0 -0.07"
+                    rpy="0 0 0"/>
+            <geometry>
+                <sphere radius="0.05"/>
+            </geometry>
+        </collision>
+        <!-- <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="../meshes/l_hand_pitch.STL" />
+        </geometry> -->
+    </link>
+    <joint
+            name="l_wrist_pitch"
+            type="revolute">
+        <origin
+                xyz="0 0 0"
+                rpy="0 0 0"/>
+        <parent
+                link="l_hand_roll"/>
+        <child
+                link="l_hand_pitch"/>
+        <axis
+                xyz="0 1 6.8349E-05"/>
+        <limit
+                lower="-0.61"
+                upper="0.61"
+                effort="3.95"
+                velocity="27.96"/>
+    </joint>
+    <link
+            name="r_upper_arm_pitch">
+        <inertial>
+            <origin
+                    xyz="0.00515734616689043 -0.0587916277840502 0.000122943533844955"
+                    rpy="0 0 0"/>
+            <mass
+                    value="0.77739058985746"/>
+            <inertia
+                    ixx="0.000546409635698001"
+                    ixy="-2.01826802297192E-07"
+                    ixz="6.15757092559119E-08"
+                    iyy="0.000340546280880716"
+                    iyz="-1.08167740978519E-06"
+                    izz="0.00031939499037216"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/r_upper_arm_pitch.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+            </material>
+        </visual>
+        <!-- <collision>
+          <origin
+            xyz="0 0 0"
+            rpy="0 0 0" />
+          <geometry>
+            <mesh
+              filename="../meshes/r_upper_arm_pitch.STL" />
+          </geometry>
+        </collision> -->
+    </link>
+    <joint
+            name="r_shoulder_pitch"
+            type="revolute">
+        <origin
+                xyz="0 -0.12664 0.21725"
+                rpy="-0.43627 0 0"/>
+        <parent
+                link="waist_roll"/>
+        <child
+                link="r_upper_arm_pitch"/>
+        <axis
+                xyz="0 1 0"/>
+        <limit
+                lower="-2.79"
+                upper="1.92"
+                effort="38"
+                velocity="9.11"/>
+    </joint>
+    <link
+            name="r_upper_arm_roll">
+        <inertial>
+            <origin
+                    xyz="0.0260058558522785 -0.0238263693878023 -0.0272108671462995"
+                    rpy="0 0 0"/>
+            <mass
+                    value="0.0840988886420018"/>
+            <inertia
+                    ixx="8.86445321866659E-05"
+                    ixy="-1.10218258816922E-05"
+                    ixz="-1.68368503833084E-05"
+                    iyy="6.47370003862284E-05"
+                    iyz="-2.9799108598388E-05"
+                    izz="5.86526415191045E-05"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/r_upper_arm_roll.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+            </material>
+        </visual>
+<!--        <collision>-->
+<!--            <origin-->
+<!--                    xyz="0 0 0"-->
+<!--                    rpy="0 0 0"/>-->
+<!--            <geometry>-->
+<!--                <sphere radius="0.03"/>-->
+<!--            </geometry>-->
+<!--        </collision>-->
+        <!-- <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="../meshes/r_upper_arm_roll.STL" />
+        </geometry> -->
+    </link>
+    <joint
+            name="r_shoulder_roll"
+            type="revolute">
+        <origin
+                xyz="0 -0.067 0"
+                rpy="0.43627 0 0"/>
+        <parent
+                link="r_upper_arm_pitch"/>
+        <child
+                link="r_upper_arm_roll"/>
+        <axis
+                xyz="1 0 -1.221E-05"/>
+        <limit
+                lower="-3.27"
+                upper="0.57"
+                effort="38"
+                velocity="9.11"/>
+    </joint>
+    <link
+            name="r_upper_arm_yaw">
+        <inertial>
+            <origin
+                    xyz="0.00017082971927132 -0.00124147770094965 -0.100980640933926"
+                    rpy="0 0 0"/>
+            <mass
+                    value="0.789929613522641"/>
+            <inertia
+                    ixx="0.000420162267467054"
+                    ixy="9.56785696731752E-07"
+                    ixz="-3.81937864165678E-08"
+                    iyy="0.000480249411637188"
+                    iyz="-1.18111475395093E-05"
+                    izz="0.000316423129953537"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/r_upper_arm_yaw.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+            </material>
+        </visual>
+        <collision>
+            <origin
+                    xyz="0 0 -0.05"
+                    rpy="0 0 0"/>
+            <geometry>
+                <cylinder length="0.11" radius="0.03"/>
+            </geometry>
+        </collision>
+        <!-- <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="../meshes/r_upper_arm_yaw.STL" />
+        </geometry> -->
+    </link>
+    <joint
+            name="r_shoulder_yaw"
+            type="revolute">
+        <origin
+                xyz="0 -0.039996 -0.057473"
+                rpy="0 0 0"/>
+        <parent
+                link="r_upper_arm_roll"/>
+        <child
+                link="r_upper_arm_yaw"/>
+        <axis
+                xyz="1.221E-05 6.8349E-05 1"/>
+        <limit
+                lower="-2.97"
+                upper="2.97"
+                effort="30"
+                velocity="7.33"/>
+    </joint>
+    <link
+            name="r_lower_arm_pitch">
+        <inertial>
+            <origin
+                    xyz="2.54537579028047E-07 -0.0196907994945518 -0.0210929007516363"
+                    rpy="0 0 0"/>
+            <mass
+                    value="0.0553958425155445"/>
+            <inertia
+                    ixx="2.63266982660393E-05"
+                    ixy="-9.17316256555676E-11"
+                    ixz="9.10503314379193E-11"
+                    iyy="2.4105304727251E-05"
+                    iyz="7.4405362533913E-06"
+                    izz="1.84132998138921E-05"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/r_lower_arm_pitch.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+            </material>
+        </visual>
+        <!-- <collision>
+          <origin
+            xyz="0 0 0"
+            rpy="0 0 0" />
+          <geometry>
+            <mesh
+              filename="../meshes/r_lower_arm_pitch.STL" />
+          </geometry>
+        </collision> -->
+    </link>
+    <joint
+            name="r_elbow_pitch"
+            type="revolute">
+        <origin
+                xyz="0 1.28858724863512E-05 -0.188530329837713"
+                rpy="0 0 0"/>
+        <parent
+                link="r_upper_arm_yaw"/>
+        <child
+                link="r_lower_arm_pitch"/>
+        <axis
+                xyz="0 0.999999997664136 -6.83500416450034E-05"/>
+        <limit
+                lower="-2.27"
+                upper="2.27"
+                effort="30"
+                velocity="7.33"/>
+    </joint>
+    <link
+            name="r_hand_yaw">
+        <inertial>
+            <origin
+                    xyz="0.000158043899506258 -0.000478821798581081 -0.0559415052246017"
+                    rpy="0 0 0"/>
+            <mass
+                    value="0.606076127067475"/>
+            <inertia
+                    ixx="0.000748031289868089"
+                    ixy="-2.3854566608948E-07"
+                    ixz="-6.67415672551873E-06"
+                    iyy="0.000707789468859292"
+                    iyz="1.63905136248605E-05"
+                    izz="0.000281472007554911"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/r_hand_yaw.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+            </material>
+        </visual>
+        <collision>
+            <origin
+                    xyz="0 0 -0.08"
+                    rpy="0 0 0"/>
+            <geometry>
+                <cylinder length="0.17" radius="0.035"/>
+            </geometry>
+        </collision>
+        <!-- <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="../meshes/r_hand_yaw.STL" />
+        </geometry> -->
+    </link>
+    <joint
+            name="r_wrist_yaw"
+            type="revolute">
+        <origin
+                xyz="0 0 -0.0404999999249678"
+                rpy="0 0 0"/>
+        <parent
+                link="r_lower_arm_pitch"/>
+        <child
+                link="r_hand_yaw"/>
+        <axis
+                xyz="1.22100046176447E-05 6.83500416387496E-05 0.999999997589594"/>
+        <limit
+                lower="-2.97"
+                upper="2.97"
+                effort="10.2"
+                velocity="24.4"/>
+    </joint>
+    <link
+            name="r_hand_roll">
+        <inertial>
+            <origin
+                    xyz="1.5545E-08 0.00044846 4.1938E-08"
+                    rpy="0 0 0"/>
+            <mass
+                    value="0.0054473"/>
+            <inertia
+                    ixx="7.344E-08"
+                    ixy="-5.3559E-13"
+                    ixz="-1.3224E-12"
+                    iyy="3.5527E-07"
+                    iyz="-5.3178E-13"
+                    izz="3.6036E-07"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/r_hand_roll.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.75294 0.75294 0.75294 1"/>
+            </material>
+        </visual>
+        <!-- <collision>
+          <origin
+            xyz="0 0 0"
+            rpy="0 0 0" />
+          <geometry>
+            <mesh
+              filename="../meshes/r_hand_roll.STL" />
+          </geometry>
+        </collision> -->
+    </link>
+    <joint
+            name="r_wrist_roll"
+            type="revolute">
+        <origin
+                xyz="0 1.3775E-05 -0.20153"
+                rpy="0 0 0"/>
+        <parent
+                link="r_hand_yaw"/>
+        <child
+                link="r_hand_roll"/>
+        <axis
+                xyz="1 0 1.221E-05"/>
+        <limit
+                lower="-0.61"
+                upper="0.61"
+                effort="3.95"
+                velocity="27.96"/>
+    </joint>
+    <link
+            name="r_hand_pitch">
+        <inertial>
+            <origin
+                    xyz="0.0060733 0.0018907 -0.084578"
+                    rpy="0 0 0"/>
+            <mass
+                    value="0.53221"/>
+            <inertia
+                    ixx="0.00018966"
+                    ixy="-1.685E-06"
+                    ixz="6.9527E-06"
+                    iyy="0.00031983"
+                    iyz="-3.4284E-06"
+                    izz="0.00018923"/>
+        </inertial>
+        <visual>
+            <origin
+                    xyz="0 0 0"
+                    rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                        filename="../meshes/r_hand_pitch.STL"/>
+            </geometry>
+            <material
+                    name="">
+                <color
+                        rgba="0.75294 0.75294 0.75294 1"/>
+            </material>
+        </visual>
+        <collision>
+            <origin
+                    xyz="0 0 -0.07"
+                    rpy="0 0 0"/>
+            <geometry>
+                <sphere radius="0.05"/>
+            </geometry>
+        </collision>
+        <!-- <origin
+          xyz="0 0 0"
+          rpy="0 0 0" />
+        <geometry>
+          <mesh
+            filename="../meshes/r_hand_pitch.STL" />
+        </geometry> -->
+    </link>
+    <joint
+            name="r_wrist_pitch"
+            type="revolute">
+        <origin
+                xyz="0 0 0"
+                rpy="0 0 0"/>
+        <parent
+                link="r_hand_roll"/>
+        <child
+                link="r_hand_pitch"/>
+        <axis
+                xyz="0 1 -6.835E-05"/>
+        <limit
+                lower="-0.61"
+                upper="0.61"
+                effort="3.95"
+                velocity="27.96"/>
+    </joint>
+
+</robot>
\ No newline at end of file
diff --git a/src/robots/gr1t1_description/urdf/GR1T1_lower_limb.urdf b/src/robots/gr1t1_description/urdf/GR1T1_lower_limb.urdf
new file mode 100644
index 0000000..c755c3f
--- /dev/null
+++ b/src/robots/gr1t1_description/urdf/GR1T1_lower_limb.urdf
@@ -0,0 +1,1400 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot name="gr1t1_description">
+  <!-- ros_control plugin -->
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>/gr1t1_gazebo</robotNamespace>
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+    </plugin>
+  </gazebo>
+  <!-- Show the trajectory of base_link center. -->
+  <!-- <gazebo>
+        <plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
+            <frequency>10</frequency>
+            <plot>
+                <link>base</link>
+                <pose>0 0 0 0 0 0</pose>
+                <material>Gazebo/Yellow</material>
+            </plot>
+        </plugin>
+    </gazebo> -->
+  <!-- Show the trajectory of foot. You can add another trajectory about another foot. -->
+  <!-- <gazebo>
+        <plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
+            <frequency>100</frequency>
+            <plot>
+                <link>FL_foot</link>
+                <pose>0 0 0 0 0 0</pose>
+                <material>Gazebo/Green</material>
+            </plot>
+        </plugin>
+    </gazebo> -->
+  <gazebo>
+    <plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
+      <bodyName>base_link</bodyName>
+      <topicName>/apply_force/base_link</topicName>
+    </plugin>
+  </gazebo>
+  <gazebo reference="imu_link">
+    <gravity>true</gravity>
+    <sensor name="imu_sensor" type="imu">
+      <always_on>true</always_on>
+      <update_rate>1000</update_rate>
+      <visualize>true</visualize>
+      <topic>__default_topic__</topic>
+      <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
+        <topicName>base_link_imu</topicName>
+        <bodyName>imu_link</bodyName>
+        <updateRateHZ>1000.0</updateRateHZ>
+        <gaussianNoise>0.0</gaussianNoise>
+        <xyzOffset>0 0 0</xyzOffset>
+        <rpyOffset>0 0 0</rpyOffset>
+        <frameName>imu_link</frameName>
+      </plugin>
+      <pose>0 0 0 0 0 0</pose>
+    </sensor>
+  </gazebo>
+
+  <gazebo reference="base">
+    <material>Gazebo/DarkGrey</material>
+    <turnGravityOff>false</turnGravityOff>
+  </gazebo>
+  <gazebo reference="base_link">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <gazebo reference="imu_link">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <material>Gazebo/Red</material>
+  </gazebo>
+
+  <!-- left leg -->
+  <gazebo reference="l_thigh_roll">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="l_thigh_yaw">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="l_thigh_pitch">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="l_shank_pitch">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="l_foot_pitch">
+    <mu1>1.0</mu1>
+    <mu2>1.0</mu2>
+    <self_collide>1</self_collide>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="l_foot_roll">
+    <mu1>1.0</mu1>
+    <mu2>1.0</mu2>
+    <self_collide>1</self_collide>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+
+  <!-- right leg -->
+  <gazebo reference="r_thigh_roll">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="r_thigh_yaw">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="r_thigh_pitch">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="r_shank_pitch">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="r_foot_pitch">
+    <mu1>1.0</mu1>
+    <mu2>1.0</mu2>
+    <self_collide>1</self_collide>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="r_foot_roll">
+    <mu1>1.0</mu1>
+    <mu2>1.0</mu2>
+    <self_collide>1</self_collide>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+
+  <material name="black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="blue">
+    <color rgba="0.0 0.0 0.8 1.0"/>
+  </material>
+  <material name="green">
+    <color rgba="0.0 0.8 0.0 1.0"/>
+  </material>
+  <material name="grey">
+    <color rgba="0.2 0.2 0.2 1.0"/>
+  </material>
+  <material name="silver">
+    <color rgba="0.913725490196 0.913725490196 0.847058823529 1.0"/>
+  </material>
+  <material name="orange">
+    <color rgba="0.12 0.15 0.2 1.0"/>
+  </material>
+  <material name="brown">
+    <color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
+  </material>
+  <material name="red">
+    <color rgba="0.8 0.0 0.0 1.0"/>
+  </material>
+  <material name="white">
+    <color rgba="1.0 1.0 1.0 1.0"/>
+  </material>
+  
+  <!-- ########################################################################## -->
+
+  <!-- <link name="world"/>
+  <joint name="base_static_joint" type="fixed">
+      <origin rpy="0 0 0" xyz="0 0 0.5"/>
+      <parent link="world"/>
+      <child link="base"/>
+  </joint> -->
+
+  <link name="base">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+  </link>
+  <joint name="floating_base" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="base"/>
+    <child link="base_link"/>
+  </joint>
+
+  <!-- ########################################################################## -->
+
+
+
+  <link name="base_link">
+    <inertial>
+      <origin xyz="-0.0508888422032162 0.000109183466089114 -0.0455794100581443" rpy="0 0 0"/>
+      <mass value="6.45116957899581"/>
+      <inertia ixx="0.0149278332076571" ixy="1.72607763235119E-05" ixz="-7.0218313084794E-05" iyy="0.00640926461574383" iyz="-3.16385131035405E-06" izz="0.0127995550738519"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/base.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <!--        <collision>-->
+    <!--            <origin-->
+    <!-- xyz="0 0 0.0"-->
+    <!-- rpy="0 0 0"/>-->
+    <!--            <geometry>-->
+    <!--                <cylinder length="0.1" radius="0.1"/>-->
+    <!--            </geometry>-->
+    <!--        </collision>-->
+    <!-- <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://gr1t1_description/meshes/base.STL" />
+        </geometry> -->
+  </link>
+  <link name="l_thigh_roll">
+    <inertial>
+      <origin xyz="0.04163 3.4568E-05 -0.0021382" rpy="0 0 0"/>
+      <mass value="1.1437"/>
+      <inertia ixx="0.0010239" ixy="9.9505E-08" ixz="5.4748E-05" iyy="0.0009439" iyz="-1.1272E-06" izz="0.0011557"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/l_thigh_roll.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://gr1t1_description/meshes/l_thigh_roll.STL" />
+          </geometry>
+        </collision> -->
+  </link>
+  <joint name="l_hip_roll" type="revolute">
+    <origin xyz="-0.047999 0.105 -0.057493" rpy="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="l_thigh_roll"/>
+    <axis xyz="1 0 1.221E-05"/>
+    <limit lower="-0.09" upper="0.79" effort="60" velocity="12.15"/>
+  </joint>
+  <link name="l_thigh_yaw">
+    <inertial>
+      <origin xyz="0.00018348 0.046462 -0.10389" rpy="0 0 0"/>
+      <mass value="3.9888"/>
+      <inertia ixx="0.010009" ixy="-4.6866E-05" ixz="7.2342E-05" iyy="0.016331" iyz="0.00022608" izz="0.0092577"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/l_thigh_yaw.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://gr1t1_description/meshes/l_thigh_yaw.STL" />
+          </geometry>
+        </collision> -->
+  </link>
+  <joint name="l_hip_yaw" type="revolute">
+    <origin xyz="0.048 0 -0.030499" rpy="0 0 0"/>
+    <parent link="l_thigh_roll"/>
+    <child link="l_thigh_yaw"/>
+    <axis xyz="1.2209E-05 6.8349E-05 1"/>
+    <limit lower="-0.7" upper="0.7" effort="45" velocity="16.76"/>
+  </joint>
+  <link name="l_thigh_pitch">
+    <inertial>
+      <origin xyz="0.0028493 -0.021688 -0.090886" rpy="0 0 0"/>
+      <mass value="6.369"/>
+      <inertia ixx="0.042477" ixy="1.4561E-05" ixz="0.00097249" iyy="0.053235" iyz="-8.0062E-05" izz="0.013469"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/l_thigh_pitch.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.25" radius="0.05"/>
+      </geometry>
+    </collision>
+    <!-- <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://gr1t1_description/meshes/l_thigh_pitch.STL" />
+        </geometry> -->
+  </link>
+  <joint name="l_hip_pitch" type="revolute">
+    <origin xyz="0 0 -0.11" rpy="0 0 0"/>
+    <parent link="l_thigh_yaw"/>
+    <child link="l_thigh_pitch"/>
+    <axis xyz="0 1 -6.8349E-05"/>
+    <limit lower="-1.75" upper="0.7" effort="130" velocity="37.38"/>
+  </joint>
+  <link name="l_shank_pitch">
+    <inertial>
+      <origin xyz="0.0024312 -2.9945E-05 -0.11554" rpy="0 0 0"/>
+      <mass value="2.1895"/>
+      <inertia ixx="0.016117" ixy="-6.426E-06" ixz="-5.8179E-05" iyy="0.01627" iyz="-2.4143E-06" izz="0.0011365"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/l_shank_pitch.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.2" radius="0.05"/>
+      </geometry>
+    </collision>
+    <!-- <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://gr1t1_description/meshes/l_shank_pitch.STL" />
+        </geometry> -->
+  </link>
+  <joint name="l_knee_pitch" type="revolute">
+    <origin xyz="0 2.4606E-05 -0.36" rpy="0 0 0"/>
+    <parent link="l_thigh_pitch"/>
+    <child link="l_shank_pitch"/>
+    <axis xyz="0 1 6.8349E-05"/>
+    <limit lower="-0.09" upper="1.92" effort="130" velocity="37.38"/>
+  </joint>
+  <link name="l_foot_pitch">
+    <inertial>
+      <origin xyz="1.748E-11 4.3804E-11 -1.1768E-14" rpy="0 0 0"/>
+      <mass value="0.080335"/>
+      <inertia ixx="5.1022E-06" ixy="-7.7589E-16" ixz="1.1105E-11" iyy="4.3976E-06" iyz="1.4349E-11" izz="4.188E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/l_foot_pitch.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://gr1t1_description/meshes/l_foot_pitch.STL" />
+          </geometry>
+        </collision> -->
+  </link>
+  <joint name="l_ankle_pitch" type="revolute">
+    <origin xyz="0 2.3239E-05 -0.34" rpy="0 0 0"/>
+    <parent link="l_shank_pitch"/>
+    <child link="l_foot_pitch"/>
+    <axis xyz="0 1 6.8349E-05"/>
+    <limit lower="-1.05" upper="0.52" effort="16" velocity="20.32"/>
+  </joint>
+  <link name="l_foot_roll">
+    <inertial>
+      <origin xyz="0.039446 0.00035757 -0.038101" rpy="0 0 0"/>
+      <mass value="0.584"/>
+      <inertia ixx="0.00036926" ixy="2.5654E-06" ixz="1.6145E-05" iyy="0.0024027" iyz="-1.171E-06" izz="0.0027102"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/l_foot_roll.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <!--        <collision>-->
+    <!--            <origin-->
+    <!-- xyz="0 0 0"-->
+    <!-- rpy="0 0 0"/>-->
+    <!--            <geometry>-->
+    <!--                <mesh-->
+    <!-- filename="package://gr1t1_description/meshes/l_foot_roll.STL"/>-->
+    <!--            </geometry>-->
+    <!--        </collision>-->
+    <collision name="l_foot_1">
+      <origin rpy="0 1.5708 0" xyz="0.05 0.02 -0.035"/>
+      <geometry>
+        <cylinder length="0.24" radius="0.02"/>
+      </geometry>
+    </collision>
+    <collision name="l_foot_2">
+      <origin rpy="0 1.5708 0" xyz="0.05 -0.02 -0.035"/>
+      <geometry>
+        <cylinder length="0.24" radius="0.02"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="l_ankle_roll" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="l_foot_pitch"/>
+    <child link="l_foot_roll"/>
+    <axis xyz="1 0 1.221E-05"/>
+    <limit lower="-0.44" upper="0.44" effort="8" velocity="20.32"/>
+  </joint>
+
+
+  <link name="r_thigh_roll">
+    <inertial>
+      <origin xyz="0.0416295155706895 3.45677738990829E-05 -0.00213823706160741" rpy="0 0 0"/>
+      <mass value="1.14366511831048"/>
+      <inertia ixx="0.00102394921532563" ixy="9.95085325681151E-08" ixz="5.47475125260362E-05" iyy="0.000943899068816994" iyz="-1.12724702713386E-06" izz="0.00115569508392434"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/r_thigh_roll.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://gr1t1_description/meshes/r_thigh_roll.STL" />
+          </geometry>
+        </collision> -->
+  </link>
+  <joint name="r_hip_roll" type="revolute">
+    <origin xyz="-0.047999 -0.105 -0.057508" rpy="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="r_thigh_roll"/>
+    <axis xyz="1 0 1.221E-05"/>
+    <limit lower="-0.79" upper="0.09" effort="60" velocity="12.15"/>
+  </joint>
+  <link name="r_thigh_yaw">
+    <inertial>
+      <origin xyz="-0.00159762056384161 -0.0464481886102113 -0.103898553073221" rpy="0 0 0"/>
+      <mass value="3.98881919610613"/>
+      <inertia ixx="0.0100089332385704" ixy="6.17707630241845E-05" ixz="6.45577121456205E-05" iyy="0.0163310979398362" iyz="-0.000225113633002584" izz="0.00925763732044222"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/r_thigh_yaw.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://gr1t1_description/meshes/r_thigh_yaw.STL" />
+          </geometry>
+        </collision> -->
+  </link>
+  <joint name="r_hip_yaw" type="revolute">
+    <origin xyz="0.048 0 -0.030499" rpy="0 0 0"/>
+    <parent link="r_thigh_roll"/>
+    <child link="r_thigh_yaw"/>
+    <axis xyz="1.2209E-05 6.8349E-05 1"/>
+    <limit lower="-0.7" upper="0.7" effort="45" velocity="16.76"/>
+  </joint>
+  <link name="r_thigh_pitch">
+    <inertial>
+      <origin xyz="0.00284199593611564 0.021700884823371 -0.0919987122372821" rpy="0 0 0"/>
+      <mass value="6.36895320063918"/>
+      <inertia ixx="0.0424483438482445" ixy="-1.47927561603623E-05" ixz="0.000984281537641053" iyy="0.0532347537596881" iyz="7.05431175910573E-05" izz="0.013497669133255"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/r_thigh_pitch.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.25" radius="0.05"/>
+      </geometry>
+    </collision>
+    <!-- <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://gr1t1_description/meshes/r_thigh_pitch.STL" />
+        </geometry> -->
+  </link>
+  <joint name="r_hip_pitch" type="revolute">
+    <origin xyz="0 0 -0.11" rpy="0 0 0"/>
+    <parent link="r_thigh_yaw"/>
+    <child link="r_thigh_pitch"/>
+    <axis xyz="0 1 6.8349E-05"/>
+    <limit lower="-1.75" upper="0.7" effort="130" velocity="37.38"/>
+  </joint>
+  <link name="r_shank_pitch">
+    <inertial>
+      <origin xyz="0.00236658249661569 4.56188310302558E-05 -0.115531948381968" rpy="0 0 0"/>
+      <mass value="2.1894662603365"/>
+      <inertia ixx="0.0161181947280801" ixy="5.07999055973116E-06" ixz="-5.80047175383452E-05" iyy="0.0162705809299741" iyz="8.20054646838796E-06" izz="0.001135584923079"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/r_shank_pitch.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.2" radius="0.05"/>
+      </geometry>
+    </collision>
+    <!-- <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://gr1t1_description/meshes/r_shank_pitch.STL" />
+        </geometry> -->
+  </link>
+  <joint name="r_knee_pitch" type="revolute">
+    <origin xyz="0 2.4606E-05 -0.36" rpy="0 0 0"/>
+    <parent link="r_thigh_pitch"/>
+    <child link="r_shank_pitch"/>
+    <axis xyz="0 1 -6.8349E-05"/>
+    <limit lower="-0.09" upper="1.92" effort="130" velocity="37.38"/>
+  </joint>
+  <link name="r_foot_pitch">
+    <inertial>
+      <origin xyz="1.75705006100202E-11 4.40342345919831E-11 -1.0991207943789E-14" rpy="0 0 0"/>
+      <mass value="0.0799218634469264"/>
+      <inertia ixx="5.11122380590824E-06" ixy="-7.90463256918838E-16" ixz="1.13183213108103E-11" iyy="4.40003788346816E-06" iyz="1.50971653307747E-11" izz="4.17953587325444E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/r_foot_pitch.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://gr1t1_description/meshes/r_foot_pitch.STL" />
+          </geometry>
+        </collision> -->
+  </link>
+  <joint name="r_ankle_pitch" type="revolute">
+    <origin xyz="0 2.3239E-05 -0.34" rpy="0 0 0"/>
+    <parent link="r_shank_pitch"/>
+    <child link="r_foot_pitch"/>
+    <axis xyz="0 1 6.8349E-05"/>
+    <limit lower="-1.05" upper="0.52" effort="16" velocity="20.32"/>
+  </joint>
+  <link name="r_foot_roll">
+    <inertial>
+      <origin xyz="0.043909 -0.00032475 -0.039419" rpy="0 0 0"/>
+      <mass value="0.58089"/>
+      <inertia ixx="0.00036866" ixy="-2.4583E-06" ixz="2.061E-05" iyy="0.0024886" iyz="1.0326E-06" izz="0.0028008"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/r_foot_roll.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <!--        <collision>-->
+    <!--            <origin-->
+    <!-- xyz="0 0 0"-->
+    <!-- rpy="0 0 0"/>-->
+    <!--            <geometry>-->
+    <!--                <mesh-->
+    <!-- filename="package://gr1t1_description/meshes/r_foot_roll.STL"/>-->
+    <!--            </geometry>-->
+    <!--        </collision>-->
+    <collision name="r_foot_1">
+      <origin rpy="0 1.5708 0" xyz="0.05 0.02 -0.035"/>
+      <geometry>
+        <cylinder length="0.24" radius="0.02"/>
+      </geometry>
+    </collision>
+    <collision name="r_foot_2">
+      <origin rpy="0 1.5708 0" xyz="0.05 -0.02 -0.035"/>
+      <geometry>
+        <cylinder length="0.24" radius="0.02"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="r_ankle_roll" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="r_foot_pitch"/>
+    <child link="r_foot_roll"/>
+    <axis xyz="1 0 1.221E-05"/>
+    <limit lower="-0.44" upper="0.44" effort="8" velocity="20.32"/>
+  </joint>
+  <link name="waist_yaw">
+    <inertial>
+      <origin xyz="-0.01038801904175 -0.0016963578434683 0.0224708834468219" rpy="0 0 0"/>
+      <mass value="0.378859790655159"/>
+      <inertia ixx="0.000196919706249791" ixy="2.12382402842255E-08" ixz="2.56981032600088E-05" iyy="0.000354992150456574" iyz="2.2415775111576E-10" izz="0.00038367592149874"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/waist_yaw.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <!--        <collision>-->
+    <!--            <origin-->
+    <!-- xyz="0 0 -0.06"-->
+    <!-- rpy="1.57 0 0"/>-->
+    <!--            <geometry>-->
+    <!--                <cylinder length="0.29" radius="0.039"/>-->
+    <!--            </geometry>-->
+    <!--        </collision>-->
+    <!--      &lt;!&ndash; <origin-->
+    <!-- xyz="0 0 0"-->
+    <!-- rpy="0 0 0" />-->
+    <!--      <geometry>-->
+    <!--        <mesh-->
+    <!-- filename="package://gr1t1_description/meshes/waist_yaw.STL" />-->
+    <!--      </geometry> &ndash;&gt;-->
+  </link>
+  <joint name="waist_yaw" type="fixed">
+    <origin xyz="0 0 0.0065" rpy="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="waist_yaw"/>
+    <axis xyz="-1.221E-05 -6.8349E-05 1"/>
+    <limit lower="-1.05" upper="1.05" effort="45" velocity="16.76"/>
+  </joint>
+  <link name="waist_pitch">
+    <inertial>
+      <origin xyz="0.00229172837325653 -0.00229363862540611 0.0425378199809092" rpy="0 0 0"/>
+      <mass value="4.40328023801883"/>
+      <inertia ixx="0.00487323232531471" ixy="-1.33065018698405E-06" ixz="-4.0266051775557E-05" iyy="0.00486933742500086" iyz="-5.32468490704019E-05" izz="0.00355689827572313"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/waist_pitch.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://gr1t1_description/meshes/waist_pitch.STL" />
+          </geometry>
+        </collision> -->
+  </link>
+  <joint name="waist_pitch" type="fixed">
+    <origin xyz="0 0 0.055" rpy="0 0 0"/>
+    <parent link="waist_yaw"/>
+    <child link="waist_pitch"/>
+    <axis xyz="0 1 -6.8349E-05"/>
+    <limit lower="-0.52" upper="1.22" effort="45" velocity="16.76"/>
+  </joint>
+  <link name="waist_roll">
+    <inertial>
+      <origin xyz="-0.00982804024116587 -1.29539615708683E-05 0.160653030053409" rpy="0 0 0"/>
+      <mass value="7.34392576050251"/>
+      <inertia ixx="0.03052609429886" ixy="-7.04406839473537E-07" ixz="8.78092229296906E-05" iyy="0.0217567392965276" iyz="2.92528051867509E-07" izz="0.013678748857541"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/waist_roll.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.2" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.25" radius="0.1"/>
+      </geometry>
+    </collision>
+    <!-- <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://gr1t1_description/meshes/waist_roll.STL" />
+          </geometry>
+        </collision> -->
+  </link>
+  <joint name="waist_roll" type="fixed">
+    <origin xyz="0 0 0.085" rpy="0 0 0"/>
+    <parent link="waist_pitch"/>
+    <child link="waist_roll"/>
+    <axis xyz="1 0 1.221E-05"/>
+    <limit lower="-0.7" upper="0.7" effort="45" velocity="16.76"/>
+  </joint>
+  <link name="head_yaw">
+    <inertial>
+      <origin xyz="0.000255272678900229 -1.43142991538659E-05 0.0571736596753861" rpy="0 0 0"/>
+      <mass value="0.168424767391324"/>
+      <inertia ixx="0.000120637037320682" ixy="-5.93229450399767E-09" ixz="-1.70979295447655E-05" iyy="0.00013240987030897" iyz="-4.69267427280031E-08" izz="7.43274136318794E-05"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/head_yaw.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://gr1t1_description/meshes/head_yaw.STL" />
+          </geometry>
+        </collision> -->
+  </link>
+  <joint name="head_yaw" type="fixed">
+    <origin xyz="0 -2.3548E-05 0.34453" rpy="0 0 0"/>
+    <parent link="waist_roll"/>
+    <child link="head_yaw"/>
+    <axis xyz="-1.221E-05 -6.8349E-05 1"/>
+    <limit lower="-2.71" upper="2.71" effort="10.2" velocity="24.4"/>
+  </joint>
+  <link name="head_roll">
+    <inertial>
+      <origin xyz="-0.00053971727334301 0.00299722575980531 -0.00194710854925029" rpy="0 0 0"/>
+      <mass value="0.0189573887284304"/>
+      <inertia ixx="3.07261028096843E-06" ixy="4.10292725305652E-08" ixz="1.78312300579722E-08" iyy="5.66077116289085E-06" iyz="-1.51708218419401E-08" izz="3.71878356025897E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/head_roll.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://gr1t1_description/meshes/head_roll.STL" />
+          </geometry>
+        </collision> -->
+  </link>
+  <joint name="head_roll" type="fixed">
+    <origin xyz="0 0 0.1225" rpy="0 0 0"/>
+    <parent link="head_yaw"/>
+    <child link="head_roll"/>
+    <axis xyz="1 0 1.221E-05"/>
+    <limit lower="-0.35" upper="0.35" effort="3.95" velocity="27.96"/>
+  </joint>
+  <link name="head_pitch">
+    <inertial>
+      <origin xyz="0.01799372239635 0.000157202623736513 -0.0107883907241233" rpy="0 0 0"/>
+      <mass value="0.315067351746468"/>
+      <inertia ixx="0.00100471588146565" ixy="-8.2389801869501E-07" ixz="0.000209876082162464" iyy="0.00105272220107397" iyz="2.28002016869807E-06" izz="0.00114154911943759"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/head_pitch.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <!--        <collision>-->
+    <!--            <origin-->
+    <!-- xyz="0 0 0"-->
+    <!-- rpy="0 0 0"/>-->
+    <!--            <geometry>-->
+    <!--                <sphere radius="0.07"/>-->
+    <!--            </geometry>-->
+    <!--        </collision>-->
+    <!-- <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://gr1t1_description/meshes/head_pitch.STL" />
+        </geometry> -->
+  </link>
+  <joint name="head_pitch" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="head_roll"/>
+    <child link="head_pitch"/>
+    <axis xyz="0 1 6.8349E-05"/>
+    <limit lower="-0.52" upper="0.35" effort="3.95" velocity="27.96"/>
+  </joint>
+  <link name="l_upper_arm_pitch">
+    <inertial>
+      <origin xyz="0.00515733406641455 0.0588336893604175 0.000122943787672525" rpy="0 0 0"/>
+      <mass value="0.777393531978708"/>
+      <inertia ixx="0.000546409518236063" ixy="4.5500097592642E-07" ixz="6.15521787285397E-08" iyy="0.000340546617645802" iyz="8.62383243316686E-07" izz="0.000319394142648864"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/l_upper_arm_pitch.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://gr1t1_description/meshes/l_upper_arm_pitch.STL" />
+          </geometry>
+        </collision> -->
+  </link>
+  <joint name="l_shoulder_pitch" type="fixed">
+    <origin xyz="0 0.12661 0.21727" rpy="0.4364 0 0"/>
+    <parent link="waist_roll"/>
+    <child link="l_upper_arm_pitch"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.79" upper="1.92" effort="18" velocity="9.11"/>
+  </joint>
+  <link name="l_upper_arm_roll">
+    <inertial>
+      <origin xyz="0.0260054889695985 0.0238304608304643 -0.0272078144143461" rpy="0 0 0"/>
+      <mass value="0.0841110150281994"/>
+      <inertia ixx="8.86545692234474E-05" ixy="1.10261615048222E-05" ixz="-1.68388747194297E-05" iyy="6.47384851947623E-05" iyz="2.98032549399858E-05" izz="5.86690192674296E-05"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/l_upper_arm_roll.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <!--        <collision>-->
+    <!--            <origin-->
+    <!-- xyz="0 0 0"-->
+    <!-- rpy="0 0 0"/>-->
+    <!--            <geometry>-->
+    <!--                <sphere radius="0.03"/>-->
+    <!--            </geometry>-->
+    <!--        </collision>-->
+    <!-- <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://gr1t1_description/meshes/l_upper_arm_roll.STL" />
+        </geometry> -->
+  </link>
+  <joint name="l_shoulder_roll" type="fixed">
+    <origin xyz="0 0.067 0" rpy="-0.2364 0 0"/>
+    <!-- rpy="-0.4364 0 0"/>-->
+    <parent link="l_upper_arm_pitch"/>
+    <child link="l_upper_arm_roll"/>
+    <axis xyz="1 0 -1.221E-05"/>
+    <limit lower="-0.57" upper="3.27" effort="18" velocity="9.11"/>
+  </joint>
+  <link name="l_upper_arm_yaw">
+    <inertial>
+      <origin xyz="-4.12427484584449E-05 0.0013503058680803 -0.101007372712731" rpy="0 0 0"/>
+      <mass value="0.789929607890225"/>
+      <inertia ixx="0.000420362349750673" ixy="-4.29935210110168E-08" ixz="2.9021678251778E-07" iyy="0.000479981540492924" iyz="1.20389492536702E-05" izz="0.00031649091251987"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/l_upper_arm_yaw.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.05" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.11" radius="0.03"/>
+      </geometry>
+    </collision>
+    <!-- <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://gr1t1_description/meshes/l_upper_arm_yaw.STL" />
+        </geometry> -->
+  </link>
+  <joint name="l_shoulder_yaw" type="fixed">
+    <origin xyz="0 0.040004 -0.057502" rpy="0 0 0"/>
+    <parent link="l_upper_arm_roll"/>
+    <child link="l_upper_arm_yaw"/>
+    <axis xyz="1.221E-05 6.8349E-05 1"/>
+    <limit lower="-2.97" upper="2.97" effort="18" velocity="7.33"/>
+  </joint>
+  <link name="l_lower_arm_pitch">
+    <inertial>
+      <origin xyz="2.6051859526588E-07 0.0196936824971259 -0.0210902088376421" rpy="0 0 0"/>
+      <mass value="0.0553958425180931"/>
+      <inertia ixx="2.63266982648187E-05" ixy="8.99558077509379E-11" ixz="1.02219606504308E-10" iyy="2.41073388509647E-05" iyz="-7.43975788176561E-06" izz="1.84112656900213E-05"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/l_lower_arm_pitch.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <!-- <collision> -->
+    <!-- <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://gr1t1_description/meshes/l_lower_arm_pitch.STL" />
+        </geometry> -->
+    <!-- </collision> -->
+  </link>
+  <joint name="l_elbow_pitch" type="fixed">
+    <origin xyz="0 1.28869477361571E-05 -0.188546025627068" rpy="0 -0.3 0"/>
+    <parent link="l_upper_arm_yaw"/>
+    <child link="l_lower_arm_pitch"/>
+    <axis xyz="0 0.999999997664201 6.83490816804566E-05"/>
+    <limit lower="-2.27" upper="2.27" effort="18" velocity="7.33"/>
+  </joint>
+  <link name="l_hand_yaw">
+    <inertial>
+      <origin xyz="-4.20995524185452E-05 0.000336116177448609 -0.0559829380105823" rpy="0 0 0"/>
+      <mass value="0.606076158384213"/>
+      <inertia ixx="0.000747174769416669" ixy="6.19684554111363E-07" ixz="6.37825833529036E-06" iyy="0.000708647092597255" iyz="-1.62066266681277E-05" izz="0.000281470921643104"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/l_hand_yaw.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.08" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.17" radius="0.035"/>
+      </geometry>
+    </collision>
+    <!-- <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://gr1t1_description/meshes/l_hand_yaw.STL" />
+        </geometry> -->
+  </link>
+  <joint name="l_wrist_yaw" type="fixed">
+    <origin xyz="0 0 -0.0405043040990825" rpy="0 0 0"/>
+    <parent link="l_lower_arm_pitch"/>
+    <child link="l_hand_yaw"/>
+    <axis xyz="1.22100045815953E-05 6.83490816738029E-05 0.999999997589659"/>
+    <limit lower="-2.97" upper="2.97" effort="10.2" velocity="24.4"/>
+  </joint>
+  <link name="l_hand_roll">
+    <inertial>
+      <origin xyz="-1.5483E-08 -0.00044846 -1.9394E-08" rpy="0 0 0"/>
+      <mass value="0.0054473"/>
+      <inertia ixx="7.344E-08" ixy="-5.3228E-13" ixz="-5.6785E-12" iyy="3.5527E-07" iyz="-1.577E-13" izz="3.6036E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/l_hand_roll.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://gr1t1_description/meshes/l_hand_roll.STL" />
+          </geometry>
+        </collision> -->
+  </link>
+  <joint name="l_wrist_roll" type="fixed">
+    <origin xyz="0 1.3775E-05 -0.20155" rpy="0 0 0"/>
+    <parent link="l_hand_yaw"/>
+    <child link="l_hand_roll"/>
+    <axis xyz="1 0 -1.2211E-05"/>
+    <limit lower="-0.61" upper="0.61" effort="3.95" velocity="27.96"/>
+  </joint>
+  <link name="l_hand_pitch">
+    <inertial>
+      <origin xyz="0.0071289 -0.0026192 -0.078567" rpy="0 0 0"/>
+      <mass value="0.5239"/>
+      <inertia ixx="0.00020226" ixy="1.8145E-06" ixz="7.7506E-06" iyy="0.00034574" iyz="2.8432E-06" izz="0.00020545"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/l_hand_pitch.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.07" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.05"/>
+      </geometry>
+    </collision>
+    <!-- <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://gr1t1_description/meshes/l_hand_pitch.STL" />
+        </geometry> -->
+  </link>
+  <joint name="l_wrist_pitch" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="l_hand_roll"/>
+    <child link="l_hand_pitch"/>
+    <axis xyz="0 1 6.8349E-05"/>
+    <limit lower="-0.61" upper="0.61" effort="3.95" velocity="27.96"/>
+  </joint>
+  <link name="r_upper_arm_pitch">
+    <inertial>
+      <origin xyz="0.00515734616689043 -0.0587916277840502 0.000122943533844955" rpy="0 0 0"/>
+      <mass value="0.77739058985746"/>
+      <inertia ixx="0.000546409635698001" ixy="-2.01826802297192E-07" ixz="6.15757092559119E-08" iyy="0.000340546280880716" iyz="-1.08167740978519E-06" izz="0.00031939499037216"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/r_upper_arm_pitch.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://gr1t1_description/meshes/r_upper_arm_pitch.STL" />
+          </geometry>
+        </collision> -->
+  </link>
+  <joint name="r_shoulder_pitch" type="fixed">
+    <origin xyz="0 -0.12664 0.21725" rpy="-0.43627 0 0"/>
+    <parent link="waist_roll"/>
+    <child link="r_upper_arm_pitch"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.79" upper="1.92" effort="18" velocity="9.11"/>
+  </joint>
+  <link name="r_upper_arm_roll">
+    <inertial>
+      <origin xyz="0.0260058558522785 -0.0238263693878023 -0.0272108671462995" rpy="0 0 0"/>
+      <mass value="0.0840988886420018"/>
+      <inertia ixx="8.86445321866659E-05" ixy="-1.10218258816922E-05" ixz="-1.68368503833084E-05" iyy="6.47370003862284E-05" iyz="-2.9799108598388E-05" izz="5.86526415191045E-05"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/r_upper_arm_roll.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <!--        <collision>-->
+    <!--            <origin-->
+    <!-- xyz="0 0 0"-->
+    <!-- rpy="0 0 0"/>-->
+    <!--            <geometry>-->
+    <!--                <sphere radius="0.03"/>-->
+    <!--            </geometry>-->
+    <!--        </collision>-->
+    <!-- <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://gr1t1_description/meshes/r_upper_arm_roll.STL" />
+        </geometry> -->
+  </link>
+  <joint name="r_shoulder_roll" type="fixed">
+    <origin xyz="0 -0.067 0" rpy="0.23627 0 0"/>
+    <!-- rpy="0.43627 0 0"/>-->
+    <parent link="r_upper_arm_pitch"/>
+    <child link="r_upper_arm_roll"/>
+    <axis xyz="1 0 -1.221E-05"/>
+    <limit lower="-3.27" upper="0.57" effort="18" velocity="9.11"/>
+  </joint>
+  <link name="r_upper_arm_yaw">
+    <inertial>
+      <origin xyz="0.00017082971927132 -0.00124147770094965 -0.100980640933926" rpy="0 0 0"/>
+      <mass value="0.789929613522641"/>
+      <inertia ixx="0.000420162267467054" ixy="9.56785696731752E-07" ixz="-3.81937864165678E-08" iyy="0.000480249411637188" iyz="-1.18111475395093E-05" izz="0.000316423129953537"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/r_upper_arm_yaw.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.05" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.11" radius="0.03"/>
+      </geometry>
+    </collision>
+    <!-- <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://gr1t1_description/meshes/r_upper_arm_yaw.STL" />
+        </geometry> -->
+  </link>
+  <joint name="r_shoulder_yaw" type="fixed">
+    <origin xyz="0 -0.039996 -0.057473" rpy="0 0 0"/>
+    <parent link="r_upper_arm_roll"/>
+    <child link="r_upper_arm_yaw"/>
+    <axis xyz="1.221E-05 6.8349E-05 1"/>
+    <limit lower="-2.97" upper="2.97" effort="18" velocity="7.33"/>
+  </joint>
+  <link name="r_lower_arm_pitch">
+    <inertial>
+      <origin xyz="2.54537579028047E-07 -0.0196907994945518 -0.0210929007516363" rpy="0 0 0"/>
+      <mass value="0.0553958425155445"/>
+      <inertia ixx="2.63266982660393E-05" ixy="-9.17316256555676E-11" ixz="9.10503314379193E-11" iyy="2.4105304727251E-05" iyz="7.4405362533913E-06" izz="1.84132998138921E-05"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/r_lower_arm_pitch.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://gr1t1_description/meshes/r_lower_arm_pitch.STL" />
+          </geometry>
+        </collision> -->
+  </link>
+  <joint name="r_elbow_pitch" type="fixed">
+    <origin xyz="0 1.28858724863512E-05 -0.188530329837713" rpy="0 -0.3 0"/>
+    <parent link="r_upper_arm_yaw"/>
+    <child link="r_lower_arm_pitch"/>
+    <axis xyz="0 0.999999997664136 -6.83500416450034E-05"/>
+    <limit lower="-2.27" upper="2.27" effort="18" velocity="7.33"/>
+  </joint>
+  <link name="r_hand_yaw">
+    <inertial>
+      <origin xyz="0.000158043899506258 -0.000478821798581081 -0.0559415052246017" rpy="0 0 0"/>
+      <mass value="0.606076127067475"/>
+      <inertia ixx="0.000748031289868089" ixy="-2.3854566608948E-07" ixz="-6.67415672551873E-06" iyy="0.000707789468859292" iyz="1.63905136248605E-05" izz="0.000281472007554911"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/r_hand_yaw.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.08" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.17" radius="0.035"/>
+      </geometry>
+    </collision>
+    <!-- <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://gr1t1_description/meshes/r_hand_yaw.STL" />
+        </geometry> -->
+  </link>
+  <joint name="r_wrist_yaw" type="fixed">
+    <origin xyz="0 0 -0.0404999999249678" rpy="0 0 0"/>
+    <parent link="r_lower_arm_pitch"/>
+    <child link="r_hand_yaw"/>
+    <axis xyz="1.22100046176447E-05 6.83500416387496E-05 0.999999997589594"/>
+    <limit lower="-2.97" upper="2.97" effort="10.2" velocity="24.4"/>
+  </joint>
+  <link name="r_hand_roll">
+    <inertial>
+      <origin xyz="1.5545E-08 0.00044846 4.1938E-08" rpy="0 0 0"/>
+      <mass value="0.0054473"/>
+      <inertia ixx="7.344E-08" ixy="-5.3559E-13" ixz="-1.3224E-12" iyy="3.5527E-07" iyz="-5.3178E-13" izz="3.6036E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/r_hand_roll.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <!-- <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="package://gr1t1_description/meshes/r_hand_roll.STL" />
+          </geometry>
+        </collision> -->
+  </link>
+  <joint name="r_wrist_roll" type="fixed">
+    <origin xyz="0 1.3775E-05 -0.20153" rpy="0 0 0"/>
+    <parent link="r_hand_yaw"/>
+    <child link="r_hand_roll"/>
+    <axis xyz="1 0 1.221E-05"/>
+    <limit lower="-0.61" upper="0.61" effort="3.95" velocity="27.96"/>
+  </joint>
+  <link name="r_hand_pitch">
+    <inertial>
+      <origin xyz="0.0060733 0.0018907 -0.084578" rpy="0 0 0"/>
+      <mass value="0.53221"/>
+      <inertia ixx="0.00018966" ixy="-1.685E-06" ixz="6.9527E-06" iyy="0.00031983" iyz="-3.4284E-06" izz="0.00018923"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://gr1t1_description/meshes/r_hand_pitch.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.07" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.05"/>
+      </geometry>
+    </collision>
+    <!-- <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://gr1t1_description/meshes/r_hand_pitch.STL" />
+        </geometry> -->
+  </link>
+  <joint name="r_wrist_pitch" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="r_hand_roll"/>
+    <child link="r_hand_pitch"/>
+    <axis xyz="0 1 -6.835E-05"/>
+    <limit lower="-0.61" upper="0.61" effort="3.95" velocity="27.96"/>
+  </joint>
+
+
+  <!-- ############################################################ -->
+
+  <link name="imu_link">
+    <visual>
+      <geometry>
+        <box size="0.05 0.05 0.02"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="z_joint_imu" type="fixed">
+    <origin xyz="-0.0648 0 -0.1015" rpy="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="imu_link"/>
+  </joint>
+
+  <!-- ############################################################ -->
+
+  <transmission name="l_hip_roll_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="l_hip_roll">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="l_hip_roll_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>&quot;1&quot;</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="l_hip_yaw_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="l_hip_yaw">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="l_hip_yaw_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>&quot;1&quot;</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="l_hip_pitch_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="l_hip_pitch">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="l_hip_pitch_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>&quot;1&quot;</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="l_knee_pitch_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="l_knee_pitch">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="l_knee_pitch_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>&quot;1&quot;</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="l_ankle_pitch_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="l_ankle_pitch">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="l_ankle_pitch_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>&quot;1&quot;</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <transmission name="r_hip_roll_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="r_hip_roll">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="r_hip_roll_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>&quot;1&quot;</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="r_hip_yaw_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="r_hip_yaw">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="r_hip_yaw_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>&quot;1&quot;</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="r_hip_pitch_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="r_hip_pitch">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="r_hip_pitch_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>&quot;1&quot;</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="r_knee_pitch_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="r_knee_pitch">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="r_knee_pitch_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>&quot;1&quot;</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="r_ankle_pitch_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="r_ankle_pitch">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="r_ankle_pitch_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>&quot;1&quot;</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+</robot>
\ No newline at end of file