diff --git a/hrpsys_ros_bridge/CMakeLists.txt b/hrpsys_ros_bridge/CMakeLists.txt
index 66848dd00..06c842ac1 100644
--- a/hrpsys_ros_bridge/CMakeLists.txt
+++ b/hrpsys_ros_bridge/CMakeLists.txt
@@ -102,11 +102,11 @@ rtmbuild_genbridge()
 # pr2_controller_msgs is not catkinized
 string(RANDOM _random_string)
 
-rtmbuild_add_executable(HrpsysSeqStateROSBridge src/HrpsysSeqStateROSBridgeImpl.cpp src/HrpsysSeqStateROSBridge.cpp src/HrpsysSeqStateROSBridgeComp.cpp)
+rtmbuild_add_executable(HrpsysSeqStateROSBridge src/HrpsysSeqStateROSBridgeImpl.cpp src/HrpsysSeqStateROSBridge.cpp src/HrpsysSeqStateROSBridgeComp.cpp src/HrpsysROSBridgeUtil.cpp)
 rtmbuild_add_executable(ImageSensorROSBridge src/ImageSensorROSBridge.cpp src/ImageSensorROSBridgeComp.cpp)
 rtmbuild_add_executable(RangeSensorROSBridge src/RangeSensorROSBridge.cpp src/RangeSensorROSBridgeComp.cpp)
 rtmbuild_add_executable(PointCloudROSBridge src/PointCloudROSBridge.cpp src/PointCloudROSBridgeComp.cpp)
-rtmbuild_add_executable(HrpsysJointTrajectoryBridge src/HrpsysJointTrajectoryBridge.cpp src/HrpsysJointTrajectoryBridgeComp.cpp)
+rtmbuild_add_executable(HrpsysJointTrajectoryBridge src/HrpsysJointTrajectoryBridge.cpp src/HrpsysJointTrajectoryBridgeComp.cpp src/HrpsysROSBridgeUtil.cpp)
 
 install(DIRECTORY launch euslisp srv idl scripts models test cmake
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
diff --git a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch
index 69e25c91f..6963b96ac 100644
--- a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch
+++ b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch
@@ -30,7 +30,7 @@
   <arg name="USE_REFERENCEFORCEUPDATER" default="false" />
   <arg name="USE_OBJECTCONTACTTURNAROUNDDETECTOR" default="false" />
   <arg name="USE_HRPSYS_PROFILE" default="true" />
-  <arg name="USE_VELOCITY_OUTPUT" default="false" />
+  <arg name="USE_VELOCITY_OUTPUT" default="false" doc="true: output actual velocity / false: output differential value of actual postition as /joint_states/velocity"/>
   <arg name="PUBLISH_SENSOR_TF" default="true" />
   <!--
       roslaunch hrpsys_ros_bridge sample.launch nameserver:=192.168.1.1
diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
index 336917762..1f28aa8c4 100644
--- a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
+++ b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
@@ -41,6 +41,9 @@
 #include "hrpsys_ros_bridge/idl/ExecutionProfileService.hh"
 #include "hrpsys_ros_bridge/idl/RobotHardwareService.hh"
 
+// Version
+#include "HrpsysROSBridgeUtil.h"
+
 // Module specification
 // <rtc-template block="module_spec">
 static const char* hrpsysjointtrajectorybridge_spec[] = {"implementation_id", "HrpsysJointTrajectoryBridge",
@@ -186,6 +189,8 @@ RTC::ReturnCode_t HrpsysJointTrajectoryBridge::onActivated(RTC::UniqueId ec_id)
     ROS_WARN_STREAM("param: " << config_name << ", param does not exist.");
   }
 
+  hrpsys_version = GetHrpsysVersion(m_SequencePlayerServicePort);
+
   return RTC::RTC_OK;
 }
 
@@ -330,6 +335,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::proc()
 
   // FIXME: need to set actual informatoin, currently we set dummy information
   trajectory_msgs::JointTrajectoryPoint commanded_joint_trajectory_point, error_joint_trajectory_point;
+  commanded_joint_trajectory_point.time_from_start = tm_on_execute - traj_start_tm;
   commanded_joint_trajectory_point.positions.resize(joint_list.size());
   commanded_joint_trajectory_point.velocities.resize(joint_list.size());
   commanded_joint_trajectory_point.accelerations.resize(joint_list.size());
@@ -341,6 +347,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::proc()
       commanded_joint_trajectory_point.accelerations[j] = parent->body->link(joint_list[j])->ddq;
       commanded_joint_trajectory_point.effort[j]        = parent->body->link(joint_list[j])->u;
     }
+  error_joint_trajectory_point.time_from_start = tm_on_execute - traj_start_tm;
   error_joint_trajectory_point.positions.resize(joint_list.size());
   error_joint_trajectory_point.velocities.resize(joint_list.size());
   error_joint_trajectory_point.accelerations.resize(joint_list.size());
