diff --git a/gundam_rx78_control/config/gundam_rx78_control.yaml b/gundam_rx78_control/config/gundam_rx78_control.yaml
index 571c002..39fd01f 100644
--- a/gundam_rx78_control/config/gundam_rx78_control.yaml
+++ b/gundam_rx78_control/config/gundam_rx78_control.yaml
@@ -47,11 +47,6 @@ lleg_ankle_r_position:
   type: effort_controllers/JointPositionController
   joint: lleg_ankle_r
   pid: {p: 10000000.000000, i: 5000.000000, d: 1000000.000000}
-  mimic_joints:
-    rx78_Null_040_joint_position:
-      type: effort_controllers/JointPositionController
-      joint: rx78_Null_040_joint
-      pid: {p: 10000000.000000, i: 5000.000000, d: 1000000.000000}
 rarm_elbow_p_position:
   type: effort_controllers/JointPositionController
   joint: rarm_elbow_p
@@ -297,11 +292,6 @@ rleg_ankle_r_position:
   type: effort_controllers/JointPositionController
   joint: rleg_ankle_r
   pid: {p: 10000000.000000, i: 5000.000000, d: 1000000.000000}
-  mimic_joints:
-    rx78_Null_090_joint_position:
-      type: effort_controllers/JointPositionController
-      joint: rx78_Null_090_joint
-      pid: {p: 10000000.000000, i: 5000.000000, d: 1000000.000000}
 rleg_crotch_r_position:
   type: effort_controllers/JointPositionController
   joint: rleg_crotch_r
@@ -311,3 +301,159 @@ rleg_crotch_r_position:
       type: effort_controllers/JointPositionController
       joint: rx78_Null_032_joint
       pid: {p: 50000.000000, i: 500.000000, d: 5000.000000}
