diff --git a/lib/io/iob.h b/lib/io/iob.h
index 0321beb7cab..b03da4b5022 100644
--- a/lib/io/iob.h
+++ b/lib/io/iob.h
@@ -175,6 +175,24 @@ extern "C"{
      */
     int write_angle_offset(int id, double offset);
 
+    /**
+     * @brief read lower joint limit[rad]
+     * @param id	joint id
+     * @param angle	lower joint limit[rad]
+     * @retval          TRUE this function is supported
+     * @retval          FALSE otherwise
+     */
+    int read_llimit_angle(int id, double *angle);
+
+    /**
+     * @brief read upper joint limit[rad]
+     * @param id	joint id
+     * @param angle	upper joint limit[rad]
+     * @retval          TRUE this function is supported
+     * @retval          FALSE otherwise
+     */
+    int read_ulimit_angle(int id, double *angle);
+    
     //@}
 
     // @name joint
diff --git a/rtc/RobotHardware/RobotHardware.cpp b/rtc/RobotHardware/RobotHardware.cpp
index 51abd91a656..e0247c1adde 100644
--- a/rtc/RobotHardware/RobotHardware.cpp
+++ b/rtc/RobotHardware/RobotHardware.cpp
@@ -51,6 +51,7 @@ RobotHardware::RobotHardware(RTC::Manager* manager)
     m_dqRefIn("dqRef", m_dqRef),
     m_ddqRefIn("ddqRef", m_ddqRef),
     m_tauRefIn("tauRef", m_tauRef),
+    m_torqueControlModeIn("torqueControlMode", m_torqueControlMode),
     m_qOut("q", m_q),
     m_dqOut("dq", m_dq),
     m_tauOut("tau", m_tau),
@@ -61,7 +62,7 @@ RobotHardware::RobotHardware(RTC::Manager* manager)
     m_rstate2Out("rstate2", m_rstate2),
     m_RobotHardwareServicePort("RobotHardwareService"),
     // </rtc-template>
-	dummy(0)
+    dummy(0)
 {
 }
 
@@ -79,6 +80,7 @@ RTC::ReturnCode_t RobotHardware::onInitialize()
   addInPort("dqRef", m_dqRefIn);
   addInPort("ddqRef", m_ddqRefIn);
   addInPort("tauRef", m_tauRefIn);
+  addInPort("torqueControlMode", m_torqueControlModeIn);
 
   addOutPort("q", m_qOut);
   addOutPort("dq", m_dqOut);
@@ -145,6 +147,7 @@ RTC::ReturnCode_t RobotHardware::onInitialize()
   m_dqRef.data.length(m_robot->numJoints());
   m_ddqRef.data.length(m_robot->numJoints());
   m_tauRef.data.length(m_robot->numJoints());
+  m_torqueControlMode.data.length(m_robot->numJoints());
 
   int ngyro = m_robot->numSensors(Sensor::RATE_GYRO);
   std::cout << "the number of gyros = " << ngyro << std::endl;
@@ -248,7 +251,7 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id)
               m_emergencySignalOut.write();
           }
       }
-  }    
+  }
 
   if (m_qRefIn.isNew()){
       m_qRefIn.read();
@@ -281,6 +284,12 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id)
       // output to iob
       m_robot->writeTorqueCommands(m_tauRef.data.get_buffer());
   }
+  if (m_torqueControlModeIn.isNew()){
+      m_torqueControlModeIn.read();
+      for (unsigned int i=0; i<m_robot->numJoints(); ++i){
+          m_robot->setJointControlMode(m_robot->joint(i)->name.c_str(), m_torqueControlMode.data[i] ? JCM_TORQUE : JCM_POSITION);
+      }
+  }
 
   // read from iob
   m_robot->readJointAngles(m_q.data.get_buffer());  
diff --git a/rtc/RobotHardware/RobotHardware.h b/rtc/RobotHardware/RobotHardware.h
index 1450cfd07c8..65e5457f12d 100644
--- a/rtc/RobotHardware/RobotHardware.h
+++ b/rtc/RobotHardware/RobotHardware.h
@@ -23,6 +23,7 @@
 #include <rtm/idl/ExtendedDataTypesSkel.h>
 
 #include <hrpModel/Body.h>
+#include <hrpModel/Link.h>
 
 // Service implementation headers
 // <rtc-template block="service_impl_h">
@@ -140,7 +141,12 @@ class RobotHardware
   */
   TimedDoubleSeq m_tauRef;
   InPort<TimedDoubleSeq> m_tauRefIn;
-  
+  /**
+     \brief array of booleans indicating which joint is in torque mode (1) or position mode (0)
+  */
+  TimedBooleanSeq m_torqueControlMode;
+  InPort<TimedBooleanSeq> m_torqueControlModeIn;
+    
   // </rtc-template>
 
   /**
@@ -216,7 +222,7 @@ class RobotHardware
   robot *robot_ptr(void) { return m_robot.get(); };
  private:
   void getStatus2(OpenHRP::RobotHardwareService::RobotState2 &rstate2);
-
+  
   int dummy;
   boost::shared_ptr<robot> m_robot;
 };
diff --git a/rtc/RobotHardware/robot.cpp b/rtc/RobotHardware/robot.cpp
index b79e2c49678..49d2b95fdd0 100644
--- a/rtc/RobotHardware/robot.cpp
+++ b/rtc/RobotHardware/robot.cpp
@@ -665,9 +665,11 @@ bool robot::checkJointCommands(const double *i_commands)
 bool robot::checkEmergency(emg_reason &o_reason, int &o_id)
 {
     int state;
+    joint_control_mode mode;
     for (unsigned int i=0; i<numJoints(); i++){
         read_servo_state(i, &state);
-        if (state == ON && m_servoErrorLimit[i] != 0){
+        read_control_mode(i, &mode);
+        if (state == ON && m_servoErrorLimit[i] != 0 && mode != JCM_TORQUE){
             double angle, command;
             read_actual_angle(i, &angle);
             read_command_angle(i, &command);