@@ -462,8 +469,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory(
       ss << " " << point.positions[j];
     }
     ROS_INFO_STREAM(
-        "[" << parent->getInstanceName() << "] @onJointTrajectoryAction (" << this->groupname << ") : time_from_start " << trajectory.points[i].time_from_start.toSec());
-    ROS_INFO_STREAM("[" << parent->getInstanceName() << "] " << ss.str());
+        "[" << parent->getInstanceName() << "] i:" << i << " : time_from_start " << trajectory.points[i].time_from_start.toSec());
 
     if (i > 0)
     {
@@ -474,6 +480,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory(
       duration[i] = trajectory.points[i].time_from_start.toSec();
       if ( std::abs(duration[i]) < 0.001 ) duration[i] = 0.001; // set magic delta ... https://github.com/start-jsk/rtmros_common/issues/1036
     }
+    ROS_INFO_STREAM("[" << parent->getInstanceName() << "] [" << ss.str() << "] " << duration[i]);
   }
 
   parent->m_mutex.unlock();
@@ -486,6 +493,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory(
     }
     else
     { // fullbody
+      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] setJointAngles");
       parent->m_service0->setJointAngles(angles[0], duration[0]);
     }
   }
@@ -493,15 +501,30 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory(
   {
     if (groupname.length() > 0)
     { // group
-      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] playPatternGroup: " << groupname);
-      parent->m_service0->playPatternOfGroup(groupname.c_str(), angles, duration);
+      // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+      // hrpsys < 315.16.0 have bugs in setJointAnglesSequenceOfGroup https://github.com/fkanehiro/hrpsys-base/pull/1237
+      if (LessThanVersion(parent->hrpsys_version, std::string("315.16.0"))) {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] playPatternOfGroup: " << groupname);
+        parent->m_service0->playPatternOfGroup(groupname.c_str(), angles, duration);
+      } else {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] setJointAnglesSequenceOfGroup: " << groupname);
+        parent->m_service0->setJointAnglesSequenceOfGroup(groupname.c_str(), angles, duration);
+      }
     }
     else
-    {
-      OpenHRP::dSequenceSequence rpy, zmp;
-      parent->m_service0->playPattern(angles, rpy, zmp, duration);
+    { // fullbody
+      // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+      if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] playPattern");
+        OpenHRP::dSequenceSequence rpy, zmp;
+        parent->m_service0->playPattern(angles, rpy, zmp, duration);
+      } else {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] setJointAnglesSequence");
+        parent->m_service0->setJointAnglesSequence(angles, duration);
+      }
     }
   }
+  traj_start_tm = ros::Time::now();
 
   interpolationp = true;
 }
@@ -527,6 +550,34 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectoryAct
 {
   ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @onJointTrajectoryActionPreempt()");
   joint_trajectory_server->setPreempted();
+  if (!joint_trajectory_server->isNewGoalAvailable())
+  { // Cancel request comes from client, so motion should be stopped immediately,
+    // while motion should be changed smoothly when new goal comes
+    if (groupname.length() > 0)
+    { // group
+      // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+      // hrpsys < 315.16.0 have bugs in setJointAnglesSequenceOfGroup https://github.com/fkanehiro/hrpsys-base/pull/1237
+      if (LessThanVersion(parent->hrpsys_version, std::string("315.16.0"))) {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearOfGroup: " << groupname);
+        parent->m_service0->clearOfGroup(groupname.c_str(), 0.05);
+      } else {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAnglesOfGroup: " << groupname);
+        parent->m_service0->clearJointAnglesOfGroup(groupname.c_str());
+      }
+    }
+    else
+    { // fullbody
+      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles ");
+      // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+      if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clear");
+        parent->m_service0->clear();
+      } else {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles");
+        parent->m_service0->clearJointAngles();
+      }
+    }
+  }
 }
 #endif
 
@@ -534,6 +585,32 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onFollowJointTraject
 {
   ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @onFollowJointTrajectoryActionPreempt()");
   follow_joint_trajectory_server->setPreempted();
+
+  if (!follow_joint_trajectory_server->isNewGoalAvailable())
+  { // Cancel request comes from client, so motion should be stopped immediately,
+    // while motion should be changed smoothly when new goal comes
+    if (groupname.length() > 0)
+    { // group
+      // hrpsys < 315.16.0 have bugs in setJointAnglesSequenceOfGroup https://github.com/fkanehiro/hrpsys-base/pull/1237
+      if (LessThanVersion(parent->hrpsys_version, std::string("315.16.0"))) {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearOfGroup: " << groupname);
+        parent->m_service0->clearOfGroup(groupname.c_str(), 0.05);
+      } else {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAnglesOfGroup: " << groupname);
+        parent->m_service0->clearJointAnglesOfGroup(groupname.c_str());
+      }
+    }
+    else
+    { // fullbody
+      if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clear");
+        parent->m_service0->clear();
+      } else {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles");
+        parent->m_service0->clearJointAngles();
+      }
+    }
+  }
 }
 
 void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onTrajectoryCommandCB(const trajectory_msgs::JointTrajectoryConstPtr& msg)
diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h
index 4a4e3bd1a..d261d2bf6 100644
--- a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h
+++ b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h
@@ -102,6 +102,7 @@ class HrpsysJointTrajectoryBridge : public RTC::DataFlowComponentBase
     std::string groupname;
     std::vector<std::string> joint_list;
     bool interpolationp;
+    ros::Time traj_start_tm;
 
   public:
     typedef boost::shared_ptr<jointTrajectoryActionObj> Ptr;
@@ -126,6 +127,8 @@ class HrpsysJointTrajectoryBridge : public RTC::DataFlowComponentBase
   RTC::CorbaPort m_SequencePlayerServicePort;
   RTC::CorbaConsumer<OpenHRP::SequencePlayerService> m_service0;
 