+
+# Joint Trajectory Controllers ---------------------------------------
+fullbody_controller:
+  type: effort_controllers/JointTrajectoryController
+  joints:
+    - lleg_ankle_p
+    - lleg_crotch_p
+    - lleg_crotch_y
+    - lleg_knee_p
+    - rarm_shoulder_p
+    - lleg_ankle_r
+    - rarm_elbow_p
+    - rarm_shoulder_r
+    - rarm_shoulder_y
+    - rarm_gripper
+    - rarm_wrist_y
+    - rarm_wrist_r
+    - larm_wrist_y
+    - larm_gripper
+    - larm_wrist_r
+    - larm_shoulder_p
+    - larm_shoulder_r
+    - larm_elbow_p
+    - larm_shoulder_y
+    - lleg_crotch_r
+    - rleg_ankle_p
+    - rleg_crotch_p
+    - rleg_knee_p
+    - rleg_crotch_y
+    - torso_waist_p
+    - head_neck_p
+    - head_neck_y
+    - torso_waist_y
+    - rleg_ankle_r
+    - rleg_crotch_r
+  mimic_joints:
+    - rx78_Null_038_joint # lleg_knee_p
+    - rx78_Null_034_joint # lleg_crotch_p
+    - rx78_Null_030_joint # rleg_ankle_p
+    - rx78_Null_031_joint # rleg_crotch_p
+    - rx78_Null_032_joint # rleg_crotch_r
+    - rx78_Null_033_joint # lleg_crotch_p
+    - rx78_Null_047_joint # lleg_crotch_r
+    - rx78_Null_046_joint # lleg_ankle_p
+    - rx78_Null_058_joint # rarm_gripper
+    - rx78_Null_059_joint # larm_gripper
+    - rx78_Null_053_joint # rarm_elbow_p
+    - rx78_Null_057_joint # rarm_gripper
+    - rx78_Null_066_joint # larm_elbow_p
+    - rx78_Null_064_joint # larm_gripper
+    - rx78_Null_063_joint # larm_gripper
+    - rx78_Null_061_joint # larm_gripper
+    - rx78_Null_060_joint # larm_gripper
+    - rx78_Null_068_joint # larm_gripper
+    - rx78_Null_088_joint # rleg_knee_p
+    - rx78_Null_084_joint # rleg_crotch_p
+    - rx78_Null_080_joint # rarm_gripper
+    - rx78_Null_070_joint # larm_gripper
+    - rx78_Null_071_joint # larm_gripper
+    - rx78_Null_072_joint # larm_gripper
+    - rx78_Null_073_joint # larm_gripper
+    - rx78_Null_074_joint # larm_gripper
+    - rx78_Null_075_joint # larm_gripper
+    - rx78_Null_076_joint # larm_gripper
+    - rx78_Null_077_joint # larm_gripper
+    - rx78_Null_078_joint # rarm_gripper
+    - rx78_Null_079_joint # rarm_gripper
+    - rx78_Null_016_joint # torso_waist_p
+    - rx78_Null_023_joint # rarm_gripper
+    - rx78_Null_022_joint # rarm_gripper
+    - rx78_Null_021_joint # rarm_gripper
+    - rx78_Null_027_joint # rarm_gripper
+    - rx78_Null_026_joint # rarm_gripper
+    - rx78_Null_025_joint # rarm_gripper
+    - rx78_Null_024_joint # rarm_gripper
+    - rx78_Null_029_joint # rarm_gripper
+    - rx78_Null_028_joint # rarm_gripper
+  gains:
+    lleg_ankle_p : {p: 10000000.000000, i: 5000.000000, d: 1000000.000000}
+    lleg_crotch_p : {p: 40000000.000000, i: 4000000.000000, d: 2000000.000000}
+    lleg_crotch_y : {p: 20000000.000000, i: 1000000.000000, d: 1000000.000000}
+    lleg_knee_p : {p: 10000000.000000, i: 100000.000000, d: 1000000.000000}
+    rarm_shoulder_p : {p: 1000000.000000, i: 500.000000, d: 200000.000000}
+    lleg_ankle_r : {p: 10000000.000000, i: 5000.000000, d: 1000000.000000}
+    rarm_elbow_p : {p: 1000000.000000, i: 500.000000, d: 200000.000000}
+    rarm_shoulder_r : {p: 1000000.000000, i: 500.000000, d: 200000.000000}
+    rarm_shoulder_y : {p: 1000000.000000, i: 500.000000, d: 200000.000000}
+    rarm_gripper : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rarm_wrist_y : {p: 100000.000000, i: 50.000000, d: 20000.000000}
+    rarm_wrist_r : {p: 100000.000000, i: 50.000000, d: 20000.000000}
+    larm_wrist_y : {p: 100000.000000, i: 50.000000, d: 20000.000000}
+    larm_gripper : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    larm_wrist_r : {p: 100000.000000, i: 50.000000, d: 20000.000000}
+    larm_shoulder_p : {p: 1000000.000000, i: 500.000000, d: 200000.000000}
+    larm_shoulder_r : {p: 1000000.000000, i: 500.000000, d: 200000.000000}
+    larm_elbow_p : {p: 1000000.000000, i: 500.000000, d: 200000.000000}
+    larm_shoulder_y : {p: 1000000.000000, i: 500.000000, d: 200000.000000}
+    lleg_crotch_r : {p: 20000000.000000, i: 1000000.000000, d: 1000000.000000}
+    rleg_ankle_p : {p: 10000000.000000, i: 5000.000000, d: 1000000.000000}
+    rleg_crotch_p : {p: 40000000.000000, i: 4000000.000000, d: 2000000.000000}
+    rleg_knee_p : {p: 10000000.000000, i: 100000.000000, d: 1000000.000000}
+    rleg_crotch_y : {p: 20000000.000000, i: 1000000.000000, d: 1000000.000000}
+    torso_waist_p : {p: 20000000.000000, i: 10000.000000, d: 2000000.000000}
+    head_neck_p : {p: 1000000.000000, i: 500.000000, d: 200000.000000}
+    head_neck_y : {p: 1000000.000000, i: 500.000000, d: 200000.000000}
+    torso_waist_y : {p: 20000000.000000, i: 10000.000000, d: 2000000.000000}
+    rleg_ankle_r : {p: 10000000.000000, i: 5000.000000, d: 1000000.000000}
+    rleg_crotch_r : {p: 20000000.000000, i: 1000000.000000, d: 1000000.000000}
+    rx78_Null_038_joint : {p: 10000000.000000, i: 100000.000000, d: 1000000.000000}
+    rx78_Null_034_joint : {p: 50000.000000, i: 500.000000, d: 5000.000000}
+    rx78_Null_030_joint : {p: 10000000.000000, i: 5000.000000, d: 1000000.000000}
+    rx78_Null_031_joint : {p: 50000.000000, i: 500.000000, d: 5000.000000}
+    rx78_Null_032_joint : {p: 50000.000000, i: 500.000000, d: 5000.000000}
+    rx78_Null_033_joint : {p: 50000.000000, i: 500.000000, d: 5000.000000}
+    rx78_Null_047_joint : {p: 50000.000000, i: 500.000000, d: 5000.000000}
+    rx78_Null_046_joint : {p: 10000000.000000, i: 5000.000000, d: 1000000.000000}
+    rx78_Null_058_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_059_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_053_joint : {p: 1000000.000000, i: 500.000000, d: 200000.000000}
+    rx78_Null_057_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_066_joint : {p: 1000000.000000, i: 500.000000, d: 200000.000000}
+    rx78_Null_064_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_063_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_061_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_060_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_068_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_088_joint : {p: 10000000.000000, i: 100000.000000, d: 1000000.000000}
+    rx78_Null_084_joint : {p: 50000.000000, i: 500.000000, d: 5000.000000}
+    rx78_Null_080_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_070_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_071_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_072_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_073_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_074_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_075_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_076_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_077_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_078_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_079_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_016_joint : {p: 20000000.000000, i: 10000.000000, d: 2000000.000000}
+    rx78_Null_023_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_022_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_021_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_027_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_026_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_025_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_024_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_029_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+    rx78_Null_028_joint : {p: 5000.000000, i: 100.000000, d: 1000.000000}
+  constraints:
+    goal_time: 0.6
+    stopped_velocity_tolerance: 0.05
+    # joint: {trajectory: 0.2, goal: 0.2}
+  stop_trajectory_duration: 0.5
+  state_publish_rate:  125
+  action_monitor_rate: 10
diff --git a/gundam_rx78_description/urdf/GGC_TestModel_rx78_20170112.urdf b/gundam_rx78_description/urdf/GGC_TestModel_rx78_20170112.urdf
index c75f683..bf6736b 100644
--- a/gundam_rx78_description/urdf/GGC_TestModel_rx78_20170112.urdf
+++ b/gundam_rx78_description/urdf/GGC_TestModel_rx78_20170112.urdf
@@ -2570,29 +2570,16 @@
     <child link="rx78_Null_089_link"/>
     <limit effort="1000000000" lower="-0.785398163397" upper="0.785398163397" velocity="1000000"/>
   </joint>
-  <link name="rx78_Null_090_link">
-    <inertial>
-      <mass value="0.8"/>
-      <inertia ixx="0.00533333333333" ixy="0.0" ixz="0.0" iyy="0.00533333333333" iyz="0.0" izz="0.00533333333333"/>
-    </inertial>
-    <collision>
-      <geometry>
-        <box size="0.2 0.2 0.2"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="rx78_Null_090_joint" type="revolute">
-    <origin xyz="2.99e-05 -5e-07 -0.716289" rpy="3.0000001061e-06 -0.0 1.5707763268"/>
-    <axis xyz="-1 0 0"/>
-    <parent link="rx78_Null_089_link"/>
+  <link name="rx78_Null_090_link"/>
+  <joint name="rx78_Null_090_joint" type="fixed">
+    <origin xyz="1.44559 0 0" rpy="0 0 0"/>
+    <parent link="rx78_Null_091_link"/>
     <child link="rx78_Null_090_link"/>
-    <limit effort="1000000000" lower="-1.57079632679" upper="1.57079632679" velocity="1000000"/>
-    <mimic joint="rleg_ankle_r"/>
   </joint>
   <link name="rx78_object_089_link">
     <inertial>
-      <origin xyz="0.854892450571 -4.76241e-05 -0.642670845985"/>
-      <mass value="1650.26392015"/>
+      <origin xyz="0.304892 -4.76241e-05 -0.292671"/>
+      <mass value="330.052784029"/>
       <inertia ixx="179.173487591" ixy="0.0" ixz="0.0" iyy="96.5170220208" iyz="0.0" izz="239.960124507"/>
     </inertial>
     <visual>
@@ -2655,7 +2642,7 @@
   </joint>
   <link name="rx78_object_091_link">
     <inertial>
-      <origin xyz="-0.916370356083 -1.97291e-05 -0.457724845409"/>
+      <origin xyz="-0.51637 -1.97291e-05 0.142275"/>
       <mass value="1382.91066929"/>
       <inertia ixx="998.128333614" ixy="0.0" ixz="0.0" iyy="1664.97828092" iyz="0.0" izz="2016.61569891"/>
     </inertial>