+  std::string hrpsys_version;
+
 protected:
   hrp::BodyPtr body;
   OpenHRP::BodyInfo_var bodyinfo;
diff --git a/hrpsys_ros_bridge/src/HrpsysROSBridgeUtil.cpp b/hrpsys_ros_bridge/src/HrpsysROSBridgeUtil.cpp
new file mode 100644
index 000000000..6e69a8d6e
--- /dev/null
+++ b/hrpsys_ros_bridge/src/HrpsysROSBridgeUtil.cpp
@@ -0,0 +1,70 @@
+// -*- C++ -*-
+/*!
+ * @file  HrpsysROSBridgeUtil.cpp * @brief hrpsys - ros bridge util files * @date  $Date$ 
+ *
+ * $Id$ 
+ */
+
+#include "HrpsysROSBridgeUtil.h"
+#include <ros/console.h>
+
+//
+void Parse(int result[3], const std::string& input)
+{
+  std::istringstream parser(input);
+  parser >> result[0];
+  for(int idx = 1; idx < 3; idx++)
+  {
+    parser.get(); //Skip period
+    parser >> result[idx];
+  }
+}
+
+bool LessThanVersion(const std::string& a,const std::string& b)
+{
+  int parsedA[3], parsedB[3];
+  try {
+    Parse(parsedA, a);
+    Parse(parsedB, b);
+    return std::lexicographical_compare(parsedA, parsedA + 4, parsedB, parsedB + 4);
+  } catch (...) {
+    ROS_ERROR_STREAM("LessThanVersion failed " << a << " < " << b <<", force return false");
+    return false;
+  }
+}
+
+//
+std::string GetHrpsysVersion(RTC::CorbaPort& m_ServicePort) {
+  std::string hrpsys_version;
+  try {
+    // copied from HrpsysSeqStateROSBridgeImpl.cpp
+    RTC::ConnectorProfileList* connector_profile_list = m_ServicePort.get_connector_profiles();
+    for (int i = 0; i < connector_profile_list->length(); i++ ) {
+      RTC::ConnectorProfile& connector_profile = (*connector_profile_list)[i];
+      RTC::PortServiceList ports_list = connector_profile.ports;
+      for (int j = 0; j < ports_list.length(); j++ ) {
+        RTC::PortService_var port = ports_list[j];
+        RTC::PortProfile* profile = port->get_port_profile();
+        if ( profile ) {
+          RTC::RTObject_var owner = profile->owner;
+          if ( owner ) {
+            RTC::ComponentProfile* component_profile = owner->get_component_profile();
+            if ( component_profile && std::string(component_profile->type_name) == std::string("SequencePlayer") ) {
+              ROS_WARN_STREAM(__PRETTY_FUNCTION__ << " Connected to " << component_profile->type_name  << " (" << component_profile->instance_name << ") version " << component_profile->version);
+              hrpsys_version = std::string(component_profile->version);
+            }
+          }
+        }
+      }
+    }
+    if ( hrpsys_version == "" ) {
+      ROS_ERROR_STREAM( __PRETTY_FUNCTION__ << " Could not get valid hrpsys_version");
+    }
+  } catch (...) {
+    ROS_ERROR_STREAM( __PRETTY_FUNCTION__ << " Could not get hrpsys_version");
+  }
+  return hrpsys_version;
+}
+
+
+
diff --git a/hrpsys_ros_bridge/src/HrpsysROSBridgeUtil.h b/hrpsys_ros_bridge/src/HrpsysROSBridgeUtil.h
new file mode 100644
index 000000000..219c09f2b
--- /dev/null
+++ b/hrpsys_ros_bridge/src/HrpsysROSBridgeUtil.h
@@ -0,0 +1,31 @@
+// -*- C++ -*-
+/*!
+ * @file  HrpsysROSBridgeUtil.h * @brief hrpsys - ros bridge util files * @date  $Date$ 
+ *
+ * $Id$ 
+ */
+#ifndef HRPSYSROSBRIDGEUTIL_H
+#define HRPSYSROSBRIDGEUTIL_H
+
+#include <string>
+// http://stackoverflow.com/questions/2941491/compare-versions-as-strings
+bool LessThanVersion(const std::string& a,const std::string& b);
+
+//
+#include <rtm/idl/BasicDataTypeSkel.h>
+#include <rtm/idl/ExtendedDataTypesSkel.h>
+#include <rtm/Manager.h>
+#include <rtm/CorbaNaming.h>
+#include <rtm/DataFlowComponentBase.h>
+#include <rtm/CorbaPort.h>
+
+#include <ros/ros.h>
+#include <ros/console.h>
+
+#include <string>
+#include <iostream>
+
+std::string GetHrpsysVersion(RTC::CorbaPort& m_ServicePort);
+
+#endif // HRPSYSROSBRIDGEUTIL_H
+
diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
index 30bd3b7c7..bebd13460 100644
--- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
+++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
@@ -215,9 +215,15 @@ void HrpsysSeqStateROSBridge::onJointTrajectory(trajectory_msgs::JointTrajectory
   if ( duration.length() == 1 ) {
     m_service0->setJointAngles(angles[0], duration[0]);
   } else {
-    OpenHRP::dSequenceSequence rpy, zmp;
-    m_service0->playPattern(angles, rpy, zmp, duration);
+    // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+    if (LessThanVersion(hrpsys_version, std::string("315.5.0"))) {
+      OpenHRP::dSequenceSequence rpy, zmp;
+      m_service0->playPattern(angles, rpy, zmp, duration);
+    } else {
+      m_service0->setJointAnglesSequence(angles, duration);
+    }
   }