@@ -3167,29 +3154,16 @@
     <child link="rx78_Null_039_link"/>
     <limit effort="1000000000" lower="-0.785398163397" upper="0.785398163397" velocity="1000000"/>
   </joint>
-  <link name="rx78_Null_040_link">
-    <inertial>
-      <mass value="0.8"/>
-      <inertia ixx="0.00533333333333" ixy="0.0" ixz="0.0" iyy="0.00533333333333" iyz="0.0" izz="0.00533333333333"/>
-    </inertial>
-    <collision>
-      <geometry>
-        <box size="0.2 0.2 0.2"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="rx78_Null_040_joint" type="revolute">
-    <origin xyz="1.9e-06 -6e-07 -0.716289" rpy="0.0 -0.0 1.57079632679"/>
-    <axis xyz="-1 0 0"/>
-    <parent link="rx78_Null_039_link"/>
+  <link name="rx78_Null_040_link"/>
+  <joint name="rx78_Null_040_joint" type="fixed">
+    <origin xyz="1.44559 0 0" rpy="0 0 0"/>
+    <parent link="rx78_Null_041_link"/>
     <child link="rx78_Null_040_link"/>
-    <limit effort="1000000000" lower="-1.57079632679" upper="1.57079632679" velocity="1000000"/>
-    <mimic joint="lleg_ankle_r"/>
   </joint>
   <link name="rx78_object_032_link">
     <inertial>
-      <origin xyz="0.854906725883 7.15256e-07 -0.642668104172"/>
-      <mass value="1650.24956648"/>
+      <origin xyz="0.304907 7.15256e-07 -0.292668"/>
+      <mass value="330.049913295"/>
       <inertia ixx="179.171837371" ixy="0.0" ixz="0.0" iyy="96.5149497242" iyz="0.0" izz="239.95677834"/>
     </inertial>
     <visual>
@@ -3252,7 +3226,7 @@
   </joint>
   <link name="rx78_object_034_link">
     <inertial>
-      <origin xyz="-0.916372740269 0.0 -0.457724368572"/>
+      <origin xyz="-0.516373 0.0 0.142276"/>
       <mass value="1382.89789608"/>
       <inertia ixx="998.116476765" ixy="0.0" ixz="0.0" iyy="1664.94531795" iyz="0.0" izz="2016.58058707"/>
     </inertial>
@@ -4766,29 +4740,6 @@
     <mu1>9000</mu1>
     <mu2>1.5</mu2>
   </gazebo>
-  <gazebo reference="rx78_Null_090_link">
-    <selfCollide>false</selfCollide>
-    <mu1>9000</mu1>
-    <mu2>1.5</mu2>
-  </gazebo>
-  <gazebo reference="rx78_Null_090_joint">
-    <provideFeedback>1</provideFeedback>
-    <implicitSpringDamper>1</implicitSpringDamper>
-  </gazebo>
-  <transmission name="rx78_Null_090_joint_trans">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="rx78_Null_090_joint">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="rx78_Null_090_joint_motor">
-      <mechanicalReduction>1.0</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <gazebo reference="rx78_Null_089_link">
-    <selfCollide>false</selfCollide>
-    <mu1>9000</mu1>
-    <mu2>1.5</mu2>
-  </gazebo>
   <gazebo reference="rx78_Null_091_link">
     <selfCollide>false</selfCollide>
     <mu1>9000</mu1>
@@ -5065,29 +5016,6 @@
     <mu1>9000</mu1>
     <mu2>1.5</mu2>
   </gazebo>
-  <gazebo reference="rx78_Null_040_link">
-    <selfCollide>false</selfCollide>
-    <mu1>9000</mu1>
-    <mu2>1.5</mu2>
-  </gazebo>
-  <gazebo reference="rx78_Null_040_joint">
-    <provideFeedback>1</provideFeedback>
-    <implicitSpringDamper>1</implicitSpringDamper>
-  </gazebo>
-  <transmission name="rx78_Null_040_joint_trans">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="rx78_Null_040_joint">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="rx78_Null_040_joint_motor">
-      <mechanicalReduction>1.0</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <gazebo reference="rx78_Null_039_link">
-    <selfCollide>false</selfCollide>
-    <mu1>9000</mu1>
-    <mu2>1.5</mu2>
-  </gazebo>
   <gazebo reference="rx78_Null_041_link">
     <selfCollide>false</selfCollide>
     <mu1>9000</mu1>