+  traj_start_tm = ros::Time::now();
 
   interpolationp = true;
 }
@@ -238,11 +244,31 @@ void HrpsysSeqStateROSBridge::onFollowJointTrajectoryActionGoal() {
 #ifdef USE_PR2_CONTROLLERS_MSGS
 void HrpsysSeqStateROSBridge::onJointTrajectoryActionPreempt() {
   joint_trajectory_server.setPreempted();
+  if (!joint_trajectory_server.isNewGoalAvailable()) {
+    // Cancel request comes from client, so motion should be stopped immediately,
+    // while motion should be changed smoothly when new goal comes.
+    // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+    if (LessThanVersion(hrpsys_version, std::string("315.5.0"))) {
+      m_service0->clear();
+    } else {
+      m_service0->clearJointAngles();
+    }
+  }
 }
 #endif
 
 void HrpsysSeqStateROSBridge::onFollowJointTrajectoryActionPreempt() {
   follow_joint_trajectory_server.setPreempted();
+  if (!follow_joint_trajectory_server.isNewGoalAvailable()) {
+    // Cancel request comes from client, so motion should be stopped immediately,
+    // while motion should be changed smoothly when new goal comes.
+    // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+    if (LessThanVersion(hrpsys_version, std::string("315.5.0"))) {
+      m_service0->clear();
+    } else {
+      m_service0->clearJointAngles();
+    }
+  }
 }
 
 void HrpsysSeqStateROSBridge::onTrajectoryCommandCB(const trajectory_msgs::JointTrajectoryConstPtr& msg) {
@@ -442,6 +468,9 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id)
         //joint_state.velocity
         //joint_state.effort
         follow_joint_trajectory_feedback.joint_names.push_back(j->name);
+        follow_joint_trajectory_feedback.desired.time_from_start = tm_on_execute - traj_start_tm;
+        follow_joint_trajectory_feedback.actual.time_from_start = tm_on_execute - traj_start_tm;
+        follow_joint_trajectory_feedback.error.time_from_start = tm_on_execute - traj_start_tm;
         follow_joint_trajectory_feedback.desired.positions.push_back(j->q);
         follow_joint_trajectory_feedback.actual.positions.push_back(j->q);
         follow_joint_trajectory_feedback.error.positions.push_back(0);
@@ -463,8 +492,16 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id)
         joint_state.velocity.push_back(m_rsvel.data[i]);
       }
     } else {
-      joint_state.velocity.resize(joint_state.name.size());
+      double time_from_prev = (joint_state.header.stamp - prev_joint_state.header.stamp).toSec();
+      if(time_from_prev > 0 && prev_joint_state.position.size() == joint_state.position.size()) {
+        for (unsigned int i = 0; i < joint_state.position.size(); i++) {
+          joint_state.velocity.push_back((joint_state.position[i] - prev_joint_state.position[i]) / time_from_prev);
+        }
+      } else {
+        joint_state.velocity.resize(joint_state.name.size());
+      }
     }
+    prev_joint_state = joint_state;
     // set effort if m_rstorque is available
     if (m_rstorque.data.length() == body->joints().size()) {
       for ( unsigned int i = 0; i < body->joints().size() ; i++ ){
@@ -574,7 +611,8 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id)
     }
     if ( !follow_joint_trajectory_feedback.joint_names.empty() &&
          !follow_joint_trajectory_feedback.actual.positions.empty() &&
-         follow_action_initialized ) {
+         follow_action_initialized &&
+         follow_joint_trajectory_server.isActive() ) {
       follow_joint_trajectory_server.publishFeedback(follow_joint_trajectory_feedback);
     }
   } // end: m_mcangleIn
diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h
index bf615738c..4068ca1a3 100644
--- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h
+++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h
@@ -75,6 +75,7 @@ class HrpsysSeqStateROSBridge  : public HrpsysSeqStateROSBridgeImpl
 
   coil::Mutex m_mutex;
   coil::TimeMeasure tm;
+  sensor_msgs::JointState prev_joint_state;
 
   std::string nameserver;
   std::string rootlink_name;
@@ -87,6 +88,7 @@ class HrpsysSeqStateROSBridge  : public HrpsysSeqStateROSBridgeImpl
   void clock_cb(const rosgraph_msgs::ClockPtr& str) {};
 
   bool follow_action_initialized;
+  ros::Time traj_start_tm;
 
   boost::mutex tf_mutex;
   double tf_rate;
diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp
index c8d7596de..ac2a92496 100644
--- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp
+++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp
@@ -375,12 +375,14 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridgeImpl::onShutdown(RTC::UniqueId ec_id)
   return RTC::RTC_OK;
 }
 */
-/*
+
 RTC::ReturnCode_t HrpsysSeqStateROSBridgeImpl::onActivated(RTC::UniqueId ec_id)
 {
+  hrpsys_version = GetHrpsysVersion(m_SequencePlayerServicePort);
+
   return RTC::RTC_OK;
 }
-*/
+
 /*
 RTC::ReturnCode_t HrpsysSeqStateROSBridgeImpl::onDeactivated(RTC::UniqueId ec_id)
 {
diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h
index 5971d639b..20c5dfb82 100644
--- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h
+++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h
@@ -36,6 +36,9 @@
 
 // </rtc-template>
 
+// version
+#include "HrpsysROSBridgeUtil.h"
+
 #include "tf/transform_broadcaster.h"
 using namespace RTC;
 
@@ -63,7 +66,7 @@ class HrpsysSeqStateROSBridgeImpl  : public RTC::DataFlowComponentBase
 
   // The activated action (Active state entry action)
   // former rtc_active_entry()
-  // virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
+  virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
 
   // The deactivated action (Active state exit action)
   // former rtc_active_exit()
@@ -199,6 +202,8 @@ class HrpsysSeqStateROSBridgeImpl  : public RTC::DataFlowComponentBase
 
   double dt;
 
+  std::string hrpsys_version;
+
  private:
 };
 
diff --git a/hrpsys_ros_bridge/test/test-pa10.py b/hrpsys_ros_bridge/test/test-pa10.py
index 0652dd292..f2b1dfcac 100755
--- a/hrpsys_ros_bridge/test/test-pa10.py
+++ b/hrpsys_ros_bridge/test/test-pa10.py
@@ -10,6 +10,7 @@
     import roslib; roslib.load_manifest(PKG)
 
 import argparse,unittest,rostest, time, sys, math, os
+from copy import deepcopy
 from numpy import *
 
 import rospy,rospkg, tf
@@ -18,12 +19,14 @@
 
 import actionlib
 
+from sensor_msgs.msg import JointState
 from trajectory_msgs.msg import JointTrajectoryPoint, JointTrajectory
 
 class TestPA10Robot(unittest.TestCase):
     listener = None
     lfsensor = None
     rfsensor = None
+    feedback = None
 
     def send_joint_angles(self,angles):
         pub = rospy.Publisher("/fullbody_controller/command", JointTrajectory)
@@ -68,6 +71,9 @@ def test_follow_joint_trajectory_action(self):
         larm = actionlib.SimpleActionClient("/fullbody_controller/follow_joint_trajectory_action", FollowJointTrajectoryAction)
         self.impl_test_joint_trajectory_action(larm, FollowJointTrajectoryGoal())
 
+    def feedback_cb(self, msg):
+        self.feedback = msg
+
     def impl_test_joint_trajectory_action(self, larm, goal):
         larm.wait_for_server()
 
@@ -90,12 +96,31 @@ def impl_test_joint_trajectory_action(self, larm, goal):
         point.positions = [ x * math.pi / 180.0 for x in [10,20,30,40,50,60,70] ]
         point.time_from_start = rospy.Duration(5.0)
         goal.trajectory.points.append(point)
-        larm.send_goal(goal)
+        larm.send_goal(goal, feedback_cb = self.feedback_cb)
+        rospy.sleep(2.5)
+        mid_feedback = deepcopy(self.feedback)
         larm.wait_for_result()
         (trans2,rot2) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0))
         rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans2,rot2))
         rospy.logwarn("difference between two /J7_LINK %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
         self.assertNotAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
+        # time_from_start in feedback should have a meaning
+        # https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/233#issuecomment-403416846
+        # JointTrajectoryAction in pr2_controllers_msgs has empty feedback
+        if hasattr(mid_feedback, 'desired'):
+            rospy.logwarn("time_from_start of desired of midpoint feedback: %r"%mid_feedback.desired.time_from_start.to_sec())
+            self.assertAlmostEqual(mid_feedback.desired.time_from_start.to_sec(), 2.5, delta=1)
+            rospy.logwarn("time_from_start of actual of midpoint feedback: %r"%mid_feedback.actual.time_from_start.to_sec())
+            self.assertAlmostEqual(mid_feedback.actual.time_from_start.to_sec(), 2.5, delta=1)
+            rospy.logwarn("time_from_start of error of midpoint feedback: %r"%mid_feedback.error.time_from_start.to_sec())
+            self.assertAlmostEqual(mid_feedback.error.time_from_start.to_sec(), 2.5, delta=1)
+        # feedback shouldn't be published after motion finishes
+        # https://github.com/start-jsk/rtmros_common/pull/1049#issuecomment-403780615
+        fb_id1 = id(self.feedback)
+        rospy.sleep(0.5)
+        fb_id2 = id(self.feedback)
+        rospy.logwarn("object id of feedback after motion: %r, 0.5sec after: %r"%(fb_id1, fb_id2))
+        self.assertEqual(fb_id1, fb_id2)
 
     def test_joint_trajectory_command(self):
         try:
@@ -112,6 +137,121 @@ def test_joint_trajectory_command(self):
         rospy.logwarn("difference between two /J7_LINK %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
         self.assertNotAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
 
+    # https://github.com/start-jsk/rtmros_common/pull/765#issuecomment-120208947
+    def test_jta_cancel_goal(self):
+        # for < kinetic
+        if os.environ['ROS_DISTRO'] >= 'kinetic' :
+            return True
+        from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal
+        larm = actionlib.SimpleActionClient("/fullbody_controller/joint_trajectory_action", JointTrajectoryAction)
+        self.impl_test_jta_cancel_goal(larm, JointTrajectoryGoal())
+
+    def test_fjta_cancel_goal(self):
+        # for >= kinetic
+        if os.environ['ROS_DISTRO'] < 'kinetic' :
+            return True
+        from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
+        larm = actionlib.SimpleActionClient("/fullbody_controller/follow_joint_trajectory_action", FollowJointTrajectoryAction)
+        self.impl_test_jta_cancel_goal(larm, FollowJointTrajectoryGoal())
+
+    def impl_test_jta_cancel_goal(self, larm, goal):
+        larm.wait_for_server()
+
+        try:
+            self.listener.waitForTransform('/BASE_LINK', '/J7_LINK', rospy.Time(), rospy.Duration(120))
+        except tf.Exception:
+            self.assertTrue(None, "could not found tf from /BASE_LINK to /J7_LINK")
+        goal.trajectory.header.stamp = rospy.get_rostime()
+        goal.trajectory.joint_names.append("J1")
+        goal.trajectory.joint_names.append("J2")
+        goal.trajectory.joint_names.append("J3")
+        goal.trajectory.joint_names.append("J4")
+        goal.trajectory.joint_names.append("J5")
+        goal.trajectory.joint_names.append("J6")
+        goal.trajectory.joint_names.append("J7")
+
+        point = JointTrajectoryPoint()
+        point.positions = [ x * math.pi / 180.0 for x in [10,20,30,40,50,60,70] ]
+        point.time_from_start = rospy.Duration(5.0)
+        goal.trajectory.points.append(point)
+        larm.send_goal(goal)
+        rospy.sleep(2.5)
+        (trans1,rot1) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0))
+        rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans1,rot1))
+        larm.cancel_goal()
+        rospy.sleep(2.5)
+        (trans2,rot2) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0))
+        rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans2,rot2))
+        larm.wait_for_result()
+        rospy.logwarn("difference between two /J7_LINK %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
+        self.assertAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
+
+    # https://github.com/start-jsk/rtmros_common/pull/765#issuecomment-392741195
+    def test_jta_overwrite_goal(self):
+        # for < kinetic
+        if os.environ['ROS_DISTRO'] >= 'kinetic' :
+            return True
+        from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal
+        larm = actionlib.SimpleActionClient("/fullbody_controller/joint_trajectory_action", JointTrajectoryAction)
+        self.impl_test_jta_overwrite_goal(larm, JointTrajectoryGoal())
+
+    def test_fjta_overwrite_goal(self):
+        # for >= kinetic
+        if os.environ['ROS_DISTRO'] < 'kinetic' :
+            return True
+        from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
+        larm = actionlib.SimpleActionClient("/fullbody_controller/follow_joint_trajectory_action", FollowJointTrajectoryAction)
+        self.impl_test_jta_overwrite_goal(larm, FollowJointTrajectoryGoal())
+
+    def impl_test_jta_overwrite_goal(self, larm, goal):
+        larm.wait_for_server()
+
+        try:
+            self.listener.waitForTransform('/BASE_LINK', '/J7_LINK', rospy.Time(), rospy.Duration(120))
+        except tf.Exception:
+            self.assertTrue(None, "could not found tf from /BASE_LINK to /J7_LINK")
+        (trans1,rot1) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0))
+        rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans1,rot1))
+        goal.trajectory.header.stamp = rospy.get_rostime()
+        goal.trajectory.joint_names.append("J1")
+        goal.trajectory.joint_names.append("J2")
+        goal.trajectory.joint_names.append("J3")
+        goal.trajectory.joint_names.append("J4")
+        goal.trajectory.joint_names.append("J5")
+        goal.trajectory.joint_names.append("J6")
+        goal.trajectory.joint_names.append("J7")
+
+        point = JointTrajectoryPoint()
+        point.positions = [ x * math.pi / 180.0 for x in [10,20,30,40,50,60,70] ]
+        point.time_from_start = rospy.Duration(5.0)
+        goal.trajectory.points.append(point)
+        # send first goal
+        larm.send_goal(goal)
+        rospy.sleep(2.5)
+        goal.trajectory.header.stamp = rospy.get_rostime()
+        point.positions = [0,0,0,0,0,0,0]
+        point.time_from_start = rospy.Duration(2.5)
+        del goal.trajectory.points[:]
+        goal.trajectory.points.append(point)
+        # send second goal
+        larm.send_goal(goal)
+        jt_pos1 = rospy.wait_for_message('/joint_states', JointState)
+        rospy.logwarn("joint position just after sending second goal: %r"%(array(jt_pos1.position)))
+        rospy.sleep(0.5)
+        jt_pos2 = rospy.wait_for_message('/joint_states', JointState)
+        rospy.logwarn("joint position 0.5 sec after sending second goal: %r"%(array(jt_pos2.position)))
+        rospy.sleep(2)
+        (trans2,rot2) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0))
+        rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans2,rot2))
+        larm.wait_for_result()
+        rospy.logwarn("difference between two joint positions %r"%(array(jt_pos2.position)-array(jt_pos1.position)))
+        # if robot suddenly stops when goal is overwritten, joint position immediately starts to decrease (heading for new goal (zero)).
+        # joint_states includes unchanged hand joints, so using assertGreaterEqual instead of assertGreater is necessary.
+        for x in array(jt_pos2.position)-array(jt_pos1.position):
+            self.assertGreaterEqual(x, 0)
+        rospy.logwarn("difference between two /J7_LINK %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
+        self.assertAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
+
 # unittest.main()
 if __name__ == '__main__':
     import rostest
diff --git a/hrpsys_ros_bridge/test/test-samplerobot.py b/hrpsys_ros_bridge/test/test-samplerobot.py
index 61fd1a162..a2591d763 100755
--- a/hrpsys_ros_bridge/test/test-samplerobot.py
+++ b/hrpsys_ros_bridge/test/test-samplerobot.py
@@ -4,6 +4,7 @@
 NAME = 'test_samplerobot'
 
 import argparse,unittest,rostest, time, sys, math, os
+from copy import deepcopy
 from numpy import *
 
 import rospy,rospkg, tf
@@ -22,6 +23,7 @@ class TestSampleRobot(unittest.TestCase):
     rfsensor = None
     odom = None
     joint_states = None
+    feedback = None
 
     def lfsensor_cb(self, sensor):
         self.lfsensor = sensor
@@ -90,6 +92,9 @@ def test_load_pattern(self):
             self.assertAlmostEqual(t2-t1, 11, delta=2)
             #self.assertNotAlmostEqual(trans[1],0,2)
 
+    def feedback_cb(self, msg):
+        self.feedback = msg
+
     def impl_test_joint_angles(self, larm, goal):
         larm.wait_for_server()
 
@@ -111,12 +116,31 @@ def impl_test_joint_angles(self, larm, goal):
         point.positions = [ x * math.pi / 180.0 for x in [30,30,30,-90,-40,-30] ]
         point.time_from_start = rospy.Duration(5.0)
         goal.trajectory.points.append(point)
-        larm.send_goal(goal)
+        larm.send_goal(goal, feedback_cb = self.feedback_cb)
+        rospy.sleep(2.5)
+        mid_feedback = deepcopy(self.feedback)
         larm.wait_for_result()
         (trans2,rot2) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
         rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans2,rot2))
         rospy.logwarn("difference between two /LARM_LINK7 %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
         self.assertNotAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
+        # time_from_start in feedback should have a meaning
+        # https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/233#issuecomment-403416846
+        # JointTrajectoryAction in pr2_controllers_msgs has empty feedback
+        if hasattr(mid_feedback, 'desired'):
+            rospy.logwarn("time_from_start of desired of midpoint feedback: %r"%mid_feedback.desired.time_from_start.to_sec())
+            self.assertAlmostEqual(mid_feedback.desired.time_from_start.to_sec(), 2.5, delta=1)
+            rospy.logwarn("time_from_start of actual of midpoint feedback: %r"%mid_feedback.actual.time_from_start.to_sec())
+            self.assertAlmostEqual(mid_feedback.actual.time_from_start.to_sec(), 2.5, delta=1)
+            rospy.logwarn("time_from_start of error of midpoint feedback: %r"%mid_feedback.error.time_from_start.to_sec())
+            self.assertAlmostEqual(mid_feedback.error.time_from_start.to_sec(), 2.5, delta=1)
+        # feedback shouldn't be published after motion finishes
+        # https://github.com/start-jsk/rtmros_common/pull/1049#issuecomment-403780615
+        fb_id1 = id(self.feedback)
+        rospy.sleep(0.5)
+        fb_id2 = id(self.feedback)
+        rospy.logwarn("object id of feedback after motion: %r, 0.5sec after: %r"%(fb_id1, fb_id2))
+        self.assertEqual(fb_id1, fb_id2)
 
         ## move less than 1.0 sec
         goal.trajectory.header.stamp = rospy.get_rostime()
@@ -224,6 +248,135 @@ def test_joint_angles_duration_0(self): # https://github.com/start-jsk/rtmros_co
         rospy.logwarn("difference between two angles %r %r"%(array([30,30,30,-90,-40,-30])-array(goal_angles),linalg.norm(array([30,30,30,-90,-40,-30])-array(goal_angles))))
         self.assertAlmostEqual(linalg.norm(array([30,30,30,-90,-40,-30])-array(goal_angles)), 0, delta=0.1)
 
+    # https://github.com/start-jsk/rtmros_common/pull/765#issuecomment-120208947
+    def test_jta_cancel_goal(self):
+        # for < kinetic
+        if os.environ['ROS_DISTRO'] >= 'kinetic' :
+            return True
+        from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal
+        larm = actionlib.SimpleActionClient("/larm_controller/joint_trajectory_action", JointTrajectoryAction)
+        self.impl_test_jta_cancel_goal(larm, JointTrajectoryGoal())
+
+    def test_fjta_cancel_goal(self):
+        # for >= kinetic
+        if os.environ['ROS_DISTRO'] < 'kinetic' :
+            return True
+        from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
+        larm = actionlib.SimpleActionClient("/larm_controller/follow_joint_trajectory_action", FollowJointTrajectoryAction)
+        self.impl_test_jta_cancel_goal(larm, FollowJointTrajectoryGoal())
+
+    def impl_test_jta_cancel_goal(self, larm, goal):
+        larm.wait_for_server()
+
+        # initialize
+        goal.trajectory.header.stamp = rospy.get_rostime()
+        goal.trajectory.joint_names.append("LARM_SHOULDER_P")
+        goal.trajectory.joint_names.append("LARM_SHOULDER_R")
+        goal.trajectory.joint_names.append("LARM_SHOULDER_Y")
+        goal.trajectory.joint_names.append("LARM_ELBOW")
+        goal.trajectory.joint_names.append("LARM_WRIST_Y")
+        goal.trajectory.joint_names.append("LARM_WRIST_P")
+        point = JointTrajectoryPoint()
+        point.positions = [0,0,0,0,0,0]
+        point.time_from_start = rospy.Duration(5.0)
+        goal.trajectory.points.append(point)
+        larm.send_goal(goal)
+        larm.wait_for_result()
+
+        try:
+            self.listener.waitForTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(), rospy.Duration(120))
+        except tf.Exception:
+            self.assertTrue(None, "could not found tf from /WAIST_LINK0 to /LARM_LINK7")
+        goal.trajectory.header.stamp = rospy.get_rostime()
+        point.positions = [ x * math.pi / 180.0 for x in [20,20,20,20,20,20] ]
+        point.time_from_start = rospy.Duration(5.0)
+        del goal.trajectory.points[:]
+        goal.trajectory.points.append(point)
+        larm.send_goal(goal)
+        rospy.sleep(2.5)
+        (trans1,rot1) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
+        rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans1,rot1))
+        larm.cancel_goal()
+        rospy.sleep(2.5)
+        (trans2,rot2) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
+        rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans2,rot2))
+        larm.wait_for_result()
+        rospy.logwarn("difference between two /LARM_LINK7 %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
+        self.assertAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
+
+    # https://github.com/start-jsk/rtmros_common/pull/765#issuecomment-392741195
+    def test_jta_overwrite_goal(self):
+        # for < kinetic
+        if os.environ['ROS_DISTRO'] >= 'kinetic' :
+            return True
+        from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal
+        larm = actionlib.SimpleActionClient("/larm_controller/joint_trajectory_action", JointTrajectoryAction)
+        self.impl_test_jta_overwrite_goal(larm, JointTrajectoryGoal())
+
+    def test_fjta_overwrite_goal(self):
+        # for >= kinetic
+        if os.environ['ROS_DISTRO'] < 'kinetic' :
+            return True
+        from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
+        larm = actionlib.SimpleActionClient("/larm_controller/follow_joint_trajectory_action", FollowJointTrajectoryAction)
+        self.impl_test_jta_overwrite_goal(larm, FollowJointTrajectoryGoal())
+
+    def impl_test_jta_overwrite_goal(self, larm, goal):
+        larm.wait_for_server()
+
+        # initialize
+        goal.trajectory.header.stamp = rospy.get_rostime()
+        goal.trajectory.joint_names.append("LARM_SHOULDER_P")
+        goal.trajectory.joint_names.append("LARM_SHOULDER_R")
+        goal.trajectory.joint_names.append("LARM_SHOULDER_Y")
+        goal.trajectory.joint_names.append("LARM_ELBOW")
+        goal.trajectory.joint_names.append("LARM_WRIST_Y")
+        goal.trajectory.joint_names.append("LARM_WRIST_P")
+        point = JointTrajectoryPoint()
+        point.positions = [0,0,0,0,0,0]
+        point.time_from_start = rospy.Duration(5.0)
+        goal.trajectory.points.append(point)
+        larm.send_goal(goal)
+        larm.wait_for_result()
+
+        try:
+            self.listener.waitForTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(), rospy.Duration(120))
+        except tf.Exception:
+            self.assertTrue(None, "could not found tf from /WAIST_LINK0 to /LARM_LINK7")
+        (trans1,rot1) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
+        rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans1,rot1))
+        goal.trajectory.header.stamp = rospy.get_rostime()
+        point.positions = [ x * math.pi / 180.0 for x in [20,20,20,20,20,20] ]
+        point.time_from_start = rospy.Duration(5.0)
+        del goal.trajectory.points[:]
+        goal.trajectory.points.append(point)
+        # send first goal
+        larm.send_goal(goal)
+        rospy.sleep(2.5)
+        goal.trajectory.header.stamp = rospy.get_rostime()
+        point.positions = [0,0,0,0,0,0]
+        point.time_from_start = rospy.Duration(2.5)
+        del goal.trajectory.points[:]
+        goal.trajectory.points.append(point)
+        # send second goal
+        larm.send_goal(goal)
+        jt_pos1 = rospy.wait_for_message('/joint_states', JointState)
+        rospy.logwarn("joint position just after sending second goal: %r"%(array(jt_pos1.position)))
+        rospy.sleep(0.5)
+        jt_pos2 = rospy.wait_for_message('/joint_states', JointState)
+        rospy.logwarn("joint position 0.5 sec after sending second goal: %r"%(array(jt_pos2.position)))
+        rospy.sleep(2)
+        (trans2,rot2) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
+        rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans2,rot2))
+        larm.wait_for_result()
+        rospy.logwarn("difference between two joint positions %r"%(array(jt_pos2.position)-array(jt_pos1.position)))
+        # if robot suddenly stops when goal is overwritten, joint position immediately starts to decrease (heading for new goal (zero)).
+        # joint_states includes unchanged hand joints, so using assertGreaterEqual instead of assertGreater is necessary.
+        for x in array(jt_pos2.position)-array(jt_pos1.position):
+            self.assertGreaterEqual(x, 0)
+        rospy.logwarn("difference between two /LARM_LINK7 %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
+        self.assertAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
+
 
 #unittest.main()
 if __name__ == '__main__':