diff --git a/planning/motion_velocity_smoother/CMakeLists.txt b/planning/motion_velocity_smoother/CMakeLists.txt
new file mode 100644
index 0000000000000..27f840cf38c5f
--- /dev/null
+++ b/planning/motion_velocity_smoother/CMakeLists.txt
@@ -0,0 +1,69 @@
+cmake_minimum_required(VERSION 3.5)
+project(motion_velocity_smoother)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+find_package(Boost REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+
+set(MOTION_VELOCITY_SMOOTHER_SRC
+  src/motion_velocity_smoother_node.cpp
+  src/trajectory_utils.cpp
+  src/linear_interpolation.cpp
+  src/resample.cpp
+)
+
+set(SMOOTHER_SRC
+  src/smoother/smoother_base.cpp
+  src/smoother/l2_pseudo_jerk_smoother.cpp
+  src/smoother/linf_pseudo_jerk_smoother.cpp
+  src/smoother/jerk_filtered_smoother.cpp
+  src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp
+  src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp
+)
+
+ament_auto_add_library(smoother SHARED
+  ${SMOOTHER_SRC}
+)
+
+ament_auto_add_library(motion_velocity_smoother_node SHARED
+  ${MOTION_VELOCITY_SMOOTHER_SRC}
+)
+
+target_link_libraries(motion_velocity_smoother_node
+  smoother
+)
+
+rclcpp_components_register_node(motion_velocity_smoother_node
+  PLUGIN "motion_velocity_smoother::MotionVelocitySmootherNode"
+  EXECUTABLE motion_velocity_smoother
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+    config
+)
+
+install(PROGRAMS
+  scripts/trajectory_visualizer.py
+  scripts/closest_velocity_checker.py
+  DESTINATION lib/${PROJECT_NAME}
+)
diff --git a/planning/motion_velocity_smoother/README.ja.md b/planning/motion_velocity_smoother/README.ja.md
new file mode 100644
index 0000000000000..e59b7087d20f3
--- /dev/null
+++ b/planning/motion_velocity_smoother/README.ja.md
@@ -0,0 +1,250 @@
+# Motion Velocity Smoother
+
+## Purpose
+
+`motion_velocity_smoother`は目標軌道上の各点における望ましい車速を計画して出力するモジュールである。
+このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。
+加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールを`motion_velocity_smoother`と呼んでいる。
+
+## Inner-workings / Algorithms
+
+### Flow chart
+
+![motion_velocity_smoother_flow](./media/motion_velocity_smoother_flow.drawio.svg)
+
+#### Extract trajectory
+
+自車後輪軸中心位置に最も近い参照経路上の点に対し、`extract_behind_dist`だけ戻った点から`extract_ahead_dist`だけ進んだ点までの参照経路を抜き出す。
+
+#### Apply external velocity limit
+
+モジュール外部から指定された速度制限を適用する。
+ここで扱う外部の速度制限は`/planning/scenario_planning/max_velocity`の topic で渡されるもので、地図上で設定された速度制限など、参照経路にすでに設定されている制限速度とは別である。
+外部から指定される速度制限は、パラメータで指定されている減速度および躍度の制限の範囲で減速可能な位置から速度制限を適用する。
+
+#### Apply stop approaching velocity
+
+停止点に近づいたときの速度を設定する。障害物近傍まで近づく場合や、正着精度向上などの目的に用いる。
+
+#### Apply lateral acceleration limit
+
+経路の曲率に応じて、指定された最大横加速度`max_lateral_accel`を超えない速度を制限速度として設定する。ただし、制限速度は`min_curve_velocity`を下回らないように設定する。
+
+#### Resample trajectory
+
+指定された時間間隔で経路の点を再サンプルする。ただし、経路全体の長さは`min_trajectory_length`から`max_trajectory_length`の間となるように再サンプルを行い、点の間隔は`min_trajectory_interval_distance`より小さくならないようにする。
+現在車速で`resample_time`の間進む距離までは密にサンプリングし、それ以降は疎にサンプリングする。
+この方法でサンプリングすることで、低速時は密に、高速時は疎にサンプルされるため、停止精度と計算負荷軽減の両立を図っている。
+
+#### Calculate initial state
+
+速度計画のための初期値を計算する。初期値は状況に応じて下表のように計算する。
+
+| 状況                     | 初期速度          | 初期加速度            |
+| ------------------------ | ----------------- | --------------------- |
+| 最初の計算時             | 現在車速          | 0.0                   |
+| 発進時                   | `engage_velocity` | `engage_acceleration` |
+| 現在車速と計画車速が乖離 | 現在車速          | 前回計画値            |
+| 通常時                   | 前回計画値        | 前回計画値            |
+
+#### Smooth velocity
+
+速度の計画を行う。速度計画のアルゴリズムは`JerkFiltered`, `L2`, `Linf`の 3 種類のうちからコンフィグで指定する。
+最適化のソルバは OSQP[1]を利用する。
+
+##### JerkFiltered
+
+速度の 2 乗（最小化で表すため負値で表現）、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、制限ジャーク逸脱量の 2 乗、ジャークの 2 乗の総和を最小化する。
+
+##### L2
+
+速度の 2 乗（最小化で表すため負値で表現）、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、疑似ジャーク[2]の 2 乗の総和を最小化する。
+
+##### Linf
+
+速度の 2 乗（最小化で表すため負値で表現）、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗の総和と疑似ジャーク[2]の絶対最大値の和の最小化する。
+
+#### Post process
+
+計画された軌道の後処理を行う。
+
+- 停止点より先の速度を 0 に設定
+- 速度がパラメータで与えられる`max_velocity`以下となるように設定
+- 自車位置より手前の点における速度を設定
+- Trajectory の仕様に合わせてリサンプリング(`post resampling`)
+- デバッグデータの出力
+
+最適化の計算が終わったあと、次のノードに経路を渡す前に`post resampling`と呼ばれるリサンプリングを行う。ここで再度リサンプリングを行っている理由としては、最適化前で必要な経路間隔と後段のモジュールに渡す経路間隔が必ずしも一致していないからであり、その差を埋めるために再度サンプリングを行っている。そのため、`post resampling`では後段モジュールの経路仕様を確認してパラメータを決める必要がある。なお、最適化アルゴリズムの計算負荷が高く、最初のリサンプリングで経路間隔が後段モジュールの経路仕様より疎になっている場合、`post resampling`で経路を蜜にリサンプリングする。逆に最適化アルゴリズムの計算負荷が小さく、最初のリサンプリングで経路間隔が後段の経路仕様より蜜になっている場合は、`post resampling`で経路をその仕様に合わせて疎にリサンプリングする。
+
+## Inputs / Outputs
+
+### Input
+
+| Name                                       | Type                                     | Description                   |
+| ------------------------------------------ | ---------------------------------------- | ----------------------------- |
+| `~/input/trajectory`                       | `autoware_auto_planning_msgs/Trajectory` | Reference trajectory          |
+| `/planning/scenario_planning/max_velocity` | `std_msgs/Float32`                       | External velocity limit [m/s] |
+| `/localization/kinematic_state`            | `nav_msgs/Odometry`                      | Current odometry              |
+| `/tf`                                      | `tf2_msgs/TFMessage`                     | TF                            |
+| `/tf_static`                               | `tf2_msgs/TFMessage`                     | TF static                     |
+
+### Output
+
+| Name                                               | Type                                     | Description                                                                                               |
+| -------------------------------------------------- | ---------------------------------------- | --------------------------------------------------------------------------------------------------------- |
+| `~/output/trajectory`                              | `autoware_auto_planning_msgs/Trajectory` | Modified trajectory                                                                                       |
+| `/planning/scenario_planning/current_max_velocity` | `std_msgs/Float32`                       | Current external velocity limit [m/s]                                                                     |
+| `~/closest_velocity`                               | `std_msgs/Float32`                       | Planned velocity closest to ego base_link (for debug)                                                     |
+| `~/closest_acceleration`                           | `std_msgs/Float32`                       | Planned acceleration closest to ego base_link (for debug)                                                 |
+| `~/closest_jerk`                                   | `std_msgs/Float32`                       | Planned jerk closest to ego base_link (for debug)                                                         |
+| `~/debug/trajectory_raw`                           | `autoware_auto_planning_msgs/Trajectory` | Extracted trajectory (for debug)                                                                          |
+| `~/debug/trajectory_external_velocity_limited`     | `autoware_auto_planning_msgs/Trajectory` | External velocity limited trajectory (for debug)                                                          |
+| `~/debug/trajectory_lateral_acc_filtered`          | `autoware_auto_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug)                                                |
+| `~/debug/trajectory_time_resampled`                | `autoware_auto_planning_msgs/Trajectory` | Time resampled trajectory (for debug)                                                                     |
+| `~/distance_to_stopline`                           | `std_msgs/Float32`                       | Distance to stop line from current ego pose (max 50 m) (for debug)                                        |
+| `~/stop_speed_exceeded`                            | `std_msgs/Bool`                          | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold |
+
+## Parameters
+
+### Constraint parameters
+
+| Name           | Type     | Description                                    | Default value |
+| :------------- | :------- | :--------------------------------------------- | :------------ |
+| `max_velocity` | `double` | Max velocity limit [m/s]                       | 20.0          |
+| `max_accel`    | `double` | Max acceleration limit [m/ss]                  | 1.0           |
+| `min_decel`    | `double` | Min deceleration limit [m/ss]                  | -0.5          |
+| `stop_decel`   | `double` | Stop deceleration value at a stop point [m/ss] | 0.0           |
+| `max_jerk`     | `double` | Max jerk limit [m/sss]                         | 1.0           |
+| `min_jerk`     | `double` | Min jerk limit [m/sss]                         | -0.5          |
+
+### External velocity limit parameter
+
+| Name                                       | Type     | Description                                           | Default value |
+| :----------------------------------------- | :------- | :---------------------------------------------------- | :------------ |
+| `margin_to_insert_external_velocity_limit` | `double` | margin distance to insert external velocity limit [m] | 0.3           |
+
+### Curve parameters
+
+| Name                          | Type     | Description                                                            | Default value |
+| :---------------------------- | :------- | :--------------------------------------------------------------------- | :------------ |
+| `max_lateral_accel`           | `double` | Max lateral acceleration limit [m/ss]                                  | 0.5           |
+| `min_curve_velocity`          | `double` | Min velocity at lateral acceleration limit [m/ss]                      | 2.74          |
+| `decel_distance_before_curve` | `double` | Distance to slowdown before a curve for lateral acceleration limit [m] | 3.5           |
+| `decel_distance_after_curve`  | `double` | Distance to slowdown after a curve for lateral acceleration limit [m]  | 2.0           |
+
+### Engage & replan parameters
+
+| Name                           | Type     | Description                                                                                                                        | Default value |
+| :----------------------------- | :------- | :--------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| `replan_vel_deviation`         | `double` | Velocity deviation to replan initial velocity [m/s]                                                                                | 5.53          |
+| `engage_velocity`              | `double` | Engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed) | 0.25          |
+| `engage_acceleration`          | `double` | Engage acceleration [m/ss] (use this acceleration when engagement)                                                                 | 0.1           |
+| `engage_exit_ratio`            | `double` | Exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity.                    | 0.5           |
+| `stop_dist_to_prohibit_engage` | `double` | If the stop point is in this distance, the speed is set to 0 not to move the vehicle [m]                                           | 0.5           |
+
+### Stopping velocity parameters
+
+| Name                | Type     | Description                                                                           | Default value |
+| :------------------ | :------- | :------------------------------------------------------------------------------------ | :------------ |
+| `stopping_velocity` | `double` | change target velocity to this value before v=0 point [m/s]                           | 2.778         |
+| `stopping_distance` | `double` | distance for the stopping_velocity [m]. 0 means the stopping velocity is not applied. | 0.0           |
+
+### Extraction parameters
+
+| Name                  | Type     | Description                                                     | Default value |
+| :-------------------- | :------- | :-------------------------------------------------------------- | :------------ |
+| `extract_ahead_dist`  | `double` | Forward trajectory distance used for planning [m]               | 200.0         |
+| `extract_behind_dist` | `double` | backward trajectory distance used for planning [m]              | 5.0           |
+| `delta_yaw_threshold` | `double` | Allowed delta yaw between ego pose and trajectory pose [radian] | 1.0472        |
+
+### Resampling parameters
+
+| Name                           | Type     | Description                                            | Default value |
+| :----------------------------- | :------- | :----------------------------------------------------- | :------------ |
+| `max_trajectory_length`        | `double` | Max trajectory length for resampling [m]               | 200.0         |
+| `min_trajectory_length`        | `double` | Min trajectory length for resampling [m]               | 30.0          |
+| `resample_time`                | `double` | Resample total time [s]                                | 10.0          |
+| `dense_dt`                     | `double` | resample time interval for dense sampling [s]          | 0.1           |
+| `dense_min_interval_distance`  | `double` | minimum points-interval length for dense sampling [m]  | 0.1           |
+| `sparse_dt`                    | `double` | resample time interval for sparse sampling [s]         | 0.5           |
+| `sparse_min_interval_distance` | `double` | minimum points-interval length for sparse sampling [m] | 4.0           |
+
+### Resampling parameters for post process
+
+| Name                                | Type     | Description                                            | Default value |
+| :---------------------------------- | :------- | :----------------------------------------------------- | :------------ |
+| `post_max_trajectory_length`        | `double` | max trajectory length for resampling [m]               | 300.0         |
+| `post_min_trajectory_length`        | `double` | min trajectory length for resampling [m]               | 30.0          |
+| `post_resample_time`                | `double` | resample total time for dense sampling [s]             | 10.0          |
+| `post_dense_dt`                     | `double` | resample time interval for dense sampling [s]          | 0.1           |
+| `post_dense_min_interval_distance`  | `double` | minimum points-interval length for dense sampling [m]  | 0.1           |
+| `post_sparse_dt`                    | `double` | resample time interval for sparse sampling [s]         | 0.1           |
+| `post_sparse_min_interval_distance` | `double` | minimum points-interval length for sparse sampling [m] | 1.0           |
+
+### Weights for optimization
+
+#### JerkFiltered
+
+| Name            | Type     | Description                           | Default value |
+| :-------------- | :------- | :------------------------------------ | :------------ |
+| `jerk_weight`   | `double` | Weight for "smoothness" cost for jerk | 10.0          |
+| `over_v_weight` | `double` | Weight for "over speed limit" cost    | 100000.0      |
+| `over_a_weight` | `double` | Weight for "over accel limit" cost    | 5000.0        |
+| `over_j_weight` | `double` | Weight for "over jerk limit" cost     | 1000.0        |
+
+#### L2
+
+| Name                 | Type     | Description                        | Default value |
+| :------------------- | :------- | :--------------------------------- | :------------ |
+| `pseudo_jerk_weight` | `double` | Weight for "smoothness" cost       | 100.0         |
+| `over_v_weight`      | `double` | Weight for "over speed limit" cost | 100000.0      |
+| `over_a_weight`      | `double` | Weight for "over accel limit" cost | 1000.0        |
+
+#### Linf
+
+| Name                 | Type     | Description                        | Default value |
+| :------------------- | :------- | :--------------------------------- | :------------ |
+| `pseudo_jerk_weight` | `double` | Weight for "smoothness" cost       | 100.0         |
+| `over_v_weight`      | `double` | Weight for "over speed limit" cost | 100000.0      |
+| `over_a_weight`      | `double` | Weight for "over accel limit" cost | 1000.0        |
+
+### Others
+
+| Name                          | Type     | Description                                                                                       | Default value |
+| :---------------------------- | :------- | :------------------------------------------------------------------------------------------------ | :------------ |
+| `over_stop_velocity_warn_thr` | `double` | Threshold to judge that the optimized velocity exceeds the input velocity on the stop point [m/s] | 1.389         |
+
+<!-- Write parameters of this package.
+
+Example:
+  ### Node Parameters
+
+  | Name                   | Type | Description                     |
+  | ---------------------- | ---- | ------------------------------- |
+  | `output_debug_markers` | bool | whether to output debug markers |
+
+  ### Core Parameters
+
+  | Name                 | Type     | Description                                                          |
+  | -------------------- | -------- | -------------------------------------------------------------------- |
+  | `min_object_size_m`  | `double` | minimum object size to be selected as avoidance target obstacles [m] |
+  | `avoidance_margin_m` | `double` | avoidance margin to obstacles [m]                                    |
+-->
+
+## Assumptions / Known limits
+
+- 参照経路上の点には制限速度（停止点）が正しく設定されていることを仮定
+- 参照経路に設定されている制限速度を指定した減速度やジャークで達成不可能な場合、可能な範囲で速度、加速度、ジャークの逸脱量を抑えながら減速
+- 各逸脱量の重視の度合いはパラメータにより指定
+
+## (Optional) Error detection and handling
+
+## (Optional) Performance characterization
+
+## (Optional) References/External links
+
+[1] B. Stellato, et al., "OSQP: an operator splitting solver for quadratic programs", Mathematical Programming Computation, 2020, [10.1007/s12532-020-00179-2](https://link.springer.com/article/10.1007/s12532-020-00179-2).
+
+[2] Y. Zhang, et al., "Toward a More Complete, Flexible, and Safer Speed Planning for Autonomous Driving via Convex Optimization", Sensors, vol. 18, no. 7, p. 2185, 2018, [10.3390/s18072185](https://doi.org/10.3390/s18072185)
+
+## (Optional) Future extensions / Unimplemented parts
diff --git a/planning/motion_velocity_smoother/README.md b/planning/motion_velocity_smoother/README.md
new file mode 100644
index 0000000000000..0254b53fc7eb0
--- /dev/null
+++ b/planning/motion_velocity_smoother/README.md
@@ -0,0 +1,256 @@
+# Motion Velocity Smoother
+
+## Purpose
+
+`motion_velocity_smoother` outputs a desired velocity profile on a reference trajectory.
+This module plans a velocity profile within the limitations of the velocity, the acceleration and the jerk to realize both the maximization of velocity and the ride quality.
+We call this module `motion_velocity_smoother` because the limitations of the acceleration and the jerk means the smoothness of the velocity profile.
+
+## Inner-workings / Algorithms
+
+### Flow chart
+
+![motion_velocity_smoother_flow](./media/motion_velocity_smoother_flow.drawio.svg)
+
+#### Extract trajectory
+
+For the point on the reference trajectory closest to the center of the rear wheel axle of the vehicle, it extracts the reference path between `extract_behind_dist` behind and `extract_ahead_dist` ahead.
+
+#### Apply external velocity limit
+
+It applies the velocity limit input from the external of `motion_velocity_smoother`.
+Remark that the external velocity limit is different from the velocity limit already set on the map and the reference trajectory.
+The external velocity is applied at the position that it is able to reach the velocity limit with the deceleration and the jerk constraints set as the parameter.
+
+#### Apply stop approaching velocity
+
+It applies the velocity limit near the stopping point.
+This function is used to approach near the obstacle or improve the accuracy of stopping.
+
+#### Apply lateral acceleration limit
+
+It applies the velocity limit to decelerate at the curve.
+It calculate the velocity limit from the curvature of the reference trajectory and the maximum lateral acceleration `max_lateral_accel`.
+The velocity limit is set as not to fall under `min_curve_velocity`.
+
+#### Resample trajectory
+
+It resamples the points on the reference trajectory with designated time interval.
+Note that the range of the length of the trajectory is set between `min_trajectory_length` and `max_trajectory_length`, and the distance between two points is longer than `min_trajectory_interval_distance`.
+It samples densely up to the distance traveled between `resample_time` with the current velocity, then samples sparsely after that.
+By sampling according to the velocity, both calculation load and accuracy are achieved since it samples finely at low velocity and coarsely at high velocity.
+
+#### Calculate initial state
+
+Calculate initial values for velocity planning.
+The initial values are calculated according to the situation as shown in the following table.
+
+| Situation                                                     | Initial velocity       | Initial acceleration   |
+| ------------------------------------------------------------- | ---------------------- | ---------------------- |
+| First calculation                                             | Current velocity       | 0.0                    |
+| Engaging                                                      | `engage_velocity`      | `engage_acceleration`  |
+| Deviate between the planned velocity and the current velocity | Current velocity       | Previous planned value |
+| Normal                                                        | Previous planned value | Previous planned value |
+
+#### Smooth velocity
+
+It plans the velocity.
+The algorithm of velocity planning is chosen from `JerkFiltered`, `L2` and `Linf`, and it is set in the launch file.
+In these algorithms, they use OSQP[1] as the solver of the optimization.
+
+##### JerkFiltered
+
+It minimizes the sum of the minus of the square of the velocity and the square of the violation of the velocity limit, the acceleration limit and the jerk limit.
+
+##### L2
+
+It minimizes the sum of the minus of the square of the velocity, the square of the the pseudo-jerk[2] and the square of the violation of the velocity limit and the acceleration limit.
+
+##### Linf
+
+It minimizes the sum of the minus of the square of the velocity, the maximum absolute value of the the pseudo-jerk[2] and the square of the violation of the velocity limit and the acceleration limit.
+
+#### Post process
+
+It performs the post-process of the planned velocity.
+
+- Set zero velocity ahead of the stopping point
+- Set maximum velocity given in the config named `max_velocity`
+- Set velocity behind the current pose
+- Resample trajectory (`post resampling`)
+- Output debug data
+
+After the optimization, a resampling called `post resampling` is performed before passing the optimized trajectory to the next node. Since the required path interval from optimization may be different from the one for the next module, `post resampling` helps to fill this gap. Therefore, in `post resampling`, it is necessary to check the path specification of the following module to determine the parameters. Note that if the computational load of the optimization algorithm is high and the path interval is sparser than the path specification of the following module in the first resampling, `post resampling` would resample the trajectory densely. On the other hand, if the computational load of the optimization algorithm is small and the path interval is denser than the path specification of the following module in the first resampling, the path is sparsely resampled according to the specification of the following module.
+
+## Inputs / Outputs
+
+### Input
+
+| Name                                       | Type                                     | Description                   |
+| ------------------------------------------ | ---------------------------------------- | ----------------------------- |
+| `~/input/trajectory`                       | `autoware_auto_planning_msgs/Trajectory` | Reference trajectory          |
+| `/planning/scenario_planning/max_velocity` | `std_msgs/Float32`                       | External velocity limit [m/s] |
+| `/localization/kinematic_state`            | `nav_msgs/Odometry`                      | Current odometry              |
+| `/tf`                                      | `tf2_msgs/TFMessage`                     | TF                            |
+| `/tf_static`                               | `tf2_msgs/TFMessage`                     | TF static                     |
+
+### Output
+
+| Name                                               | Type                                     | Description                                                                                               |
+| -------------------------------------------------- | ---------------------------------------- | --------------------------------------------------------------------------------------------------------- |
+| `~/output/trajectory`                              | `autoware_auto_planning_msgs/Trajectory` | Modified trajectory                                                                                       |
+| `/planning/scenario_planning/current_max_velocity` | `std_msgs/Float32`                       | Current external velocity limit [m/s]                                                                     |
+| `~/closest_velocity`                               | `std_msgs/Float32`                       | Planned velocity closest to ego base_link (for debug)                                                     |
+| `~/closest_acceleration`                           | `std_msgs/Float32`                       | Planned acceleration closest to ego base_link (for debug)                                                 |
+| `~/closest_jerk`                                   | `std_msgs/Float32`                       | Planned jerk closest to ego base_link (for debug)                                                         |
+| `~/debug/trajectory_raw`                           | `autoware_auto_planning_msgs/Trajectory` | Extracted trajectory (for debug)                                                                          |
+| `~/debug/trajectory_external_velocity_limited`     | `autoware_auto_planning_msgs/Trajectory` | External velocity limited trajectory (for debug)                                                          |
+| `~/debug/trajectory_lateral_acc_filtered`          | `autoware_auto_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug)                                                |
+| `~/debug/trajectory_time_resampled`                | `autoware_auto_planning_msgs/Trajectory` | Time resampled trajectory (for debug)                                                                     |
+| `~/distance_to_stopline`                           | `std_msgs/Float32`                       | Distance to stop line from current ego pose (max 50 m) (for debug)                                        |
+| `~/stop_speed_exceeded`                            | `std_msgs/Bool`                          | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold |
+
+## Parameters
+
+### Constraint parameters
+
+| Name           | Type     | Description                                    | Default value |
+| :------------- | :------- | :--------------------------------------------- | :------------ |
+| `max_velocity` | `double` | Max velocity limit [m/s]                       | 20.0          |
+| `max_accel`    | `double` | Max acceleration limit [m/ss]                  | 1.0           |
+| `min_decel`    | `double` | Min deceleration limit [m/ss]                  | -0.5          |
+| `stop_decel`   | `double` | Stop deceleration value at a stop point [m/ss] | 0.0           |
+| `max_jerk`     | `double` | Max jerk limit [m/sss]                         | 1.0           |
+| `min_jerk`     | `double` | Min jerk limit [m/sss]                         | -0.5          |
+
+### External velocity limit parameter
+
+| Name                                       | Type     | Description                                           | Default value |
+| :----------------------------------------- | :------- | :---------------------------------------------------- | :------------ |
+| `margin_to_insert_external_velocity_limit` | `double` | margin distance to insert external velocity limit [m] | 0.3           |
+
+### Curve parameters
+
+| Name                          | Type     | Description                                                            | Default value |
+| :---------------------------- | :------- | :--------------------------------------------------------------------- | :------------ |
+| `max_lateral_accel`           | `double` | Max lateral acceleration limit [m/ss]                                  | 0.5           |
+| `min_curve_velocity`          | `double` | Min velocity at lateral acceleration limit [m/ss]                      | 2.74          |
+| `decel_distance_before_curve` | `double` | Distance to slowdown before a curve for lateral acceleration limit [m] | 3.5           |
+| `decel_distance_after_curve`  | `double` | Distance to slowdown after a curve for lateral acceleration limit [m]  | 2.0           |
+
+### Engage & replan parameters
+
+| Name                           | Type     | Description                                                                                                                        | Default value |
+| :----------------------------- | :------- | :--------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| `replan_vel_deviation`         | `double` | Velocity deviation to replan initial velocity [m/s]                                                                                | 5.53          |
+| `engage_velocity`              | `double` | Engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed) | 0.25          |
+| `engage_acceleration`          | `double` | Engage acceleration [m/ss] (use this acceleration when engagement)                                                                 | 0.1           |
+| `engage_exit_ratio`            | `double` | Exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity.                    | 0.5           |
+| `stop_dist_to_prohibit_engage` | `double` | If the stop point is in this distance, the speed is set to 0 not to move the vehicle [m]                                           | 0.5           |
+
+### Stopping velocity parameters
+
+| Name                | Type     | Description                                                                           | Default value |
+| :------------------ | :------- | :------------------------------------------------------------------------------------ | :------------ |
+| `stopping_velocity` | `double` | change target velocity to this value before v=0 point [m/s]                           | 2.778         |
+| `stopping_distance` | `double` | distance for the stopping_velocity [m]. 0 means the stopping velocity is not applied. | 0.0           |
+
+### Extraction parameters
+
+| Name                  | Type     | Description                                                     | Default value |
+| :-------------------- | :------- | :-------------------------------------------------------------- | :------------ |
+| `extract_ahead_dist`  | `double` | Forward trajectory distance used for planning [m]               | 200.0         |
+| `extract_behind_dist` | `double` | backward trajectory distance used for planning [m]              | 5.0           |
+| `delta_yaw_threshold` | `double` | Allowed delta yaw between ego pose and trajectory pose [radian] | 1.0472        |
+
+### Resampling parameters
+
+| Name                           | Type     | Description                                            | Default value |
+| :----------------------------- | :------- | :----------------------------------------------------- | :------------ |
+| `max_trajectory_length`        | `double` | Max trajectory length for resampling [m]               | 200.0         |
+| `min_trajectory_length`        | `double` | Min trajectory length for resampling [m]               | 30.0          |
+| `resample_time`                | `double` | Resample total time [s]                                | 10.0          |
+| `dense_dt`                     | `double` | resample time interval for dense sampling [s]          | 0.1           |
+| `dense_min_interval_distance`  | `double` | minimum points-interval length for dense sampling [m]  | 0.1           |
+| `sparse_dt`                    | `double` | resample time interval for sparse sampling [s]         | 0.5           |
+| `sparse_min_interval_distance` | `double` | minimum points-interval length for sparse sampling [m] | 4.0           |
+
+### Resampling parameters for post process
+
+| Name                                | Type     | Description                                            | Default value |
+| :---------------------------------- | :------- | :----------------------------------------------------- | :------------ |
+| `post_max_trajectory_length`        | `double` | max trajectory length for resampling [m]               | 300.0         |
+| `post_min_trajectory_length`        | `double` | min trajectory length for resampling [m]               | 30.0          |
+| `post_resample_time`                | `double` | resample total time for dense sampling [s]             | 10.0          |
+| `post_dense_dt`                     | `double` | resample time interval for dense sampling [s]          | 0.1           |
+| `post_dense_min_interval_distance`  | `double` | minimum points-interval length for dense sampling [m]  | 0.1           |
+| `post_sparse_dt`                    | `double` | resample time interval for sparse sampling [s]         | 0.1           |
+| `post_sparse_min_interval_distance` | `double` | minimum points-interval length for sparse sampling [m] | 1.0           |
+
+### Weights for optimization
+
+#### JerkFiltered
+
+| Name            | Type     | Description                           | Default value |
+| :-------------- | :------- | :------------------------------------ | :------------ |
+| `jerk_weight`   | `double` | Weight for "smoothness" cost for jerk | 10.0          |
+| `over_v_weight` | `double` | Weight for "over speed limit" cost    | 100000.0      |
+| `over_a_weight` | `double` | Weight for "over accel limit" cost    | 5000.0        |
+| `over_j_weight` | `double` | Weight for "over jerk limit" cost     | 1000.0        |
+
+#### L2
+
+| Name                 | Type     | Description                        | Default value |
+| :------------------- | :------- | :--------------------------------- | :------------ |
+| `pseudo_jerk_weight` | `double` | Weight for "smoothness" cost       | 100.0         |
+| `over_v_weight`      | `double` | Weight for "over speed limit" cost | 100000.0      |
+| `over_a_weight`      | `double` | Weight for "over accel limit" cost | 1000.0        |
+
+#### Linf
+
+| Name                 | Type     | Description                        | Default value |
+| :------------------- | :------- | :--------------------------------- | :------------ |
+| `pseudo_jerk_weight` | `double` | Weight for "smoothness" cost       | 100.0         |
+| `over_v_weight`      | `double` | Weight for "over speed limit" cost | 100000.0      |
+| `over_a_weight`      | `double` | Weight for "over accel limit" cost | 1000.0        |
+
+### Others
+
+| Name                          | Type     | Description                                                                                       | Default value |
+| :---------------------------- | :------- | :------------------------------------------------------------------------------------------------ | :------------ |
+| `over_stop_velocity_warn_thr` | `double` | Threshold to judge that the optimized velocity exceeds the input velocity on the stop point [m/s] | 1.389         |
+
+<!-- Write parameters of this package.
+
+Example:
+  ### Node Parameters
+
+  | Name                   | Type | Description                     |
+  | ---------------------- | ---- | ------------------------------- |
+  | `output_debug_markers` | bool | whether to output debug markers |
+
+  ### Core Parameters
+
+  | Name                 | Type     | Description                                                          |
+  | -------------------- | -------- | -------------------------------------------------------------------- |
+  | `min_object_size_m`  | `double` | minimum object size to be selected as avoidance target obstacles [m] |
+  | `avoidance_margin_m` | `double` | avoidance margin to obstacles [m]                                    |
+-->
+
+## Assumptions / Known limits
+
+- Assume that the velocity limit or the stopping point is properly set at the point on the reference trajectory
+- If the velocity limit set in the reference path cannot be achieved by the designated constraints of the deceleration and the jerk, decelerate while suppressing the velocity, the acceleration and the jerk deviation as much as possible
+- The importance of the deviations is set in the config file
+
+## (Optional) Error detection and handling
+
+## (Optional) Performance characterization
+
+## (Optional) References/External links
+
+[1] B. Stellato, et al., "OSQP: an operator splitting solver for quadratic programs", Mathematical Programming Computation, 2020, [10.1007/s12532-020-00179-2](https://link.springer.com/article/10.1007/s12532-020-00179-2).
+
+[2] Y. Zhang, et al., "Toward a More Complete, Flexible, and Safer Speed Planning for Autonomous Driving via Convex Optimization", Sensors, vol. 18, no. 7, p. 2185, 2018, [10.3390/s18072185](https://doi.org/10.3390/s18072185)
+
+## (Optional) Future extensions / Unimplemented parts
diff --git a/planning/motion_velocity_smoother/config/Analytical.param.yaml b/planning/motion_velocity_smoother/config/Analytical.param.yaml
new file mode 100644
index 0000000000000..329714e3d371e
--- /dev/null
+++ b/planning/motion_velocity_smoother/config/Analytical.param.yaml
@@ -0,0 +1,25 @@
+/**:
+  ros__parameters:
+    resample:
+      ds_resample: 0.1
+      num_resample: 1
+      delta_yaw_threshold: 0.785
+
+    latacc:
+      enable_constant_velocity_while_turning: false
+      constant_velocity_dist_threshold: 2.0
+
+    forward:
+      max_acc: 1.0
+      min_acc: -1.0
+      max_jerk: 0.3
+      min_jerk: -0.3
+      kp: 0.3
+
+    backward:
+      start_jerk: -0.1
+      min_jerk_mild_stop: -0.3
+      min_jerk: -1.5
+      min_acc_mild_stop: -1.0
+      min_acc: -2.5
+      span_jerk: -0.01
diff --git a/planning/motion_velocity_smoother/config/JerkFiltered.param.yaml b/planning/motion_velocity_smoother/config/JerkFiltered.param.yaml
new file mode 100644
index 0000000000000..78c088b0843a2
--- /dev/null
+++ b/planning/motion_velocity_smoother/config/JerkFiltered.param.yaml
@@ -0,0 +1,7 @@
+/**:
+  ros__parameters:
+    jerk_weight: 10.0         # weight for "smoothness" cost for jerk
+    over_v_weight: 100000.0   # weight for "over speed limit" cost
+    over_a_weight: 5000.0     # weight for "over accel limit" cost
+    over_j_weight: 2000.0     # weight for "over jerk limit" cost
+    jerk_filter_ds: 0.1      # resampling ds for jerk filter
diff --git a/planning/motion_velocity_smoother/config/L2.param.yaml b/planning/motion_velocity_smoother/config/L2.param.yaml
new file mode 100644
index 0000000000000..c993978204f10
--- /dev/null
+++ b/planning/motion_velocity_smoother/config/L2.param.yaml
@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    pseudo_jerk_weight: 100.0 # weight for "smoothness" cost
+    over_v_weight: 100000.0   # weight for "over speed limit" cost
+    over_a_weight: 1000.0     # weight for "over accel limit" cost
diff --git a/planning/motion_velocity_smoother/config/Linf.param.yaml b/planning/motion_velocity_smoother/config/Linf.param.yaml
new file mode 100644
index 0000000000000..ec38010621cc9
--- /dev/null
+++ b/planning/motion_velocity_smoother/config/Linf.param.yaml
@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    pseudo_jerk_weight: 200.0 # weight for "smoothness" cost
+    over_v_weight: 100000.0   # weight for "over speed limit" cost
+    over_a_weight: 5000.0     # weight for "over accel limit" cost
diff --git a/planning/motion_velocity_smoother/config/default_common.param.yaml b/planning/motion_velocity_smoother/config/default_common.param.yaml
new file mode 100644
index 0000000000000..a23570a5fc791
--- /dev/null
+++ b/planning/motion_velocity_smoother/config/default_common.param.yaml
@@ -0,0 +1,15 @@
+/**:
+  ros__parameters:
+    # constraints param for normal driving
+    normal:
+      min_acc: -0.5         # min deceleration [m/ss]
+      max_acc: 1.0          # max acceleration [m/ss]
+      min_jerk: -0.5        # min jerk [m/sss]
+      max_jerk: 1.0         # max jerk [m/sss]
+
+    # constraints to be observed
+    limit:
+      min_acc: -2.5         # min deceleration limit [m/ss]
+      max_acc: 1.0          # max acceleration limit [m/ss]
+      min_jerk: -1.5        # min jerk limit [m/sss]
+      max_jerk: 1.5         # max jerk limit [m/sss]
diff --git a/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml b/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml
new file mode 100644
index 0000000000000..904a80d6f4ffd
--- /dev/null
+++ b/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml
@@ -0,0 +1,51 @@
+/**:
+  ros__parameters:
+    # motion state constraints
+    max_velocity: 20.0      # max velocity limit [m/s]
+    stop_decel: 0.0         # deceleration at a stop point[m/ss]
+
+    # external velocity limit parameter
+    margin_to_insert_external_velocity_limit: 0.3   #  margin distance to insert external velocity limit [m]
+
+    # curve parameters
+    max_lateral_accel: 0.8             # max lateral acceleration limit [m/ss]
+    min_curve_velocity: 2.74           # min velocity at lateral acceleration limit [m/ss]
+    decel_distance_before_curve: 3.5   # slow speed distance before a curve for lateral acceleration limit
+    decel_distance_after_curve: 2.0    # slow speed distance after a curve for lateral acceleration limit
+
+    # engage & replan parameters
+    replan_vel_deviation: 5.53         # velocity deviation to replan initial velocity [m/s]
+    engage_velocity: 0.25              # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed)
+    engage_acceleration: 0.1           # engage acceleration [m/ss] (use this acceleration when engagement)
+    engage_exit_ratio: 0.5             # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity.
+    stop_dist_to_prohibit_engage: 0.5  # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m]
+
+    # stop velocity
+    stopping_velocity: 2.778  # change target velocity to this value before v=0 point [m/s]
+    stopping_distance: 0.0    # distance for the stopping_velocity [m]. 0 means the stopping velocity is not applied.
+
+    # path extraction parameters
+    extract_ahead_dist: 200.0         # forward trajectory distance used for planning [m]
+    extract_behind_dist: 5.0          # backward trajectory distance used for planning [m]
+    delta_yaw_threshold: 1.0472       # Allowed delta yaw between ego pose and trajectory pose [radian]
+
+    # resampling parameters for optimization
+    max_trajectory_length: 200.0        # max trajectory length for resampling [m]
+    min_trajectory_length: 150.0        # min trajectory length for resampling [m]
+    resample_time: 5.0                  # resample total time for dense sampling [s]
+    dense_dt: 0.1                       # resample time interval for dense sampling [s]
+    dense_min_interval_distance: 0.1    # minimum points-interval length for dense sampling [m]
+    sparse_dt: 0.5                      # resample time interval for sparse sampling [s]
+    sparse_min_interval_distance: 4.0   # minimum points-interval length for sparse sampling [m]
+
+    # resampling parameters for post process
+    post_max_trajectory_length: 300.0        # max trajectory length for resampling [m]
+    post_min_trajectory_length: 30.0        # min trajectory length for resampling [m]
+    post_resample_time: 10.0                 # resample total time for dense sampling [s]
+    post_dense_dt: 0.1                       # resample time interval for dense sampling [s]
+    post_dense_min_interval_distance: 0.1    # minimum points-interval length for dense sampling [m]
+    post_sparse_dt: 0.1                      # resample time interval for sparse sampling [s]
+    post_sparse_min_interval_distance: 1.0   # minimum points-interval length for sparse sampling [m]
+
+    # system
+    over_stop_velocity_warn_thr: 1.389  # used to check if the optimization exceeds the input velocity on the stop point
diff --git a/planning/motion_velocity_smoother/config/rqt_multiplot.xml b/planning/motion_velocity_smoother/config/rqt_multiplot.xml
new file mode 100644
index 0000000000000..ad0ff4b68251e
--- /dev/null
+++ b/planning/motion_velocity_smoother/config/rqt_multiplot.xml
@@ -0,0 +1,317 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<rqt_multiplot>
+    <table>
+        <background_color>#babdb6</background_color>
+        <foreground_color>#000000</foreground_color>
+        <link_cursor>false</link_cursor>
+        <link_scale>false</link_scale>
+        <plots>
+            <row_0>
+                <column_0>
+                    <axes>
+                        <axes>
+                            <x_axis>
+                                <custom_title>Untitled Axis</custom_title>
+                                <title_type>0</title_type>
+                                <title_visible>true</title_visible>
+                            </x_axis>
+                            <y_axis>
+                                <custom_title>Untitled Axis</custom_title>
+                                <title_type>0</title_type>
+                                <title_visible>true</title_visible>
+                            </y_axis>
+                        </axes>
+                    </axes>
+                    <curves>
+                        <curve_0>
+                            <axes>
+                                <x_axis>
+                                    <field></field>
+                                    <field_type>1</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/planning/scenario_planning/motion_velocity_smoother/closest_velocity</topic>
+                                    <type>std_msgs/Float32</type>
+                                </x_axis>
+                                <y_axis>
+                                    <field>data</field>
+                                    <field_type>0</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/planning/scenario_planning/motion_velocity_smoother/closest_velocity</topic>
+                                    <type>std_msgs/Float32</type>
+                                </y_axis>
+                            </axes>
+                            <color>
+                                <custom_color>#000000</custom_color>
+                                <type>1</type>
+                            </color>
+                            <data>
+                                <circular_buffer_capacity>1000</circular_buffer_capacity>
+                                <time_frame_length>10</time_frame_length>
+                                <type>0</type>
+                            </data>
+                            <style>
+                                <lines_interpolate>false</lines_interpolate>
+                                <pen_style>1</pen_style>
+                                <pen_width>3</pen_width>
+                                <render_antialias>false</render_antialias>
+                                <steps_invert>false</steps_invert>
+                                <sticks_baseline>0</sticks_baseline>
+                                <sticks_orientation>2</sticks_orientation>
+                                <type>0</type>
+                            </style>
+                            <subscriber_queue_size>100</subscriber_queue_size>
+                            <title>Optimum Velocity</title>
+                        </curve_0>
+                        <curve_1>
+                            <axes>
+                                <x_axis>
+                                    <field></field>
+                                    <field_type>1</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/planning/scenario_planning/motion_velocity_smoother/closest_merged_velocity</topic>
+                                    <type>std_msgs/Float32</type>
+                                </x_axis>
+                                <y_axis>
+                                    <field>data</field>
+                                    <field_type>0</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/planning/scenario_planning/motion_velocity_smoother/closest_merged_velocity</topic>
+                                    <type>std_msgs/Float32</type>
+                                </y_axis>
+                            </axes>
+                            <color>
+                                <custom_color>#73d216</custom_color>
+                                <type>1</type>
+                            </color>
+                            <data>
+                                <circular_buffer_capacity>1000</circular_buffer_capacity>
+                                <time_frame_length>10</time_frame_length>
+                                <type>0</type>
+                            </data>
+                            <style>
+                                <lines_interpolate>false</lines_interpolate>
+                                <pen_style>2</pen_style>
+                                <pen_width>3</pen_width>
+                                <render_antialias>false</render_antialias>
+                                <steps_invert>false</steps_invert>
+                                <sticks_baseline>0</sticks_baseline>
+                                <sticks_orientation>2</sticks_orientation>
+                                <type>0</type>
+                            </style>
+                            <subscriber_queue_size>100</subscriber_queue_size>
+                            <title>Jerk Filtered Velocity</title>
+                        </curve_1>
+                        <curve_2>
+                            <axes>
+                                <x_axis>
+                                    <field></field>
+                                    <field_type>1</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/planning/scenario_planning/motion_velocity_smoother/closest_max_velocity</topic>
+                                    <type>std_msgs/Float32</type>
+                                </x_axis>
+                                <y_axis>
+                                    <field>data</field>
+                                    <field_type>0</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/planning/scenario_planning/motion_velocity_smoother/closest_max_velocity</topic>
+                                    <type>std_msgs/Float32</type>
+                                </y_axis>
+                            </axes>
+                            <color>
+                                <custom_color>#75507b</custom_color>
+                                <type>1</type>
+                            </color>
+                            <data>
+                                <circular_buffer_capacity>1000</circular_buffer_capacity>
+                                <time_frame_length>10</time_frame_length>
+                                <type>0</type>
+                            </data>
+                            <style>
+                                <lines_interpolate>false</lines_interpolate>
+                                <pen_style>1</pen_style>
+                                <pen_width>3</pen_width>
+                                <render_antialias>false</render_antialias>
+                                <steps_invert>false</steps_invert>
+                                <sticks_baseline>0</sticks_baseline>
+                                <sticks_orientation>2</sticks_orientation>
+                                <type>0</type>
+                            </style>
+                            <subscriber_queue_size>100</subscriber_queue_size>
+                            <title>Maximum Velocity</title>
+                        </curve_2>
+                        <curve_3>
+                            <axes>
+                                <x_axis>
+                                    <field></field>
+                                    <field_type>1</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/planning/scenario_planning/motion_velocity_smoother/closest_acceleration</topic>
+                                    <type>std_msgs/Float32</type>
+                                </x_axis>
+                                <y_axis>
+                                    <field>data</field>
+                                    <field_type>0</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/planning/scenario_planning/motion_velocity_smoother/closest_acceleration</topic>
+                                    <type>std_msgs/Float32</type>
+                                </y_axis>
+                            </axes>
+                            <color>
+                                <custom_color>#ef2929</custom_color>
+                                <type>1</type>
+                            </color>
+                            <data>
+                                <circular_buffer_capacity>1000</circular_buffer_capacity>
+                                <time_frame_length>10</time_frame_length>
+                                <type>0</type>
+                            </data>
+                            <style>
+                                <lines_interpolate>false</lines_interpolate>
+                                <pen_style>1</pen_style>
+                                <pen_width>3</pen_width>
+                                <render_antialias>false</render_antialias>
+                                <steps_invert>false</steps_invert>
+                                <sticks_baseline>0</sticks_baseline>
+                                <sticks_orientation>2</sticks_orientation>
+                                <type>0</type>
+                            </style>
+                            <subscriber_queue_size>100</subscriber_queue_size>
+                            <title>Acceleration</title>
+                        </curve_3>
+                    </curves>
+                    <legend>
+                        <visible>true</visible>
+                    </legend>
+                    <plot_rate>30</plot_rate>
+                    <title>Velocity</title>
+                </column_0>
+            </row_0>
+            <row_1>
+                <column_0>
+                    <axes>
+                        <axes>
+                            <x_axis>
+                                <custom_title>Untitled Axis</custom_title>
+                                <title_type>0</title_type>
+                                <title_visible>true</title_visible>
+                            </x_axis>
+                            <y_axis>
+                                <custom_title>Untitled Axis</custom_title>
+                                <title_type>0</title_type>
+                                <title_visible>true</title_visible>
+                            </y_axis>
+                        </axes>
+                    </axes>
+                    <curves>
+                        <curve_0>
+                            <axes>
+                                <x_axis>
+                                    <field></field>
+                                    <field_type>1</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/planning/scenario_planning/motion_velocity_smoother/calculation_time</topic>
+                                    <type>std_msgs/Float32</type>
+                                </x_axis>
+                                <y_axis>
+                                    <field>data</field>
+                                    <field_type>0</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/planning/scenario_planning/motion_velocity_smoother/calculation_time</topic>
+                                    <type>std_msgs/Float32</type>
+                                </y_axis>
+                            </axes>
+                            <color>
+                                <custom_color>#000000</custom_color>
+                                <type>0</type>
+                            </color>
+                            <data>
+                                <circular_buffer_capacity>1000</circular_buffer_capacity>
+                                <time_frame_length>10</time_frame_length>
+                                <type>0</type>
+                            </data>
+                            <style>
+                                <lines_interpolate>false</lines_interpolate>
+                                <pen_style>1</pen_style>
+                                <pen_width>3</pen_width>
+                                <render_antialias>false</render_antialias>
+                                <steps_invert>false</steps_invert>
+                                <sticks_baseline>0</sticks_baseline>
+                                <sticks_orientation>2</sticks_orientation>
+                                <type>0</type>
+                            </style>
+                            <subscriber_queue_size>100</subscriber_queue_size>
+                            <title>Calculation Time</title>
+                        </curve_0>
+                    </curves>
+                    <legend>
+                        <visible>true</visible>
+                    </legend>
+                    <plot_rate>30</plot_rate>
+                    <title>Calculation Time</title>
+                </column_0>
+            </row_1>
+        </plots>
+        <track_points>false</track_points>
+    </table>
+</rqt_multiplot>
diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/linear_interpolation.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/linear_interpolation.hpp
new file mode 100644
index 0000000000000..10c0bca1ddddb
--- /dev/null
+++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/linear_interpolation.hpp
@@ -0,0 +1,34 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MOTION_VELOCITY_SMOOTHER__LINEAR_INTERPOLATION_HPP_
+#define MOTION_VELOCITY_SMOOTHER__LINEAR_INTERPOLATION_HPP_
+
+#include <boost/optional.hpp>
+
+#include <cmath>
+#include <iostream>
+#include <vector>
+
+namespace motion_velocity_smoother
+{
+namespace linear_interpolation
+{
+boost::optional<std::vector<double>> interpolate(
+  const std::vector<double> & base_index, const std::vector<double> & base_value,
+  const std::vector<double> & return_index);
+}
+}  // namespace motion_velocity_smoother
+
+#endif  // MOTION_VELOCITY_SMOOTHER__LINEAR_INTERPOLATION_HPP_
diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp
new file mode 100644
index 0000000000000..b4b50655f17b1
--- /dev/null
+++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp
@@ -0,0 +1,234 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MOTION_VELOCITY_SMOOTHER__MOTION_VELOCITY_SMOOTHER_NODE_HPP_
+#define MOTION_VELOCITY_SMOOTHER__MOTION_VELOCITY_SMOOTHER_NODE_HPP_
+
+#include "autoware_utils/geometry/geometry.hpp"
+#include "autoware_utils/math/unit_conversion.hpp"
+#include "autoware_utils/ros/self_pose_listener.hpp"
+#include "autoware_utils/system/stop_watch.hpp"
+#include "autoware_utils/trajectory/tmp_conversion.hpp"
+#include "autoware_utils/trajectory/trajectory.hpp"
+#include "motion_velocity_smoother/resample.hpp"
+#include "motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp"
+#include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp"
+#include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp"
+#include "osqp_interface/osqp_interface.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2/utils.h"
+#include "tf2_ros/transform_listener.h"
+
+#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+#include "autoware_debug_msgs/msg/float32_stamped.hpp"         // temporary
+#include "autoware_planning_msgs/msg/stop_speed_exceeded.hpp"  // temporary
+#include "autoware_planning_msgs/msg/velocity_limit.hpp"       // temporary
+#include "nav_msgs/msg/odometry.hpp"
+
+#include <iostream>
+#include <memory>
+#include <string>
+#include <tuple>
+#include <vector>
+
+// *INDENT-OFF*
+#include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp"
+// *INDENT-ON*
+#include "motion_velocity_smoother/smoother/smoother_base.hpp"
+#include "motion_velocity_smoother/trajectory_utils.hpp"
+
+namespace motion_velocity_smoother
+{
+using autoware_auto_planning_msgs::msg::Trajectory;
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+using TrajectoryPoints = std::vector<TrajectoryPoint>;
+using autoware_debug_msgs::msg::Float32Stamped;        // temporary
+using autoware_planning_msgs::msg::StopSpeedExceeded;  // temporary
+using autoware_planning_msgs::msg::VelocityLimit;      // temporary
+using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::PoseStamped;
+using nav_msgs::msg::Odometry;
+
+class MotionVelocitySmootherNode : public rclcpp::Node
+{
+public:
+  explicit MotionVelocitySmootherNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  rclcpp::Publisher<Trajectory>::SharedPtr
+    pub_trajectory_;  //!< @brief publisher for output trajectory
+  rclcpp::Publisher<StopSpeedExceeded>::SharedPtr
+    pub_over_stop_velocity_;  //!< @brief publisher for over stop velocity warning
+  rclcpp::Subscription<Odometry>::SharedPtr
+    sub_current_odometry_;  //!< @brief subscriber for current velocity
+  rclcpp::Subscription<Trajectory>::SharedPtr
+    sub_current_trajectory_;  //!< @brief subscriber for reference trajectory
+  rclcpp::Subscription<VelocityLimit>::SharedPtr
+    sub_external_velocity_limit_;  //!< @brief subscriber for external velocity limit
+
+  PoseStamped::ConstSharedPtr current_pose_ptr_;   // current vehicle pose
+  Odometry::ConstSharedPtr current_odometry_ptr_;  // current odometry
+  Trajectory::ConstSharedPtr base_traj_raw_ptr_;   // current base_waypoints
+  double external_velocity_limit_;                 // current external_velocity_limit
+  double max_velocity_with_deceleration_;          // maximum velocity with deceleration
+                                                   // for external velocity limit
+  double external_velocity_limit_dist_{0.0};       // distance to set external velocity limit
+
+  TrajectoryPoints prev_output_;                           // previously published trajectory
+  boost::optional<TrajectoryPoint> prev_closest_point_{};  // previous trajectory point
+                                                           // closest to ego vehicle
+
+  autoware_utils::SelfPoseListener self_pose_listener_{this};
+
+  enum class AlgorithmType {
+    INVALID = 0,
+    JERK_FILTERED = 1,
+    L2 = 2,
+    LINF = 3,
+    ANALYTICAL = 4,
+  };
+
+  enum class InitializeType {
+    INIT = 0,
+    LARGE_DEVIATION_REPLAN = 1,
+    ENGAGING = 2,
+    NORMAL = 3,
+  };
+
+  struct Param
+  {
+    double max_velocity;                              // max velocity [m/s]
+    double margin_to_insert_external_velocity_limit;  // for external velocity limit [m]
+    double replan_vel_deviation;                      // if speed error exceeds this [m/s],
+                                                      // replan from current velocity
+    double engage_velocity;                           // use this speed when start moving [m/s]
+    double engage_acceleration;           // use this acceleration when start moving [m/ss]
+    double engage_exit_ratio;             // exit engage sequence
+                                          // when the speed exceeds ratio x engage_vel.
+    double stopping_velocity;             // change target velocity to this value before v=0 point.
+    double stopping_distance;             // distance for the stopping_velocity
+    double extract_ahead_dist;            // forward waypoints distance from current position [m]
+    double extract_behind_dist;           // backward waypoints distance from current position [m]
+    double stop_dist_to_prohibit_engage;  // prevent to move toward close stop point
+    double delta_yaw_threshold;           // for closest index calculation
+    resampling::ResampleParam post_resample_param;
+    AlgorithmType algorithm_type;  // Option : JerkFiltered, Linf, L2
+  } node_param_{};
+
+  SmootherBase::BaseParam base_param_{};
+  JerkFilteredSmoother::Param jerk_filtered_smoother_param_{};
+  L2PseudoJerkSmoother::Param l2_pseudo_jerk_smoother_param_{};
+  LinfPseudoJerkSmoother::Param linf_pseudo_jerk_smoother_param_{};
+  AnalyticalJerkConstrainedSmoother::Param analytical_jerk_constrained_smoother_param_{};
+
+  std::shared_ptr<SmootherBase> smoother_;
+
+  bool publish_debug_trajs_;  // publish planned trajectories
+
+  double over_stop_velocity_warn_thr_;  // threshold to publish over velocity warn
+
+  // parameter update
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+  rcl_interfaces::msg::SetParametersResult onParameter(
+    const std::vector<rclcpp::Parameter> & parameters);
+
+  // topic callback
+  void onCurrentOdometry(const Odometry::ConstSharedPtr msg);
+
+  void onCurrentTrajectory(const Trajectory::ConstSharedPtr msg);
+
+  void onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg);
+
+  // publish methods
+  void publishTrajectory(const TrajectoryPoints & traj) const;
+
+  void publishStopDistance(const TrajectoryPoints & trajectory, const size_t closest) const;
+
+  // non-const methods
+  void publishClosestState(const TrajectoryPoint & closest_point);
+
+  // const methods
+  bool checkData() const;
+
+  AlgorithmType getAlgorithmType(const std::string & algorithm_name) const;
+
+  TrajectoryPoints calcTrajectoryVelocity(const TrajectoryPoints & input) const;
+
+  bool smoothVelocity(const TrajectoryPoints & input, TrajectoryPoints & traj_smoothed) const;
+
+  std::tuple<double, double, InitializeType> calcInitialMotion(
+    const TrajectoryPoints & input_traj, const size_t input_closest,
+    const TrajectoryPoints & prev_traj) const;
+
+  void applyExternalVelocityLimit(TrajectoryPoints & traj) const;
+
+  void insertBehindVelocity(
+    const size_t output_closest, const InitializeType type, TrajectoryPoints & output) const;
+
+  void applyStopApproachingVelocity(TrajectoryPoints & traj) const;
+
+  void overwriteStopPoint(const TrajectoryPoints & input, TrajectoryPoints & output) const;
+
+  double calcTravelDistance() const;
+
+  bool isEngageStatus(const double target_vel) const;
+
+  void publishDebugTrajectories(const std::vector<TrajectoryPoints> & debug_trajectories) const;
+
+  void publishClosestVelocity(
+    const TrajectoryPoints & trajectory, const Pose & current_pose,
+    const rclcpp::Publisher<Float32Stamped>::SharedPtr pub) const;
+
+  Trajectory toTrajectoryMsg(
+    const TrajectoryPoints & points, const std_msgs::msg::Header & header) const;
+
+  // parameter handling
+  void initCommonParam();
+
+  void initSmootherBaseParam();
+
+  void initJerkFilteredSmootherParam();
+
+  void initL2PseudoJerkSmootherParam();
+
+  void initLinfPseudoJerkSmootherParam();
+
+  void initAnalyticalJerkConstrainedSmootherParam();
+
+  // debug
+  autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch_;
+  std::shared_ptr<rclcpp::Time> prev_time_;
+  double prev_acc_;
+  rclcpp::Publisher<Float32Stamped>::SharedPtr pub_dist_to_stopline_;
+  rclcpp::Publisher<Trajectory>::SharedPtr pub_trajectory_raw_;
+  rclcpp::Publisher<VelocityLimit>::SharedPtr pub_velocity_limit_;
+  rclcpp::Publisher<Trajectory>::SharedPtr pub_trajectory_vel_lim_;
+  rclcpp::Publisher<Trajectory>::SharedPtr pub_trajectory_latacc_filtered_;
+  rclcpp::Publisher<Trajectory>::SharedPtr pub_trajectory_resampled_;
+  rclcpp::Publisher<Float32Stamped>::SharedPtr debug_closest_velocity_;
+  rclcpp::Publisher<Float32Stamped>::SharedPtr debug_closest_acc_;
+  rclcpp::Publisher<Float32Stamped>::SharedPtr debug_closest_jerk_;
+  rclcpp::Publisher<Float32Stamped>::SharedPtr debug_calculation_time_;
+  rclcpp::Publisher<Float32Stamped>::SharedPtr debug_closest_max_velocity_;
+
+  // For Jerk Filtered Algorithm Debug
+  rclcpp::Publisher<Trajectory>::SharedPtr pub_forward_filtered_trajectory_;
+  rclcpp::Publisher<Trajectory>::SharedPtr pub_backward_filtered_trajectory_;
+  rclcpp::Publisher<Trajectory>::SharedPtr pub_merged_filtered_trajectory_;
+  rclcpp::Publisher<Float32Stamped>::SharedPtr pub_closest_merged_velocity_;
+};
+}  // namespace motion_velocity_smoother
+
+#endif  // MOTION_VELOCITY_SMOOTHER__MOTION_VELOCITY_SMOOTHER_NODE_HPP_
diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp
new file mode 100644
index 0000000000000..cd13a081e94a2
--- /dev/null
+++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp
@@ -0,0 +1,55 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_
+#define MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_
+
+#include "autoware_utils/trajectory/trajectory.hpp"
+#include "motion_velocity_smoother/trajectory_utils.hpp"
+
+#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+
+#include "boost/optional.hpp"
+
+#include <vector>
+
+namespace motion_velocity_smoother
+{
+namespace resampling
+{
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+using TrajectoryPoints = std::vector<TrajectoryPoint>;
+
+struct ResampleParam
+{
+  double max_trajectory_length;         // max length of the objective trajectory for resample
+  double min_trajectory_length;         // min length of the objective trajectory for resample
+  double resample_time;                 // max time to calculate trajectory length
+  double dense_resample_dt;             // resample time interval for dense sampling [s]
+  double dense_min_interval_distance;   // minimum points-interval length for dense sampling [m]
+  double sparse_resample_dt;            // resample time interval for sparse sampling [s]
+  double sparse_min_interval_distance;  // minimum points-interval length for sparse sampling [m]
+};
+
+boost::optional<TrajectoryPoints> resampleTrajectory(
+  const TrajectoryPoints & input, const double v_current, const size_t closest_id,
+  const ResampleParam & param);
+
+boost::optional<TrajectoryPoints> resampleTrajectory(
+  const TrajectoryPoints & input, const size_t closest_id, const ResampleParam & param,
+  const double nominal_ds);
+}  // namespace resampling
+}  // namespace motion_velocity_smoother
+
+#endif  // MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_
diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp
new file mode 100644
index 0000000000000..412427f0277f9
--- /dev/null
+++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp
@@ -0,0 +1,117 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// *INDENT-OFF*
+#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_  // NOLINT
+#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_  // NOLINT
+// *INDENT-ON*
+
+#include "autoware_utils/trajectory/trajectory.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2/utils.h"
+
+#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+
+#include <string>
+#include <utility>
+#include <vector>
+
+// *INDENT-OFF*
+#include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp"
+// *INDENT-ON*
+#include "motion_velocity_smoother/smoother/smoother_base.hpp"
+
+namespace motion_velocity_smoother
+{
+class AnalyticalJerkConstrainedSmoother : public SmootherBase
+{
+public:
+  struct Param
+  {
+    struct
+    {
+      double ds_resample;
+      double num_resample;
+      double delta_yaw_threshold;
+    } resample;
+    struct
+    {
+      bool enable_constant_velocity_while_turning;
+      double constant_velocity_dist_threshold;
+    } latacc;
+    struct
+    {
+      double max_acc;
+      double min_acc;
+      double max_jerk;
+      double min_jerk;
+      double kp;
+    } forward;
+    struct
+    {
+      double start_jerk;
+      double min_jerk_mild_stop;
+      double min_jerk;
+      double min_acc_mild_stop;
+      double min_acc;
+      double span_jerk;
+    } backward;
+  };
+
+  explicit AnalyticalJerkConstrainedSmoother(const Param & p);
+
+  bool apply(
+    const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
+    TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
+
+  boost::optional<TrajectoryPoints> resampleTrajectory(
+    const TrajectoryPoints & input, const double v_current, const int closest_id) const override;
+
+  boost::optional<TrajectoryPoints> applyLateralAccelerationFilter(
+    const TrajectoryPoints & input) const override;
+
+  void setParam(const Param & param);
+
+private:
+  Param smoother_param_;
+  rclcpp::Logger logger_{
+    rclcpp::get_logger("smoother").get_child("analytical_jerk_constrained_smoother")};
+
+  bool searchDecelTargetIndices(
+    const TrajectoryPoints & trajectory, const size_t closest_index,
+    std::vector<std::pair<size_t, double>> & decel_target_indices) const;
+  bool applyForwardJerkFilter(
+    const TrajectoryPoints & base_trajectory, const size_t start_index, const double initial_vel,
+    const double initial_acc, const Param & params, TrajectoryPoints & output_trajectory) const;
+  bool applyBackwardDecelFilter(
+    const std::vector<size_t> & start_indices, const size_t decel_target_index,
+    const double decel_target_vel, const Param & params,
+    TrajectoryPoints & output_trajectory) const;
+  bool calcEnoughDistForDecel(
+    const TrajectoryPoints & trajectory, const size_t start_index, const double decel_target_vel,
+    const double planning_jerk, const Param & params, const std::vector<double> & dist_to_target,
+    bool & is_enough_dist, int & type, std::vector<double> & times, double & stop_dist) const;
+  bool applyDecelVelocityFilter(
+    const size_t decel_start_index, const double decel_target_vel, const double planning_jerk,
+    const Param & params, const int type, const std::vector<double> & times,
+    TrajectoryPoints & output_trajectory) const;
+
+  // debug
+  std::string strTimes(const std::vector<double> & times) const;
+  std::string strStartIndices(const std::vector<size_t> & start_indices) const;
+};
+}  // namespace motion_velocity_smoother
+
+#endif
diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp
new file mode 100644
index 0000000000000..6c0156257cd96
--- /dev/null
+++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp
@@ -0,0 +1,59 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// *INDENT-OFF*
+#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_  // NOLINT
+#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_  // NOLINT
+// *INDENT-ON*
+
+#include "autoware_utils/trajectory/trajectory.hpp"
+#include "motion_velocity_smoother/linear_interpolation.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+
+#include <cmath>
+#include <iostream>
+#include <numeric>
+#include <vector>
+
+namespace motion_velocity_smoother
+{
+namespace analytical_velocity_planning_utils
+{
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+using TrajectoryPoints = std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>;
+
+bool calcStopDistWithJerkAndAccConstraints(
+  const double v0, const double a0, const double jerk_acc, const double jerk_dec,
+  const double min_acc, const double target_vel, int & type, std::vector<double> & times,
+  double & stop_dist);
+bool validCheckCalcStopDist(
+  const double v_end, const double a_end, const double v_target, const double a_target,
+  const double v_margin, const double a_margin);
+bool calcStopVelocityWithConstantJerkAccLimit(
+  const double v0, const double a0, const double jerk_acc, const double jerk_dec,
+  const double min_acc, const double decel_target_vel, const int type,
+  const std::vector<double> & times, const size_t start_index,
+  TrajectoryPoints & output_trajectory);
+void updateStopVelocityStatus(
+  double v0, double a0, double jerk_acc, double jerk_dec, int type, std::vector<double> times,
+  double t, double & x, double & v, double & a, double & j);
+double integ_x(double x0, double v0, double a0, double j0, double t);
+double integ_v(double v0, double a0, double j0, double t);
+double integ_a(double a0, double j0, double t);
+}  // namespace analytical_velocity_planning_utils
+}  // namespace motion_velocity_smoother
+
+#endif
diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp
new file mode 100644
index 0000000000000..c3e4f16cc344b
--- /dev/null
+++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp
@@ -0,0 +1,71 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_
+#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_
+
+#include "autoware_utils/geometry/geometry.hpp"
+#include "autoware_utils/trajectory/trajectory.hpp"
+#include "motion_velocity_smoother/smoother/smoother_base.hpp"
+#include "osqp_interface/osqp_interface.hpp"
+
+#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+
+#include "boost/optional.hpp"
+
+#include <vector>
+
+namespace motion_velocity_smoother
+{
+class JerkFilteredSmoother : public SmootherBase
+{
+public:
+  struct Param
+  {
+    double jerk_weight;
+    double over_v_weight;
+    double over_a_weight;
+    double over_j_weight;
+    double jerk_filter_ds;
+  };
+
+  explicit JerkFilteredSmoother(const Param & p);
+
+  bool apply(
+    const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
+    TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
+
+  boost::optional<TrajectoryPoints> resampleTrajectory(
+    const TrajectoryPoints & input, const double v_current, const int closest_id) const override;
+
+  void setParam(const Param & param);
+
+private:
+  Param smoother_param_;
+  autoware::common::osqp::OSQPInterface qp_solver_;
+  rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("jerk_filtered_smoother")};
+
+  TrajectoryPoints forwardJerkFilter(
+    const double v0, const double a0, const double a_max, const double a_stop, const double j_max,
+    const TrajectoryPoints & input) const;
+  TrajectoryPoints backwardJerkFilter(
+    const double v0, const double a0, const double a_min, const double a_stop, const double j_min,
+    const TrajectoryPoints & input) const;
+  TrajectoryPoints mergeFilteredTrajectory(
+    const double v0, const double a0, const double a_min, const double j_min,
+    const TrajectoryPoints & forward_filtered, const TrajectoryPoints & backward_filtered) const;
+};
+}  // namespace motion_velocity_smoother
+
+#endif  // MOTION_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_
diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp
new file mode 100644
index 0000000000000..fd203dee78a67
--- /dev/null
+++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp
@@ -0,0 +1,59 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_
+#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_
+
+#include "autoware_utils/geometry/geometry.hpp"
+#include "autoware_utils/trajectory/trajectory.hpp"
+#include "motion_velocity_smoother/smoother/smoother_base.hpp"
+#include "osqp_interface/osqp_interface.hpp"
+
+#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+
+#include "boost/optional.hpp"
+
+#include <vector>
+
+namespace motion_velocity_smoother
+{
+class L2PseudoJerkSmoother : public SmootherBase
+{
+public:
+  struct Param
+  {
+    double pseudo_jerk_weight;
+    double over_v_weight;
+    double over_a_weight;
+  };
+
+  explicit L2PseudoJerkSmoother(const Param & smoother_param);
+
+  bool apply(
+    const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
+    TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
+
+  boost::optional<TrajectoryPoints> resampleTrajectory(
+    const TrajectoryPoints & input, const double v_current, const int closest_id) const override;
+
+  void setParam(const Param & smoother_param);
+
+private:
+  Param smoother_param_;
+  autoware::common::osqp::OSQPInterface qp_solver_;
+  rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("l2_pseudo_jerk_smoother")};
+};
+}  // namespace motion_velocity_smoother
+
+#endif  // MOTION_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_
diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp
new file mode 100644
index 0000000000000..89e641d183751
--- /dev/null
+++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp
@@ -0,0 +1,59 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_
+#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_
+
+#include "autoware_utils/geometry/geometry.hpp"
+#include "autoware_utils/trajectory/trajectory.hpp"
+#include "motion_velocity_smoother/smoother/smoother_base.hpp"
+#include "osqp_interface/osqp_interface.hpp"
+
+#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+
+#include "boost/optional.hpp"
+
+#include <vector>
+
+namespace motion_velocity_smoother
+{
+class LinfPseudoJerkSmoother : public SmootherBase
+{
+public:
+  struct Param
+  {
+    double pseudo_jerk_weight;
+    double over_v_weight;
+    double over_a_weight;
+  };
+
+  explicit LinfPseudoJerkSmoother(const Param & smoother_param);
+
+  bool apply(
+    const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
+    TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
+
+  boost::optional<TrajectoryPoints> resampleTrajectory(
+    const TrajectoryPoints & input, const double v_current, const int closest_id) const override;
+
+  void setParam(const Param & smoother_param);
+
+private:
+  Param smoother_param_;
+  autoware::common::osqp::OSQPInterface qp_solver_;
+  rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("linf_pseudo_jerk_smoother")};
+};
+}  // namespace motion_velocity_smoother
+
+#endif  // MOTION_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_
diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp
new file mode 100644
index 0000000000000..c14bdc0c21ad5
--- /dev/null
+++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp
@@ -0,0 +1,76 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_
+#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_
+
+#include "autoware_utils/geometry/geometry.hpp"
+#include "autoware_utils/trajectory/trajectory.hpp"
+#include "motion_velocity_smoother/resample.hpp"
+#include "motion_velocity_smoother/trajectory_utils.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+
+#include "boost/optional.hpp"
+
+#include <limits>
+#include <vector>
+
+namespace motion_velocity_smoother
+{
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+using TrajectoryPoints = std::vector<TrajectoryPoint>;
+
+class SmootherBase
+{
+public:
+  struct BaseParam
+  {
+    double max_accel;   // max acceleration in planning [m/s2] > 0
+    double min_decel;   // min deceleration in planning [m/s2] < 0
+    double stop_decel;  // deceleration at a stop point [m/s2] <= 0
+    double max_jerk;
+    double min_jerk;
+    double max_lateral_accel;            // max lateral acceleration [m/ss] > 0
+    double min_curve_velocity;           // min velocity at curve [m/s]
+    double decel_distance_before_curve;  // distance before slow down for lateral acc at a curve
+    double decel_distance_after_curve;   // distance after slow down for lateral acc at a curve
+    resampling::ResampleParam resample_param;
+  };
+
+  virtual ~SmootherBase() = default;
+  virtual bool apply(
+    const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
+    TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) = 0;
+
+  virtual boost::optional<TrajectoryPoints> resampleTrajectory(
+    const TrajectoryPoints & input, const double v_current, const int closest_id) const = 0;
+
+  virtual boost::optional<TrajectoryPoints> applyLateralAccelerationFilter(
+    const TrajectoryPoints & input) const;
+
+  double getMaxAccel() const;
+  double getMinDecel() const;
+  double getMaxJerk() const;
+  double getMinJerk() const;
+
+  void setParam(const BaseParam & param);
+
+protected:
+  BaseParam base_param_;
+};
+}  // namespace motion_velocity_smoother
+
+#endif  // MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_
diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp
new file mode 100644
index 0000000000000..aea9fed6641a9
--- /dev/null
+++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp
@@ -0,0 +1,91 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_
+#define MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_
+
+#include "autoware_utils/geometry/geometry.hpp"
+#include "autoware_utils/trajectory/trajectory.hpp"
+
+#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+
+#include "boost/optional.hpp"
+
+#include <iostream>
+#include <map>
+#include <numeric>
+#include <tuple>
+#include <vector>
+
+namespace motion_velocity_smoother
+{
+namespace trajectory_utils
+{
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+using TrajectoryPoints = std::vector<TrajectoryPoint>;
+using geometry_msgs::msg::Pose;
+
+TrajectoryPoint calcInterpolatedTrajectoryPoint(
+  const TrajectoryPoints & trajectory, const Pose & target_pose);
+
+boost::optional<TrajectoryPoints> extractPathAroundIndex(
+  const TrajectoryPoints & trajectory, const size_t index, const double & ahead_length,
+  const double & behind_length);
+
+double calcArcLength(const TrajectoryPoints & trajectory, const int idx1, const int idx2);
+
+std::vector<double> calcArclengthArray(const TrajectoryPoints & trajectory);
+
+std::vector<double> calcTrajectoryIntervalDistance(const TrajectoryPoints & trajectory);
+
+boost::optional<std::vector<double>> calcTrajectoryCurvatureFrom3Points(
+  const TrajectoryPoints & trajectory, const size_t & idx_dist);
+
+void setZeroVelocity(TrajectoryPoints & trajectory);
+
+double getMaxVelocity(const TrajectoryPoints & trajectory);
+
+double getMaxAbsVelocity(const TrajectoryPoints & trajectory);
+
+void applyMaximumVelocityLimit(
+  const size_t from, const size_t to, const double max_vel, TrajectoryPoints & trajectory);
+
+boost::optional<size_t> searchZeroVelocityIdx(const TrajectoryPoints & trajectory);
+
+boost::optional<TrajectoryPoints> applyLinearInterpolation(
+  const std::vector<double> & base_index, const TrajectoryPoints & base_trajectory,
+  const std::vector<double> & out_index, const bool use_spline_for_pose = false);
+
+bool calcStopDistWithJerkConstraints(
+  const double v0, const double a0, const double jerk_acc, const double jerk_dec,
+  const double min_acc, const double target_vel, std::map<double, double> & jerk_profile,
+  double & stop_dist);
+
+bool isValidStopDist(
+  const double v_end, const double a_end, const double v_target, const double a_target,
+  const double v_margin, const double a_margin);
+
+boost::optional<TrajectoryPoints> applyDecelFilterWithJerkConstraint(
+  const TrajectoryPoints & input, const size_t start_index, const double v0, const double a0,
+  const double min_acc, const double decel_target_vel,
+  const std::map<double, double> & jerk_profile);
+
+boost::optional<std::tuple<double, double, double, double>> updateStateWithJerkConstraint(
+  const double v0, const double a0, const std::map<double, double> & jerk_profile, const double t);
+
+}  // namespace trajectory_utils
+}  // namespace motion_velocity_smoother
+
+#endif  // MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_
diff --git a/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml b/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml
new file mode 100644
index 0000000000000..9c5768fccfad3
--- /dev/null
+++ b/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml
@@ -0,0 +1,28 @@
+<launch>
+  <!-- common param -->
+  <arg name="common_param_path" default="$(find-pkg-share motion_velocity_smoother)/config/default_common.param.yaml"/>
+
+  <!-- input/output -->
+  <arg name="input_trajectory" default="/planning/scenario_planning/scenario_selector/trajectory"/>
+  <arg name="output_trajectory" default="/planning/scenario_planning/trajectory"/>
+
+  <!-- debug flags -->
+  <arg name="publish_debug_trajs" default="true"/>
+  <arg name="smoother_type" default="JerkFiltered"/> <!-- Analytical, JerkFiltered, L2, or Linf -->
+
+  <arg name="param_path" default="$(find-pkg-share motion_velocity_smoother)/config/default_motion_velocity_smoother.param.yaml" />
+  <arg name="smoother_param_path" default="$(find-pkg-share motion_velocity_smoother)/config/$(var smoother_type).param.yaml" />
+
+  <node pkg="motion_velocity_smoother" exec="motion_velocity_smoother" name="motion_velocity_smoother" output="screen">
+    <param from="$(var common_param_path)" />
+    <param from="$(var param_path)" />
+    <param from="$(var smoother_param_path)" />
+    <param name="publish_debug_trajs" value="$(var publish_debug_trajs)"/>
+    <param name="algorithm_type" value="$(var smoother_type)" />
+    <remap from="~/input/trajectory" to="$(var input_trajectory)"/>
+    <remap from="~/output/trajectory" to="$(var output_trajectory)"/>
+    <remap from="~/input/external_velocity_limit_mps" to="/planning/scenario_planning/max_velocity"/> <!-- Topic for setting maximum speed from the outside (input topic) -->
+    <remap from="~/output/current_velocity_limit_mps" to="/planning/scenario_planning/current_max_velocity"/> <!-- Topic for displaying the set maximum speed (output topic) -->
+  </node>
+
+</launch>
diff --git a/planning/motion_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg b/planning/motion_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg
new file mode 100644
index 0000000000000..4d8ffd9eb8ae2
--- /dev/null
+++ b/planning/motion_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg
@@ -0,0 +1,3 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="1127px" height="1531px" viewBox="-0.5 -0.5 1127 1531" content="&lt;mxfile host=&quot;app.diagrams.net&quot; modified=&quot;2021-06-28T09:40:55.320Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/88.0.4324.96 Safari/537.36&quot; etag=&quot;mkVlTmMT86s69d6RVbAq&quot; version=&quot;14.6.13&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;VyaQj6gfKOac8UcvojAl&quot; name=&quot;Page-1&quot;&gt;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&lt;/diagram&gt;&lt;/mxfile&gt;"><defs><clipPath id="mx-clip-149-5-141-26-0"><rect x="149" y="5" width="141" height="26"/></clipPath><clipPath id="mx-clip-150-865-141-26-0"><rect x="150" y="865" width="141" height="26"/></clipPath></defs><g><rect x="507.5" y="1340" width="617" height="190" fill="#ffffff" stroke="#000000" pointer-events="all"/><path d="M 596 1490.42 L 596 1383.66" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 596 1377.66 L 600 1385.66 L 596 1383.66 L 592 1385.66 Z" fill="#000000" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/><path d="M 586 1480.42 L 987.76 1480.42" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 993.76 1480.42 L 985.76 1484.42 L 987.76 1480.42 L 985.76 1476.42 Z" fill="#000000" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/><rect x="509" y="1130" width="617" height="190" fill="#ffffff" stroke="#000000" pointer-events="all"/><rect x="507.5" y="690" width="617" height="400" fill="#ffffff" stroke="#000000" pointer-events="all"/><rect x="507.5" y="435" width="617" height="230" fill="#ffffff" stroke="#000000" pointer-events="all"/><rect x="509" y="180" width="617" height="230" fill="#ffffff" stroke="#000000" pointer-events="all"/><rect x="509" y="10" width="617" height="140" fill="#ffffff" stroke="#000000" pointer-events="all"/><path d="M 696 1430 L 746 1430" fill="none" stroke="#3333ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 617 1420 L 626.5 1418 Q 636 1416 645.41 1419.38 L 665.59 1426.62 Q 675 1430 685 1430 L 698 1430" fill="none" stroke="#3333ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 746 1430 L 751 1430 Q 756 1430 764.75 1425.16 L 785.25 1413.84 Q 794 1409 801.49 1415.62 L 855.51 1463.38 Q 863 1470 869.5 1470 L 876 1470" fill="none" stroke="#3333ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 936 1480 L 931 1480 Q 926 1480 917.06 1475.53 L 911.53 1472.76 Q 906 1470 896 1470 L 876 1470" fill="none" stroke="#3333ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 746 1430.42 L 696 1430.42" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 616 1193 L 616 1270" fill="none" stroke="#000000" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/><path d="M 596 970 L 676 970" fill="none" stroke="#3333ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 696 1000 L 676 970" fill="none" stroke="#3333ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 746 1000 L 696 1000" fill="none" stroke="#3333ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 766 970 L 746 1000" fill="none" stroke="#3333ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 766 970 L 826 970" fill="none" stroke="#3333ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 826 990 L 826 970" fill="none" stroke="#3333ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 856 990 L 826 990" fill="none" stroke="#3333ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 856 1020 L 856 990" fill="none" stroke="#3333ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 856 1020 L 876 1020" fill="none" stroke="#3333ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 876 1040 L 876 1020" fill="none" stroke="#3333ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 876 1040 L 936 1040" fill="none" stroke="#3333ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 936 1050 L 936 1040" fill="none" stroke="#3333ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 876 580 L 936 580" fill="none" stroke="#0000ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 936 590 L 936 580" fill="none" stroke="#0000ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 876 580 L 876 560" fill="none" stroke="#0000ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 856 560 L 876 560" fill="none" stroke="#0000ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 856 530 L 856 560" fill="none" stroke="#0000ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 826 530 L 856 530" fill="none" stroke="#0000ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 826 510 L 826 530" fill="none" stroke="#0000ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 596 510 L 826 510" fill="none" stroke="#0000ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 936 310 L 846 310" fill="none" stroke="#3333ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 586 310 L 976 310" fill="none" stroke="#ff3333" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 846 310 L 846 280" fill="none" stroke="#3333ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 846 280 L 816 280" fill="none" stroke="#3333ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 816 280 L 816 260" fill="none" stroke="#3333ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 586 260 L 816 260" fill="none" stroke="#3333ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 586 260 L 816 260" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 816 280 L 816 260" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 816 280 L 936 280" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><rect x="0" y="50" width="446" height="790" fill="#ffffff" stroke="#000000" stroke-dasharray="3 3" pointer-events="all"/><path d="M 220 30 L 220 93.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 220 98.88 L 216.5 91.88 L 220 93.63 L 223.5 91.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/><path d="M 220 220 L 220 253.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 220 258.88 L 216.5 251.88 L 220 253.63 L 223.5 251.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/><rect x="55" y="180" width="330" height="40" rx="7" ry="7" fill="#ffffff" stroke="#000000" stroke-width="2" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 328px; height: 1px; padding-top: 200px; margin-left: 56px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 14px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">Extract trajectory</div></div></div></foreignObject><text x="220" y="204" fill="#000000" font-family="Helvetica" font-size="14px" text-anchor="middle">Extract trajectory</text></switch></g><path d="M 220 300 L 220 333.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 220 338.88 L 216.5 331.88 L 220 333.63 L 223.5 331.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/><rect x="55" y="260" width="330" height="40" rx="7" ry="7" fill="#ffffff" stroke="#000000" stroke-width="2" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 328px; height: 1px; padding-top: 280px; margin-left: 56px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 14px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">Apply external velocity limit</div></div></div></foreignObject><text x="220" y="284" fill="#000000" font-family="Helvetica" font-size="14px" text-anchor="middle">Apply external velocity limit</text></switch></g><path d="M 220 460 L 220 493.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 220 498.88 L 216.5 491.88 L 220 493.63 L 223.5 491.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/><rect x="55" y="420" width="330" height="40" rx="7" ry="7" fill="#ffffff" stroke="#000000" stroke-width="2" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 328px; height: 1px; padding-top: 440px; margin-left: 56px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 14px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">Apply lateral acceleration limit</div></div></div></foreignObject><text x="220" y="444" fill="#000000" font-family="Helvetica" font-size="14px" text-anchor="middle">Apply lateral acceleration limit</text></switch></g><path d="M 220 540 L 220 573.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 220 578.88 L 216.5 571.88 L 220 573.63 L 223.5 571.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/><rect x="55" y="500" width="330" height="40" rx="7" ry="7" fill="#ffffff" stroke="#000000" stroke-width="2" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 328px; height: 1px; padding-top: 520px; margin-left: 56px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 14px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">Resample trajectory</div></div></div></foreignObject><text x="220" y="524" fill="#000000" font-family="Helvetica" font-size="14px" text-anchor="middle">Resample trajectory</text></switch></g><path d="M 220 620 L 220 653.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 220 658.88 L 216.5 651.88 L 220 653.63 L 223.5 651.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/><rect x="55" y="580" width="330" height="40" rx="7" ry="7" fill="#ffffff" stroke="#000000" stroke-width="2" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 328px; height: 1px; padding-top: 600px; margin-left: 56px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 14px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">Calculate initial state</div></div></div></foreignObject><text x="220" y="604" fill="#000000" font-family="Helvetica" font-size="14px" text-anchor="middle">Calculate initial state</text></switch></g><path d="M 220 700 L 220 733.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 220 738.88 L 216.5 731.88 L 220 733.63 L 223.5 731.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/><rect x="55" y="660" width="330" height="40" rx="7" ry="7" fill="#ffffff" stroke="#000000" stroke-width="2" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 328px; height: 1px; padding-top: 680px; margin-left: 56px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 14px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">Smooth velocity</div></div></div></foreignObject><text x="220" y="684" fill="#000000" font-family="Helvetica" font-size="14px" text-anchor="middle">Smooth velocity</text></switch></g><path d="M 220 780 L 220.03 820 L 220.38 853.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 220.44 858.88 L 216.86 851.92 L 220.38 853.63 L 223.86 851.85 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/><rect x="55" y="740" width="330" height="40" rx="7" ry="7" fill="#ffffff" stroke="#000000" stroke-width="2" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 328px; height: 1px; padding-top: 760px; margin-left: 56px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 14px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">Postprocess</div></div></div></foreignObject><text x="220" y="764" fill="#000000" font-family="Helvetica" font-size="14px" text-anchor="middle">Postprocess</text></switch></g><rect x="145" y="0" width="149" height="26" fill="none" stroke="none" pointer-events="all"/><g fill="#000000" font-family="Helvetica" clip-path="url(#mx-clip-149-5-141-26-0)" text-anchor="middle" font-size="14px"><text x="219" y="19.5">Reference trajectory</text></g><path d="M 219 700 L 219 733.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 219 738.88 L 215.5 731.88 L 219 733.63 L 222.5 731.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/><rect x="145.5" y="860" width="149" height="26" fill="none" stroke="none" pointer-events="all"/><g fill="#000000" font-family="Helvetica" clip-path="url(#mx-clip-150-865-141-26-0)" text-anchor="middle" font-size="14px"><text x="219.5" y="879.5">Output trajectory</text></g><rect x="16" y="40" width="160" height="20" fill="#ffffff" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 158px; height: 1px; padding-top: 50px; margin-left: 17px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 14px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">onCurrentTrajectory</div></div></div></foreignObject><text x="96" y="54" fill="#000000" font-family="Helvetica" font-size="14px" text-anchor="middle">onCurrentTrajectory</text></switch></g><path d="M 220 140 L 220 173.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 220 178.88 L 216.5 171.88 L 220 173.63 L 223.5 171.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/><rect x="55" y="100" width="330" height="40" rx="7" ry="7" fill="#ffffff" stroke="#000000" stroke-width="2" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 328px; height: 1px; padding-top: 120px; margin-left: 56px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 14px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">Calculate travel distance</div></div></div></foreignObject><text x="220" y="124" fill="#000000" font-family="Helvetica" font-size="14px" text-anchor="middle">Calculate travel distance</text></switch></g><path d="M 220 380 L 220 413.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 220 418.88 L 216.5 411.88 L 220 413.63 L 223.5 411.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/><rect x="55" y="340" width="330" height="40" rx="7" ry="7" fill="#ffffff" stroke="#000000" stroke-width="2" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 328px; height: 1px; padding-top: 360px; margin-left: 56px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 14px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">Apply stop approaching velocity</div></div></div></foreignObject><text x="220" y="364" fill="#000000" font-family="Helvetica" font-size="14px" text-anchor="middle">Apply stop approaching velocity</text></switch></g><rect x="716" y="50" width="104.2" height="40" fill="none" stroke="none" transform="translate(768.1,0)scale(-1,1)translate(-768.1,0)" pointer-events="all"/><path d="M 820.12 66.63 C 820.06 66.34 819.95 65.99 819.74 65.53 L 800.19 50.05 L 760.86 50.05 C 759.94 50 759.45 50.43 759.02 50.93 L 745.88 64.06 L 717.12 68.83 C 716.73 69.18 716.33 69.52 716 70.42 L 716.06 80.28 C 716.68 81.86 717.11 81.65 717.59 81.88 L 723.13 81.88 C 723.13 68.47 743.16 68.51 743.16 81.88 L 794.23 81.88 C 794.23 68.44 814.1 68.46 814.1 81.88 L 818.64 81.88 C 819.4 81.81 819.83 81.14 820.2 80.35 L 820.12 66.63 Z M 777.9 66 L 750.65 66 C 749.61 65.59 749.48 64.97 749.91 64.22 L 759.7 54.57 C 759.92 54.39 760.1 54.13 760.48 54.19 L 777.9 54.19 L 777.9 66 Z M 806.12 66 L 781.13 66 L 781.13 54.18 L 795.78 54.18 C 796.16 54.12 796.34 54.38 796.56 54.56 L 804.96 62.99 C 805.74 64.17 807.29 65.76 806.12 66 Z M 733.09 73.78 C 728.6 73.78 724.95 77.41 724.95 81.89 C 724.95 86.37 728.59 90 733.09 90 C 737.59 90 741.22 86.37 741.22 81.89 C 741.22 77.41 737.58 73.78 733.09 73.78 Z M 733.09 85.3 C 731.2 85.3 729.65 83.77 729.65 81.88 C 729.65 79.99 731.19 78.46 733.09 78.46 C 734.98 78.46 736.52 79.99 736.52 81.88 C 736.52 83.77 734.98 85.3 733.09 85.3 Z M 804.16 73.78 C 799.67 73.78 796.03 77.41 796.03 81.89 C 796.03 86.37 799.66 90 804.16 90 C 808.66 90 812.29 86.37 812.29 81.89 C 812.29 77.41 808.65 73.78 804.16 73.78 Z M 804.16 85.3 C 802.27 85.3 800.72 83.77 800.72 81.88 C 800.72 79.99 802.26 78.46 804.16 78.46 C 806.05 78.46 807.59 79.99 807.59 81.88 C 807.59 83.77 806.05 85.3 804.16 85.3 Z" fill="#000000" stroke="none" transform="translate(768.1,0)scale(-1,1)translate(-768.1,0)" pointer-events="all"/><path d="M 593 90 L 1058 90" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 733 90 L 1033 90" fill="none" stroke="#3333ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 621.24 100 L 724.76 100" fill="none" stroke="#ff3333" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 615.24 100 L 623.24 96 L 621.24 100 L 623.24 104 Z" fill="#ff3333" stroke="#ff3333" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/><path d="M 730.76 100 L 722.76 104 L 724.76 100 L 722.76 96 Z" fill="#ff3333" stroke="#ff3333" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/><path d="M 741.24 100 L 1024.76 100" fill="none" stroke="#3333ff" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 735.24 100 L 743.24 96 L 741.24 100 L 743.24 104 Z" fill="#3333ff" stroke="#3333ff" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/><path d="M 1030.76 100 L 1022.76 104 L 1024.76 100 L 1022.76 96 Z" fill="#3333ff" stroke="#3333ff" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/><rect x="613" y="110" width="120" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 118px; height: 1px; padding-top: 120px; margin-left: 614px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">extract_behind_dist</div></div></div></foreignObject><text x="673" y="124" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">extract_behind_dist</text></switch></g><rect x="820.2" y="110" width="120" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 118px; height: 1px; padding-top: 120px; margin-left: 821px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">extract_ahead_dist</div></div></div></foreignObject><text x="880" y="124" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">extract_ahead_dist</text></switch></g><path d="M 613 90 L 733 90" fill="none" stroke="#ff3333" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><rect x="526" y="0" width="100" height="20" fill="#ffffff" stroke="#000000" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 98px; height: 1px; padding-top: 10px; margin-left: 527px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">Extract trajectory</div></div></div></foreignObject><text x="576" y="14" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">Extract trajecto...</text></switch></g><rect x="526" y="170" width="160" height="20" fill="#ffffff" stroke="#000000" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 158px; height: 1px; padding-top: 180px; margin-left: 527px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">Apply external velocity limit</div></div></div></foreignObject><text x="606" y="184" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">Apply external velocity li...</text></switch></g><path d="M 586 350 L 586 243.24" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 586 237.24 L 590 245.24 L 586 243.24 L 582 245.24 Z" fill="#000000" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/><path d="M 576 340 L 977.76 340" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 983.76 340 L 975.76 344 L 977.76 340 L 975.76 336 Z" fill="#000000" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/><rect x="866" y="190" width="120" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 118px; height: 1px; padding-top: 200px; margin-left: 868px;"><div style="box-sizing: border-box; font-size: 0; text-align: left; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">default velocity limit</div></div></div></foreignObject><text x="868" y="204" fill="#000000" font-family="Helvetica" font-size="12px">default velocity lim...</text></switch></g><rect x="866" y="210" width="120" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 118px; height: 1px; padding-top: 220px; margin-left: 868px;"><div style="box-sizing: border-box; font-size: 0; text-align: left; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">external velocity limit</div></div></div></foreignObject><text x="868" y="224" fill="#000000" font-family="Helvetica" font-size="12px">external velocity li...</text></switch></g><ellipse cx="606" cy="280" rx="5" ry="5" fill="#33ff33" stroke="#000000" pointer-events="all"/><path d="M 612.37 350 L 839.63 350" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 607.12 350 L 614.12 346.5 L 612.37 350 L 614.12 353.5 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/><path d="M 844.88 350 L 837.88 353.5 L 839.63 350 L 837.88 346.5 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/><rect x="526" y="240" width="50" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 48px; height: 1px; padding-top: 250px; margin-left: 527px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">velocity</div></div></div></foreignObject><text x="551" y="254" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">velocity</text></switch></g><rect x="966" y="350" width="50" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 48px; height: 1px; padding-top: 360px; margin-left: 967px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">distance</div></div></div></foreignObject><text x="991" y="364" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">distance</text></switch></g><path d="M 776 200 L 856 199.66" fill="none" stroke="#000000" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/><path d="M 776 219.66 L 856 220" fill="none" stroke="#ff3333" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/><path d="M 776 240 L 856 240.34" fill="none" stroke="#3333ff" stroke-miterlimit="10" pointer-events="stroke"/><rect x="866" y="230" width="120" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 118px; height: 1px; padding-top: 240px; margin-left: 868px;"><div style="box-sizing: border-box; font-size: 0; text-align: left; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">applied velocity limit</div></div></div></foreignObject><text x="868" y="244" fill="#000000" font-family="Helvetica" font-size="12px">applied velocity lim...</text></switch></g><path d="M 606 285 L 606 340" fill="none" stroke="#000000" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/><path d="M 846 310 L 846 340" fill="none" stroke="#000000" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/><rect x="636" y="360" width="183" height="30" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 181px; height: 1px; padding-top: 375px; margin-left: 637px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">distance required for deceleration under jerk constraints</div></div></div></foreignObject><text x="728" y="379" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">distance required for decelera...</text></switch></g><rect x="609" y="265" width="57" height="30" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 55px; height: 1px; padding-top: 280px; margin-left: 610px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">current pose</div></div></div></foreignObject><text x="637" y="284" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">current p...</text></switch></g><rect x="526" y="425" width="160" height="20" fill="#ffffff" stroke="#000000" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 158px; height: 1px; padding-top: 435px; margin-left: 527px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">Apply stopping velocity limit</div></div></div></foreignObject><text x="606" y="439" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">Apply stopping velocity li...</text></switch></g><path d="M 936 560 L 856 560.34" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 856 560.34 L 856 530.34" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 856 530.34 L 826 530.34" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 826 530.34 L 826 510.34" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 596 510.34 L 826 510.34" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 596 600.34 L 596 493.58" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 596 487.58 L 600 495.58 L 596 493.58 L 592 495.58 Z" fill="#000000" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/><path d="M 586 590.34 L 987.76 590.34" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 993.76 590.34 L 985.76 594.34 L 987.76 590.34 L 985.76 586.34 Z" fill="#000000" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/><rect x="876" y="460.34" width="120" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 118px; height: 1px; padding-top: 470px; margin-left: 878px;"><div style="box-sizing: border-box; font-size: 0; text-align: left; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">stopping velocity</div></div></div></foreignObject><text x="878" y="474" fill="#000000" font-family="Helvetica" font-size="12px">stopping velocity</text></switch></g><ellipse cx="616" cy="530.34" rx="5" ry="5" fill="#33ff33" stroke="#000000" pointer-events="all"/><path d="M 882.37 600 L 929.63 600" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 877.12 600 L 884.12 596.5 L 882.37 600 L 884.12 603.5 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/><path d="M 934.88 600 L 927.88 603.5 L 929.63 600 L 927.88 596.5 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/><rect x="536" y="490.34" width="50" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 48px; height: 1px; padding-top: 500px; margin-left: 537px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">velocity</div></div></div></foreignObject><text x="561" y="504" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">velocity</text></switch></g><path d="M 786 450.34 L 866 450" fill="none" stroke="#000000" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/><path d="M 786 470 L 866 470.34" fill="none" stroke="#ff0000" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/><path d="M 786 490.34 L 866 490.68" fill="none" stroke="#0000ff" stroke-miterlimit="10" pointer-events="stroke"/><rect x="876" y="480.34" width="120" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 118px; height: 1px; padding-top: 490px; margin-left: 878px;"><div style="box-sizing: border-box; font-size: 0; text-align: left; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">applied velocity limit</div></div></div></foreignObject><text x="878" y="494" fill="#000000" font-family="Helvetica" font-size="12px">applied velocity lim...</text></switch></g><path d="M 876 580.34 L 876 590" fill="none" stroke="#000000" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/><rect x="876" y="610" width="60.2" height="30" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 58px; height: 1px; padding-top: 625px; margin-left: 877px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">stopping<br />distance</div></div></div></foreignObject><text x="906" y="629" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">stopping...</text></switch></g><rect x="619" y="515.34" width="57" height="30" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 55px; height: 1px; padding-top: 530px; margin-left: 620px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">current pose</div></div></div></foreignObject><text x="647" y="534" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">current p...</text></switch></g><path d="M 936 340 L 936 280" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 936 310 L 936 340" fill="none" stroke="#3333ff" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 936 590 L 936 560" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 596 580 L 996 580" fill="none" stroke="#ff0000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><rect x="876" y="440.34" width="210" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 208px; height: 1px; padding-top: 450px; margin-left: 878px;"><div style="box-sizing: border-box; font-size: 0; text-align: left; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">velocity limit with external velocity limit</div></div></div></foreignObject><text x="878" y="454" fill="#000000" font-family="Helvetica" font-size="12px">velocity limit with external veloci...</text></switch></g><path d="M 876 1039.66 L 936 1039.66" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 936 1049.66 L 936 1039.66" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 876 1039.66 L 876 1019.66" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 856 1019.66 L 876 1019.66" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 856 989.66 L 856 1019.66" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 826 989.66 L 856 989.66" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 826 969.66 L 826 989.66" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 596 969.66 L 826 969.66" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><rect x="526" y="680" width="180" height="20" fill="#ffffff" stroke="#000000" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 178px; height: 1px; padding-top: 690px; margin-left: 527px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">Apply lateral acceleration limit</div></div></div></foreignObject><text x="616" y="694" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">Apply lateral acceleration lim...</text></switch></g><path d="M 596 1060 L 596 953.24" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 596 947.24 L 600 955.24 L 596 953.24 L 592 955.24 Z" fill="#000000" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/><path d="M 586 1050 L 987.76 1050" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 993.76 1050 L 985.76 1054 L 987.76 1050 L 985.76 1046 Z" fill="#000000" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/><ellipse cx="616" cy="990" rx="5" ry="5" fill="#33ff33" stroke="#000000" pointer-events="all"/><rect x="536" y="950" width="50" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 48px; height: 1px; padding-top: 960px; margin-left: 537px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">velocity</div></div></div></foreignObject><text x="561" y="964" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">velocity</text></switch></g><path d="M 786 929.92 L 866 929.58" fill="none" stroke="#000000" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/><path d="M 786 950 L 866 950.34" fill="none" stroke="#0000ff" stroke-miterlimit="10" pointer-events="stroke"/><rect x="876" y="940" width="120" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 118px; height: 1px; padding-top: 950px; margin-left: 878px;"><div style="box-sizing: border-box; font-size: 0; text-align: left; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">applied velocity limit</div></div></div></foreignObject><text x="878" y="954" fill="#000000" font-family="Helvetica" font-size="12px">applied velocity lim...</text></switch></g><rect x="619" y="975" width="57" height="30" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 55px; height: 1px; padding-top: 990px; margin-left: 620px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">current pose</div></div></div></foreignObject><text x="647" y="994" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">current p...</text></switch></g><rect x="876" y="920" width="210" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 208px; height: 1px; padding-top: 930px; margin-left: 878px;"><div style="box-sizing: border-box; font-size: 0; text-align: left; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">velocity limit with stopping velocity</div></div></div></foreignObject><text x="878" y="934" fill="#000000" font-family="Helvetica" font-size="12px">velocity limit with stopping veloci...</text></switch></g><path d="M 596 800 L 666 800" fill="none" stroke="#000000" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 666 800 Q 726 800 766 780" fill="none" stroke="#000000" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 766 780 L 936 700" fill="none" stroke="#000000" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><rect x="611" y="790" width="30" height="20" fill="#ffffff" stroke="#000000" pointer-events="all"/><path d="M 611 790 L 626 800 L 611 810 Z" fill="#ffffff" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/><rect x="586" y="760" width="57" height="30" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 55px; height: 1px; padding-top: 775px; margin-left: 587px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">ego car</div></div></div></foreignObject><text x="614" y="779" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">ego car</text></switch></g><path d="M 596 900 L 596 828.24" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 596 822.24 L 600 830.24 L 596 828.24 L 592 830.24 Z" fill="#000000" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/><path d="M 586 890 L 987.76 890" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 993.76 890 L 985.76 894 L 987.76 890 L 985.76 886 Z" fill="#000000" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/><path d="M 596 890 L 676 890" fill="none" stroke="#ff8000" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 676 890 L 695.9 850" fill="none" stroke="#ff8000" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 746 850 L 696 850" fill="none" stroke="#ff8000" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 746 850 L 766 890" fill="none" stroke="#ff8000" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 766 890 L 936 890" fill="none" stroke="#ff8000" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><rect x="966" y="600" width="50" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 48px; height: 1px; padding-top: 610px; margin-left: 967px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">distance</div></div></div></foreignObject><text x="991" y="614" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">distance</text></switch></g><rect x="966" y="900" width="50" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 48px; height: 1px; padding-top: 910px; margin-left: 967px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">distance</div></div></div></foreignObject><text x="991" y="914" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">distance</text></switch></g><rect x="536" y="810" width="50" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 48px; height: 1px; padding-top: 820px; margin-left: 537px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">curvature</div></div></div></foreignObject><text x="561" y="824" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">curvature</text></switch></g><rect x="966" y="1060" width="50" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 48px; height: 1px; padding-top: 1070px; margin-left: 967px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">distance</div></div></div></foreignObject><text x="991" y="1074" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">distance</text></switch></g><rect x="526" y="1120" width="110" height="20" fill="#ffffff" stroke="#000000" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 108px; height: 1px; padding-top: 1130px; margin-left: 527px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">Resample trajectory</div></div></div></foreignObject><text x="581" y="1134" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">Resample trajectory</text></switch></g><path d="M 676 890 L 676 800" fill="none" stroke="#000000" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/><path d="M 766 890 L 766 780" fill="none" stroke="#000000" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/><rect x="846" y="690" width="60.2" height="30" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 58px; height: 1px; padding-top: 705px; margin-left: 847px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">trajectory</div></div></div></foreignObject><text x="876" y="709" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">trajectory</text></switch></g><path d="M 596 1190.42 L 676 1190.42" fill="none" stroke="#000000" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 696 1220.42 L 676 1190.42" fill="none" stroke="#000000" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 746 1220.42 L 696 1220.42" fill="none" stroke="#000000" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 766 1190.42 L 746 1220.42" fill="none" stroke="#000000" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 766 1190.42 L 826 1190.42" fill="none" stroke="#000000" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 826 1210.42 L 826 1190.42" fill="none" stroke="#000000" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 856 1210.42 L 826 1210.42" fill="none" stroke="#000000" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 856 1240.42 L 856 1210.42" fill="none" stroke="#000000" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 856 1240.42 L 876 1240.42" fill="none" stroke="#000000" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 876 1260.42 L 876 1240.42" fill="none" stroke="#000000" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 876 1260.42 L 936 1260.42" fill="none" stroke="#000000" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 936 1270.42 L 936 1260.42" fill="none" stroke="#000000" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 596 1280.42 L 596 1173.66" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 596 1167.66 L 600 1175.66 L 596 1173.66 L 592 1175.66 Z" fill="#000000" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/><path d="M 586 1270.42 L 987.76 1270.42" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 993.76 1270.42 L 985.76 1274.42 L 987.76 1270.42 L 985.76 1266.42 Z" fill="#000000" stroke="#000000" stroke-width="2" stroke-miterlimit="10" pointer-events="all"/><ellipse cx="616" cy="1210.42" rx="5" ry="5" fill="#33ff33" stroke="#000000" pointer-events="all"/><rect x="536" y="1170.42" width="50" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 48px; height: 1px; padding-top: 1180px; margin-left: 537px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">velocity</div></div></div></foreignObject><text x="561" y="1184" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">velocity</text></switch></g><rect x="619" y="1195.42" width="57" height="30" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 55px; height: 1px; padding-top: 1210px; margin-left: 620px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">current pose</div></div></div></foreignObject><text x="647" y="1214" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">current p...</text></switch></g><rect x="966" y="1280.42" width="50" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 48px; height: 1px; padding-top: 1290px; margin-left: 967px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">distance</div></div></div></foreignObject><text x="991" y="1294" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">distance</text></switch></g><ellipse cx="616" cy="1190" rx="3" ry="3" fill="#3333ff" stroke="#000000" pointer-events="all"/><ellipse cx="626" cy="1190" rx="3" ry="3" fill="#3333ff" stroke="#000000" pointer-events="all"/><ellipse cx="636" cy="1190" rx="3" ry="3" fill="#3333ff" stroke="#000000" pointer-events="all"/><ellipse cx="596" cy="1190" rx="3" ry="3" fill="#ff3333" stroke="#000000" pointer-events="all"/><ellipse cx="646" cy="1190" rx="3" ry="3" fill="#3333ff" stroke="#000000" pointer-events="all"/><ellipse cx="656" cy="1190" rx="3" ry="3" fill="#3333ff" stroke="#000000" pointer-events="all"/><ellipse cx="666" cy="1190" rx="3" ry="3" fill="#3333ff" stroke="#000000" pointer-events="all"/><ellipse cx="676" cy="1190" rx="3" ry="3" fill="#3333ff" stroke="#000000" pointer-events="all"/><ellipse cx="686" cy="1205" rx="3" ry="3" fill="#3333ff" stroke="#000000" pointer-events="all"/><ellipse cx="696" cy="1220" rx="3" ry="3" fill="#3333ff" stroke="#000000" pointer-events="all"/><ellipse cx="736" cy="1220" rx="3" ry="3" fill="#3333ff" stroke="#000000" pointer-events="all"/><ellipse cx="776" cy="1190" rx="3" ry="3" fill="#ff3333" stroke="#000000" pointer-events="all"/><ellipse cx="816" cy="1190" rx="3" ry="3" fill="#ff3333" stroke="#000000" pointer-events="all"/><ellipse cx="856" cy="1240" rx="3" ry="3" fill="#ff3333" stroke="#000000" pointer-events="all"/><ellipse cx="896" cy="1260" rx="3" ry="3" fill="#ff3333" stroke="#000000" pointer-events="all"/><ellipse cx="936" cy="1270" rx="3" ry="3" fill="#ff3333" stroke="#000000" pointer-events="all"/><path d="M 736 1223 L 736 1270" fill="none" stroke="#000000" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/><rect x="593" y="1280" width="173" height="26.58" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 171px; height: 1px; padding-top: 1293px; margin-left: 594px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">resample time * current velocity</div></div></div></foreignObject><text x="680" y="1297" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">resample time * current veloc...</text></switch></g><ellipse cx="856" cy="1150" rx="3" ry="3" fill="#3333ff" stroke="#000000" pointer-events="all"/><ellipse cx="856" cy="1170" rx="3" ry="3" fill="#ff3333" stroke="#000000" pointer-events="all"/><rect x="866" y="1140" width="100" height="17" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 98px; height: 1px; padding-top: 1149px; margin-left: 868px;"><div style="box-sizing: border-box; font-size: 0; text-align: left; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">dense sampling</div></div></div></foreignObject><text x="868" y="1152" fill="#000000" font-family="Helvetica" font-size="12px">dense sampling</text></switch></g><rect x="866" y="1160" width="100" height="17" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 98px; height: 1px; padding-top: 1169px; margin-left: 868px;"><div style="box-sizing: border-box; font-size: 0; text-align: left; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">sparse sampling</div></div></div></foreignObject><text x="868" y="1172" fill="#000000" font-family="Helvetica" font-size="12px">sparse sampling</text></switch></g><ellipse cx="706" cy="1220" rx="3" ry="3" fill="#3333ff" stroke="#000000" pointer-events="all"/><ellipse cx="716" cy="1220" rx="3" ry="3" fill="#3333ff" stroke="#000000" pointer-events="all"/><ellipse cx="726" cy="1220" rx="3" ry="3" fill="#3333ff" stroke="#000000" pointer-events="all"/><path d="M 622.37 1280 L 729.63 1280" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="stroke"/><path d="M 617.12 1280 L 624.12 1276.5 L 622.37 1280 L 624.12 1283.5 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/><path d="M 734.88 1280 L 727.88 1283.5 L 729.63 1280 L 727.88 1276.5 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="all"/><rect x="526" y="1330" width="100" height="20" fill="#ffffff" stroke="#000000" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 98px; height: 1px; padding-top: 1340px; margin-left: 527px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">Smooth velocity</div></div></div></foreignObject><text x="576" y="1344" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">Smooth velocity</text></switch></g><path d="M 596 1400.42 L 676 1400.42" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 696 1430.42 L 676 1400.42" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 766 1400.42 L 746 1430.42" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 766 1400.42 L 826 1400.42" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 826 1420.42 L 826 1400.42" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 856 1420.42 L 826 1420.42" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 856 1450.42 L 856 1420.42" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 856 1450.42 L 876 1450.42" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 876 1470.42 L 876 1450.42" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 876 1470.42 L 936 1470.42" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><path d="M 936 1480.42 L 936 1470.42" fill="none" stroke="#000000" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="6 6" pointer-events="stroke"/><ellipse cx="616" cy="1420.42" rx="5" ry="5" fill="#33ff33" stroke="#000000" pointer-events="all"/><rect x="536" y="1380.42" width="50" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 48px; height: 1px; padding-top: 1390px; margin-left: 537px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">velocity</div></div></div></foreignObject><text x="561" y="1394" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">velocity</text></switch></g><path d="M 786 1370.34 L 866 1370" fill="none" stroke="#000000" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/><path d="M 786 1390.42 L 866 1390.76" fill="none" stroke="#0000ff" stroke-miterlimit="10" pointer-events="stroke"/><rect x="876" y="1380.42" width="120" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 118px; height: 1px; padding-top: 1390px; margin-left: 878px;"><div style="box-sizing: border-box; font-size: 0; text-align: left; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">smoothed velocity</div></div></div></foreignObject><text x="878" y="1394" fill="#000000" font-family="Helvetica" font-size="12px">smoothed velocity</text></switch></g><rect x="876" y="1360.42" width="210" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 208px; height: 1px; padding-top: 1370px; margin-left: 878px;"><div style="box-sizing: border-box; font-size: 0; text-align: left; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">velocity limit</div></div></div></foreignObject><text x="878" y="1374" fill="#000000" font-family="Helvetica" font-size="12px">velocity limit</text></switch></g><rect x="966" y="1490.42" width="50" height="20" fill="none" stroke="none" pointer-events="all"/><g transform="translate(-0.5 -0.5)"><switch><foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 48px; height: 1px; padding-top: 1500px; margin-left: 967px;"><div style="box-sizing: border-box; font-size: 0; text-align: center; "><div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">distance</div></div></div></foreignObject><text x="991" y="1504" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">distance</text></switch></g></g><switch><g requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"/><a transform="translate(0,-5)" xlink:href="https://www.diagrams.net/doc/faq/svg-export-text-problems" target="_blank"><text text-anchor="middle" font-size="10px" x="50%" y="100%">Viewer does not support full SVG 1.1</text></a></switch></svg>
\ No newline at end of file
diff --git a/planning/motion_velocity_smoother/package.xml b/planning/motion_velocity_smoother/package.xml
new file mode 100644
index 0000000000000..43e958c205a37
--- /dev/null
+++ b/planning/motion_velocity_smoother/package.xml
@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>motion_velocity_smoother</name>
+  <version>0.1.0</version>
+  <description>The motion_velocity_smoother package</description>
+
+  <maintainer email="horibe.takamasa@gmail.com">Takamasa Horibe</maintainer>
+  <maintainer email="fumiya.watanabe@tier4.jp">Fumiya Watanabe</maintainer>
+  <maintainer email="yutaka.shimizu@tier4.jp">Yutaka Shimizu</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
+
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_debug_msgs</depend>
+  <depend>autoware_planning_msgs</depend>
+  <depend>autoware_utils</depend>
+  <depend>geometry_msgs</depend>
+  <depend>interpolation</depend>
+  <depend>libboost-dev</depend>
+  <depend>nav_msgs</depend>
+  <depend>osqp_interface</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/planning/motion_velocity_smoother/scripts/closest_velocity_checker.py b/planning/motion_velocity_smoother/scripts/closest_velocity_checker.py
new file mode 100755
index 0000000000000..fa904db0b22f9
--- /dev/null
+++ b/planning/motion_velocity_smoother/scripts/closest_velocity_checker.py
@@ -0,0 +1,332 @@
+#!/usr/bin/env python3
+
+# Copyright 2020 Tier IV, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import time
+
+from autoware_auto_control_msgs.msg import AckermannControlCommand
+from autoware_auto_planning_msgs.msg import Path
+from autoware_auto_planning_msgs.msg import PathWithLaneId
+from autoware_auto_planning_msgs.msg import Trajectory
+from autoware_auto_vehicle_msgs.msg import Engage
+from autoware_auto_vehicle_msgs.msg import VelocityReport
+from autoware_debug_msgs.msg import Float32MultiArrayStamped
+from autoware_planning_msgs.msg import VelocityLimit
+from geometry_msgs.msg import Pose
+from nav_msgs.msg import Odometry
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from tf2_ros import LookupException
+from tf2_ros.buffer import Buffer
+from tf2_ros.transform_listener import TransformListener
+
+REF_LINK = "map"
+SELF_LINK = "base_link"
+
+LANE_CHANGE = 0
+BEHAVIOR_VELOCITY = 1
+OBSTACLE_AVOID = 2
+OBSTACLE_STOP = 3
+LAT_ACC = 4
+VELOCITY_OPTIMIZE = 5
+CONTROL_CMD = 6
+VEHICLE_CMD = 7
+CONTROL_CMD_ACC = 8
+VEHICLE_CMD_ACC = 9
+DATA_NUM = 10
+
+
+class VelocityChecker(Node):
+    def __init__(self):
+        super().__init__("velocity_checker")
+
+        self.autoware_engage = None
+        self.vehicle_engage = None
+        self.external_v_lim = np.nan
+        self.localization_twist_vx = np.nan
+        self.vehicle_twist_vx = np.nan
+        self.self_pose = Pose()
+        self.data_arr = [np.nan] * DATA_NUM
+        self.count = 0
+
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+
+        # planning path and trajectories
+        profile = rclpy.qos.QoSProfile(depth=1)
+        lane_drv = "/planning/scenario_planning/lane_driving"
+        scenario = "/planning/scenario_planning"
+        self.sub0 = self.create_subscription(
+            PathWithLaneId,
+            lane_drv + "/behavior_planning/path_with_lane_id",
+            self.CallBackBehaviorPathWLid,
+            1,
+        )
+        self.sub1 = self.create_subscription(
+            Path, lane_drv + "/behavior_planning/path", self.CallBackBehaviorPath, 1
+        )
+        self.sub2 = self.create_subscription(
+            Trajectory,
+            lane_drv + "/motion_planning/obstacle_avoidance_planner/trajectory",
+            self.CallBackAvoidTrajectory,
+            1,
+        )
+        self.sub3 = self.create_subscription(
+            Trajectory, lane_drv + "/trajectory", self.CallBackLaneDriveTrajectory, 1
+        )
+        self.sub4 = self.create_subscription(
+            Trajectory,
+            scenario + "/motion_velocity_smoother/debug/trajectory_lateral_acc_filtered",
+            # TODO: change to following string after fixing bug of autoware
+            # '/motion_velocity_optimizer/debug/trajectory_lateral_acc_filtered',
+            self.CallBackLataccTrajectory,
+            1,
+        )
+        self.sub5 = self.create_subscription(
+            Trajectory, scenario + "/trajectory", self.CallBackScenarioTrajectory, 1
+        )
+
+        # control commands
+        self.sub6 = self.create_subscription(
+            AckermannControlCommand,
+            "/control/trajectory_follower/control_cmd",
+            self.CallBackControlCmd,
+            1,
+        )
+        self.sub7 = self.create_subscription(
+            AckermannControlCommand, "/control/command/control_cmd", self.CallBackVehicleCmd, 1
+        )
+
+        # others related to velocity
+        self.sub8 = self.create_subscription(
+            Engage, "/autoware/engage", self.CallBackAwEngage, profile
+        )
+        self.sub12 = self.create_subscription(
+            Engage, "/vehicle/engage", self.CallBackVehicleEngage, profile
+        )
+        self.sub9 = self.create_subscription(
+            VelocityLimit,
+            "/planning/scenario_planning/current_max_velocity",
+            self.CallBackExternalVelLim,
+            profile,
+        )
+
+        # self twist
+        self.sub10 = self.create_subscription(
+            Odometry, "/localization/kinematic_state", self.CallBackLocalizationTwist, 1
+        )
+        self.sub11 = self.create_subscription(
+            VelocityReport, "/vehicle/status/velocity_status", self.CallBackVehicleTwist, 1
+        )
+
+        # publish data
+        self.pub_v_arr = self.create_publisher(Float32MultiArrayStamped, "closest_speeds", 1)
+
+        time.sleep(1.0)  # wait for ready to publish/subscribe
+
+        # for publish traffic signal image
+        self.create_timer(0.1, self.timerCallback)
+
+    def printInfo(self):
+        self.count = self.count % 30
+        if self.count == 0:
+            self.get_logger().info("")
+            self.get_logger().info(
+                "| Map Limit | Behavior | Obs Avoid | Obs Stop | External Lim | LatAcc Filtered "
+                "| Optimized | Control VelCmd | Control AccCmd | Vehicle VelCmd | Vehicle AccCmd "
+                "| AW Engage | VC Engage | Localization Vel | Vehicle Vel | [km/h]"
+            )  # noqa: E501
+        mps2kmph = 3.6
+        vel_map_lim = self.data_arr[LANE_CHANGE] * mps2kmph
+        vel_behavior = self.data_arr[BEHAVIOR_VELOCITY] * mps2kmph
+        vel_obs_avoid = self.data_arr[OBSTACLE_AVOID] * mps2kmph
+        vel_obs_stop = self.data_arr[OBSTACLE_STOP] * mps2kmph
+        vel_external_lim = self.external_v_lim * mps2kmph
+        vel_latacc_filtered = self.data_arr[LAT_ACC] * mps2kmph
+        vel_optimized = self.data_arr[VELOCITY_OPTIMIZE] * mps2kmph
+        vel_ctrl_cmd = self.data_arr[CONTROL_CMD] * mps2kmph
+        acc_ctrl_cmd = self.data_arr[CONTROL_CMD_ACC]
+        vel_vehicle_cmd = self.data_arr[VEHICLE_CMD] * mps2kmph
+        acc_vehicle_cmd = self.data_arr[VEHICLE_CMD_ACC]
+        vel_localization = self.localization_twist_vx * mps2kmph
+        vel_vehicle = self.vehicle_twist_vx * mps2kmph
+        engage = (
+            "None"
+            if self.autoware_engage is None
+            else ("True" if self.autoware_engage is True else "False")
+        )
+        vehicle_engage = (
+            "None"
+            if self.vehicle_engage is None
+            else ("True" if self.vehicle_engage is True else "False")
+        )
+        self.get_logger().info(
+            "| {0: 9.2f} | {1: 8.2f} | {2: 9.2f} | {3: 8.2f} | {4: 12.2f} "
+            "| {5: 15.2f} | {6: 9.2f} | {7: 14.2f} | {8: 14.2f} | {9: 14.2f} | {10: 14.2f} "
+            "| {11:>9s} | {12:>9s} | {13: 16.2f} | {14: 11.2f} |".format(  # noqa: E501
+                vel_map_lim,
+                vel_behavior,
+                vel_obs_avoid,
+                vel_obs_stop,
+                vel_external_lim,
+                vel_latacc_filtered,
+                vel_optimized,
+                vel_ctrl_cmd,
+                acc_ctrl_cmd,
+                vel_vehicle_cmd,
+                acc_vehicle_cmd,
+                engage,
+                vehicle_engage,
+                vel_localization,
+                vel_vehicle,
+            )
+        )
+        self.count += 1
+
+    def timerCallback(self):
+        # self.get_logger().info('timer called')
+        self.updatePose(REF_LINK, SELF_LINK)
+        self.pub_v_arr.publish(Float32MultiArrayStamped(data=self.data_arr))
+        self.printInfo()
+
+    def CallBackAwEngage(self, msg):
+        self.autoware_engage = msg.engage
+
+    def CallBackVehicleEngage(self, msg):
+        self.vehicle_engage = msg.engage
+
+    def CallBackExternalVelLim(self, msg):
+        self.external_v_lim = msg.max_velocity
+
+    def CallBackLocalizationTwist(self, msg):
+        self.localization_twist_vx = msg.twist.twist.linear.x
+
+    def CallBackVehicleTwist(self, msg):
+        self.vehicle_twist_vx = msg.longitudinal_velocity
+
+    def CallBackBehaviorPathWLid(self, msg):
+        # self.get_logger().info('LANE_CHANGE called')
+        closest = self.calcClosestPathWLid(msg)
+        self.data_arr[LANE_CHANGE] = msg.points[closest].point.longitudinal_velocity_mps
+        return
+
+    def CallBackBehaviorPath(self, msg):
+        # self.get_logger().info('BEHAVIOR_VELOCITY called')
+        closest = self.calcClosestPath(msg)
+        self.data_arr[BEHAVIOR_VELOCITY] = msg.points[closest].longitudinal_velocity_mps
+        return
+
+    def CallBackAvoidTrajectory(self, msg):
+        # self.get_logger().info('OBSTACLE_AVOID called')
+        closest = self.calcClosestTrajectory(msg)
+        self.data_arr[OBSTACLE_AVOID] = msg.points[closest].longitudinal_velocity_mps
+        return
+
+    def CallBackLaneDriveTrajectory(self, msg):
+        # self.get_logger().info('OBSTACLE_STOP called')
+        closest = self.calcClosestTrajectory(msg)
+        self.data_arr[OBSTACLE_STOP] = msg.points[closest].longitudinal_velocity_mps
+        return
+
+    def CallBackLataccTrajectory(self, msg):
+        # self.get_logger().info('LAT_ACC called')
+        closest = self.calcClosestTrajectory(msg)
+        self.data_arr[LAT_ACC] = msg.points[closest].longitudinal_velocity_mps
+        return
+
+    def CallBackScenarioTrajectory(self, msg):
+        # self.get_logger().info('VELOCITY_OPTIMIZE called')
+        closest = self.calcClosestTrajectory(msg)
+        self.data_arr[VELOCITY_OPTIMIZE] = msg.points[closest].longitudinal_velocity_mps
+        return
+
+    def CallBackControlCmd(self, msg):
+        # self.get_logger().info('CONTROL_CMD called')
+        self.data_arr[CONTROL_CMD] = msg.longitudinal.speed
+        self.data_arr[CONTROL_CMD_ACC] = msg.longitudinal.acceleration
+        return
+
+    def CallBackVehicleCmd(self, msg):
+        # self.get_logger().info('VEHICLE_CMD called')
+        self.data_arr[VEHICLE_CMD] = msg.longitudinal.speed
+        self.data_arr[VEHICLE_CMD_ACC] = msg.longitudinal.acceleration
+        return
+
+    def calcClosestPath(self, path):
+        closest = -1
+        min_dist_squared = 1.0e10
+        for i in range(0, len(path.points)):
+            dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].pose)
+            if dist_sq < min_dist_squared:
+                min_dist_squared = dist_sq
+                closest = i
+        return closest
+
+    def calcClosestPathWLid(self, path):
+        closest = -1
+        min_dist_squared = 1.0e10
+        for i in range(0, len(path.points)):
+            dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].point.pose)
+            if dist_sq < min_dist_squared:
+                min_dist_squared = dist_sq
+                closest = i
+        return closest
+
+    def calcClosestTrajectory(self, path):
+        closest = -1
+        min_dist_squared = 1.0e10
+        for i in range(0, len(path.points)):
+            dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].pose)
+            if dist_sq < min_dist_squared:
+                min_dist_squared = dist_sq
+                closest = i
+        return closest
+
+    def calcSquaredDist2d(self, p1, p2):
+        dx = p1.position.x - p2.position.x
+        dy = p1.position.y - p2.position.y
+        return dx * dx + dy * dy
+
+    def updatePose(self, from_link, to_link):
+        try:
+            tf = self.tf_buffer.lookup_transform(from_link, to_link, rclpy.time.Time())
+            self.self_pose.position.x = tf.transform.translation.x
+            self.self_pose.position.y = tf.transform.translation.y
+            self.self_pose.position.z = tf.transform.translation.z
+            self.self_pose.orientation.x = tf.transform.rotation.x
+            self.self_pose.orientation.y = tf.transform.rotation.y
+            self.self_pose.orientation.z = tf.transform.rotation.z
+            self.self_pose.orientation.w = tf.transform.rotation.w
+            return
+        except LookupException as e:
+            self.get_logger().warn("No required transformation found: `{}`".format(str(e)))
+            return
+
+
+def main(args=None):
+    try:
+        rclpy.init(args=args)
+        node = VelocityChecker()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/planning/motion_velocity_smoother/scripts/trajectory_visualizer.py b/planning/motion_velocity_smoother/scripts/trajectory_visualizer.py
new file mode 100755
index 0000000000000..cdddaf483c196
--- /dev/null
+++ b/planning/motion_velocity_smoother/scripts/trajectory_visualizer.py
@@ -0,0 +1,656 @@
+#!/usr/bin/env python3
+
+# Copyright 2020 Tier IV, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import argparse
+
+from autoware_auto_planning_msgs.msg import Path
+from autoware_auto_planning_msgs.msg import PathWithLaneId
+from autoware_auto_planning_msgs.msg import Trajectory
+from autoware_auto_vehicle_msgs.msg import VelocityReport
+from geometry_msgs.msg import Pose
+from matplotlib import animation
+import matplotlib.pyplot as plt
+import message_filters
+from nav_msgs.msg import Odometry
+import numpy as np
+import rclpy
+from rclpy.node import Node
+import tf2_ros
+from tf2_ros.buffer import Buffer
+from tf2_ros.transform_listener import TransformListener
+
+parser = argparse.ArgumentParser()
+parser.add_argument("-l", "--length", help="max arclength in plot")
+parser.add_argument(
+    "-t",
+    "--type",
+    help="Options  VEL(default): show velocity only, VEL_ACC_JERK: show vel & acc & jerk",
+)
+
+args = parser.parse_args()
+
+if args.length is None:
+    PLOT_MAX_ARCLENGTH = 200
+else:
+    PLOT_MAX_ARCLENGTH = int(args.length)
+print("max arclength = " + str(PLOT_MAX_ARCLENGTH))
+
+if args.type is None:
+    PLOT_TYPE = "VEL"
+elif args.type == "VEL":
+    PLOT_TYPE = "VEL"
+elif args.type == "VEL_ACC_JERK":
+    PLOT_TYPE = "VEL_ACC_JERK"
+else:
+    print("invalid type. set default VEL.")
+    PLOT_TYPE = "VEL"
+print("plot type = " + PLOT_TYPE)
+
+PATH_ORIGIN_FRAME = "map"
+SELF_POSE_FRAME = "base_link"
+
+
+class TrajectoryVisualizer(Node):
+    def __init__(self):
+
+        super().__init__("trajectory_visualizer")
+
+        self.fig = plt.figure()
+
+        self.max_vel = 0.0
+        self.min_vel = 0.0
+        self.min_accel = 0.0
+        self.max_accel = 0.0
+        self.min_jerk = 0.0
+        self.max_jerk = 0.0
+
+        # update flag
+        self.update_ex_vel_lim = False
+        self.update_lat_acc_fil = False
+        self.update_traj_raw = False
+        self.update_traj_resample = False
+        self.update_traj_final = False
+        self.update_lanechange_path = False
+        self.update_behavior_path = False
+        self.update_traj_ob_avoid = False
+        self.update_traj_ob_stop = False
+
+        self.tf_buffer = Buffer(node=self)
+        self.tf_listener = TransformListener(self.tf_buffer, self, spin_thread=True)
+
+        self.self_pose = Pose()
+        self.self_pose_received = False
+        self.localization_vx = 0.0
+        self.vehicle_vx = 0.0
+
+        self.trajectory_external_velocity_limited = Trajectory()
+        self.trajectory_lateral_acc_filtered = Trajectory()
+        self.trajectory_raw = Trajectory()
+        self.trajectory_time_resampled = Trajectory()
+        self.trajectory_final = Trajectory()
+
+        self.lane_change_path = PathWithLaneId()
+        self.behavior_path = Path()
+        self.obstacle_avoid_traj = Trajectory()
+        self.obstacle_stop_traj = Trajectory()
+
+        self.plotted = [False] * 9
+        self.sub_localization_twist = self.create_subscription(
+            Odometry, "/localization/kinematic_state", self.CallbackLocalizationTwist, 1
+        )
+        self.sub_vehicle_twist = self.create_subscription(
+            VelocityReport, "/vehicle/status/velocity_status", self.CallbackVehicleTwist, 1
+        )
+
+        # BUFFER_SIZE = 65536*100
+        optimizer_debug = "/planning/scenario_planning/motion_velocity_smoother/debug/"
+        self.sub1 = message_filters.Subscriber(
+            self, Trajectory, optimizer_debug + "trajectory_external_velocity_limited"
+        )
+        self.sub2 = message_filters.Subscriber(
+            self, Trajectory, optimizer_debug + "trajectory_lateral_acc_filtered"
+        )
+        self.sub3 = message_filters.Subscriber(self, Trajectory, optimizer_debug + "trajectory_raw")
+        self.sub4 = message_filters.Subscriber(
+            self, Trajectory, optimizer_debug + "trajectory_time_resampled"
+        )
+        self.sub5 = message_filters.Subscriber(
+            self, Trajectory, "/planning/scenario_planning/trajectory"
+        )
+
+        lane_driving = "/planning/scenario_planning/lane_driving"
+        self.sub6 = message_filters.Subscriber(
+            self, PathWithLaneId, lane_driving + "/behavior_planning/path_with_lane_id"
+        )
+        self.sub7 = message_filters.Subscriber(self, Path, lane_driving + "/behavior_planning/path")
+        self.sub8 = message_filters.Subscriber(
+            self,
+            Trajectory,
+            lane_driving + "/motion_planning/obstacle_avoidance_planner/trajectory",
+        )
+        self.sub9 = message_filters.Subscriber(self, Trajectory, lane_driving + "/trajectory")
+
+        self.ts1 = message_filters.ApproximateTimeSynchronizer(
+            [self.sub1, self.sub2, self.sub3, self.sub4, self.sub5], 30, 0.5
+        )
+        self.ts1.registerCallback(self.CallbackMotionVelOptTraj)
+
+        self.ts2 = message_filters.ApproximateTimeSynchronizer(
+            [self.sub6, self.sub7, self.sub8, self.sub9], 30, 1, 0
+        )
+        self.ts2.registerCallback(self.CallBackLaneDrivingTraj)
+
+        # main process
+        if PLOT_TYPE == "VEL_ACC_JERK":
+            self.ani = animation.FuncAnimation(
+                self.fig, self.plotTrajectory, interval=100, blit=True
+            )
+            self.setPlotTrajectory()
+        else:
+            self.ani = animation.FuncAnimation(
+                self.fig, self.plotTrajectoryVelocity, interval=100, blit=True
+            )
+            self.setPlotTrajectoryVelocity()
+
+        plt.show()
+
+        return
+
+    def test(self):
+        self.updatePose("map", "base_link")
+
+    def CallbackLocalizationTwist(self, cmd):
+        self.localization_vx = cmd.twist.twist.linear.x
+
+    def CallbackVehicleTwist(self, cmd):
+        self.vehicle_vx = cmd.longitudinal_velocity
+
+    def CallbackMotionVelOptTraj(self, cmd1, cmd2, cmd3, cmd4, cmd5):
+        print("CallbackMotionVelOptTraj called")
+        self.CallBackTrajExVelLim(cmd1)
+        self.CallBackTrajLatAccFiltered(cmd2)
+        self.CallBackTrajRaw(cmd3)
+        self.CallBackTrajTimeResampled(cmd4)
+        self.CallBackTrajFinal(cmd5)
+
+    def CallBackTrajExVelLim(self, cmd):
+        self.trajectory_external_velocity_limited = cmd
+        self.update_ex_vel_lim = True
+
+    def CallBackTrajLatAccFiltered(self, cmd):
+        self.trajectory_lateral_acc_filtered = cmd
+        self.update_lat_acc_fil = True
+
+    def CallBackTrajRaw(self, cmd):
+        self.trajectory_raw = cmd
+        self.update_traj_raw = True
+
+    def CallBackTrajTimeResampled(self, cmd):
+        self.trajectory_time_resampled = cmd
+        self.update_traj_resample = True
+
+    def CallBackTrajFinal(self, cmd):
+        self.trajectory_final = cmd
+        self.update_traj_final = True
+
+    def CallBackLaneDrivingTraj(self, cmd6, cmd7, cmd8, cmd9):
+        print("CallBackLaneDrivingTraj called")
+        self.CallBackLaneChangePath(cmd6)
+        self.CallBackBehaviorPath(cmd7)
+        self.CallbackObstacleAvoidTraj(cmd8)
+        self.CallbackObstacleStopTraj(cmd9)
+
+    def CallBackLaneChangePath(self, cmd):
+        self.lane_change_path = cmd
+        self.update_lanechange_path = True
+
+    def CallBackBehaviorPath(self, cmd):
+        self.behavior_path = cmd
+        self.update_behavior_path = True
+
+    def CallbackObstacleAvoidTraj(self, cmd):
+        self.obstacle_avoid_traj = cmd
+        self.update_traj_ob_avoid = True
+
+    def CallbackObstacleStopTraj(self, cmd):
+        self.obstacle_stop_traj = cmd
+        self.update_traj_ob_stop = True
+
+    def setPlotTrajectoryVelocity(self):
+        self.ax1 = plt.subplot(1, 1, 1)  # row, col, index(<raw*col)
+        (self.im1,) = self.ax1.plot([], [], label="0: lane_change_path", marker="")
+        (self.im2,) = self.ax1.plot([], [], label="1: behavior_path", marker="", ls="--")
+        (self.im3,) = self.ax1.plot([], [], label="2: obstacle_avoid_traj", marker="", ls="-.")
+        (self.im4,) = self.ax1.plot([], [], label="3: obstacle_stop_traj", marker="", ls="--")
+        (self.im5,) = self.ax1.plot([], [], label="4-1: opt input", marker="", ls="--")
+        (self.im6,) = self.ax1.plot(
+            [], [], label="4-2: opt external_velocity_limited", marker="", ls="--"
+        )
+        (self.im7,) = self.ax1.plot([], [], label="4-3: opt lat_acc_filtered", marker="", ls="--")
+        (self.im8,) = self.ax1.plot([], [], label="4-4: opt time_resampled", marker="", ls="--")
+        (self.im9,) = self.ax1.plot([], [], label="4-5: opt final", marker="", ls="-")
+        (self.im10,) = self.ax1.plot(
+            [], [], label="localization twist vx", color="r", marker="*", ls=":", markersize=10
+        )
+        (self.im11,) = self.ax1.plot(
+            [], [], label="vehicle twist vx", color="k", marker="+", ls=":", markersize=10
+        )
+        self.ax1.set_title("trajectory's velocity")
+        self.ax1.legend()
+        self.ax1.set_xlim([0, PLOT_MAX_ARCLENGTH])
+        self.ax1.set_ylabel("vel [m/s]")
+
+        return (
+            self.im1,
+            self.im2,
+            self.im3,
+            self.im4,
+            self.im5,
+            self.im6,
+            self.im7,
+            self.im8,
+            self.im9,
+            self.im10,
+            self.im11,
+        )
+
+    def plotTrajectoryVelocity(self, data):
+        self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME)
+        if self.self_pose_received is False:
+            print("plot start but self pose is not received")
+            return (
+                self.im1,
+                self.im2,
+                self.im3,
+                self.im4,
+                self.im5,
+                self.im6,
+                self.im7,
+                self.im8,
+                self.im9,
+                self.im10,
+                self.im11,
+            )
+        print("plot start")
+
+        # copy
+        lane_change_path = self.lane_change_path
+        behavior_path = self.behavior_path
+        obstacle_avoid_traj = self.obstacle_avoid_traj
+        obstacle_stop_traj = self.obstacle_stop_traj
+        trajectory_raw = self.trajectory_raw
+        trajectory_external_velocity_limited = self.trajectory_external_velocity_limited
+        trajectory_lateral_acc_filtered = self.trajectory_lateral_acc_filtered
+        trajectory_time_resampled = self.trajectory_time_resampled
+        trajectory_final = self.trajectory_final
+
+        if self.update_lanechange_path:
+            x = self.CalcArcLengthPathWLid(lane_change_path)
+            y = self.ToVelListPathWLid(lane_change_path)
+            self.im1.set_data(x, y)
+            self.update_lanechange_path = False
+            if len(y) != 0:
+                self.max_vel = max(10.0, np.max(y))
+                self.min_vel = np.min(y)
+
+        if self.update_behavior_path:
+            x = self.CalcArcLengthPath(behavior_path)
+            y = self.ToVelListPath(behavior_path)
+            self.im2.set_data(x, y)
+            self.update_behavior_path = False
+
+        if self.update_traj_ob_avoid:
+            x = self.CalcArcLength(obstacle_avoid_traj)
+            y = self.ToVelList(obstacle_avoid_traj)
+            self.im3.set_data(x, y)
+            self.update_traj_ob_avoid = False
+
+        if self.update_traj_ob_stop:
+            x = self.CalcArcLength(obstacle_stop_traj)
+            y = self.ToVelList(obstacle_stop_traj)
+            self.im4.set_data(x, y)
+            self.update_traj_ob_stop = False
+
+        if self.update_traj_raw:
+            x = self.CalcArcLength(trajectory_raw)
+            y = self.ToVelList(trajectory_raw)
+            self.im5.set_data(x, y)
+            self.update_traj_raw = False
+
+        if self.update_ex_vel_lim:
+            x = self.CalcArcLength(trajectory_external_velocity_limited)
+            y = self.ToVelList(trajectory_external_velocity_limited)
+            self.im6.set_data(x, y)
+            self.update_ex_vel_lim = False
+
+        if self.update_lat_acc_fil:
+            x = self.CalcArcLength(trajectory_lateral_acc_filtered)
+            y = self.ToVelList(trajectory_lateral_acc_filtered)
+            self.im7.set_data(x, y)
+            self.update_lat_acc_fil = False
+
+        if self.update_traj_resample:
+            x = self.CalcArcLength(trajectory_time_resampled)
+            y = self.ToVelList(trajectory_time_resampled)
+            self.im8.set_data(x, y)
+            self.update_traj_resample = False
+
+        if self.update_traj_final:
+            x = self.CalcArcLength(trajectory_final)
+            y = self.ToVelList(trajectory_final)
+            self.im9.set_data(x, y)
+            self.update_traj_final = False
+
+            closest = self.calcClosestTrajectory(trajectory_final)
+            if closest >= 0:
+                x_closest = x[closest]
+                self.im10.set_data(x_closest, self.localization_vx)
+                self.im11.set_data(x_closest, self.vehicle_vx)
+
+        # change y-range
+        self.ax1.set_ylim([self.min_vel - 1.0, self.max_vel + 1.0])
+
+        return (
+            self.im1,
+            self.im2,
+            self.im3,
+            self.im4,
+            self.im5,
+            self.im6,
+            self.im7,
+            self.im8,
+            self.im9,
+            self.im10,
+            self.im11,
+        )
+
+    def CalcArcLength(self, traj):
+        s_arr = []
+        ds = 0.0
+        s_sum = 0.0
+
+        if len(traj.points) > 0:
+            s_arr.append(s_sum)
+
+        for i in range(1, len(traj.points)):
+            p0 = traj.points[i - 1]
+            p1 = traj.points[i]
+            dx = p1.pose.position.x - p0.pose.position.x
+            dy = p1.pose.position.y - p0.pose.position.y
+            ds = np.sqrt(dx ** 2 + dy ** 2)
+            s_sum += ds
+            s_arr.append(s_sum)
+        return s_arr
+
+    def CalcArcLengthPathWLid(self, traj):
+        s_arr = []
+        ds = 0.0
+        s_sum = 0.0
+
+        if len(traj.points) > 0:
+            s_arr.append(s_sum)
+
+        for i in range(1, len(traj.points)):
+            p0 = traj.points[i - 1].point
+            p1 = traj.points[i].point
+            dx = p1.pose.position.x - p0.pose.position.x
+            dy = p1.pose.position.y - p0.pose.position.y
+            ds = np.sqrt(dx ** 2 + dy ** 2)
+            s_sum += ds
+            s_arr.append(s_sum)
+        return s_arr
+
+    def CalcArcLengthPath(self, traj):
+        s_arr = []
+        ds = 0.0
+        s_sum = 0.0
+
+        if len(traj.points) > 0:
+            s_arr.append(s_sum)
+
+        for i in range(1, len(traj.points)):
+            p0 = traj.points[i - 1]
+            p1 = traj.points[i]
+            dx = p1.pose.position.x - p0.pose.position.x
+            dy = p1.pose.position.y - p0.pose.position.y
+            ds = np.sqrt(dx ** 2 + dy ** 2)
+            s_sum += ds
+            s_arr.append(s_sum)
+        return s_arr
+
+    def ToVelList(self, traj):
+        v_list = []
+        for p in traj.points:
+            v_list.append(p.longitudinal_velocity_mps)
+        return v_list
+
+    def ToVelListPathWLid(self, traj):
+        v_list = []
+        for p in traj.points:
+            v_list.append(p.point.longitudinal_velocity_mps)
+        return v_list
+
+    def ToVelListPath(self, traj):
+        v_list = []
+        for p in traj.points:
+            v_list.append(p.longitudinal_velocity_mps)
+        return v_list
+
+    def CalcAcceleration(self, traj):
+        a_arr = []
+        for i in range(1, len(traj.points) - 1):
+            p0 = traj.points[i - 1]
+            p1 = traj.points[i]
+            v0 = p0.longitudinal_velocity_mps
+            v1 = p1.longitudinal_velocity_mps
+            v = 0.5 * (v1 + v0)
+            dx = p1.pose.position.x - p0.pose.position.x
+            dy = p1.pose.position.y - p0.pose.position.y
+            ds = np.sqrt(dx ** 2 + dy ** 2)
+            dt = ds / max(abs(v), 0.001)
+            a = (v1 - v0) / dt
+            a_arr.append(a)
+        if len(traj.points) > 0:
+            a_arr.append(0)
+            a_arr.append(0)
+        return a_arr
+
+    def CalcJerk(self, traj):
+        j_arr = []
+        for i in range(1, len(traj.points) - 2):
+            p0 = traj.points[i - 1]
+            p1 = traj.points[i]
+            p2 = traj.points[i + 1]
+            v0 = p0.longitudinal_velocity_mps
+            v1 = p1.longitudinal_velocity_mps
+            v2 = p2.longitudinal_velocity_mps
+
+            dx0 = p1.pose.position.x - p0.pose.position.x
+            dy0 = p1.pose.position.y - p0.pose.position.y
+            ds0 = np.sqrt(dx0 ** 2 + dy0 ** 2)
+
+            dx1 = p2.pose.position.x - p1.pose.position.x
+            dy1 = p2.pose.position.y - p1.pose.position.y
+            ds1 = np.sqrt(dx1 ** 2 + dy1 ** 2)
+
+            dt0 = ds0 / max(abs(0.5 * (v1 + v0)), 0.001)
+            dt1 = ds1 / max(abs(0.5 * (v2 + v1)), 0.001)
+
+            a0 = (v1 - v0) / max(dt0, 0.001)
+            a1 = (v2 - v1) / max(dt1, 0.001)
+            j = (a1 - a0) / max(dt1, 0.001)
+            j_arr.append(j)
+        if len(traj.points) > 0:
+            j_arr.append(0)
+            j_arr.append(0)
+            j_arr.append(0)
+        return j_arr
+
+    def setPlotTrajectory(self):
+        self.ax1 = plt.subplot(3, 1, 1)  # row, col, index(<raw*col)
+        (self.im0,) = self.ax1.plot([], [], label="0: raw", marker="")
+        (self.im1,) = self.ax1.plot([], [], label="3: time_resampled", marker="")
+        (self.im2,) = self.ax1.plot([], [], label="4: final velocity", marker="")
+        self.ax1.set_title("trajectory's velocity")
+        self.ax1.legend()
+        self.ax1.set_xlim([0, PLOT_MAX_ARCLENGTH])
+        self.ax1.set_ylabel("vel [m/s]")
+
+        self.ax2 = plt.subplot(3, 1, 2)
+        self.ax2.set_xlim([0, PLOT_MAX_ARCLENGTH])
+        self.ax2.set_ylim([-1, 1])
+        self.ax2.set_ylabel("acc [m/ss]")
+        (self.im3,) = self.ax2.plot([], [], label="final accel")
+
+        self.ax3 = plt.subplot(3, 1, 3)
+        self.ax3.set_xlim([0, PLOT_MAX_ARCLENGTH])
+        self.ax3.set_ylim([-2, 2])
+        self.ax3.set_xlabel("arclength [m]")
+        self.ax3.set_ylabel("jerk [m/sss]")
+        (self.im4,) = self.ax3.plot([], [], label="final jerk")
+
+        return self.im0, self.im1, self.im2, self.im3, self.im4
+
+    def plotTrajectory(self, data):
+        print("plot called")
+        self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME)
+
+        # copy
+        trajectory_raw = self.trajectory_raw
+        # trajectory_external_velocity_limited = self.trajectory_external_velocity_limited
+        # trajectory_lateral_acc_filtered = self.trajectory_lateral_acc_filtered
+        trajectory_time_resampled = self.trajectory_time_resampled
+        trajectory_final = self.trajectory_final
+
+        # ax1
+        if self.update_traj_raw:
+            x = self.CalcArcLength(trajectory_raw)
+            y = self.ToVelList(trajectory_raw)
+            self.im0.set_data(x, y)
+            self.update_traj_raw = False
+            if len(y) != 0:
+                self.max_vel = max(10.0, np.max(y))
+                self.min_vel = np.min(y)
+                # change y-range
+                self.ax1.set_ylim([self.min_vel - 1.0, self.max_vel + 1.0])
+
+        if self.update_traj_resample:
+            x = self.CalcArcLength(trajectory_time_resampled)
+            y = self.ToVelList(trajectory_time_resampled)
+            self.im1.set_data(x, y)
+            self.update_traj_resample = False
+
+        if self.update_traj_final:
+            x = self.CalcArcLength(trajectory_final)
+            y = self.ToVelList(trajectory_final)
+            self.im2.set_data(x, y)
+            self.update_traj_final = False
+
+            # ax2
+            y = self.CalcAcceleration(trajectory_final)
+            if len(y) != 0:
+                self.max_accel = max(0.0, np.max(y))
+                self.min_accel = min(0.0, np.min(y))
+                # change y-range
+                self.ax2.set_ylim([self.min_accel - 1.0, self.max_accel + 1.0])
+                if len(x) == len(y):
+                    self.im3.set_data(x, y)
+
+            # ax3
+            y = self.CalcJerk(trajectory_final)
+            if len(y) != 0:
+                self.max_jerk = max(0.0, np.max(y))
+                self.min_jerk = min(0.0, np.min(y))
+                # change y-range
+                # self.ax3.set_ylim([self.min_jerk - 1.0, self.max_jerk + 1.0])
+                self.ax3.set_ylim([-2.0, 2.0])  # fixed range
+                if len(x) == len(y):
+                    self.im4.set_data(x, y)
+
+        return self.im0, self.im1, self.im2, self.im3, self.im4
+
+    def calcClosestPath(self, path):
+        closest = -1
+        min_dist_squared = 1.0e10
+        for i in range(0, len(path.points)):
+            dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].pose)
+            if dist_sq < min_dist_squared:
+                min_dist_squared = dist_sq
+                closest = i
+        return closest
+
+    def calcClosestPathWLid(self, path):
+        closest = -1
+        min_dist_squared = 1.0e10
+        for i in range(0, len(path.points)):
+            dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].point.pose)
+            if dist_sq < min_dist_squared:
+                min_dist_squared = dist_sq
+                closest = i
+        return closest
+
+    def calcClosestTrajectory(self, path):
+        closest = -1
+        min_dist_squared = 1.0e10
+        for i in range(0, len(path.points)):
+            dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].pose)
+            if dist_sq < min_dist_squared:
+                min_dist_squared = dist_sq
+                closest = i
+        return closest
+
+    def calcSquaredDist2d(self, p1, p2):
+        dx = p1.position.x - p2.position.x
+        dy = p1.position.y - p2.position.y
+        return dx * dx + dy * dy
+
+    def updatePose(self, from_link, to_link):
+        try:
+            tf = self.tf_buffer.lookup_transform(from_link, to_link, rclpy.time.Time())
+            self.self_pose.position.x = tf.transform.translation.x
+            self.self_pose.position.y = tf.transform.translation.y
+            self.self_pose.position.z = tf.transform.translation.z
+            self.self_pose.orientation.x = tf.transform.rotation.x
+            self.self_pose.orientation.y = tf.transform.rotation.y
+            self.self_pose.orientation.z = tf.transform.rotation.z
+            self.self_pose.orientation.w = tf.transform.rotation.w
+            print("updatePose succeeded")
+            self.self_pose_received = True
+            return
+        except tf2_ros.TransformException:
+            self.get_logger().warn(
+                "lookup transform failed: from {} to {}".format(from_link, to_link)
+            )
+            return
+
+    def closeFigure(self):
+        plt.close(self.fig)
+
+
+def main(args=None):
+    try:
+        rclpy.init(args=args)
+        node = TrajectoryVisualizer()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/planning/motion_velocity_smoother/src/linear_interpolation.cpp b/planning/motion_velocity_smoother/src/linear_interpolation.cpp
new file mode 100644
index 0000000000000..d77134302a486
--- /dev/null
+++ b/planning/motion_velocity_smoother/src/linear_interpolation.cpp
@@ -0,0 +1,101 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "motion_velocity_smoother/linear_interpolation.hpp"
+
+#include <vector>
+
+namespace motion_velocity_smoother
+{
+/*
+ * linear interpolation
+ */
+namespace linear_interpolation
+{
+boost::optional<std::vector<double>> interpolate(
+  const std::vector<double> & sample_x, const std::vector<double> & sample_value,
+  const std::vector<double> & query_x)
+{
+  std::vector<double> query_value;
+  auto isIncrease = [](const std::vector<double> & x) {
+    if (x.empty()) {
+      return false;
+    }
+    for (size_t i = 0; i < x.size() - 1; ++i) {
+      if (x.at(i) > x.at(i + 1)) {
+        return false;
+      }
+    }
+    return true;
+  };
+
+  if (sample_x.empty() || sample_value.empty() || query_x.empty()) {
+    printf(
+      "[interpolate] some vector size is zero: sample_x.size() = %lu, sample_value.size() = %lu, "
+      "query_x.size() = %lu\n",
+      sample_x.size(), sample_value.size(), query_x.size());
+    return {};
+  }
+
+  // check if inputs are valid
+  if (
+    !isIncrease(sample_x) || !isIncrease(query_x) || query_x.front() < sample_x.front() ||
+    sample_x.back() < query_x.back() || sample_x.size() != sample_value.size()) {
+    std::cerr << "[isIncrease] bad index, return false" << std::endl;
+    const bool b1 = !isIncrease(sample_x);
+    const bool b2 = !isIncrease(query_x);
+    const bool b3 = query_x.front() < sample_x.front();
+    const bool b4 = sample_x.back() < query_x.back();
+    const bool b5 = sample_x.size() != sample_value.size();
+    printf("%d, %d, %d, %d, %d\n", b1, b2, b3, b4, b5);
+    printf("sample_x = [%f, %f]\n", sample_x.front(), sample_x.back());
+    printf("query_x = [%f, %f]\n", query_x.front(), query_x.back());
+    printf(
+      "sample_x.size() = %lu, sample_value.size() = %lu\n", sample_x.size(), sample_value.size());
+    return {};
+  }
+
+  // calculate linear interpolation
+  int i = 0;
+  for (const auto idx : query_x) {
+    if (sample_x.at(i) == idx) {
+      query_value.push_back(sample_value.at(i));
+      continue;
+    }
+    while (sample_x.at(i) < idx) {
+      ++i;
+    }
+    if (i <= 0 || static_cast<int>(sample_x.size()) - 1 < i) {
+      std::cerr << "? something wrong. skip this idx." << std::endl;
+      continue;
+    }
+
+    const double dist_base_idx = sample_x.at(i) - sample_x.at(i - 1);
+    const double dist_to_forward = sample_x.at(i) - idx;
+    const double dist_to_backward = idx - sample_x.at(i - 1);
+    if (dist_to_forward < 0.0 || dist_to_backward < 0.0) {
+      std::cerr << "?? something wrong. skip this idx. sample_x.at(i - 1) = " << sample_x.at(i - 1)
+                << ", idx = " << idx << ", sample_x.at(i) = " << sample_x.at(i) << std::endl;
+      continue;
+    }
+
+    const double value =
+      (dist_to_backward * sample_value.at(i) + dist_to_forward * sample_value.at(i - 1)) /
+      dist_base_idx;
+    query_value.push_back(value);
+  }
+  return query_value;
+}
+}  // namespace linear_interpolation
+}  // namespace motion_velocity_smoother
diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
new file mode 100644
index 0000000000000..c51a6ec640f9a
--- /dev/null
+++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
@@ -0,0 +1,1012 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "motion_velocity_smoother/motion_velocity_smoother_node.hpp"
+
+#include "autoware_utils/ros/update_param.hpp"
+#include "motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp"
+#include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp"
+#include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp"
+
+#include <algorithm>
+#include <chrono>
+#include <limits>
+#include <map>
+#include <memory>
+#include <string>
+#include <tuple>
+#include <vector>
+
+// clang-format on
+namespace motion_velocity_smoother
+{
+MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions & node_options)
+: Node("motion_velocity_smoother", node_options)
+{
+  using std::placeholders::_1;
+
+  // set common params
+  initCommonParam();
+  over_stop_velocity_warn_thr_ =
+    declare_parameter("over_stop_velocity_warn_thr", autoware_utils::kmph2mps(5.0));
+
+  // create smoother
+  initSmootherBaseParam();
+  switch (node_param_.algorithm_type) {
+    case AlgorithmType::JERK_FILTERED: {
+      initJerkFilteredSmootherParam();
+      smoother_ = std::make_shared<JerkFilteredSmoother>(jerk_filtered_smoother_param_);
+      smoother_->setParam(base_param_);
+
+      // Set Publisher for jerk filtered algorithm
+      pub_forward_filtered_trajectory_ =
+        create_publisher<Trajectory>("~/debug/forward_filtered_trajectory", 1);
+      pub_backward_filtered_trajectory_ =
+        create_publisher<Trajectory>("~/debug/backward_filtered_trajectory", 1);
+      pub_merged_filtered_trajectory_ =
+        create_publisher<Trajectory>("~/debug/merged_filtered_trajectory", 1);
+      pub_closest_merged_velocity_ =
+        create_publisher<Float32Stamped>("~/closest_merged_velocity", 1);
+      break;
+    }
+    case AlgorithmType::L2: {
+      initL2PseudoJerkSmootherParam();
+      smoother_ = std::make_shared<L2PseudoJerkSmoother>(l2_pseudo_jerk_smoother_param_);
+      smoother_->setParam(base_param_);
+      break;
+    }
+    case AlgorithmType::LINF: {
+      initLinfPseudoJerkSmootherParam();
+      smoother_ = std::make_shared<LinfPseudoJerkSmoother>(linf_pseudo_jerk_smoother_param_);
+      smoother_->setParam(base_param_);
+      break;
+    }
+    case AlgorithmType::ANALYTICAL: {
+      initAnalyticalJerkConstrainedSmootherParam();
+      smoother_ = std::make_shared<AnalyticalJerkConstrainedSmoother>(
+        analytical_jerk_constrained_smoother_param_);
+      smoother_->setParam(base_param_);
+      break;
+    }
+    default:
+      throw std::domain_error("[MotionVelocitySmootherNode] invalid algorithm");
+  }
+
+  // publishers, subscribers
+  pub_trajectory_ = create_publisher<Trajectory>("~/output/trajectory", 1);
+  pub_velocity_limit_ = create_publisher<VelocityLimit>(
+    "~/output/current_velocity_limit_mps", rclcpp::QoS{1}.transient_local());
+  pub_dist_to_stopline_ = create_publisher<Float32Stamped>("~/distance_to_stopline", 1);
+  pub_over_stop_velocity_ = create_publisher<StopSpeedExceeded>("~/stop_speed_exceeded", 1);
+  sub_current_trajectory_ = create_subscription<Trajectory>(
+    "~/input/trajectory", 1, std::bind(&MotionVelocitySmootherNode::onCurrentTrajectory, this, _1));
+  sub_current_odometry_ = create_subscription<Odometry>(
+    "/localization/kinematic_state", 1,
+    std::bind(&MotionVelocitySmootherNode::onCurrentOdometry, this, _1));
+  sub_external_velocity_limit_ = create_subscription<VelocityLimit>(
+    "~/input/external_velocity_limit_mps", 1,
+    std::bind(&MotionVelocitySmootherNode::onExternalVelocityLimit, this, _1));
+
+  // parameter update
+  set_param_res_ = this->add_on_set_parameters_callback(
+    std::bind(&MotionVelocitySmootherNode::onParameter, this, _1));
+
+  // debug
+  publish_debug_trajs_ = declare_parameter("publish_debug_trajs", false);
+  debug_closest_velocity_ = create_publisher<Float32Stamped>("~/closest_velocity", 1);
+  debug_closest_acc_ = create_publisher<Float32Stamped>("~/closest_acceleration", 1);
+  debug_closest_jerk_ = create_publisher<Float32Stamped>("~/closest_jerk", 1);
+  debug_closest_max_velocity_ = create_publisher<Float32Stamped>("~/closest_max_velocity", 1);
+  debug_calculation_time_ = create_publisher<Float32Stamped>("~/calculation_time", 1);
+  pub_trajectory_raw_ = create_publisher<Trajectory>("~/debug/trajectory_raw", 1);
+  pub_trajectory_vel_lim_ =
+    create_publisher<Trajectory>("~/debug/trajectory_external_velocity_limited", 1);
+  pub_trajectory_latacc_filtered_ =
+    create_publisher<Trajectory>("~/debug/trajectory_lateral_acc_filtered", 1);
+  pub_trajectory_resampled_ = create_publisher<Trajectory>("~/debug/trajectory_time_resampled", 1);
+
+  // Wait for first self pose
+  self_pose_listener_.waitForFirstPose();
+
+  external_velocity_limit_ = node_param_.max_velocity;
+  max_velocity_with_deceleration_ = node_param_.max_velocity;
+
+  // publish default max velocity
+  VelocityLimit max_vel_msg{};
+  max_vel_msg.stamp = this->now();
+  max_vel_msg.max_velocity = node_param_.max_velocity;
+  pub_velocity_limit_->publish(max_vel_msg);
+}
+
+rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter(
+  const std::vector<rclcpp::Parameter> & parameters)
+{
+  auto update_param = [&](const std::string & name, double & v) {
+    auto it = std::find_if(
+      parameters.cbegin(), parameters.cend(),
+      [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; });
+    if (it != parameters.cend()) {
+      v = it->as_double();
+      return true;
+    }
+    return false;
+  };
+  {
+    auto & p = node_param_;
+    update_param("max_velocity", p.max_velocity);
+    update_param(
+      "margin_to_insert_external_velocity_limit", p.margin_to_insert_external_velocity_limit);
+    update_param("replan_vel_deviation", p.replan_vel_deviation);
+    update_param("engage_velocity", p.engage_velocity);
+    update_param("engage_acceleration", p.engage_acceleration);
+    update_param("engage_exit_ratio", p.engage_exit_ratio);
+    update_param("stopping_velocity", p.stopping_velocity);
+    update_param("stopping_distance", p.stopping_distance);
+    update_param("extract_ahead_dist", p.extract_ahead_dist);
+    update_param("extract_behind_dist", p.extract_behind_dist);
+    update_param("stop_dist_to_prohibit_engage", p.stop_dist_to_prohibit_engage);
+    update_param("delta_yaw_threshold", p.delta_yaw_threshold);
+  }
+
+  {
+    auto & p = base_param_;
+    update_param("normal.max_acc", p.max_accel);
+    update_param("normal.min_acc", p.min_decel);
+    update_param("stop_decel", p.stop_decel);
+    update_param("normal.max_jerk", p.max_jerk);
+    update_param("normal.min_jerk", p.min_jerk);
+    update_param("max_lateral_accel", p.max_lateral_accel);
+    update_param("min_curve_velocity", p.min_curve_velocity);
+    update_param("decel_distance_before_curve", p.decel_distance_before_curve);
+    update_param("decel_distance_after_curve", p.decel_distance_after_curve);
+    update_param("max_trajectory_length", p.resample_param.max_trajectory_length);
+    update_param("min_trajectory_length", p.resample_param.min_trajectory_length);
+    update_param("resample_time", p.resample_param.resample_time);
+    update_param("dense_resample_dt", p.resample_param.dense_resample_dt);
+    update_param("min_interval_distance", p.resample_param.dense_min_interval_distance);
+    update_param("sparse_resample_dt", p.resample_param.sparse_resample_dt);
+    update_param("sparse_min_interval_distance", p.resample_param.sparse_min_interval_distance);
+    smoother_->setParam(p);
+  }
+
+  switch (node_param_.algorithm_type) {
+    case AlgorithmType::JERK_FILTERED: {
+      auto & p = jerk_filtered_smoother_param_;
+      update_param("jerk_weight", p.jerk_weight);
+      update_param("over_v_weight", p.over_v_weight);
+      update_param("over_a_weight", p.over_a_weight);
+      update_param("over_j_weight", p.over_j_weight);
+      update_param("jerk_filter_ds", p.jerk_filter_ds);
+      std::dynamic_pointer_cast<JerkFilteredSmoother>(smoother_)->setParam(p);
+      break;
+    }
+    case AlgorithmType::L2: {
+      auto & p = l2_pseudo_jerk_smoother_param_;
+      update_param("pseudo_jerk_weight", p.pseudo_jerk_weight);
+      update_param("over_v_weight", p.over_v_weight);
+      update_param("over_a_weight", p.over_a_weight);
+      std::dynamic_pointer_cast<L2PseudoJerkSmoother>(smoother_)->setParam(p);
+      break;
+    }
+    case AlgorithmType::LINF: {
+      auto & p = linf_pseudo_jerk_smoother_param_;
+      update_param("pseudo_jerk_weight", p.pseudo_jerk_weight);
+      update_param("over_v_weight", p.over_v_weight);
+      update_param("over_a_weight", p.over_a_weight);
+      std::dynamic_pointer_cast<LinfPseudoJerkSmoother>(smoother_)->setParam(p);
+      break;
+    }
+    case AlgorithmType::ANALYTICAL: {
+      auto & p = analytical_jerk_constrained_smoother_param_;
+      update_param("resample.ds_resample", p.resample.ds_resample);
+      update_param("resample.num_resample", p.resample.num_resample);
+      update_param("resample.delta_yaw_threshold", p.resample.delta_yaw_threshold);
+      update_param(
+        "latacc.constant_velocity_dist_threshold", p.latacc.constant_velocity_dist_threshold);
+      update_param("forward.max_acc", p.forward.max_acc);
+      update_param("forward.min_acc", p.forward.min_acc);
+      update_param("forward.max_jerk", p.forward.max_jerk);
+      update_param("forward.min_jerk", p.forward.min_jerk);
+      update_param("forward.kp", p.forward.kp);
+      update_param("backward.start_jerk", p.backward.start_jerk);
+      update_param("backward.min_jerk_mild_stop", p.backward.min_jerk_mild_stop);
+      update_param("backward.min_jerk", p.backward.min_jerk);
+      update_param("backward.min_acc_mild_stop", p.backward.min_acc_mild_stop);
+      update_param("backward.min_acc", p.backward.min_acc);
+      update_param("backward.span_jerk", p.backward.span_jerk);
+      std::dynamic_pointer_cast<AnalyticalJerkConstrainedSmoother>(smoother_)->setParam(p);
+      break;
+    }
+    default:
+      throw std::domain_error("[MotionVelocitySmootherNode] invalid algorithm");
+  }
+
+  rcl_interfaces::msg::SetParametersResult result{};
+  result.successful = true;
+  result.reason = "success";
+  return result;
+}
+
+void MotionVelocitySmootherNode::initCommonParam()
+{
+  auto & p = node_param_;
+  p.max_velocity = declare_parameter("max_velocity", 20.0);  // 72.0 kmph
+  p.margin_to_insert_external_velocity_limit =
+    declare_parameter("margin_to_insert_external_velocity_limit", 0.3);
+  p.replan_vel_deviation = declare_parameter("replan_vel_deviation", 3.0);
+  p.engage_velocity = declare_parameter("engage_velocity", 0.3);
+  p.engage_acceleration = declare_parameter("engage_acceleration", 0.1);
+  p.engage_exit_ratio = declare_parameter("engage_exit_ratio", 0.5);
+  p.engage_exit_ratio = std::min(std::max(p.engage_exit_ratio, 0.0), 1.0);
+  p.stopping_velocity = declare_parameter("stopping_velocity", autoware_utils::kmph2mps(10.0));
+  p.stopping_distance = declare_parameter("stopping_distance", 0.0);
+  p.extract_ahead_dist = declare_parameter("extract_ahead_dist", 200.0);
+  p.extract_behind_dist = declare_parameter("extract_behind_dist", 3.0);
+  p.stop_dist_to_prohibit_engage = declare_parameter("stop_dist_to_prohibit_engage", 1.5);
+  p.delta_yaw_threshold = declare_parameter("delta_yaw_threshold", M_PI / 3.0);
+  p.post_resample_param.max_trajectory_length =
+    declare_parameter("post_max_trajectory_length", 300.0);
+  p.post_resample_param.min_trajectory_length =
+    declare_parameter("post_min_trajectory_length", 30.0);
+  p.post_resample_param.resample_time = declare_parameter("post_resample_time", 10.0);
+  p.post_resample_param.dense_resample_dt = declare_parameter("post_dense_resample_dt", 0.1);
+  p.post_resample_param.dense_min_interval_distance =
+    declare_parameter("post_dense_min_interval_distance", 0.1);
+  p.post_resample_param.sparse_resample_dt = declare_parameter("post_sparse_resample_dt", 0.1);
+  p.post_resample_param.sparse_min_interval_distance =
+    declare_parameter("post_sparse_min_interval_distance", 1.0);
+  p.algorithm_type = getAlgorithmType(declare_parameter("algorithm_type", "JerkFiltered"));
+}
+
+void MotionVelocitySmootherNode::initSmootherBaseParam()
+{
+  auto & p = base_param_;
+  p.max_accel = declare_parameter("normal.max_acc", 2.0);   // 0.11G
+  p.min_decel = declare_parameter("normal.min_acc", -3.0);  // -0.2G
+  p.stop_decel = declare_parameter("stop_decel", 0.0);
+  p.max_jerk = declare_parameter("normal.max_jerk", 0.3);
+  p.min_jerk = declare_parameter("normal.min_jerk", -0.1);
+  p.max_lateral_accel = declare_parameter("max_lateral_accel", 0.2);
+  p.decel_distance_before_curve = declare_parameter("decel_distance_before_curve", 3.5);
+  p.decel_distance_after_curve = declare_parameter("decel_distance_after_curve", 0.0);
+  p.min_curve_velocity = declare_parameter("min_curve_velocity", 1.38);
+  p.resample_param.max_trajectory_length = declare_parameter("max_trajectory_length", 200.0);
+  p.resample_param.min_trajectory_length = declare_parameter("min_trajectory_length", 30.0);
+  p.resample_param.resample_time = declare_parameter("resample_time", 10.0);
+  p.resample_param.dense_resample_dt = declare_parameter("dense_resample_dt", 0.1);
+  p.resample_param.dense_min_interval_distance =
+    declare_parameter("dense_min_interval_distance", 0.1);
+  p.resample_param.sparse_resample_dt = declare_parameter("sparse_resample_dt", 0.5);
+  p.resample_param.sparse_min_interval_distance =
+    declare_parameter("sparse_min_interval_distance", 4.0);
+}
+
+void MotionVelocitySmootherNode::initJerkFilteredSmootherParam()
+{
+  auto & p = jerk_filtered_smoother_param_;
+  p.jerk_weight = declare_parameter("jerk_weight", 10.0);
+  p.over_v_weight = declare_parameter("over_v_weight", 100000.0);
+  p.over_a_weight = declare_parameter("over_a_weight", 5000.0);
+  p.over_j_weight = declare_parameter("over_j_weight", 2000.0);
+  p.jerk_filter_ds = declare_parameter("jerk_filter_ds", 0.1);
+}
+
+void MotionVelocitySmootherNode::initL2PseudoJerkSmootherParam()
+{
+  auto & p = l2_pseudo_jerk_smoother_param_;
+  p.pseudo_jerk_weight = declare_parameter("pseudo_jerk_weight", 100.0);
+  p.over_v_weight = declare_parameter("over_v_weight", 100000.0);
+  p.over_a_weight = declare_parameter("over_a_weight", 1000.0);
+}
+
+void MotionVelocitySmootherNode::initLinfPseudoJerkSmootherParam()
+{
+  auto & p = linf_pseudo_jerk_smoother_param_;
+  p.pseudo_jerk_weight = declare_parameter("pseudo_jerk_weight", 200.0);
+  p.over_v_weight = declare_parameter("over_v_weight", 100000.0);
+  p.over_a_weight = declare_parameter("over_a_weight", 5000.0);
+}
+
+void MotionVelocitySmootherNode::initAnalyticalJerkConstrainedSmootherParam()
+{
+  auto & p = analytical_jerk_constrained_smoother_param_;
+  p.resample.ds_resample = declare_parameter("resample.ds_resample", 0.1);
+  p.resample.num_resample = declare_parameter("resample.num_resample", 1);
+  p.resample.delta_yaw_threshold = declare_parameter("resample.delta_yaw_threshold", 0.785);
+
+  p.latacc.enable_constant_velocity_while_turning =
+    declare_parameter("latacc.enable_constant_velocity_while_turning", false);
+  p.latacc.constant_velocity_dist_threshold =
+    declare_parameter("latacc.constant_velocity_dist_threshold", 2.0);
+
+  p.forward.max_acc = declare_parameter("forward.max_acc", 1.0);
+  p.forward.min_acc = declare_parameter("forward.min_acc", -1.0);
+  p.forward.max_jerk = declare_parameter("forward.max_jerk", 0.3);
+  p.forward.min_jerk = declare_parameter("forward.min_jerk", -0.3);
+  p.forward.kp = declare_parameter("forward.kp", 0.3);
+
+  p.backward.start_jerk = declare_parameter("backward.start_jerk", -0.1);
+  p.backward.min_jerk_mild_stop = declare_parameter("backward.min_jerk_mild_stop", -0.3);
+  p.backward.min_jerk = declare_parameter("backward.min_jerk", -1.5);
+  p.backward.min_acc_mild_stop = declare_parameter("backward.min_acc_mild_stop", -1.0);
+  p.backward.min_acc = declare_parameter("backward.min_acc", -2.5);
+  p.backward.span_jerk = declare_parameter("backward.span_jerk", -0.01);
+}
+
+void MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const
+{
+  Trajectory publishing_trajectory = autoware_utils::convertToTrajectory(trajectory);
+  publishing_trajectory.header = base_traj_raw_ptr_->header;
+  pub_trajectory_->publish(publishing_trajectory);
+}
+
+void MotionVelocitySmootherNode::onCurrentOdometry(const Odometry::ConstSharedPtr msg)
+{
+  current_odometry_ptr_ = msg;
+}
+
+void MotionVelocitySmootherNode::onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg)
+{
+  constexpr double eps = 1.0E-04;
+  const double margin = node_param_.margin_to_insert_external_velocity_limit;
+
+  // calculate distance and maximum velocity
+  // to decelerate to external velocity limit with jerk and acceleration
+  // constraints
+  if (!prev_output_.empty()) {
+    // if external velocity limit decreases
+    if ((external_velocity_limit_ - msg->max_velocity) > eps) {
+      if (prev_closest_point_) {
+        const double v0 = prev_closest_point_->longitudinal_velocity_mps;
+        const double a0 = prev_closest_point_->acceleration_mps2;
+
+        if (isEngageStatus(v0)) {
+          max_velocity_with_deceleration_ = external_velocity_limit_;
+          external_velocity_limit_dist_ = 0.0;
+        } else {
+          const double a_min =
+            msg->use_constraints ? msg->constraints.min_acceleration : smoother_->getMinDecel();
+          const double j_max =
+            msg->use_constraints ? msg->constraints.max_jerk : smoother_->getMaxJerk();
+          const double j_min =
+            msg->use_constraints ? msg->constraints.min_jerk : smoother_->getMinJerk();
+          double stop_dist;
+          std::map<double, double> jerk_profile;
+          if (!trajectory_utils::calcStopDistWithJerkConstraints(
+                v0, a0, j_max, j_min, a_min, msg->max_velocity, jerk_profile, stop_dist)) {
+            RCLCPP_WARN(get_logger(), "Stop distance calculation is failed!");
+          }
+          external_velocity_limit_dist_ = stop_dist + margin;
+          // If the closest acceleration is positive, velocity will increase
+          // until the acceleration becomes zero
+          // So we set the maximum increased velocity as the velocity limit
+          if (a0 > 0) {
+            max_velocity_with_deceleration_ = v0 - 0.5 * a0 * a0 / j_min;
+          } else {
+            max_velocity_with_deceleration_ = v0;
+          }
+
+          if (max_velocity_with_deceleration_ < msg->max_velocity) {
+            max_velocity_with_deceleration_ = msg->max_velocity;
+            external_velocity_limit_dist_ = 0.0;
+          }
+        }
+      }
+      // if external velocity limit increases
+    } else if ((msg->max_velocity - external_velocity_limit_) > eps) {
+      max_velocity_with_deceleration_ = msg->max_velocity;
+    }
+  }
+
+  external_velocity_limit_ = msg->max_velocity;
+  pub_velocity_limit_->publish(*msg);
+}
+
+bool MotionVelocitySmootherNode::checkData() const
+{
+  if (!current_pose_ptr_ || !current_odometry_ptr_ || !base_traj_raw_ptr_) {
+    RCLCPP_DEBUG(
+      get_logger(), "wait topics : current_pose = %d, current_vel = %d, base_traj = %d",
+      (bool)current_pose_ptr_, (bool)current_odometry_ptr_, (bool)base_traj_raw_ptr_);
+    return false;
+  }
+  if (base_traj_raw_ptr_->points.size() < 2) {
+    RCLCPP_ERROR(get_logger(), "input trajectory size must > 1. Skip computation.");
+    return false;
+  }
+
+  return true;
+}
+
+void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr msg)
+{
+  base_traj_raw_ptr_ = msg;
+
+  stop_watch_.tic();
+  RCLCPP_DEBUG(get_logger(), "========================= run start =========================");
+
+  current_pose_ptr_ = self_pose_listener_.getCurrentPose();
+
+  // guard
+  if (!checkData()) {
+    return;
+  }
+
+  // calculate distance to insert external velocity limit
+  if (!prev_output_.empty()) {
+    const double travel_dist = calcTravelDistance();
+    external_velocity_limit_dist_ -= travel_dist;
+    external_velocity_limit_dist_ = std::max(external_velocity_limit_dist_, 0.0);
+    RCLCPP_DEBUG(
+      get_logger(), "run: travel_dist = %f, external_velocity_limit_dist_ = %f", travel_dist,
+      external_velocity_limit_dist_);
+  }
+
+  // calculate trajectory velocity
+  TrajectoryPoints output =
+    calcTrajectoryVelocity(autoware_utils::convertToTrajectoryPointArray(*base_traj_raw_ptr_));
+  if (output.empty()) {
+    RCLCPP_WARN(get_logger(), "Output Point is empty");
+    return;
+  }
+
+  // Get the nearest point
+  const auto output_closest_idx = autoware_utils::findNearestIndex(
+    output, current_pose_ptr_->pose, std::numeric_limits<double>::max(),
+    node_param_.delta_yaw_threshold);
+  const auto output_closest_point =
+    trajectory_utils::calcInterpolatedTrajectoryPoint(output, current_pose_ptr_->pose);
+  if (!output_closest_idx) {
+    RCLCPP_WARN_THROTTLE(
+      get_logger(), *get_clock(), 5000, "Cannot find closest waypoint for output trajectory");
+    return;
+  }
+
+  // Resample the optimized trajectory
+  auto output_resampled = resampling::resampleTrajectory(
+    output, current_odometry_ptr_->twist.twist.linear.x, *output_closest_idx,
+    node_param_.post_resample_param);
+  if (!output_resampled) {
+    RCLCPP_WARN(get_logger(), "Failed to get the resampled output trajectory");
+    return;
+  }
+
+  // Set 0 at the end of the trajectory
+  if (!output_resampled->empty()) {
+    output_resampled->back().longitudinal_velocity_mps = 0.0;
+  }
+
+  // publish message
+  publishTrajectory(*output_resampled);
+
+  // publish debug message
+  publishStopDistance(output, *output_closest_idx);
+  publishClosestState(output_closest_point);
+
+  prev_output_ = output;
+  prev_closest_point_ = output_closest_point;
+
+  // Publish Calculation Time
+  Float32Stamped calculation_time_data{};
+  calculation_time_data.stamp = this->now();
+  calculation_time_data.data = stop_watch_.toc();
+  debug_calculation_time_->publish(calculation_time_data);
+  RCLCPP_DEBUG(get_logger(), "run: calculation time = %f [ms]", calculation_time_data.data);
+  RCLCPP_DEBUG(get_logger(), "========================== run() end ==========================\n\n");
+}
+
+TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity(
+  const TrajectoryPoints & traj_input) const
+{
+  TrajectoryPoints output{};  // velocity is optimized by qp solver
+
+  // Extract trajectory around self-position with desired forward-backward length
+  const auto input_closest = autoware_utils::findNearestIndex(
+    traj_input, current_pose_ptr_->pose, std::numeric_limits<double>::max(),
+    node_param_.delta_yaw_threshold);
+  if (!input_closest) {
+    auto clock{rclcpp::Clock{RCL_ROS_TIME}};
+    RCLCPP_WARN_THROTTLE(
+      get_logger(), clock, 5000, "Cannot find the closest point from input trajectory");
+    return prev_output_;
+  }
+
+  auto traj_extracted = trajectory_utils::extractPathAroundIndex(
+    traj_input, *input_closest, node_param_.extract_ahead_dist, node_param_.extract_behind_dist);
+  if (!traj_extracted) {
+    RCLCPP_WARN(get_logger(), "Fail to extract the path from the input trajectory");
+    return prev_output_;
+  }
+
+  // Smoother can not handle negative velocity,
+  // so multiple -1 to velocity if any trajectory points have reverse
+  // velocity
+  const bool is_reverse = std::any_of(
+    traj_extracted->begin(), traj_extracted->end(),
+    [](auto & pt) { return pt.longitudinal_velocity_mps < 0; });
+  if (is_reverse) {
+    for (auto & pt : *traj_extracted) {
+      pt.longitudinal_velocity_mps *= -1.0;
+    }
+  }
+
+  // Debug
+  if (publish_debug_trajs_) {
+    pub_trajectory_raw_->publish(toTrajectoryMsg(*traj_extracted, base_traj_raw_ptr_->header));
+  }
+
+  // Apply external velocity limit
+  applyExternalVelocityLimit(*traj_extracted);
+
+  // Change trajectory velocity to zero when current_velocity == 0 & stop_dist is close
+  const auto traj_extracted_closest = autoware_utils::findNearestIndex(
+    *traj_extracted, current_pose_ptr_->pose, std::numeric_limits<double>::max(),
+    node_param_.delta_yaw_threshold);
+  if (!traj_extracted_closest) {
+    RCLCPP_WARN(get_logger(), "Cannot find the closest point from extracted trajectory");
+    return prev_output_;
+  }
+
+  // Apply velocity to approach stop point
+  applyStopApproachingVelocity(*traj_extracted);
+
+  // Debug
+  if (publish_debug_trajs_) {
+    pub_trajectory_vel_lim_->publish(toTrajectoryMsg(*traj_extracted, base_traj_raw_ptr_->header));
+  }
+
+  // Smoothing velocity
+  if (!smoothVelocity(*traj_extracted, output)) {
+    return prev_output_;
+  }
+
+  // for reverse velocity
+  if (is_reverse) {
+    for (auto & pt : output) {
+      pt.longitudinal_velocity_mps *= -1.0;
+    }
+  }
+
+  return output;
+}
+
+bool MotionVelocitySmootherNode::smoothVelocity(
+  const TrajectoryPoints & input, TrajectoryPoints & traj_smoothed) const
+{
+  // Lateral acceleration limit
+  const auto traj_lateral_acc_filtered = smoother_->applyLateralAccelerationFilter(input);
+  if (!traj_lateral_acc_filtered) {
+    return false;
+  }
+
+  // Resample trajectory with ego-velocity based interval distance
+  const auto traj_pre_resampled_closest = autoware_utils::findNearestIndex(
+    *traj_lateral_acc_filtered, current_pose_ptr_->pose, std::numeric_limits<double>::max(),
+    node_param_.delta_yaw_threshold);
+  auto traj_resampled = smoother_->resampleTrajectory(
+    *traj_lateral_acc_filtered, current_odometry_ptr_->twist.twist.linear.x,
+    *traj_pre_resampled_closest);
+  if (!traj_resampled) {
+    RCLCPP_WARN(get_logger(), "Fail to do resampling before the optimization");
+    return false;
+  }
+
+  // Set 0[m/s] in the terminal point
+  if (!traj_resampled->empty()) {
+    traj_resampled->back().longitudinal_velocity_mps = 0.0;
+  }
+
+  // Publish Closest Resample Trajectory Velocity
+  publishClosestVelocity(*traj_resampled, current_pose_ptr_->pose, debug_closest_max_velocity_);
+
+  // Calculate initial motion for smoothing
+  double initial_vel{};
+  double initial_acc{};
+  InitializeType type{};
+  const auto traj_resampled_closest = autoware_utils::findNearestIndex(
+    *traj_resampled, current_pose_ptr_->pose, std::numeric_limits<double>::max(),
+    node_param_.delta_yaw_threshold);
+  if (!traj_resampled_closest) {
+    RCLCPP_WARN(get_logger(), "Cannot find closest waypoint for resampled trajectory");
+    return false;
+  }
+  std::tie(initial_vel, initial_acc, type) =
+    calcInitialMotion(*traj_resampled, *traj_resampled_closest, prev_output_);
+
+  // Clip trajectory from closest point
+  TrajectoryPoints clipped;
+  clipped.insert(
+    clipped.end(), traj_resampled->begin() + *traj_resampled_closest, traj_resampled->end());
+
+  std::vector<TrajectoryPoints> debug_trajectories;
+  if (!smoother_->apply(initial_vel, initial_acc, clipped, traj_smoothed, debug_trajectories)) {
+    RCLCPP_WARN(get_logger(), "Fail to solve optimization.");
+  }
+
+  traj_smoothed.insert(
+    traj_smoothed.begin(), traj_resampled->begin(),
+    traj_resampled->begin() + *traj_resampled_closest);
+
+  // Set 0 velocity after input-stop-point
+  overwriteStopPoint(*traj_resampled, traj_smoothed);
+
+  // For the endpoint of the trajectory
+  if (!traj_smoothed.empty()) {
+    traj_smoothed.back().longitudinal_velocity_mps = 0.0;
+  }
+
+  // Max velocity filter for safety
+  trajectory_utils::applyMaximumVelocityLimit(
+    *traj_resampled_closest, traj_smoothed.size(), node_param_.max_velocity, traj_smoothed);
+
+  // Insert behind velocity for output's consistency
+  insertBehindVelocity(*traj_resampled_closest, type, traj_smoothed);
+
+  RCLCPP_DEBUG(get_logger(), "smoothVelocity : traj_smoothed.size() = %lu", traj_smoothed.size());
+  if (publish_debug_trajs_) {
+    pub_trajectory_latacc_filtered_->publish(
+      toTrajectoryMsg(*traj_lateral_acc_filtered, base_traj_raw_ptr_->header));
+    pub_trajectory_resampled_->publish(
+      toTrajectoryMsg(*traj_resampled, base_traj_raw_ptr_->header));
+
+    if (!debug_trajectories.empty()) {
+      for (auto & debug_trajectory : debug_trajectories) {
+        debug_trajectory.insert(
+          debug_trajectory.begin(), traj_resampled->begin(),
+          traj_resampled->begin() + *traj_resampled_closest);
+        for (size_t i = 0; i < *traj_resampled_closest; ++i) {
+          debug_trajectory.at(i).longitudinal_velocity_mps =
+            debug_trajectory.at(*traj_resampled_closest).longitudinal_velocity_mps;
+        }
+      }
+    }
+    publishDebugTrajectories(debug_trajectories);
+  }
+
+  return true;
+}
+
+void MotionVelocitySmootherNode::insertBehindVelocity(
+  const size_t output_closest, const InitializeType type, TrajectoryPoints & output) const
+{
+  const bool keep_closest_vel_for_behind =
+    (type == InitializeType::INIT || type == InitializeType::LARGE_DEVIATION_REPLAN ||
+     type == InitializeType::ENGAGING);
+
+  for (size_t i = output_closest - 1; i < output.size(); --i) {
+    if (keep_closest_vel_for_behind) {
+      output.at(i).longitudinal_velocity_mps = output.at(output_closest).longitudinal_velocity_mps;
+      output.at(i).acceleration_mps2 = output.at(output_closest).acceleration_mps2;
+    } else {
+      const auto prev_output_point =
+        trajectory_utils::calcInterpolatedTrajectoryPoint(prev_output_, output.at(i).pose);
+
+      // output should be always positive: TODO(Horibe) think better way
+      output.at(i).longitudinal_velocity_mps =
+        std::abs(prev_output_point.longitudinal_velocity_mps);
+      output.at(i).acceleration_mps2 = prev_output_point.acceleration_mps2;
+    }
+  }
+}
+
+void MotionVelocitySmootherNode::publishStopDistance(
+  const TrajectoryPoints & trajectory, const size_t closest) const
+{
+  // stop distance calculation
+  const double stop_dist_lim{50.0};
+  double stop_dist{stop_dist_lim};
+  const auto stop_idx{autoware_utils::searchZeroVelocityIndex(trajectory)};
+  if (stop_idx) {
+    stop_dist = trajectory_utils::calcArcLength(trajectory, closest, *stop_idx);
+    stop_dist = closest > *stop_idx ? stop_dist : -stop_dist;
+  } else {
+    stop_dist = closest > 0 ? stop_dist : -stop_dist;
+  }
+  Float32Stamped dist_to_stopline{};
+  dist_to_stopline.stamp = this->now();
+  dist_to_stopline.data = std::max(-stop_dist_lim, std::min(stop_dist_lim, stop_dist));
+  pub_dist_to_stopline_->publish(dist_to_stopline);
+}
+
+std::tuple<double, double, MotionVelocitySmootherNode::InitializeType>
+MotionVelocitySmootherNode::calcInitialMotion(
+  const TrajectoryPoints & input_traj, const size_t input_closest,
+  const TrajectoryPoints & prev_traj) const
+{
+  const double vehicle_speed{std::fabs(current_odometry_ptr_->twist.twist.linear.x)};
+  const double target_vel{std::fabs(input_traj.at(input_closest).longitudinal_velocity_mps)};
+
+  double initial_vel{};
+  double initial_acc{};
+  InitializeType type{};
+
+  // first time
+  if (prev_traj.empty()) {
+    initial_vel = vehicle_speed;
+    initial_acc = 0.0;
+    type = InitializeType::INIT;
+    return std::make_tuple(initial_vel, initial_acc, type);
+  }
+
+  const auto prev_output_closest_point =
+    trajectory_utils::calcInterpolatedTrajectoryPoint(prev_traj, input_traj.at(input_closest).pose);
+
+  // when velocity tracking deviation is large
+  const double desired_vel{prev_output_closest_point.longitudinal_velocity_mps};
+  const double vel_error{vehicle_speed - std::fabs(desired_vel)};
+  if (std::fabs(vel_error) > node_param_.replan_vel_deviation) {
+    type = InitializeType::LARGE_DEVIATION_REPLAN;
+    initial_vel = vehicle_speed;  // use current vehicle speed
+    initial_acc = prev_output_closest_point.acceleration_mps2;
+    RCLCPP_DEBUG(
+      get_logger(),
+      "calcInitialMotion : Large deviation error for speed control. Use current speed for "
+      "initial value, desired_vel = %f, vehicle_speed = %f, vel_error = %f, error_thr = %f",
+      desired_vel, vehicle_speed, vel_error, node_param_.replan_vel_deviation);
+    return std::make_tuple(initial_vel, initial_acc, type);
+  }
+
+  // if current vehicle velocity is low && base_desired speed is high,
+  // use engage_velocity for engage vehicle
+  const double engage_vel_thr = node_param_.engage_velocity * node_param_.engage_exit_ratio;
+  if (vehicle_speed < engage_vel_thr) {
+    if (target_vel >= node_param_.engage_velocity) {
+      const auto idx = autoware_utils::searchZeroVelocityIndex(input_traj);
+      const double stop_dist =
+        idx ? autoware_utils::calcDistance2d(input_traj.at(*idx), input_traj.at(input_closest))
+            : 0.0;
+      if (!idx || stop_dist > node_param_.stop_dist_to_prohibit_engage) {
+        type = InitializeType::ENGAGING;
+        initial_vel = node_param_.engage_velocity;
+        initial_acc = node_param_.engage_acceleration;
+        RCLCPP_DEBUG(
+          get_logger(),
+          "calcInitialMotion : vehicle speed is low (%.3f), and desired speed is high (%.3f). Use "
+          "engage speed (%.3f) until vehicle speed reaches engage_vel_thr (%.3f). stop_dist = %.3f",
+          vehicle_speed, target_vel, node_param_.engage_velocity, engage_vel_thr, stop_dist);
+        return std::make_tuple(initial_vel, initial_acc, type);
+      } else {
+        RCLCPP_DEBUG(
+          get_logger(), "calcInitialMotion : stop point is close (%.3f[m]). no engage.", stop_dist);
+      }
+    } else if (target_vel > 0.0) {
+      auto clock{rclcpp::Clock{RCL_ROS_TIME}};
+      RCLCPP_WARN_THROTTLE(
+        get_logger(), clock, 3000,
+        "calcInitialMotion : target velocity(%.3f[m/s]) is lower than engage velocity(%.3f[m/s]). ",
+        target_vel, node_param_.engage_velocity);
+    }
+  }
+
+  // normal update: use closest in prev_output
+  type = InitializeType::NORMAL;
+  initial_vel = prev_output_closest_point.longitudinal_velocity_mps;
+  initial_acc = prev_output_closest_point.acceleration_mps2;
+  RCLCPP_DEBUG(
+    get_logger(),
+    "calcInitialMotion : normal update. v0 = %f, a0 = %f, vehicle_speed = %f, target_vel = %f",
+    initial_vel, initial_acc, vehicle_speed, target_vel);
+  return std::make_tuple(initial_vel, initial_acc, type);
+}
+
+void MotionVelocitySmootherNode::overwriteStopPoint(
+  const TrajectoryPoints & input, TrajectoryPoints & output) const
+{
+  const auto stop_idx = autoware_utils::searchZeroVelocityIndex(input);
+  if (!stop_idx) {
+    return;
+  }
+
+  // Get Closest Point from Output
+  const auto nearest_output_point_idx = autoware_utils::findNearestIndex(
+    output, input.at(*stop_idx).pose, std::numeric_limits<double>::max(),
+    node_param_.delta_yaw_threshold);
+
+  // check over velocity
+  bool is_stop_velocity_exceeded{false};
+  double input_stop_vel{};
+  double output_stop_vel{};
+  if (nearest_output_point_idx) {
+    double optimized_stop_point_vel =
+      output.at(*nearest_output_point_idx).longitudinal_velocity_mps;
+    is_stop_velocity_exceeded = (optimized_stop_point_vel > over_stop_velocity_warn_thr_);
+    input_stop_vel = input.at(*nearest_output_point_idx).longitudinal_velocity_mps;
+    output_stop_vel = output.at(*nearest_output_point_idx).longitudinal_velocity_mps;
+    trajectory_utils::applyMaximumVelocityLimit(
+      *nearest_output_point_idx, output.size(), 0.0, output);
+    RCLCPP_DEBUG(
+      get_logger(),
+      "replan : input_stop_idx = %lu, stop velocity : input = %f, output = %f, thr = %f",
+      *nearest_output_point_idx, input_stop_vel, output_stop_vel, over_stop_velocity_warn_thr_);
+  } else {
+    input_stop_vel = -1.0;
+    output_stop_vel = -1.0;
+    RCLCPP_DEBUG(
+      get_logger(),
+      "replan : input_stop_idx = -1, stop velocity : input = %f, output = %f, thr = %f",
+      input_stop_vel, output_stop_vel, over_stop_velocity_warn_thr_);
+  }
+
+  {
+    StopSpeedExceeded msg{};
+    msg.stamp = this->now();
+    msg.stop_speed_exceeded = is_stop_velocity_exceeded;
+    pub_over_stop_velocity_->publish(msg);
+  }
+}
+
+void MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) const
+{
+  if (traj.size() < 1) {
+    return;
+  }
+
+  trajectory_utils::applyMaximumVelocityLimit(
+    0, traj.size(), max_velocity_with_deceleration_, traj);
+
+  const auto closest_idx = autoware_utils::findNearestIndex(
+    traj, current_pose_ptr_->pose, std::numeric_limits<double>::max(),
+    node_param_.delta_yaw_threshold);
+  if (!closest_idx) {
+    return;
+  }
+
+  double dist = 0.0;
+  for (size_t idx = *closest_idx; idx < traj.size() - 1; ++idx) {
+    dist += autoware_utils::calcDistance2d(traj.at(idx), traj.at(idx + 1));
+    if (dist > external_velocity_limit_dist_) {
+      trajectory_utils::applyMaximumVelocityLimit(
+        idx + 1, traj.size(), external_velocity_limit_, traj);
+      return;
+    }
+  }
+  traj.back().longitudinal_velocity_mps =
+    std::min(traj.back().longitudinal_velocity_mps, static_cast<float>(external_velocity_limit_));
+  RCLCPP_DEBUG(get_logger(), "externalVelocityLimit : limit_vel = %.3f", external_velocity_limit_);
+}
+
+void MotionVelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & traj) const
+{
+  const auto stop_idx = autoware_utils::searchZeroVelocityIndex(traj);
+  if (!stop_idx) {
+    return;  // no stop point.
+  }
+  double distance_sum = 0.0;
+  for (size_t i = *stop_idx - 1; i < traj.size(); --i) {  // search backward
+    distance_sum += autoware_utils::calcDistance2d(traj.at(i), traj.at(i + 1));
+    if (distance_sum > node_param_.stopping_distance) {
+      break;
+    }
+    if (traj.at(i).longitudinal_velocity_mps > node_param_.stopping_velocity) {
+      traj.at(i).longitudinal_velocity_mps = node_param_.stopping_velocity;
+    }
+  }
+}
+
+void MotionVelocitySmootherNode::publishDebugTrajectories(
+  const std::vector<TrajectoryPoints> & debug_trajectories) const
+{
+  if (node_param_.algorithm_type == AlgorithmType::JERK_FILTERED) {
+    if (debug_trajectories.size() != 3) {
+      RCLCPP_DEBUG(get_logger(), "Size of the debug trajectories is incorrect");
+      return;
+    }
+    const auto & h = base_traj_raw_ptr_->header;
+    pub_forward_filtered_trajectory_->publish(toTrajectoryMsg(debug_trajectories.at(0), h));
+    pub_backward_filtered_trajectory_->publish(toTrajectoryMsg(debug_trajectories.at(1), h));
+    pub_merged_filtered_trajectory_->publish(toTrajectoryMsg(debug_trajectories.at(2), h));
+    publishClosestVelocity(
+      debug_trajectories.at(2), current_pose_ptr_->pose, pub_closest_merged_velocity_);
+  }
+}
+
+void MotionVelocitySmootherNode::publishClosestVelocity(
+  const TrajectoryPoints & trajectory, const Pose & current_pose,
+  const rclcpp::Publisher<Float32Stamped>::SharedPtr pub) const
+{
+  const auto closest_point =
+    trajectory_utils::calcInterpolatedTrajectoryPoint(trajectory, current_pose);
+
+  Float32Stamped vel_data{};
+  vel_data.stamp = this->now();
+  vel_data.data = std::max(closest_point.longitudinal_velocity_mps, static_cast<float>(0.0));
+  pub->publish(vel_data);
+}
+
+void MotionVelocitySmootherNode::publishClosestState(const TrajectoryPoint & closest_point)
+{
+  auto publishFloat = [=](
+                        const double data, const rclcpp::Publisher<Float32Stamped>::SharedPtr pub) {
+    Float32Stamped msg{};
+    msg.stamp = this->now();
+    msg.data = data;
+    pub->publish(msg);
+    return;
+  };
+
+  const double curr_vel{closest_point.longitudinal_velocity_mps};
+  const double curr_acc{closest_point.acceleration_mps2};
+  if (!prev_time_) {
+    prev_time_ = std::make_shared<rclcpp::Time>(this->now());
+    prev_acc_ = curr_acc;
+    return;
+  }
+
+  // Calculate jerk
+  rclcpp::Time curr_time{this->now()};
+  double dt = (curr_time - *prev_time_).seconds();
+  double curr_jerk = (curr_acc - prev_acc_) / dt;
+
+  // Publish data
+  publishFloat(curr_vel, debug_closest_velocity_);
+  publishFloat(curr_acc, debug_closest_acc_);
+  publishFloat(curr_jerk, debug_closest_jerk_);
+
+  // Update
+  prev_acc_ = curr_acc;
+  *prev_time_ = curr_time;
+}
+
+MotionVelocitySmootherNode::AlgorithmType MotionVelocitySmootherNode::getAlgorithmType(
+  const std::string & algorithm_name) const
+{
+  if (algorithm_name == "JerkFiltered") {
+    return AlgorithmType::JERK_FILTERED;
+  }
+  if (algorithm_name == "L2") {
+    return AlgorithmType::L2;
+  }
+  if (algorithm_name == "Linf") {
+    return AlgorithmType::LINF;
+  }
+  if (algorithm_name == "Analytical") {
+    return AlgorithmType::ANALYTICAL;
+  }
+
+  throw std::domain_error("[MotionVelocitySmootherNode] undesired algorithm is selected.");
+  return AlgorithmType::INVALID;
+}
+
+double MotionVelocitySmootherNode::calcTravelDistance() const
+{
+  const auto closest_point =
+    trajectory_utils::calcInterpolatedTrajectoryPoint(prev_output_, current_pose_ptr_->pose);
+
+  if (prev_closest_point_) {
+    const double travel_dist = autoware_utils::calcDistance2d(*prev_closest_point_, closest_point);
+    return travel_dist;
+  }
+
+  return 0.0;
+}
+
+bool MotionVelocitySmootherNode::isEngageStatus(const double target_vel) const
+{
+  const double vehicle_speed = std::fabs(current_odometry_ptr_->twist.twist.linear.x);
+  const double engage_vel_thr = node_param_.engage_velocity * node_param_.engage_exit_ratio;
+  return vehicle_speed < engage_vel_thr && target_vel >= node_param_.engage_velocity;
+}
+
+Trajectory MotionVelocitySmootherNode::toTrajectoryMsg(
+  const TrajectoryPoints & points, const std_msgs::msg::Header & header) const
+{
+  auto trajectory = autoware_utils::convertToTrajectory(points);
+  trajectory.header = header;
+  return trajectory;
+}
+
+}  // namespace motion_velocity_smoother
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(motion_velocity_smoother::MotionVelocitySmootherNode)
diff --git a/planning/motion_velocity_smoother/src/resample.cpp b/planning/motion_velocity_smoother/src/resample.cpp
new file mode 100644
index 0000000000000..aa70a3ec99eb5
--- /dev/null
+++ b/planning/motion_velocity_smoother/src/resample.cpp
@@ -0,0 +1,266 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "motion_velocity_smoother/resample.hpp"
+
+#include <algorithm>
+#include <vector>
+
+namespace motion_velocity_smoother
+{
+namespace resampling
+{
+boost::optional<TrajectoryPoints> resampleTrajectory(
+  const TrajectoryPoints & input, const double v_current, const size_t closest_id,
+  const ResampleParam & param)
+{
+  // Arc length from the initial point to the closest point
+  const double front_arclength_value = trajectory_utils::calcArcLength(input, 0, closest_id);
+
+  // Get the nearest point where velocity is zero
+  auto zero_vel_id = autoware_utils::searchZeroVelocityIndex(input, closest_id, input.size());
+  // Arc length from the closest point to the point where velocity is zero
+  double zero_vel_arclength_value = param.max_trajectory_length;
+  if (zero_vel_id) {
+    zero_vel_arclength_value = std::min(
+      zero_vel_arclength_value,
+      autoware_utils::calcSignedArcLength(input, closest_id, *zero_vel_id));
+  }
+
+  // Get the resample size from the closest point
+  const std::vector<double> in_arclength = trajectory_utils::calcArclengthArray(input);
+  const double Nt = param.resample_time / std::max(param.dense_resample_dt, 0.001);
+  const double ds_nominal =
+    std::max(v_current * param.dense_resample_dt, param.dense_min_interval_distance);
+  const double Ns = param.min_trajectory_length / std::max(ds_nominal, 0.001);
+  const double N = std::max(Nt, Ns);
+  std::vector<double> out_arclength;
+
+  // Step1. Resample front trajectory
+  constexpr double front_ds = 0.1;
+  for (double ds = 0.0; ds <= front_arclength_value; ds += front_ds) {
+    out_arclength.push_back(ds);
+  }
+  if (std::fabs(out_arclength.back() - front_arclength_value) < 1e-3) {
+    out_arclength.back() = front_arclength_value;
+  } else {
+    out_arclength.push_back(front_arclength_value);
+  }
+
+  // Step2. Resample behind trajectory
+  double dist_i = 0.0;
+  bool is_zero_point_included = false;
+  bool is_endpoint_included = false;
+  for (size_t i = 1; static_cast<double>(i) <= N; ++i) {
+    double ds = ds_nominal;
+    if (i > Nt) {
+      // if the planning time is not enough to see the desired distance,
+      // change the interval distance to see far.
+      ds = std::max(param.sparse_min_interval_distance, param.sparse_resample_dt * v_current);
+    }
+
+    dist_i += ds;
+
+    // Check if the distance is longer than max_trajectory_length
+    if (dist_i > param.max_trajectory_length) {
+      break;  // distance is over max.
+    }
+
+    // Check if the distance is longer than input arclength
+    if (dist_i + front_arclength_value >= in_arclength.back()) {
+      is_endpoint_included = true;  // distance is over input endpoint.
+      break;
+    }
+
+    // Check if the distance is longer than minimum_trajectory_length
+    if (i > Nt && dist_i >= param.min_trajectory_length) {
+      if (
+        std::fabs(out_arclength.back() - (param.min_trajectory_length + front_arclength_value)) <
+        1e-3) {
+        out_arclength.back() = param.min_trajectory_length + front_arclength_value;
+      } else {
+        out_arclength.push_back(param.min_trajectory_length + front_arclength_value);
+      }
+      break;
+    }
+
+    // Handle Close Stop Point
+    if (dist_i > zero_vel_arclength_value && !is_zero_point_included) {
+      if (std::fabs(dist_i - zero_vel_arclength_value) > 1e-3) {
+        // dist_i is much bigger than zero_vel_arclength_value
+        if (
+          !out_arclength.empty() &&
+          std::fabs(out_arclength.back() - (zero_vel_arclength_value + front_arclength_value)) <
+            1e-3) {
+          out_arclength.back() = zero_vel_arclength_value + front_arclength_value;
+        } else {
+          out_arclength.push_back(zero_vel_arclength_value + front_arclength_value);
+        }
+      } else {
+        // dist_i is close to the zero_vel_arclength_value
+        dist_i = zero_vel_arclength_value;
+      }
+
+      is_zero_point_included = true;
+    }
+
+    out_arclength.push_back(dist_i + front_arclength_value);
+  }
+
+  auto output =
+    trajectory_utils::applyLinearInterpolation(in_arclength, input, out_arclength, true);
+  if (!output) {
+    RCLCPP_WARN(
+      rclcpp::get_logger("motion_velocity_smoother").get_child("resample"),
+      "fail trajectory interpolation. size : in_arclength = %lu, "
+      "input = %lu, out_arclength = %lu",
+      in_arclength.size(), input.size(), out_arclength.size());
+    return boost::none;
+  }
+
+  // add end point directly to consider the endpoint velocity.
+  if (is_endpoint_included) {
+    constexpr double ep_dist = 1.0E-3;
+    if (autoware_utils::calcDistance2d(output->back(), input.back()) < ep_dist) {
+      output->back() = input.back();
+    } else {
+      output->push_back(input.back());
+    }
+  }
+
+  return output;
+}
+
+boost::optional<TrajectoryPoints> resampleTrajectory(
+  const TrajectoryPoints & input, const size_t closest_id, const ResampleParam & param,
+  const double nominal_ds)
+{
+  // input arclength
+  std::vector<double> in_arclength = trajectory_utils::calcArclengthArray(input);
+
+  // Get the nearest point where velocity is zero
+  // to avoid getting closest_id as a stop point, search zero velocity index from closest_id + 1.
+  auto stop_id = autoware_utils::searchZeroVelocityIndex(input, closest_id + 1, input.size());
+  // Arc length from the closest point to the point where velocity is zero
+  double stop_arclength_value = param.max_trajectory_length;
+  if (stop_id) {
+    stop_arclength_value = std::min(
+      stop_arclength_value, autoware_utils::calcSignedArcLength(input, closest_id, *stop_id));
+  }
+
+  // Do dense resampling before the stop line(3[m] ahead of the stop line)
+  if (param.min_trajectory_length < stop_arclength_value) {
+    stop_arclength_value = param.min_trajectory_length;
+  }
+  if (in_arclength.back() < stop_arclength_value) {
+    stop_arclength_value = in_arclength.back();
+  }
+  double start_stop_arclength_value = std::max(stop_arclength_value - 3.0, 0.0);
+
+  std::vector<double> out_arclength;
+
+  // Step1. Resample front trajectory
+  // Arc length from the initial point to the closest point
+  const double front_arclength_value = trajectory_utils::calcArcLength(input, 0, closest_id);
+  for (double s = 0.0; s <= front_arclength_value; s += nominal_ds) {
+    out_arclength.push_back(s);
+  }
+  if (std::fabs(out_arclength.back() - front_arclength_value) < 1e-3) {
+    out_arclength.back() = front_arclength_value;
+  } else {
+    out_arclength.push_back(front_arclength_value);
+  }
+
+  // Step2. Resample behind trajectory
+  double dist_i{0.0};
+  bool is_zero_point_included{false};
+  bool is_endpoint_included{false};
+  while (true) {
+    double ds = nominal_ds;
+    if (start_stop_arclength_value <= dist_i && dist_i <= stop_arclength_value) {
+      // Dense sampling before the stop point
+      ds = 0.01;
+    }
+    dist_i += ds;
+
+    // Check if the distance is longer than max_trajectory_length
+    if (dist_i > param.max_trajectory_length) {
+      break;  // distance is over max.
+    }
+
+    // Check if the distance is longer than input arclength
+    if (dist_i + front_arclength_value >= in_arclength.back()) {
+      is_endpoint_included = true;  // distance is over input endpoint.
+      break;
+    }
+
+    // Check if the distance is longer than minimum_trajectory_length
+    if (dist_i >= param.min_trajectory_length) {
+      if (
+        std::fabs(out_arclength.back() - (param.min_trajectory_length + front_arclength_value)) <
+        1e-3) {
+        out_arclength.back() = param.min_trajectory_length + front_arclength_value;
+      } else {
+        out_arclength.push_back(param.min_trajectory_length + front_arclength_value);
+      }
+      break;
+    }
+
+    // Handle Close Stop Point
+    if (dist_i > stop_arclength_value && !is_zero_point_included) {
+      if (std::fabs(dist_i - stop_arclength_value) > 1e-3) {
+        // dist_i is much bigger than zero_vel_arclength_value
+        if (
+          !out_arclength.empty() &&
+          std::fabs(out_arclength.back() - (stop_arclength_value + front_arclength_value)) < 1e-3) {
+          out_arclength.back() = stop_arclength_value + front_arclength_value;
+        } else {
+          out_arclength.push_back(stop_arclength_value + front_arclength_value);
+        }
+      } else {
+        // dist_i is close to the zero_vel_arclength_value
+        dist_i = stop_arclength_value;
+      }
+
+      is_zero_point_included = true;
+    }
+
+    out_arclength.push_back(dist_i + front_arclength_value);
+  }
+
+  auto output = trajectory_utils::applyLinearInterpolation(in_arclength, input, out_arclength);
+  if (!output) {
+    RCLCPP_WARN(
+      rclcpp::get_logger("motion_velocity_smoother").get_child("resample"),
+      "fail trajectory interpolation. size : in_arclength = %lu, "
+      "input = %lu, out_arclength = %lu",
+      in_arclength.size(), input.size(), out_arclength.size());
+    return boost::none;
+  }
+
+  // add end point directly to consider the endpoint velocity.
+  if (is_endpoint_included) {
+    constexpr double ep_dist = 1.0E-3;
+    if (autoware_utils::calcDistance2d(output->back(), input.back()) < ep_dist) {
+      output->back() = input.back();
+    } else {
+      output->push_back(input.back());
+    }
+  }
+
+  return output;
+}
+
+}  // namespace resampling
+}  // namespace motion_velocity_smoother
diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp
new file mode 100644
index 0000000000000..da0c630a05c55
--- /dev/null
+++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp
@@ -0,0 +1,633 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <algorithm>
+#include <string>
+#include <tuple>
+#include <utility>
+#include <vector>
+
+// *INDENT-OFF*
+#include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp"
+// *INDENT-ON*
+
+namespace
+{
+using TrajectoryPoints = std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>;
+
+geometry_msgs::msg::Pose lerpByPose(
+  const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const double t)
+{
+  tf2::Transform tf_transform1, tf_transform2;
+  tf2::fromMsg(p1, tf_transform1);
+  tf2::fromMsg(p2, tf_transform2);
+  const auto & tf_point = tf2::lerp(tf_transform1.getOrigin(), tf_transform2.getOrigin(), t);
+  const auto & tf_quaternion =
+    tf2::slerp(tf_transform1.getRotation(), tf_transform2.getRotation(), t);
+
+  geometry_msgs::msg::Pose pose;
+  pose.position.x = tf_point.getX();
+  pose.position.y = tf_point.getY();
+  pose.position.z = tf_point.getZ();
+  pose.orientation = tf2::toMsg(tf_quaternion);
+  return pose;
+}
+
+bool applyMaxVelocity(
+  const double max_velocity, const size_t start_index, const size_t end_index,
+  TrajectoryPoints & output_trajectory)
+{
+  if (end_index < start_index || output_trajectory.size() < end_index) {
+    return false;
+  }
+
+  for (size_t i = start_index; i <= end_index; ++i) {
+    output_trajectory.at(i).longitudinal_velocity_mps =
+      std::min(output_trajectory.at(i).longitudinal_velocity_mps, static_cast<float>(max_velocity));
+    output_trajectory.at(i).acceleration_mps2 = 0.0;
+  }
+  return true;
+}
+
+}  // namespace
+
+namespace motion_velocity_smoother
+{
+AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother(const Param & smoother_param)
+: smoother_param_(smoother_param)
+{
+}
+void AnalyticalJerkConstrainedSmoother::setParam(const Param & smoother_param)
+{
+  smoother_param_ = smoother_param;
+}
+
+bool AnalyticalJerkConstrainedSmoother::apply(
+  const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
+  TrajectoryPoints & output, [[maybe_unused]] std::vector<TrajectoryPoints> & debug_trajectories)
+{
+  RCLCPP_DEBUG(logger_, "-------------------- Start --------------------");
+
+  // guard
+  if (input.empty()) {
+    RCLCPP_DEBUG(logger_, "Fail. input trajectory is empty");
+    return false;
+  }
+
+  // intput trajectory is cropped, so closest_index = 0
+  const size_t closest_index = 0;
+
+  // Find deceleration targets
+  if (input.size() == 1) {
+    RCLCPP_DEBUG(
+      logger_,
+      "Input trajectory size is too short. Cannot find decel targets and "
+      "return v0, a0");
+    output = input;
+    output.front().longitudinal_velocity_mps = initial_vel;
+    output.front().acceleration_mps2 = initial_acc;
+    return true;
+  }
+  std::vector<std::pair<size_t, double>> decel_target_indices;
+  searchDecelTargetIndices(input, closest_index, decel_target_indices);
+  RCLCPP_DEBUG(logger_, "Num deceleration targets: %zd", decel_target_indices.size());
+  for (auto & index : decel_target_indices) {
+    RCLCPP_DEBUG(
+      logger_, "Target deceleration index: %ld, target velocity: %f", index.first, index.second);
+  }
+
+  // Apply filters according to deceleration targets
+  TrajectoryPoints reference_trajectory = input;
+  TrajectoryPoints filtered_trajectory = input;
+  for (size_t i = 0; i < decel_target_indices.size(); ++i) {
+    size_t fwd_start_index;
+    double fwd_start_vel;
+    double fwd_start_acc;
+    if (i == 0) {
+      fwd_start_index = closest_index;
+      fwd_start_vel = initial_vel;
+      fwd_start_acc = initial_acc;
+    } else {
+      fwd_start_index = decel_target_indices.at(i - 1).first;
+      fwd_start_vel = filtered_trajectory.at(fwd_start_index).longitudinal_velocity_mps;
+      fwd_start_acc = filtered_trajectory.at(fwd_start_index).acceleration_mps2;
+    }
+
+    RCLCPP_DEBUG(logger_, "Apply forward jerk filter from: %ld", fwd_start_index);
+    applyForwardJerkFilter(
+      reference_trajectory, fwd_start_index, fwd_start_vel, fwd_start_acc, smoother_param_,
+      filtered_trajectory);
+
+    size_t bwd_start_index = closest_index;
+    double bwd_start_vel = initial_vel;
+    double bwd_start_acc = initial_acc;
+    for (int j = i; j >= 0; --j) {
+      if (j == 0) {
+        bwd_start_index = closest_index;
+        bwd_start_vel = initial_vel;
+        bwd_start_acc = initial_acc;
+        break;
+      }
+      if (decel_target_indices.at(j - 1).second < decel_target_indices.at(j).second) {
+        bwd_start_index = decel_target_indices.at(j - 1).first;
+        bwd_start_vel = filtered_trajectory.at(bwd_start_index).longitudinal_velocity_mps;
+        bwd_start_acc = filtered_trajectory.at(bwd_start_index).acceleration_mps2;
+        break;
+      }
+    }
+    std::vector<size_t> start_indices;
+    if (bwd_start_index != fwd_start_index) {
+      start_indices.push_back(bwd_start_index);
+      start_indices.push_back(fwd_start_index);
+    } else {
+      start_indices.push_back(bwd_start_index);
+    }
+
+    const size_t decel_target_index = decel_target_indices.at(i).first;
+    const double decel_target_vel = decel_target_indices.at(i).second;
+    RCLCPP_DEBUG(
+      logger_, "Apply backward decel filter from: %s, to: %ld (%f)",
+      strStartIndices(start_indices).c_str(), decel_target_index, decel_target_vel);
+    if (!applyBackwardDecelFilter(
+          start_indices, decel_target_index, decel_target_vel, smoother_param_,
+          filtered_trajectory)) {
+      RCLCPP_DEBUG(
+        logger_,
+        "Failed to apply backward decel filter, so apply max velocity filter. max velocity = %f, "
+        "start_index = %s, end_index = %zd",
+        decel_target_vel, strStartIndices(start_indices).c_str(), filtered_trajectory.size() - 1);
+
+      const double ep = 0.001;
+      if (std::abs(decel_target_vel) < ep) {
+        applyMaxVelocity(0.0, bwd_start_index, filtered_trajectory.size() - 1, filtered_trajectory);
+        output = filtered_trajectory;
+        RCLCPP_DEBUG(logger_, "-------------------- Finish --------------------");
+        return true;
+      }
+      applyMaxVelocity(decel_target_vel, bwd_start_index, decel_target_index, reference_trajectory);
+      RCLCPP_DEBUG(logger_, "Apply forward jerk filter from: %ld", bwd_start_index);
+      applyForwardJerkFilter(
+        reference_trajectory, bwd_start_index, bwd_start_vel, bwd_start_acc, smoother_param_,
+        filtered_trajectory);
+    }
+  }
+
+  size_t start_index;
+  double start_vel;
+  double start_acc;
+  if (decel_target_indices.empty() == true) {
+    start_index = closest_index;
+    start_vel = initial_vel;
+    start_acc = initial_acc;
+  } else {
+    start_index = decel_target_indices.back().first;
+    start_vel = filtered_trajectory.at(start_index).longitudinal_velocity_mps;
+    start_acc = filtered_trajectory.at(start_index).acceleration_mps2;
+  }
+  RCLCPP_DEBUG(logger_, "Apply forward jerk filter from: %ld", start_index);
+  applyForwardJerkFilter(
+    reference_trajectory, start_index, start_vel, start_acc, smoother_param_, filtered_trajectory);
+
+  output = filtered_trajectory;
+
+  RCLCPP_DEBUG(logger_, "-------------------- Finish --------------------");
+  return true;
+}
+
+boost::optional<TrajectoryPoints> AnalyticalJerkConstrainedSmoother::resampleTrajectory(
+  const TrajectoryPoints & input, [[maybe_unused]] const double v_current,
+  [[maybe_unused]] const int closest_id) const
+{
+  TrajectoryPoints output;
+  if (input.empty()) {
+    RCLCPP_WARN(logger_, "Input trajectory is empty");
+    return {};
+  }
+
+  const double ds = 1.0 / static_cast<double>(smoother_param_.resample.num_resample);
+
+  for (size_t i = 0; i < input.size() - 1; ++i) {
+    double s = 0.0;
+    const auto tp0 = input.at(i);
+    const auto tp1 = input.at(i + 1);
+
+    const double dist_thr = 0.001;  // 1mm
+    const double dist_tp0_tp1 = autoware_utils::calcDistance2d(tp0, tp1);
+    if (std::fabs(dist_tp0_tp1) < dist_thr) {
+      output.push_back(input.at(i));
+      continue;
+    }
+
+    for (size_t j = 0; j < smoother_param_.resample.num_resample; ++j) {
+      auto tp = input.at(i);
+
+      tp.pose = lerpByPose(tp0.pose, tp1.pose, s);
+      tp.longitudinal_velocity_mps = tp0.longitudinal_velocity_mps;
+      tp.heading_rate_rps = (1.0 - s) * tp0.heading_rate_rps + s * tp1.heading_rate_rps;
+      tp.acceleration_mps2 = tp0.acceleration_mps2;
+      // tp.accel.angular.z = (1.0 - s) * tp0.accel.angular.z + s * tp1.accel.angular.z;
+
+      output.push_back(tp);
+
+      s += ds;
+    }
+  }
+
+  output.push_back(input.back());
+
+  return boost::optional<TrajectoryPoints>(output);
+}
+
+boost::optional<TrajectoryPoints> AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilter(
+  const TrajectoryPoints & input) const
+{
+  if (input.empty()) {
+    return boost::none;
+  }
+
+  if (input.size() < 3) {
+    return boost::optional<TrajectoryPoints>(input);  // cannot calculate lateral acc. do nothing.
+  }
+
+  // Interpolate with constant interval distance for lateral acceleration calculation.
+  constexpr double points_interval = 0.1;  // [m]
+  std::vector<double> out_arclength;
+  const std::vector<double> in_arclength = trajectory_utils::calcArclengthArray(input);
+  for (double s = 0; s < in_arclength.back(); s += points_interval) {
+    out_arclength.push_back(s);
+  }
+  auto output = trajectory_utils::applyLinearInterpolation(in_arclength, input, out_arclength);
+  if (!output) {
+    RCLCPP_WARN(logger_, "Interpolation failed at lateral acceleration filter.");
+    return boost::none;
+  }
+  output->back() = input.back();  // keep the final speed.
+
+  constexpr double curvature_calc_dist = 5.0;  // [m] calc curvature with 5m away points
+  const size_t idx_dist =
+    static_cast<size_t>(std::max(static_cast<int>((curvature_calc_dist) / points_interval), 1));
+
+  // Calculate curvature assuming the trajectory points interval is constant
+  const auto curvature_v = trajectory_utils::calcTrajectoryCurvatureFrom3Points(*output, idx_dist);
+  if (!curvature_v) {
+    return boost::optional<TrajectoryPoints>(input);
+  }
+
+  // Decrease speed according to lateral G
+  const size_t before_decel_index =
+    static_cast<size_t>(std::round(base_param_.decel_distance_before_curve / points_interval));
+  const size_t after_decel_index =
+    static_cast<size_t>(std::round(base_param_.decel_distance_after_curve / points_interval));
+  const double max_lateral_accel_abs = std::fabs(base_param_.max_lateral_accel);
+
+  std::vector<int> filtered_points;
+  for (size_t i = 0; i < output->size(); ++i) {
+    double curvature = 0.0;
+    const size_t start = i > before_decel_index ? i - before_decel_index : 0;
+    const size_t end = std::min(output->size(), i + after_decel_index);
+    for (size_t j = start; j < end; ++j) {
+      curvature = std::max(curvature, std::fabs(curvature_v->at(j)));
+    }
+    double v_curvature_max = std::sqrt(max_lateral_accel_abs / std::max(curvature, 1.0E-5));
+    v_curvature_max = std::max(v_curvature_max, base_param_.min_curve_velocity);
+    if (output->at(i).longitudinal_velocity_mps > v_curvature_max) {
+      output->at(i).longitudinal_velocity_mps = v_curvature_max;
+      filtered_points.push_back(i);
+    }
+  }
+
+  // Keep constant velocity while turning
+  const double dist_threshold = smoother_param_.latacc.constant_velocity_dist_threshold;
+  std::vector<std::tuple<size_t, size_t, double>> latacc_filtered_ranges;
+  size_t start_index = 0;
+  size_t end_index = 0;
+  bool is_updated = false;
+  double min_latacc_velocity;
+  for (size_t i = 0; i < filtered_points.size(); ++i) {
+    const size_t index = filtered_points.at(i);
+
+    if (is_updated == false) {
+      start_index = index;
+      end_index = index;
+      min_latacc_velocity = output->at(index).longitudinal_velocity_mps;
+      is_updated = true;
+      continue;
+    }
+
+    if (autoware_utils::calcDistance2d(output->at(end_index), output->at(index)) < dist_threshold) {
+      end_index = index;
+      min_latacc_velocity = std::min(
+        static_cast<double>(output->at(index).longitudinal_velocity_mps), min_latacc_velocity);
+    } else {
+      latacc_filtered_ranges.emplace_back(start_index, end_index, min_latacc_velocity);
+      start_index = index;
+      end_index = index;
+      min_latacc_velocity = output->at(index).longitudinal_velocity_mps;
+    }
+  }
+  if (is_updated) {
+    latacc_filtered_ranges.emplace_back(start_index, end_index, min_latacc_velocity);
+  }
+
+  for (size_t i = 0; i < output->size(); ++i) {
+    for (const auto & lat_acc_filtered_range : latacc_filtered_ranges) {
+      const size_t start_index = std::get<0>(lat_acc_filtered_range);
+      const size_t end_index = std::get<1>(lat_acc_filtered_range);
+      const double min_latacc_velocity = std::get<2>(lat_acc_filtered_range);
+
+      if (
+        start_index <= i && i <= end_index &&
+        smoother_param_.latacc.enable_constant_velocity_while_turning) {
+        output->at(i).longitudinal_velocity_mps = min_latacc_velocity;
+        break;
+      }
+    }
+  }
+
+  return output;
+}
+
+bool AnalyticalJerkConstrainedSmoother::searchDecelTargetIndices(
+  const TrajectoryPoints & trajectory, const size_t closest_index,
+  std::vector<std::pair<size_t, double>> & decel_target_indices) const
+{
+  const double ep = -0.00001;
+  const size_t start_index = std::max<size_t>(1, closest_index);
+  std::vector<std::pair<size_t, double>> tmp_indices;
+  for (size_t i = start_index; i < trajectory.size() - 1; ++i) {
+    const double dv_before =
+      trajectory.at(i).longitudinal_velocity_mps - trajectory.at(i - 1).longitudinal_velocity_mps;
+    const double dv_after =
+      trajectory.at(i + 1).longitudinal_velocity_mps - trajectory.at(i).longitudinal_velocity_mps;
+    if (dv_before < ep && dv_after > ep) {
+      tmp_indices.emplace_back(i, trajectory.at(i).longitudinal_velocity_mps);
+    }
+  }
+
+  const unsigned int i = trajectory.size() - 1;
+  const double dv_before =
+    trajectory.at(i).longitudinal_velocity_mps - trajectory.at(i - 1).longitudinal_velocity_mps;
+  if (dv_before < ep) {
+    tmp_indices.emplace_back(i, trajectory.at(i).longitudinal_velocity_mps);
+  }
+
+  if (!tmp_indices.empty()) {
+    for (unsigned int i = 0; i < tmp_indices.size() - 1; ++i) {
+      const size_t index_err = 10;
+      if (
+        (tmp_indices.at(i + 1).first - tmp_indices.at(i).first < index_err) &&
+        (tmp_indices.at(i + 1).second < tmp_indices.at(i).second)) {
+        continue;
+      }
+
+      decel_target_indices.emplace_back(tmp_indices.at(i).first, tmp_indices.at(i).second);
+    }
+  }
+  if (!tmp_indices.empty()) {
+    decel_target_indices.emplace_back(tmp_indices.back().first, tmp_indices.back().second);
+  }
+  return true;
+}
+
+bool AnalyticalJerkConstrainedSmoother::applyForwardJerkFilter(
+  const TrajectoryPoints & base_trajectory, const size_t start_index, const double initial_vel,
+  const double initial_acc, const Param & params, TrajectoryPoints & output_trajectory) const
+{
+  output_trajectory.at(start_index).longitudinal_velocity_mps = initial_vel;
+  output_trajectory.at(start_index).acceleration_mps2 = initial_acc;
+
+  for (size_t i = start_index + 1; i < base_trajectory.size(); ++i) {
+    const double prev_vel = output_trajectory.at(i - 1).longitudinal_velocity_mps;
+    const double ds =
+      autoware_utils::calcDistance2d(base_trajectory.at(i - 1), base_trajectory.at(i));
+    const double dt = ds / std::max(prev_vel, 1.0);
+
+    const double prev_acc = output_trajectory.at(i - 1).acceleration_mps2;
+    const double curr_vel = std::max(prev_vel + prev_acc * dt, 0.0);
+
+    const double error_vel = base_trajectory.at(i).longitudinal_velocity_mps - curr_vel;
+    const double fb_acc = params.forward.kp * error_vel;
+    const double limited_acc =
+      std::max(params.forward.min_acc, std::min(params.forward.max_acc, fb_acc));
+    const double fb_jerk = (limited_acc - prev_acc) / dt;
+    const double limited_jerk =
+      std::max(params.forward.min_jerk, std::min(params.forward.max_jerk, fb_jerk));
+
+    const double curr_acc = prev_acc + limited_jerk * dt;
+
+    output_trajectory.at(i).longitudinal_velocity_mps = curr_vel;
+    output_trajectory.at(i).acceleration_mps2 = curr_acc;
+  }
+
+  return true;
+}
+
+bool AnalyticalJerkConstrainedSmoother::applyBackwardDecelFilter(
+  const std::vector<size_t> & start_indices, const size_t decel_target_index,
+  const double decel_target_vel, const Param & params, TrajectoryPoints & output_trajectory) const
+{
+  const double ep = 0.001;
+
+  double output_planning_jerk = -100.0;
+  size_t output_start_index = 0;
+  std::vector<double> output_dist_to_target;
+  int output_type;
+  std::vector<double> output_times;
+
+  for (size_t start_index : start_indices) {
+    double dist = 0.0;
+    std::vector<double> dist_to_target(output_trajectory.size(), 0);
+    dist_to_target.at(decel_target_index) = dist;
+    for (size_t i = start_index; i < decel_target_index; ++i) {
+      if (output_trajectory.at(i).longitudinal_velocity_mps >= decel_target_vel) {
+        start_index = i;
+        break;
+      }
+    }
+    for (size_t i = decel_target_index; i > start_index; --i) {
+      dist += autoware_utils::calcDistance2d(output_trajectory.at(i - 1), output_trajectory.at(i));
+      dist_to_target.at(i - 1) = dist;
+    }
+
+    RCLCPP_DEBUG(logger_, "Check enough dist to decel. start_index: %ld", start_index);
+    double planning_jerk;
+    int type;
+    std::vector<double> times;
+    double stop_dist;
+    bool is_enough_dist = false;
+    for (planning_jerk = params.backward.start_jerk; planning_jerk > params.backward.min_jerk - ep;
+         planning_jerk += params.backward.span_jerk) {
+      if (calcEnoughDistForDecel(
+            output_trajectory, start_index, decel_target_vel, planning_jerk, params, dist_to_target,
+            is_enough_dist, type, times, stop_dist)) {
+        break;
+      }
+    }
+
+    if (!is_enough_dist) {
+      RCLCPP_DEBUG(logger_, "Distance is not enough for decel with all jerk condition");
+      continue;
+    }
+
+    if (planning_jerk >= output_planning_jerk) {
+      output_planning_jerk = planning_jerk;
+      output_start_index = start_index;
+      output_dist_to_target = dist_to_target;
+      output_type = type;
+      output_times = times;
+      RCLCPP_DEBUG(
+        logger_, "Update planning jerk: %f, start_index: %ld", planning_jerk, start_index);
+    }
+  }
+
+  if (output_planning_jerk == -100.0) {
+    RCLCPP_DEBUG(
+      logger_,
+      "Distance is not enough for decel with all jerk and start index "
+      "condition");
+    return false;
+  }
+
+  RCLCPP_DEBUG(logger_, "Search decel start index");
+  size_t decel_start_index = output_start_index;
+  bool is_enough_dist = false;
+  double stop_dist;
+  if (output_planning_jerk == params.backward.start_jerk) {
+    for (size_t i = decel_target_index - 1; i >= output_start_index; --i) {
+      if (calcEnoughDistForDecel(
+            output_trajectory, i, decel_target_vel, output_planning_jerk, params,
+            output_dist_to_target, is_enough_dist, output_type, output_times, stop_dist)) {
+        decel_start_index = i;
+        break;
+      }
+    }
+  }
+
+  RCLCPP_DEBUG(
+    logger_,
+    "Apply filter. decel_start_index: %ld, target_vel: %f, "
+    "planning_jerk: %f, type: %d, times: %s",
+    decel_start_index, decel_target_vel, output_planning_jerk, output_type,
+    strTimes(output_times).c_str());
+  if (!applyDecelVelocityFilter(
+        decel_start_index, decel_target_vel, output_planning_jerk, params, output_type,
+        output_times, output_trajectory)) {
+    RCLCPP_DEBUG(
+      logger_,
+      "[applyDecelVelocityFilter] dist is enough, but fail to plan backward decel velocity");
+    return false;
+  }
+
+  return true;
+}
+
+bool AnalyticalJerkConstrainedSmoother::calcEnoughDistForDecel(
+  const TrajectoryPoints & trajectory, const size_t start_index, const double decel_target_vel,
+  const double planning_jerk, const Param & params, const std::vector<double> & dist_to_target,
+  bool & is_enough_dist, int & type, std::vector<double> & times, double & stop_dist) const
+{
+  const double v0 = trajectory.at(start_index).longitudinal_velocity_mps;
+  const double a0 = trajectory.at(start_index).acceleration_mps2;
+  const double jerk_acc = std::abs(planning_jerk);
+  const double jerk_dec = planning_jerk;
+  // *INDENT-OFF*
+  auto calcMinAcc = [&params](const double planning_jerk) {
+    if (planning_jerk < params.backward.min_jerk_mild_stop) {
+      return params.backward.min_acc;
+    }
+    return params.backward.min_acc_mild_stop;
+  };
+  // *INDENT-ON*
+  const double min_acc = calcMinAcc(planning_jerk);
+  type = 0;
+  times.clear();
+  stop_dist = 0.0;
+
+  if (!analytical_velocity_planning_utils::calcStopDistWithJerkAndAccConstraints(
+        v0, a0, jerk_acc, jerk_dec, min_acc, decel_target_vel, type, times, stop_dist)) {
+    return false;
+  }
+
+  const double allowed_dist = dist_to_target.at(start_index);
+  if (0.0 <= stop_dist && stop_dist <= allowed_dist) {
+    RCLCPP_DEBUG(
+      logger_,
+      "Distance is enough. v0: %f, a0: %f, jerk: %f, stop_dist: %f, "
+      "allowed_dist: %f",
+      v0, a0, planning_jerk, stop_dist, allowed_dist);
+    is_enough_dist = true;
+    return true;
+  }
+  RCLCPP_DEBUG(
+    logger_,
+    "Distance is not enough. v0: %f, a0: %f, jerk: %f, stop_dist: %f, "
+    "allowed_dist: %f",
+    v0, a0, planning_jerk, stop_dist, allowed_dist);
+  return false;
+}
+
+bool AnalyticalJerkConstrainedSmoother::applyDecelVelocityFilter(
+  const size_t decel_start_index, const double decel_target_vel, const double planning_jerk,
+  const Param & params, const int type, const std::vector<double> & times,
+  TrajectoryPoints & output_trajectory) const
+{
+  const double v0 = output_trajectory.at(decel_start_index).longitudinal_velocity_mps;
+  const double a0 = output_trajectory.at(decel_start_index).acceleration_mps2;
+  const double jerk_acc = std::abs(planning_jerk);
+  const double jerk_dec = planning_jerk;
+  // *INDENT-OFF*
+  auto calcMinAcc = [&params](const double planning_jerk) {
+    if (planning_jerk < params.backward.min_jerk_mild_stop) {
+      return params.backward.min_acc;
+    }
+    return params.backward.min_acc_mild_stop;
+  };
+  // *INDENT-ON*
+  const double min_acc = calcMinAcc(planning_jerk);
+
+  if (!analytical_velocity_planning_utils::calcStopVelocityWithConstantJerkAccLimit(
+        v0, a0, jerk_acc, jerk_dec, min_acc, decel_target_vel, type, times, decel_start_index,
+        output_trajectory)) {
+    return false;
+  }
+
+  return true;
+}
+
+std::string AnalyticalJerkConstrainedSmoother::strTimes(const std::vector<double> & times) const
+{
+  std::stringstream ss;
+  unsigned int i = 0;
+  for (double time : times) {
+    ss << "time[" << i << "] = " << time << ", ";
+    i++;
+  }
+  return ss.str();
+}
+
+std::string AnalyticalJerkConstrainedSmoother::strStartIndices(
+  const std::vector<size_t> & start_indices) const
+{
+  std::stringstream ss;
+  for (size_t i = 0; i < start_indices.size(); ++i) {
+    if (i != (start_indices.size() - 1)) {
+      ss << start_indices.at(i) << ", ";
+    } else {
+      ss << start_indices.at(i);
+    }
+  }
+  return ss.str();
+}
+
+}  // namespace motion_velocity_smoother
diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp
new file mode 100644
index 0000000000000..4d9da44ab8fbe
--- /dev/null
+++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp
@@ -0,0 +1,342 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <algorithm>
+#include <vector>
+
+// *INDENT-OFF*
+#include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp"
+// *INDENT-ON*
+
+namespace motion_velocity_smoother
+{
+namespace analytical_velocity_planning_utils
+{
+bool calcStopDistWithJerkAndAccConstraints(
+  const double v0, const double a0, const double jerk_acc, const double jerk_dec,
+  const double min_acc, const double target_vel, int & type, std::vector<double> & times,
+  double & stop_dist)
+{
+  const double t_min =
+    (target_vel - v0 - 0.5 * (0 - a0) / jerk_dec * a0 - 0.5 * min_acc / jerk_dec * min_acc -
+     0.5 * (0 - min_acc) / jerk_acc * min_acc) /
+    min_acc;
+
+  if (t_min > 0) {
+    double t1 = (min_acc - a0) / jerk_dec;
+    if (t1 < 0.01) {
+      t1 = 0;
+    }
+
+    const double a1 = a0 + jerk_dec * t1;
+    const double v1 = v0 + a0 * t1 + 0.5 * jerk_dec * t1 * t1;
+    const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * jerk_dec * t1 * t1 * t1;
+
+    double t2 = t_min;
+    if (t2 < 0.01) {
+      t2 = 0;
+    }
+
+    const double a2 = a1;
+    const double v2 = v1 + a1 * t2;
+    const double x2 = x1 + v1 * t2 + 0.5 * a1 * t2 * t2;
+
+    double t3 = (0 - min_acc) / jerk_acc;
+    if (t3 < 0.01) {
+      t3 = 0;
+    }
+
+    const double a3 = a2 + jerk_acc * t3;
+    const double v3 = v2 + a2 * t3 + 0.5 * jerk_acc * t3 * t3;
+    const double x3 = x2 + v2 * t3 + 0.5 * a2 * t3 * t3 + (1.0 / 6.0) * jerk_acc * t3 * t3 * t3;
+
+    const double a_target = 0.0;
+    const double v_margin = 0.3;  // [m/s]
+    const double a_margin = 0.1;  // [m/s^2]
+    if (!validCheckCalcStopDist(v3, a3, target_vel, a_target, v_margin, a_margin)) {
+      RCLCPP_DEBUG(rclcpp::get_logger("velocity_planning_utils"), "Valid check error. type = 1");
+      return false;
+    }
+
+    type = 1;
+    times.push_back(t1);
+    times.push_back(t2);
+    times.push_back(t3);
+    stop_dist = x3;
+  } else {
+    const double is_decel_needed = 0.5 * (0 - a0) / jerk_acc * a0 - (target_vel - v0);
+    if (is_decel_needed > 0 || a0 > 0) {
+      const double a1_square = (target_vel - v0 - 0.5 * (0 - a0) / jerk_dec * a0) *
+                               (2 * jerk_acc * jerk_dec / (jerk_acc - jerk_dec));
+      const double a1 = -std::sqrt(a1_square);
+
+      double t1 = (a1 - a0) / jerk_dec;
+      if (t1 < 0.01) {
+        t1 = 0;
+      }
+
+      const double v1 = v0 + a0 * t1 + 0.5 * jerk_dec * t1 * t1;
+      const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * jerk_dec * t1 * t1 * t1;
+
+      double t2 = (0 - a1) / jerk_acc;
+      if (t2 < 0.01) {
+        t2 = 0;
+      }
+
+      const double a2 = a1 + jerk_acc * t2;
+      const double v2 = v1 + a1 * t2 + 0.5 * jerk_acc * t2 * t2;
+      const double x2 = x1 + v1 * t2 + 0.5 * a1 * t2 * t2 + (1.0 / 6.0) * jerk_acc * t2 * t2 * t2;
+
+      const double a_target = 0.0;
+      const double v_margin = 0.3;
+      const double a_margin = 0.1;
+      if (!validCheckCalcStopDist(v2, a2, target_vel, a_target, v_margin, a_margin)) {
+        RCLCPP_DEBUG(rclcpp::get_logger("velocity_planning_utils"), "Valid check error. type = 2");
+        return false;
+      }
+
+      type = 2;
+      times.push_back(t1);
+      times.push_back(t2);
+      stop_dist = x2;
+    } else {
+      double t1 = (0 - a0) / jerk_acc;
+      if (t1 < 0) {
+        RCLCPP_DEBUG(
+          rclcpp::get_logger("velocity_planning_utils"), "t1 < 0. unexpected condition.");
+        return false;
+      }
+      if (t1 < 0.01) {
+        t1 = 0;
+      }
+
+      const double a1 = a0 + jerk_acc * t1;
+      const double v1 = v0 + a0 * t1 + 0.5 * jerk_acc * t1 * t1;
+      const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * jerk_acc * t1 * t1 * t1;
+
+      const double a_target = 0.0;
+      const double v_margin = 0.3;
+      const double a_margin = 0.1;
+      if (!validCheckCalcStopDist(v1, a1, target_vel, a_target, v_margin, a_margin)) {
+        RCLCPP_DEBUG(rclcpp::get_logger("velocity_planning_utils"), "Valid check error. type = 3");
+        return false;
+      }
+
+      type = 3;
+      times.push_back(t1);
+      stop_dist = x1;
+    }
+  }
+  return true;
+}
+
+bool validCheckCalcStopDist(
+  const double v_end, const double a_end, const double v_target, const double a_target,
+  const double v_margin, const double a_margin)
+{
+  const double v_min = v_target - std::abs(v_margin);
+  const double v_max = v_target + std::abs(v_margin);
+  const double a_min = a_target - std::abs(a_margin);
+  const double a_max = a_target + std::abs(a_margin);
+  if (v_end < v_min || v_max < v_end) {
+    RCLCPP_DEBUG(
+      rclcpp::get_logger("velocity_planning_utils"), "Valid check error! v_target = %f, v_end = %f",
+      v_target, v_end);
+    return false;
+  }
+  if (a_end < a_min || a_max < a_end) {
+    RCLCPP_DEBUG(
+      rclcpp::get_logger("velocity_planning_utils"), "Valid check error! a_target = %f, a_end = %f",
+      a_target, a_end);
+    return false;
+  }
+  return true;
+}
+
+bool calcStopVelocityWithConstantJerkAccLimit(
+  const double v0, const double a0, const double jerk_acc, const double jerk_dec,
+  const double min_acc, const double decel_target_vel, const int type,
+  const std::vector<double> & times, const size_t start_index, TrajectoryPoints & output_trajectory)
+{
+  const double t_total = std::accumulate(times.begin(), times.end(), 0.0);
+  std::vector<double> ts, xs, vs, as, js;
+  const double dt = 0.1;
+  double x = 0.0;
+  double v = 0.0;
+  double a = 0.0;
+  double j = 0.0;
+
+  for (double t = 0.0; t < t_total; t += dt) {
+    updateStopVelocityStatus(v0, a0, jerk_acc, jerk_dec, type, times, t, x, v, a, j);
+    if (v > 0.0) {
+      a = std::max(a, min_acc);
+      ts.push_back(t);
+      xs.push_back(x);
+      vs.push_back(v);
+      as.push_back(a);
+      js.push_back(j);
+    }
+  }
+  updateStopVelocityStatus(v0, a0, jerk_acc, jerk_dec, type, times, t_total, x, v, a, j);
+  if (v > 0.0 && !xs.empty() && xs.back() < x) {
+    a = std::max(a, min_acc);
+    ts.push_back(t_total);
+    xs.push_back(x);
+    vs.push_back(v);
+    as.push_back(a);
+    js.push_back(j);
+  }
+
+  // for debug
+  std::stringstream ss;
+  for (unsigned int i = 0; i < ts.size(); ++i) {
+    ss << "t: " << ts.at(i) << ", x: " << xs.at(i) << ", v: " << vs.at(i) << ", a: " << as.at(i)
+       << ", j: " << js.at(i) << std::endl;
+  }
+  RCLCPP_DEBUG(
+    rclcpp::get_logger("velocity_planning_utils"), "Calculate stop velocity. %s", ss.str().c_str());
+
+  const double a_target = 0.0;
+  const double v_margin = 0.3;
+  const double a_margin = 0.1;
+  if (!validCheckCalcStopDist(v, a, decel_target_vel, a_target, v_margin, a_margin)) {
+    return false;
+  }
+
+  if (xs.empty()) {
+    for (size_t i = start_index; i < output_trajectory.size(); ++i) {
+      output_trajectory.at(i).longitudinal_velocity_mps = decel_target_vel;
+      output_trajectory.at(i).acceleration_mps2 = 0.0;
+    }
+    return true;
+  }
+
+  double dist = 0.0;
+  std::vector<double> dists;
+  dists.push_back(dist);
+  for (size_t i = start_index; i < output_trajectory.size() - 1; ++i) {
+    dist += autoware_utils::calcDistance2d(output_trajectory.at(i), output_trajectory.at(i + 1));
+    if (dist > xs.back()) {
+      break;
+    }
+    dists.push_back(dist);
+  }
+
+  const auto vel_at_wp = linear_interpolation::interpolate(xs, vs, dists);
+  const auto acc_at_wp = linear_interpolation::interpolate(xs, as, dists);
+  const auto jerk_at_wp = linear_interpolation::interpolate(xs, js, dists);
+  if (!vel_at_wp || !acc_at_wp || !jerk_at_wp) {
+    RCLCPP_DEBUG(rclcpp::get_logger("velocity_planning_utils"), "Interpolation error");
+    return false;
+  }
+
+  // for debug
+  std::stringstream ssi;
+  for (unsigned int i = 0; i < dists.size(); ++i) {
+    ssi << "d: " << dists.at(i) << ", v: " << vel_at_wp->at(i) << ", a: " << acc_at_wp->at(i)
+        << ", j: " << jerk_at_wp->at(i) << std::endl;
+  }
+  RCLCPP_DEBUG(
+    rclcpp::get_logger("velocity_planning_utils"), "Interpolated = %s", ssi.str().c_str());
+
+  for (size_t i = 0; i < vel_at_wp->size(); ++i) {
+    output_trajectory.at(start_index + i).longitudinal_velocity_mps = vel_at_wp->at(i);
+    output_trajectory.at(start_index + i).acceleration_mps2 = acc_at_wp->at(i);
+  }
+  for (size_t i = start_index + vel_at_wp->size(); i < output_trajectory.size(); ++i) {
+    output_trajectory.at(i).longitudinal_velocity_mps = decel_target_vel;
+    output_trajectory.at(i).acceleration_mps2 = 0.0;
+  }
+
+  return true;
+}
+
+void updateStopVelocityStatus(
+  double v0, double a0, double jerk_acc, double jerk_dec, int type, std::vector<double> times,
+  double t, double & x, double & v, double & a, double & j)
+{
+  if (type == 1) {
+    if (0 <= t && t < times.at(0)) {
+      j = jerk_dec;
+      a = integ_a(a0, j, t);
+      v = integ_v(v0, a0, j, t);
+      x = integ_x(0, v0, a0, j, t);
+    } else if (times.at(0) <= t && t < times.at(0) + times.at(1)) {
+      const double t1 = times.at(0);
+      const double a1 = integ_a(a0, jerk_dec, t1);
+      const double v1 = integ_v(v0, a0, jerk_dec, t1);
+      const double x1 = integ_x(0, v0, a0, jerk_dec, t1);
+
+      const double dt = t - t1;
+      j = 0;
+      a = integ_a(a1, j, dt);
+      v = integ_v(v1, a1, j, dt);
+      x = integ_x(x1, v1, a1, j, dt);
+    } else if (times.at(0) + times.at(1) <= t && t <= times.at(0) + times.at(1) + times.at(2)) {
+      const double t1 = times.at(0);
+      const double a1 = integ_a(a0, jerk_dec, t1);
+      const double v1 = integ_v(v0, a0, jerk_dec, t1);
+      const double x1 = integ_x(0, v0, a0, jerk_dec, t1);
+
+      const double t2 = times.at(1);
+      const double a2 = integ_a(a1, 0, t2);
+      const double v2 = integ_v(v1, a1, 0, t2);
+      const double x2 = integ_x(x1, v1, a1, 0, t2);
+
+      const double dt = t - (t1 + t2);
+      j = jerk_acc;
+      a = integ_a(a2, j, dt);
+      v = integ_v(v2, a2, j, dt);
+      x = integ_x(x2, v2, a2, j, dt);
+    }
+  } else if (type == 2) {
+    if (0 <= t && t < times.at(0)) {
+      j = jerk_dec;
+      a = integ_a(a0, j, t);
+      v = integ_v(v0, a0, j, t);
+      x = integ_x(0, v0, a0, j, t);
+    } else if (times.at(0) <= t && t <= times.at(0) + times.at(1)) {
+      const double t1 = times.at(0);
+      const double a1 = integ_a(a0, jerk_dec, t1);
+      const double v1 = integ_v(v0, a0, jerk_dec, t1);
+      const double x1 = integ_x(0, v0, a0, jerk_dec, t1);
+
+      const double dt = t - t1;
+      j = jerk_acc;
+      a = integ_a(a1, j, dt);
+      v = integ_v(v1, a1, j, dt);
+      x = integ_x(x1, v1, a1, j, dt);
+    }
+  } else if (type == 3) {
+    if (0 <= t && t <= times.at(0)) {
+      j = jerk_acc;
+      a = integ_a(a0, j, t);
+      v = integ_v(v0, a0, j, t);
+      x = integ_x(0, v0, a0, j, t);
+    }
+  } else {
+  }
+}
+
+double integ_x(double x0, double v0, double a0, double j0, double t)
+{
+  return x0 + v0 * t + 0.5 * a0 * t * t + (1.0 / 6.0) * j0 * t * t * t;
+}
+
+double integ_v(double v0, double a0, double j0, double t) { return v0 + a0 * t + 0.5 * j0 * t * t; }
+
+double integ_a(double a0, double j0, double t) { return a0 + j0 * t; }
+
+}  // namespace analytical_velocity_planning_utils
+}  // namespace motion_velocity_smoother
diff --git a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp
new file mode 100644
index 0000000000000..596df3d26f6f1
--- /dev/null
+++ b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp
@@ -0,0 +1,466 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp"
+
+#include "eigen3/Eigen/Core"
+#include "motion_velocity_smoother/trajectory_utils.hpp"
+
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <limits>
+#include <numeric>
+#include <vector>
+
+namespace motion_velocity_smoother
+{
+JerkFilteredSmoother::JerkFilteredSmoother(const Param & smoother_param)
+: smoother_param_{smoother_param}
+{
+  qp_solver_.updateMaxIter(20000);
+  qp_solver_.updateRhoInterval(0);  // 0 means automatic
+  qp_solver_.updateEpsRel(1.0e-4);  // def: 1.0e-4
+  qp_solver_.updateEpsAbs(1.0e-8);  // def: 1.0e-4
+  qp_solver_.updateVerbose(false);
+}
+
+void JerkFilteredSmoother::setParam(const Param & smoother_param)
+{
+  smoother_param_ = smoother_param;
+}
+
+bool JerkFilteredSmoother::apply(
+  const double v0, const double a0, const TrajectoryPoints & input, TrajectoryPoints & output,
+  std::vector<TrajectoryPoints> & debug_trajectories)
+{
+  output = input;
+
+  if (input.empty()) {
+    RCLCPP_WARN(logger_, "Input TrajectoryPoints to the jerk filtered optimization is empty.");
+    return false;
+  }
+
+  if (input.size() == 1) {
+    // No need to do optimization
+    output.front().longitudinal_velocity_mps = v0;
+    output.front().acceleration_mps2 = a0;
+    debug_trajectories.resize(3);
+    debug_trajectories[0] = output;
+    debug_trajectories[1] = output;
+    debug_trajectories[2] = output;
+    return true;
+  }
+
+  bool TMP_SHOW_DEBUG_INFO = false;
+
+  const auto ts = std::chrono::system_clock::now();
+
+  const double a_max = base_param_.max_accel;
+  const double a_min = base_param_.min_decel;
+  const double a_stop_accel = 0.0;
+  const double a_stop_decel = base_param_.stop_decel;
+  const double j_max = base_param_.max_jerk;
+  const double j_min = base_param_.min_jerk;
+  const double over_j_weight = smoother_param_.over_j_weight;
+  const double over_v_weight = smoother_param_.over_v_weight;
+  const double over_a_weight = smoother_param_.over_a_weight;
+
+  // jerk filter
+  const auto forward_filtered =
+    forwardJerkFilter(v0, std::max(a0, a_min), a_max, a_stop_accel, j_max, input);
+  const auto backward_filtered = backwardJerkFilter(
+    input.back().longitudinal_velocity_mps, a_stop_decel, a_min, a_stop_decel, j_min, input);
+  const auto filtered =
+    mergeFilteredTrajectory(v0, a0, a_min, j_min, forward_filtered, backward_filtered);
+
+  // Set debug trajectories
+  debug_trajectories.resize(3);
+  debug_trajectories[0] = forward_filtered;
+  debug_trajectories[1] = backward_filtered;
+  debug_trajectories[2] = filtered;
+
+  // Resample TrajectoryPoints for Optimization
+  auto opt_resampled_trajectory =
+    resampling::resampleTrajectory(filtered, v0, 0, base_param_.resample_param);
+
+  if (!opt_resampled_trajectory) {
+    RCLCPP_WARN(logger_, "Resample failed!");
+    return false;
+  }
+  // Ensure terminal velocity is zero
+  opt_resampled_trajectory->back().longitudinal_velocity_mps = 0.0;
+
+  // If Resampled Size is too small, we don't do optimization
+  if (opt_resampled_trajectory->size() == 1) {
+    // No need to do optimization
+    output.front().longitudinal_velocity_mps = v0;
+    output.front().acceleration_mps2 = a0;
+    debug_trajectories[0] = output;
+    debug_trajectories[1] = output;
+    debug_trajectories[2] = output;
+    return true;
+  }
+
+  // to avoid getting 0 as a stop point, search zero velocity index from 1.
+  // the size of the resampled trajectory must not be less than 2.
+  const auto zero_vel_id = autoware_utils::searchZeroVelocityIndex(
+    *opt_resampled_trajectory, 1, opt_resampled_trajectory->size());
+
+  if (!zero_vel_id) {
+    RCLCPP_WARN(logger_, "opt_resampled_trajectory must have stop point.");
+    return false;
+  }
+
+  // Clip trajectory from 0 to zero_vel_id (the size becomes zero_vel_id_ + 1)
+  const size_t N = *zero_vel_id + 1;
+
+  output = *opt_resampled_trajectory;
+
+  const std::vector<double> interval_dist_arr =
+    trajectory_utils::calcTrajectoryIntervalDistance(*opt_resampled_trajectory);
+
+  std::vector<double> v_max_arr(N, 0.0);
+  for (size_t i = 0; i < N; ++i) {
+    v_max_arr.at(i) = opt_resampled_trajectory->at(i).longitudinal_velocity_mps;
+  }
+
+  /*
+   * x = [
+   *      b[0], b[1], ..., b[N],               : 0~N
+   *      a[0], a[1], .... a[N],               : N~2N
+   *      delta[0], ..., delta[N],             : 2N~3N
+   *      sigma[0], sigma[1], ...., sigma[N],  : 3N~4N
+   *      gamma[0], gamma[1], ..., gamma[N]    : 4N~5N
+   *     ]
+   *
+   * b[i]  : velocity^2
+   * delta : 0 < b[i]-delta[i] < max_vel[i]*max_vel[i]
+   * sigma : a_min < a[i] - sigma[i] < a_max
+   * gamma : jerk_min < pseudo_jerk[i] * ref_vel[i] - gamma[i] < jerk_max
+   */
+  const uint32_t IDX_B0 = 0;
+  const uint32_t IDX_A0 = N;
+  const uint32_t IDX_DELTA0 = 2 * N;
+  const uint32_t IDX_SIGMA0 = 3 * N;
+  const uint32_t IDX_GAMMA0 = 4 * N;
+
+  const uint32_t l_variables = 5 * N;
+  const uint32_t l_constraints = 4 * N + 1;
+
+  // the matrix size depends on constraint numbers.
+  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(l_constraints, l_variables);
+
+  std::vector<double> lower_bound(l_constraints, 0.0);
+  std::vector<double> upper_bound(l_constraints, 0.0);
+
+  Eigen::MatrixXd P = Eigen::MatrixXd::Zero(l_variables, l_variables);
+  std::vector<double> q(l_variables, 0.0);
+
+  /**************************************************************/
+  /**************************************************************/
+  /**************** design objective function *******************/
+  /**************************************************************/
+  /**************************************************************/
+
+  // jerk: d(ai)/ds * v_ref -> minimize weight * ((a1 - a0) / ds * v_ref)^2 * ds
+  constexpr double ZERO_VEL_THR_FOR_DT_CALC = 0.3;
+  const double smooth_weight = smoother_param_.jerk_weight;
+  for (size_t i = 0; i < N - 1; ++i) {
+    const double ref_vel = v_max_arr.at(i);
+    const double interval_dist = std::max(interval_dist_arr.at(i), 0.0001);
+    const double w_x_ds_inv = (1.0 / interval_dist) * ref_vel;
+    P(IDX_A0 + i, IDX_A0 + i) += smooth_weight * w_x_ds_inv * w_x_ds_inv * interval_dist;
+    P(IDX_A0 + i, IDX_A0 + i + 1) -= smooth_weight * w_x_ds_inv * w_x_ds_inv * interval_dist;
+    P(IDX_A0 + i + 1, IDX_A0 + i) -= smooth_weight * w_x_ds_inv * w_x_ds_inv * interval_dist;
+    P(IDX_A0 + i + 1, IDX_A0 + i + 1) += smooth_weight * w_x_ds_inv * w_x_ds_inv * interval_dist;
+  }
+
+  for (size_t i = 0; i < N; ++i) {
+    const double v_max = std::max(v_max_arr.at(i), 0.1);
+    q.at(IDX_B0 + i) =
+      -1.0 / (v_max * v_max);  // |v_max_i^2 - b_i|/v_max^2 -> minimize (-bi) * ds / v_max^2
+    if (i < N - 1) {
+      q.at(IDX_B0 + i) *= std::max(interval_dist_arr.at(i), 0.0001);
+    }
+    P(IDX_DELTA0 + i, IDX_DELTA0 + i) += over_v_weight;  // over velocity cost
+    P(IDX_SIGMA0 + i, IDX_SIGMA0 + i) += over_a_weight;  // over acceleration cost
+    P(IDX_GAMMA0 + i, IDX_GAMMA0 + i) += over_j_weight;  // over jerk cost
+  }
+
+  /**************************************************************/
+  /**************************************************************/
+  /**************** design constraint matrix ********************/
+  /**************************************************************/
+  /**************************************************************/
+
+  /*
+  NOTE: The delta allows b to be negative. This is actually invalid because the definition is b=v^2.
+  But mathematically, the strict b>0 constraint may make the problem infeasible, such as the case of
+  v=0 & a<0. To avoid the infeasibility, we allow b<0. The negative b is dealt as b=0 when it is
+  converted to v with sqrt. If the weight of delta^2 is large (the value of delta is very small),
+  b is almost 0, and is not a big problem.
+  */
+
+  size_t constr_idx = 0;
+
+  // Soft Constraint Velocity Limit: 0 < b - delta < v_max^2
+  for (size_t i = 0; i < N; ++i, ++constr_idx) {
+    A(constr_idx, IDX_B0 + i) = 1.0;       // b_i
+    A(constr_idx, IDX_DELTA0 + i) = -1.0;  // -delta_i
+    upper_bound[constr_idx] = v_max_arr.at(i) * v_max_arr.at(i);
+    lower_bound[constr_idx] = 0.0;
+  }
+
+  // Soft Constraint Acceleration Limit: a_min < a - sigma < a_max
+  for (size_t i = 0; i < N; ++i, ++constr_idx) {
+    A(constr_idx, IDX_A0 + i) = 1.0;       // a_i
+    A(constr_idx, IDX_SIGMA0 + i) = -1.0;  // -sigma_i
+
+    constexpr double stop_vel = 1e-3;
+    if (v_max_arr.at(i) < stop_vel) {
+      // Stop Point
+      upper_bound[constr_idx] = a_stop_decel;
+      lower_bound[constr_idx] = a_stop_decel;
+    } else {
+      upper_bound[constr_idx] = a_max;
+      lower_bound[constr_idx] = a_min;
+    }
+  }
+
+  // Soft Constraint Jerk Limit: jerk_min < pseudo_jerk[i] * ref_vel[i] - gamma[i] < jerk_max
+  // -> jerk_min * ds < (a[i+1] - a[i]) * ref_vel[i] - gamma[i] * ds < jerk_max * ds
+  for (size_t i = 0; i < N - 1; ++i, ++constr_idx) {
+    const double ref_vel = std::max(v_max_arr.at(i), ZERO_VEL_THR_FOR_DT_CALC);
+    const double ds = interval_dist_arr.at(i);
+    A(constr_idx, IDX_A0 + i) = -ref_vel;     // -a[i] * ref_vel
+    A(constr_idx, IDX_A0 + i + 1) = ref_vel;  //  a[i+1] * ref_vel
+    A(constr_idx, IDX_GAMMA0 + i) = -ds;      // -gamma[i] * ds
+    upper_bound[constr_idx] = j_max * ds;     //  jerk_max * ds
+    lower_bound[constr_idx] = j_min * ds;     //  jerk_min * ds
+  }
+
+  // b' = 2a ... (b(i+1) - b(i)) / ds = 2a(i)
+  for (size_t i = 0; i < N - 1; ++i, ++constr_idx) {
+    A(constr_idx, IDX_B0 + i) = -1.0;                            // b(i)
+    A(constr_idx, IDX_B0 + i + 1) = 1.0;                         // b(i+1)
+    A(constr_idx, IDX_A0 + i) = -2.0 * interval_dist_arr.at(i);  // a(i) * ds
+    upper_bound[constr_idx] = 0.0;
+    lower_bound[constr_idx] = 0.0;
+  }
+
+  // initial condition
+  {
+    A(constr_idx, IDX_B0) = 1.0;  // b0
+    upper_bound[constr_idx] = v0 * v0;
+    lower_bound[constr_idx] = v0 * v0;
+    ++constr_idx;
+
+    A(constr_idx, IDX_A0) = 1.0;  // a0
+    upper_bound[constr_idx] = a0;
+    lower_bound[constr_idx] = a0;
+    ++constr_idx;
+  }
+
+  // execute optimization
+  const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound);
+  const std::vector<double> optval = std::get<0>(result);
+
+  const auto tf1 = std::chrono::system_clock::now();
+  const double dt_ms1 =
+    std::chrono::duration_cast<std::chrono::nanoseconds>(tf1 - ts).count() * 1.0e-6;
+  RCLCPP_DEBUG(logger_, "optimization time = %f [ms]", dt_ms1);
+
+  // get velocity & acceleration
+  for (size_t i = 0; i < N; ++i) {
+    double b = optval.at(IDX_B0 + i);
+    output.at(i).longitudinal_velocity_mps = std::sqrt(std::max(b, 0.0));
+    output.at(i).acceleration_mps2 = optval.at(IDX_A0 + i);
+  }
+  for (size_t i = N; i < output.size(); ++i) {
+    output.at(i).longitudinal_velocity_mps = 0.0;
+    output.at(i).acceleration_mps2 = a_stop_decel;
+  }
+
+  const int status_val = std::get<3>(result);
+  if (status_val != 1) {
+    RCLCPP_ERROR(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str());
+  }
+
+  if (TMP_SHOW_DEBUG_INFO) {
+    // jerk calculation
+    std::vector<double> p_jerk, jerk_converted, jerk_ideal;
+    for (size_t i = 0; i < input.size() - 1; ++i) {
+      double v_ref0 = filtered.at(i).longitudinal_velocity_mps;
+      double v0 = output.at(i).longitudinal_velocity_mps;
+      double a0 = output.at(i).acceleration_mps2;
+      double a1 = output.at(i + 1).acceleration_mps2;
+      double ds = interval_dist_arr.at(i);
+      p_jerk.push_back((a1 - a0) / ds);
+      jerk_converted.push_back((a1 - a0) / ds * v_ref0);
+      jerk_ideal.push_back((a1 - a0) / ds * v0);
+    }
+    p_jerk.push_back(0.0);
+    jerk_converted.push_back(0.0);
+    jerk_ideal.push_back(0.0);
+
+    // print
+    size_t i = 0;
+    auto getVx = [&i](const TrajectoryPoints & trajectory) {
+      return trajectory.at(i).longitudinal_velocity_mps;
+    };
+    auto getAx = [&i](const TrajectoryPoints & trajectory) {
+      return trajectory.at(i).acceleration_mps2;
+    };
+    printf("v0 = %.3f, a0 = %.3f\n", v0, a0);
+    for (; i < input.size(); ++i) {
+      double gamma = optval.at(IDX_GAMMA0 + i);
+      printf(
+        "i = %lu, input: %.3f, filt_f: (%.3f, %.3f), filt_b: (%.3f, %.3f), filt_fb: (%.3f, %.3f), "
+        "opt: (%.3f, %f), p_jerk = %.3f, p_jerk*v_ref: %.3f (min: %.3f, mac: %.3f), p_jerk*v_opt "
+        "(to "
+        "check): %.3f, gamma: %.3f\n",
+        i, getVx(input), getVx(forward_filtered), getAx(forward_filtered), getVx(backward_filtered),
+        getAx(backward_filtered), getVx(filtered), getAx(filtered), getVx(output), getAx(output),
+        p_jerk.at(i), jerk_converted.at(i), j_min, j_max, jerk_ideal.at(i), gamma);
+    }
+    printf("\n");
+  }
+
+  return true;
+}
+
+TrajectoryPoints JerkFilteredSmoother::forwardJerkFilter(
+  const double v0, const double a0, const double a_max, const double a_start, const double j_max,
+  const TrajectoryPoints & input) const
+{
+  auto applyLimits = [&input, &a_start](double & v, double & a, size_t i) {
+    double v_lim = input.at(i).longitudinal_velocity_mps;
+    static constexpr double ep = 1.0e-5;
+    if (v > v_lim + ep) {
+      v = v_lim;
+      a = 0.0;
+
+      if (v_lim < 1e-3 && i < input.size() - 1) {
+        double next_v_lim = input.at(i + 1).longitudinal_velocity_mps;
+        if (next_v_lim >= 1e-3) {
+          a = a_start;  // start from stop velocity
+        }
+      }
+    }
+
+    if (v < 0.0) {
+      v = a = 0.0;
+    }
+  };
+
+  auto output = input;
+
+  double current_vel = v0;
+  double current_acc = a0;
+  applyLimits(current_vel, current_acc, 0);
+
+  output.front().longitudinal_velocity_mps = current_vel;
+  output.front().acceleration_mps2 = current_acc;
+  for (size_t i = 1; i < input.size(); ++i) {
+    const double ds = autoware_utils::calcDistance2d(input.at(i), input.at(i - 1));
+    const double max_dt = std::pow(6.0 * ds / j_max, 1.0 / 3.0);  // assuming v0 = a0 = 0.
+    const double dt = std::min(ds / std::max(current_vel, 1.0e-6), max_dt);
+
+    if (current_acc + j_max * dt >= a_max) {
+      const double tmp_jerk = std::min((a_max - current_acc) / dt, j_max);
+      current_vel = current_vel + current_acc * dt + 0.5 * tmp_jerk * dt * dt;
+      current_acc = a_max;
+    } else {
+      current_vel = current_vel + current_acc * dt + 0.5 * j_max * dt * dt;
+      current_acc = current_acc + j_max * dt;
+    }
+    applyLimits(current_vel, current_acc, i);
+    output.at(i).longitudinal_velocity_mps = current_vel;
+    output.at(i).acceleration_mps2 = current_acc;
+  }
+  return output;
+}
+
+TrajectoryPoints JerkFilteredSmoother::backwardJerkFilter(
+  const double v0, const double a0, const double a_min, const double a_stop, const double j_min,
+  const TrajectoryPoints & input) const
+{
+  auto input_rev = input;
+  std::reverse(input_rev.begin(), input_rev.end());
+  auto filtered = forwardJerkFilter(
+    v0, std::fabs(a0), std::fabs(a_min), std::fabs(a_stop), std::fabs(j_min), input_rev);
+  std::reverse(filtered.begin(), filtered.end());
+  for (size_t i = 0; i < filtered.size(); ++i) {
+    filtered.at(i).acceleration_mps2 *= -1.0;  // Deceleration
+  }
+  return filtered;
+}
+
+TrajectoryPoints JerkFilteredSmoother::mergeFilteredTrajectory(
+  const double v0, const double a0, const double a_min, const double j_min,
+  const TrajectoryPoints & forward_filtered, const TrajectoryPoints & backward_filtered) const
+{
+  TrajectoryPoints merged;
+  merged = forward_filtered;
+
+  auto getVx = [](const TrajectoryPoints & trajectory, int i) {
+    return trajectory.at(i).longitudinal_velocity_mps;
+  };
+
+  size_t i = 0;
+
+  if (getVx(backward_filtered, 0) < v0) {
+    double current_vel = v0;
+    double current_acc = a0;
+    while (getVx(backward_filtered, i) < current_vel && current_vel <= getVx(forward_filtered, i) &&
+           i < merged.size() - 1) {
+      merged.at(i).longitudinal_velocity_mps = current_vel;
+      merged.at(i).acceleration_mps2 = current_acc;
+
+      const double ds =
+        autoware_utils::calcDistance2d(forward_filtered.at(i + 1), forward_filtered.at(i));
+      const double max_dt =
+        std::pow(6.0 * ds / std::fabs(j_min), 1.0 / 3.0);  // assuming v0 = a0 = 0.
+      const double dt = std::min(ds / std::max(current_vel, 1.0e-6), max_dt);
+
+      if (current_acc + j_min * dt < a_min) {
+        const double tmp_jerk = std::max((a_min - current_acc) / dt, j_min);
+        current_vel = current_vel + current_acc * dt + 0.5 * tmp_jerk * dt * dt;
+        current_acc = std::max(current_acc + tmp_jerk * dt, a_min);
+      } else {
+        current_vel = current_vel + current_acc * dt + 0.5 * j_min * dt * dt;
+        current_acc = current_acc + j_min * dt;
+      }
+      ++i;
+    }
+  }
+
+  // take smaller velocity point
+  for (; i < merged.size(); ++i) {
+    merged.at(i) = (getVx(forward_filtered, i) < getVx(backward_filtered, i))
+                     ? forward_filtered.at(i)
+                     : backward_filtered.at(i);
+  }
+  return merged;
+}
+
+boost::optional<TrajectoryPoints> JerkFilteredSmoother::resampleTrajectory(
+  const TrajectoryPoints & input, const double /*v_current*/, const int closest_id) const
+{
+  return resampling::resampleTrajectory(
+    input, closest_id, base_param_.resample_param, smoother_param_.jerk_filter_ds);
+}
+
+}  // namespace motion_velocity_smoother
diff --git a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp
new file mode 100644
index 0000000000000..2e7bafe1bbfa6
--- /dev/null
+++ b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp
@@ -0,0 +1,224 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp"
+
+#include "eigen3/Eigen/Core"
+#include "motion_velocity_smoother/trajectory_utils.hpp"
+
+#include <algorithm>
+#include <chrono>
+#include <limits>
+#include <vector>
+
+namespace motion_velocity_smoother
+{
+L2PseudoJerkSmoother::L2PseudoJerkSmoother(const Param & smoother_param)
+: smoother_param_{smoother_param}
+{
+  qp_solver_.updateMaxIter(4000);
+  qp_solver_.updateRhoInterval(0);  // 0 means automatic
+  qp_solver_.updateEpsRel(1.0e-4);  // def: 1.0e-4
+  qp_solver_.updateEpsAbs(1.0e-4);  // def: 1.0e-4
+  qp_solver_.updateVerbose(false);
+}
+
+void L2PseudoJerkSmoother::setParam(const Param & smoother_param)
+{
+  smoother_param_ = smoother_param;
+}
+
+bool L2PseudoJerkSmoother::apply(
+  const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
+  TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories)
+{
+  debug_trajectories.clear();
+
+  const auto ts = std::chrono::system_clock::now();
+
+  output = input;
+
+  if (std::fabs(input.front().longitudinal_velocity_mps) < 0.1) {
+    RCLCPP_DEBUG(logger_, "closest v_max < 0.1. assume vehicle stopped. return.");
+    return false;
+  }
+
+  const unsigned int N = input.size();
+
+  if (N < 2) {
+    RCLCPP_WARN(logger_, "trajectory length is not enough.");
+    return false;
+  }
+
+  const std::vector<double> interval_dist_arr =
+    trajectory_utils::calcTrajectoryIntervalDistance(input);
+
+  std::vector<double> v_max(N, 0.0);
+  for (unsigned int i = 0; i < N; ++i) {
+    v_max.at(i) = input.at(i).longitudinal_velocity_mps;
+  }
+  /*
+   * x = [b0, b1, ..., bN, |  a0, a1, ..., aN, | delta0, delta1, ..., deltaN, | sigma0, sigma1, ...,
+   * sigmaN] in R^{4N} b: velocity^2 a: acceleration delta: 0 < bi < v_max^2 + delta sigma: a_min <
+   * ai - sigma < a_max
+   */
+
+  const uint32_t l_variables = 4 * N;
+  const uint32_t l_constraints = 3 * N + 1;
+
+  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(
+    l_constraints, l_variables);  // the matrix size depends on constraint numbers.
+
+  std::vector<double> lower_bound(l_constraints, 0.0);
+  std::vector<double> upper_bound(l_constraints, 0.0);
+
+  Eigen::MatrixXd P = Eigen::MatrixXd::Zero(l_variables, l_variables);
+  std::vector<double> q(l_variables, 0.0);
+
+  const double a_max = base_param_.max_accel;
+  const double a_min = base_param_.min_decel;
+  const double smooth_weight = smoother_param_.pseudo_jerk_weight;
+  const double over_v_weight = smoother_param_.over_v_weight;
+  const double over_a_weight = smoother_param_.over_a_weight;
+
+  // design objective function
+  for (unsigned int i = 0; i < N; ++i) {  // bi
+    q[i] = -1.0;                          // |v_max^2 - b| -> minimize (-bi)
+  }
+
+  // pseudo jerk: d(ai)/ds -> minimize weight * (a1 - a0)^2
+  for (unsigned int i = N; i < 2 * N - 1; ++i) {
+    unsigned int j = i - N;
+    const double w_x_ds_inv = smooth_weight * (1.0 / std::max(interval_dist_arr.at(j), 0.0001));
+    P(i, i) += w_x_ds_inv * w_x_ds_inv;
+    P(i, i + 1) -= w_x_ds_inv * w_x_ds_inv;
+    P(i + 1, i) -= w_x_ds_inv * w_x_ds_inv;
+    P(i + 1, i + 1) += w_x_ds_inv * w_x_ds_inv;
+  }
+
+  for (unsigned int i = 2 * N; i < 3 * N; ++i) {  // over velocity cost
+    P(i, i) += over_v_weight;
+  }
+
+  for (unsigned int i = 3 * N; i < 4 * N; ++i) {  // over acceleration cost
+    P(i, i) += over_a_weight;
+  }
+
+  /* design constraint matrix
+  0 < b - delta < v_max^2
+  NOTE: The delta allows b to be negative. This is actually invalid because the definition is b=v^2.
+  But mathematically, the strict b>0 constraint may make the problem infeasible, such as the case of
+  v=0 & a<0. To avoid the infeasibility, we allow b<0. The negative b is dealt as b=0 when it is
+  converted to v with sqrt. If the weight of delta^2 is large (the value of delta is very small),
+  b is almost 0, and is not a big problem.
+  */
+  for (unsigned int i = 0; i < N; ++i) {
+    const int j = 2 * N + i;
+    A(i, i) = 1.0;   // b_i
+    A(i, j) = -1.0;  // -delta_i
+    upper_bound[i] = v_max[i] * v_max[i];
+    lower_bound[i] = 0.0;
+  }
+
+  // a_min < a - sigma < a_max
+  for (unsigned int i = N; i < 2 * N; ++i) {
+    const int j = 2 * N + i;
+    A(i, i) = 1.0;   // a_i
+    A(i, j) = -1.0;  // -sigma_i
+    if (i != N && v_max[i - N] < std::numeric_limits<double>::epsilon()) {
+      upper_bound[i] = 0.0;
+      lower_bound[i] = 0.0;
+    } else {
+      upper_bound[i] = a_max;
+      lower_bound[i] = a_min;
+    }
+  }
+
+  // b' = 2a ... (b(i+1) - b(i)) / ds = 2a(i)
+  for (unsigned int i = 2 * N; i < 3 * N - 1; ++i) {
+    const unsigned int j = i - 2 * N;
+    const double ds_inv = 1.0 / std::max(interval_dist_arr.at(j), 0.0001);
+    A(i, j) = -ds_inv;     // b(i)
+    A(i, j + 1) = ds_inv;  // b(i+1)
+    A(i, j + N) = -2.0;    // a(i)
+    upper_bound[i] = 0.0;
+    lower_bound[i] = 0.0;
+  }
+
+  // initial condition
+  const double v0 = initial_vel;
+  {
+    const unsigned int i = 3 * N - 1;
+    A(i, 0) = 1.0;  // b0
+    upper_bound[i] = v0 * v0;
+    lower_bound[i] = v0 * v0;
+
+    A(i + 1, N) = 1.0;  // a0
+    upper_bound[i + 1] = initial_acc;
+    lower_bound[i + 1] = initial_acc;
+  }
+
+  const auto tf1 = std::chrono::system_clock::now();
+  const double dt_ms1 =
+    std::chrono::duration_cast<std::chrono::nanoseconds>(tf1 - ts).count() * 1.0e-6;
+
+  // execute optimization
+  const auto ts2 = std::chrono::system_clock::now();
+  const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound);
+
+  // [b0, b1, ..., bN, |  a0, a1, ..., aN, |
+  //  delta0, delta1, ..., deltaN, | sigma0, sigma1, ..., sigmaN]
+  const std::vector<double> optval = std::get<0>(result);
+
+  for (unsigned int i = 0; i < N; ++i) {
+    double v = optval.at(i);
+    // std::cout << "[smoother] v[" << i << "] : " << std::sqrt(std::max(v, 0.0)) <<
+    // ", v_max[" << i << "] : " << v_max[i] << std::endl;
+    output.at(i).longitudinal_velocity_mps = std::sqrt(std::max(v, 0.0));
+    output.at(i).acceleration_mps2 = optval.at(i + N);
+  }
+  for (unsigned int i = N; i < output.size(); ++i) {
+    output.at(i).longitudinal_velocity_mps = 0.0;
+    output.at(i).acceleration_mps2 = 0.0;
+  }
+
+  // -- to check the all optimization variables --
+  // ROS_DEBUG("[after optimize Linf] idx, vel, acc, over_vel, over_acc ");
+  // for (unsigned int i = 0; i < N; ++i) {
+  //   ROS_DEBUG(
+  //     "i = %d, v: %f, v_max: %f a: %f, b: %f, delta: %f, sigma: %f\n",
+  //     i, std::sqrt(optval.at(i)),
+  //     v_max[i], optval.at(i + N), optval.at(i), optval.at(i + 2 * N), optval.at(i + 3 * N));
+  // }
+
+  const int status_val = std::get<3>(result);
+  if (status_val != 1) {
+    RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str());
+  }
+
+  const auto tf2 = std::chrono::system_clock::now();
+  const double dt_ms2 =
+    std::chrono::duration_cast<std::chrono::nanoseconds>(tf2 - ts2).count() * 1.0e-6;
+  RCLCPP_DEBUG(logger_, "init time = %f [ms], optimization time = %f [ms]", dt_ms1, dt_ms2);
+
+  return true;
+}
+
+boost::optional<TrajectoryPoints> L2PseudoJerkSmoother::resampleTrajectory(
+  const TrajectoryPoints & input, const double v_current, const int closest_id) const
+{
+  return resampling::resampleTrajectory(input, v_current, closest_id, base_param_.resample_param);
+}
+
+}  // namespace motion_velocity_smoother
diff --git a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp
new file mode 100644
index 0000000000000..fceb9432787f7
--- /dev/null
+++ b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp
@@ -0,0 +1,236 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp"
+
+#include "eigen3/Eigen/Core"
+#include "motion_velocity_smoother/trajectory_utils.hpp"
+
+#include <algorithm>
+#include <chrono>
+#include <limits>
+#include <vector>
+
+namespace motion_velocity_smoother
+{
+LinfPseudoJerkSmoother::LinfPseudoJerkSmoother(const Param & smoother_param)
+: smoother_param_(smoother_param)
+{
+  qp_solver_.updateMaxIter(20000);
+  qp_solver_.updateRhoInterval(5000);
+  qp_solver_.updateEpsRel(1.0e-4);  // def: 1.0e-4
+  qp_solver_.updateEpsAbs(1.0e-8);  // def: 1.0e-4
+  qp_solver_.updateVerbose(false);
+}
+
+void LinfPseudoJerkSmoother::setParam(const Param & smoother_param)
+{
+  smoother_param_ = smoother_param;
+}
+
+bool LinfPseudoJerkSmoother::apply(
+  const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
+  TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories)
+{
+  debug_trajectories.clear();
+
+  const auto ts = std::chrono::system_clock::now();
+
+  output = input;
+
+  if (std::fabs(input.front().longitudinal_velocity_mps) < 0.1) {
+    RCLCPP_DEBUG(
+      logger_,
+      "closest v_max < 0.1, keep stopping. "
+      "return.");
+    return false;
+  }
+
+  const size_t N{input.size()};
+
+  if (N < 2) {
+    return false;
+  }
+
+  std::vector<double> interval_dist_arr = trajectory_utils::calcTrajectoryIntervalDistance(input);
+
+  std::vector<double> v_max(N, 0.0);
+  for (size_t i = 0; i < N; ++i) {
+    v_max.at(i) = input.at(i).longitudinal_velocity_mps;
+  }
+
+  /*
+   * x = [b0, b1, ..., bN, |  a0, a1, ..., aN, | delta0, delta1, ..., deltaN, | sigma0, sigma1, ...,
+   * sigmaN, | psi] in R^{4N+1} b: velocity^2 a: acceleration delta: 0 < bi < v_max^2 + delta sigma:
+   * a_min < ai - sigma < a_max psi: a'*curr_v -  psi < 0, - a'*curr_v - psi < 0 (<=> |a'|*curr_v <
+   * psi)
+   */
+  const size_t l_variables{4 * N + 1};
+  const size_t l_constraints{3 * N + 1 + 2 * (N - 1)};
+
+  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(
+    l_constraints, l_variables);  // the matrix size depends on constraint numbers.
+
+  std::vector<double> lower_bound(l_constraints, 0.0);
+  std::vector<double> upper_bound(l_constraints, 0.0);
+
+  Eigen::MatrixXd P = Eigen::MatrixXd::Zero(l_variables, l_variables);
+  std::vector<double> q(l_variables, 0.0);
+
+  const double a_max{base_param_.max_accel};
+  const double a_min{base_param_.min_decel};
+  const double smooth_weight{smoother_param_.pseudo_jerk_weight};
+  const double over_v_weight{smoother_param_.over_v_weight};
+  const double over_a_weight{smoother_param_.over_a_weight};
+
+  // design objective function
+  for (unsigned int i = 0; i < N; ++i) {  // bi
+    q[i] = -1.0;                          // |v_max^2 - b| -> minimize (-bi)
+  }
+
+  for (unsigned int i = 2 * N; i < 3 * N; ++i) {  // over velocity cost
+    P(i, i) += over_v_weight;
+  }
+
+  for (unsigned int i = 3 * N; i < 4 * N; ++i) {  // over acceleration cost
+    P(i, i) += over_a_weight;
+  }
+
+  // pseudo jerk (Linf): minimize psi, subject to |a'|*curr_v < psi
+  q[4 * N] = smooth_weight;
+
+  /* design constraint matrix
+  0 < b - delta < v_max^2
+  NOTE: The delta allows b to be negative. This is actually invalid because the definition is b=v^2.
+  But mathematically, the strict b>0 constraint may make the problem infeasible, such as the case of
+  v=0 & a<0. To avoid the infeasibility, we allow b<0. The negative b is dealt as b=0 when it is
+  converted to v with sqrt. If the weight of delta^2 is large (the value of delta is very small),
+  b is almost 0, and is not a big problem.
+  */
+  for (unsigned int i = 0; i < N; ++i) {
+    const int j = 2 * N + i;
+    A(i, i) = 1.0;   // b_i
+    A(i, j) = -1.0;  // -delta_i
+    upper_bound[i] = v_max[i] * v_max[i];
+    lower_bound[i] = 0.0;
+  }
+
+  // a_min < a - sigma < a_max
+  for (unsigned int i = N; i < 2 * N; ++i) {
+    const int j = 2 * N + i;
+    A(i, i) = 1.0;   // a_i
+    A(i, j) = -1.0;  // -sigma_i
+    if (i != N && v_max[i - N] < std::numeric_limits<double>::epsilon()) {
+      upper_bound[i] = 0.0;
+      lower_bound[i] = 0.0;
+    } else {
+      upper_bound[i] = a_max;
+      lower_bound[i] = a_min;
+    }
+  }
+
+  // b' = 2a
+  for (unsigned int i = 2 * N; i < 3 * N - 1; ++i) {
+    const unsigned int j = i - 2 * N;
+    const double ds_inv = 1.0 / std::max(interval_dist_arr.at(j), 0.0001);
+    A(i, j) = -ds_inv;
+    A(i, j + 1) = ds_inv;
+    A(i, j + N) = -2.0;
+    upper_bound[i] = 0.0;
+    lower_bound[i] = 0.0;
+  }
+
+  // initial condition
+  const double v0 = initial_vel;
+  {
+    const unsigned int i = 3 * N - 1;
+    A(i, 0) = 1.0;  // b0
+    upper_bound[i] = v0 * v0;
+    lower_bound[i] = v0 * v0;
+
+    A(i + 1, N) = 1.0;  // a0
+    upper_bound[i + 1] = initial_acc;
+    lower_bound[i + 1] = initial_acc;
+  }
+
+  // constraint for slack variable (a[i+1] - a[i] <= psi[i], a[i] - a[i+1] <= psi[i])
+  for (unsigned int i = 3 * N + 1; i < 4 * N; ++i) {
+    const unsigned int ia = i - (3 * N + 1) + N;
+    const unsigned int ip = 4 * N;
+    const unsigned int j = i - (3 * N + 1);
+    const double ds_inv = 1.0 / std::max(interval_dist_arr.at(j), 0.0001);
+
+    A(i, ia) = -ds_inv;
+    A(i, ia + 1) = ds_inv;
+    A(i, ip) = -1;
+    lower_bound[i] = -OSQP_INFTY;
+    upper_bound[i] = 0;
+
+    A(i + N - 1, ia) = ds_inv;
+    A(i + N - 1, ia + 1) = -ds_inv;
+    A(i + N - 1, ip) = -1;
+    lower_bound[i + N - 1] = -OSQP_INFTY;
+    upper_bound[i + N - 1] = 0;
+  }
+
+  const auto tf1 = std::chrono::system_clock::now();
+  const double dt_ms1 =
+    std::chrono::duration_cast<std::chrono::nanoseconds>(tf1 - ts).count() * 1.0e-6;
+
+  // execute optimization
+  const auto ts2 = std::chrono::system_clock::now();
+  const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound);
+
+  // [b0, b1, ..., bN, |  a0, a1, ..., aN, |
+  //  delta0, delta1, ..., deltaN, | sigma0, sigma1, ..., sigmaN]
+  const std::vector<double> optval = std::get<0>(result);
+
+  /* get velocity & acceleration */
+  for (unsigned int i = 0; i < N; ++i) {
+    double v = optval.at(i);
+    output.at(i).longitudinal_velocity_mps = std::sqrt(std::max(v, 0.0));
+    output.at(i).acceleration_mps2 = optval.at(i + N);
+  }
+  for (unsigned int i = N; i < output.size(); ++i) {
+    output.at(i).longitudinal_velocity_mps = 0.0;
+    output.at(i).acceleration_mps2 = 0.0;
+  }
+
+  // -- to check the all optimization variables --
+  // ROS_DEBUG("[after optimize Linf] idx, vel, acc, over_vel, over_acc ");
+  // for (unsigned int i = 0; i < N; ++i) {
+  //   ROS_DEBUG(
+  //     "i = %d, v: %f, v_max: %f a: %f, b: %f, delta: %f, sigma: %f", i, std::sqrt(optval.at(i)),
+  //     v_max[i], optval.at(i + N), optval.at(i), optval.at(i + 2 * N), optval.at(i + 3 * N));
+  // }
+
+  const int status_val = std::get<3>(result);
+  if (status_val != 1) {
+    RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str());
+  }
+
+  const auto tf2 = std::chrono::system_clock::now();
+  const double dt_ms2 =
+    std::chrono::duration_cast<std::chrono::nanoseconds>(tf2 - ts2).count() * 1.0e-6;
+  RCLCPP_DEBUG(logger_, "init time = %f [ms], optimization time = %f [ms]", dt_ms1, dt_ms2);
+  return true;
+}
+
+boost::optional<TrajectoryPoints> LinfPseudoJerkSmoother::resampleTrajectory(
+  const TrajectoryPoints & input, const double v_current, const int closest_id) const
+{
+  return resampling::resampleTrajectory(input, v_current, closest_id, base_param_.resample_param);
+}
+
+}  // namespace motion_velocity_smoother
diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp
new file mode 100644
index 0000000000000..adaaa71d68a09
--- /dev/null
+++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp
@@ -0,0 +1,96 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "motion_velocity_smoother/smoother/smoother_base.hpp"
+
+#include "motion_velocity_smoother/resample.hpp"
+#include "motion_velocity_smoother/trajectory_utils.hpp"
+
+#include <algorithm>
+#include <cmath>
+#include <vector>
+
+namespace motion_velocity_smoother
+{
+void SmootherBase::setParam(const BaseParam & param) { base_param_ = param; }
+
+double SmootherBase::getMaxAccel() const { return base_param_.max_accel; }
+
+double SmootherBase::getMinDecel() const { return base_param_.min_decel; }
+
+double SmootherBase::getMaxJerk() const { return base_param_.max_jerk; }
+
+double SmootherBase::getMinJerk() const { return base_param_.min_jerk; }
+
+boost::optional<TrajectoryPoints> SmootherBase::applyLateralAccelerationFilter(
+  const TrajectoryPoints & input) const
+{
+  if (input.empty()) {
+    return boost::none;
+  }
+
+  if (input.size() < 3) {
+    return boost::optional<TrajectoryPoints>(input);  // cannot calculate lateral acc. do nothing.
+  }
+
+  // Interpolate with constant interval distance for lateral acceleration calculation.
+  constexpr double points_interval = 0.1;  // [m]
+  std::vector<double> out_arclength;
+  const std::vector<double> in_arclength = trajectory_utils::calcArclengthArray(input);
+  for (double s = 0; s < in_arclength.back(); s += points_interval) {
+    out_arclength.push_back(s);
+  }
+  auto output = trajectory_utils::applyLinearInterpolation(in_arclength, input, out_arclength);
+  if (!output) {
+    RCLCPP_WARN(
+      rclcpp::get_logger("smoother").get_child("smoother_base"),
+      "interpolation failed at lateral acceleration filter.");
+    return boost::none;
+  }
+  output->back() = input.back();  // keep the final speed.
+
+  constexpr double curvature_calc_dist = 5.0;  // [m] calc curvature with 5m away points
+  const size_t idx_dist =
+    static_cast<size_t>(std::max(static_cast<int>((curvature_calc_dist) / points_interval), 1));
+
+  // Calculate curvature assuming the trajectory points interval is constant
+  const auto curvature_v = trajectory_utils::calcTrajectoryCurvatureFrom3Points(*output, idx_dist);
+  if (!curvature_v) {
+    return boost::optional<TrajectoryPoints>(input);
+  }
+
+  //  Decrease speed according to lateral G
+  const size_t before_decel_index =
+    static_cast<size_t>(std::round(base_param_.decel_distance_before_curve / points_interval));
+  const size_t after_decel_index =
+    static_cast<size_t>(std::round(base_param_.decel_distance_after_curve / points_interval));
+  const double max_lateral_accel_abs = std::fabs(base_param_.max_lateral_accel);
+
+  for (size_t i = 0; i < output->size(); ++i) {
+    double curvature = 0.0;
+    const size_t start = i > before_decel_index ? i - before_decel_index : 0;
+    const size_t end = std::min(output->size(), i + after_decel_index);
+    for (size_t j = start; j < end; ++j) {
+      curvature = std::max(curvature, std::fabs(curvature_v->at(j)));
+    }
+    double v_curvature_max = std::sqrt(max_lateral_accel_abs / std::max(curvature, 1.0E-5));
+    v_curvature_max = std::max(v_curvature_max, base_param_.min_curve_velocity);
+    if (output->at(i).longitudinal_velocity_mps > v_curvature_max) {
+      output->at(i).longitudinal_velocity_mps = v_curvature_max;
+    }
+  }
+  return output;
+}
+
+}  // namespace motion_velocity_smoother
diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp
new file mode 100644
index 0000000000000..8f0618c39b574
--- /dev/null
+++ b/planning/motion_velocity_smoother/src/trajectory_utils.cpp
@@ -0,0 +1,643 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "motion_velocity_smoother/trajectory_utils.hpp"
+
+#include "interpolation/spline_interpolation.hpp"
+#include "motion_velocity_smoother/linear_interpolation.hpp"
+
+#include <algorithm>
+#include <limits>
+#include <map>
+#include <tuple>
+#include <utility>
+#include <vector>
+
+namespace motion_velocity_smoother
+{
+using geometry_msgs::msg::Point;
+namespace trajectory_utils
+{
+inline void convertEulerAngleToMonotonic(std::vector<double> & a)
+{
+  for (unsigned int i = 1; i < a.size(); ++i) {
+    const double da = a[i] - a[i - 1];
+    a[i] = a[i - 1] + autoware_utils::normalizeRadian(da);
+  }
+}
+
+inline tf2::Vector3 getTransVector3(const Pose & from, const Pose & to)
+{
+  double dx = to.position.x - from.position.x;
+  double dy = to.position.y - from.position.y;
+  double dz = to.position.z - from.position.z;
+  return tf2::Vector3(dx, dy, dz);
+}
+
+inline double integ_x(double x0, double v0, double a0, double j0, double t)
+{
+  return x0 + v0 * t + 0.5 * a0 * t * t + (1.0 / 6.0) * j0 * t * t * t;
+}
+
+inline double integ_v(double v0, double a0, double j0, double t)
+{
+  return v0 + a0 * t + 0.5 * j0 * t * t;
+}
+
+inline double integ_a(double a0, double j0, double t) { return a0 + j0 * t; }
+
+TrajectoryPoint calcInterpolatedTrajectoryPoint(
+  const TrajectoryPoints & trajectory, const Pose & target_pose)
+{
+  TrajectoryPoint traj_p{};
+  traj_p.pose = target_pose;
+
+  if (trajectory.empty()) {
+    traj_p.longitudinal_velocity_mps = 0.0;
+    traj_p.acceleration_mps2 = 0.0;
+    return traj_p;
+  }
+
+  if (trajectory.size() == 1) {
+    traj_p.longitudinal_velocity_mps = trajectory.at(0).longitudinal_velocity_mps;
+    traj_p.acceleration_mps2 = trajectory.at(0).acceleration_mps2;
+    return traj_p;
+  }
+
+  const size_t segment_idx =
+    autoware_utils::findNearestSegmentIndex(trajectory, target_pose.position);
+
+  auto v1 = getTransVector3(trajectory.at(segment_idx).pose, trajectory.at(segment_idx + 1).pose);
+  auto v2 = getTransVector3(trajectory.at(segment_idx).pose, target_pose);
+  // calc internal proportion
+  const double prop{std::max(0.0, std::min(1.0, v1.dot(v2) / v1.length2()))};
+
+  auto interpolate = [&prop](double x1, double x2) { return prop * x1 + (1.0 - prop) * x2; };
+
+  {
+    const auto & seg_pt = trajectory.at(segment_idx);
+    const auto & next_pt = trajectory.at(segment_idx + 1);
+    traj_p.longitudinal_velocity_mps =
+      interpolate(next_pt.longitudinal_velocity_mps, seg_pt.longitudinal_velocity_mps);
+    traj_p.acceleration_mps2 = interpolate(next_pt.acceleration_mps2, seg_pt.acceleration_mps2);
+    traj_p.pose.position.x = interpolate(next_pt.pose.position.x, seg_pt.pose.position.x);
+    traj_p.pose.position.y = interpolate(next_pt.pose.position.y, seg_pt.pose.position.y);
+    traj_p.pose.position.z = interpolate(next_pt.pose.position.z, seg_pt.pose.position.z);
+  }
+
+  return traj_p;
+}
+
+boost::optional<TrajectoryPoints> extractPathAroundIndex(
+  const TrajectoryPoints & trajectory, const size_t index, const double & ahead_length,
+  const double & behind_length)
+{
+  if (trajectory.size() == 0 || trajectory.size() - 1 < index) {
+    return {};
+  }
+
+  // calc ahead distance
+  size_t ahead_index{trajectory.size() - 1};
+  {
+    double dist_sum = 0.0;
+    for (size_t i = index; i < trajectory.size() - 1; ++i) {
+      dist_sum += autoware_utils::calcDistance2d(trajectory.at(i), trajectory.at(i + 1));
+      if (dist_sum > ahead_length) {
+        ahead_index = i + 1;
+        break;
+      }
+    }
+  }
+
+  // calc behind distance
+  size_t behind_index{0};
+  {
+    double dist_sum{0.0};
+    for (size_t i = index; i != 0; --i) {
+      dist_sum += autoware_utils::calcDistance2d(trajectory.at(i), trajectory[i - 1]);
+      if (dist_sum > behind_length) {
+        behind_index = i - 1;
+        break;
+      }
+    }
+  }
+
+  // extract trajectory
+  TrajectoryPoints extracted_traj{};
+  for (size_t i = behind_index; i < ahead_index + 1; ++i) {
+    extracted_traj.push_back(trajectory.at(i));
+  }
+
+  return boost::optional<TrajectoryPoints>(extracted_traj);
+}
+
+double calcArcLength(const TrajectoryPoints & path, const int idx1, const int idx2)
+{
+  if (idx1 == idx2) {  // zero distance
+    return 0.0;
+  }
+
+  if (
+    idx1 < 0 || idx2 < 0 || static_cast<int>(path.size()) - 1 < idx1 ||
+    static_cast<int>(path.size()) - 1 < idx2) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"),
+      "invalid index");
+    return 0.0;
+  }
+
+  const int idx_from = std::min(idx1, idx2);
+  const int idx_to = std::max(idx1, idx2);
+  double dist_sum = 0.0;
+  for (int i = idx_from; i < idx_to; ++i) {
+    dist_sum += autoware_utils::calcDistance2d(path.at(i), path.at(i + 1));
+  }
+  return dist_sum;
+}
+
+std::vector<double> calcArclengthArray(const TrajectoryPoints & trajectory)
+{
+  std::vector<double> arclength;
+  double dist = 0.0;
+  arclength.clear();
+  arclength.push_back(dist);
+  for (unsigned int i = 1; i < trajectory.size(); ++i) {
+    const TrajectoryPoint tp = trajectory.at(i);
+    const TrajectoryPoint tp_prev = trajectory.at(i - 1);
+    dist += autoware_utils::calcDistance2d(tp.pose, tp_prev.pose);
+    arclength.push_back(dist);
+  }
+  return arclength;
+}
+
+std::vector<double> calcTrajectoryIntervalDistance(const TrajectoryPoints & trajectory)
+{
+  std::vector<double> intervals;
+  for (unsigned int i = 1; i < trajectory.size(); ++i) {
+    const TrajectoryPoint tp = trajectory.at(i);
+    const TrajectoryPoint tp_prev = trajectory.at(i - 1);
+    const double dist = autoware_utils::calcDistance2d(tp.pose, tp_prev.pose);
+    intervals.push_back(dist);
+  }
+  return intervals;
+}
+
+boost::optional<std::vector<double>> calcTrajectoryCurvatureFrom3Points(
+  const TrajectoryPoints & trajectory, const size_t & idx_dist)
+{
+  std::vector<double> k_arr;
+  if (trajectory.size() < 2 * idx_dist + 1) {
+    RCLCPP_DEBUG(
+      rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"),
+      "cannot calc curvature idx_dist = %lu, trajectory.size() = %lu", idx_dist, trajectory.size());
+    return {};
+  }
+
+  // calculate curvature by circle fitting from three points
+  Point p1{};
+  Point p2{};
+  Point p3{};
+  for (size_t i = idx_dist; i < trajectory.size() - idx_dist; ++i) {
+    p1.x = trajectory.at(i - idx_dist).pose.position.x;
+    p2.x = trajectory.at(i).pose.position.x;
+    p3.x = trajectory.at(i + idx_dist).pose.position.x;
+    p1.y = trajectory.at(i - idx_dist).pose.position.y;
+    p2.y = trajectory.at(i).pose.position.y;
+    p3.y = trajectory.at(i + idx_dist).pose.position.y;
+    double den = std::max(
+      autoware_utils::calcDistance2d(p1, p2) * autoware_utils::calcDistance2d(p2, p3) *
+        autoware_utils::calcDistance2d(p3, p1),
+      0.0001);
+    double curvature = 2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / den;
+    k_arr.push_back(curvature);
+  }
+
+  // for debug
+  if (k_arr.empty()) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"),
+      "k_arr.size() = 0, something wrong. pls check.");
+    return {};
+  }
+
+  // first and last curvature is copied from next value
+  for (size_t i = 0; i < idx_dist; ++i) {
+    k_arr.insert(k_arr.begin(), k_arr.front());
+    k_arr.push_back(k_arr.back());
+  }
+  return boost::optional<std::vector<double>>(k_arr);
+}
+
+void setZeroVelocity(TrajectoryPoints & trajectory)
+{
+  for (auto & tp : trajectory) {
+    tp.longitudinal_velocity_mps = 0.0;
+  }
+}
+
+double getMaxVelocity(const TrajectoryPoints & trajectory)
+{
+  double max_vel = 0.0;
+  for (auto & tp : trajectory) {
+    if (tp.longitudinal_velocity_mps > max_vel) {
+      max_vel = tp.longitudinal_velocity_mps;
+    }
+  }
+  return max_vel;
+}
+
+double getMaxAbsVelocity(const TrajectoryPoints & trajectory)
+{
+  double max_vel = 0.0;
+  for (auto & tp : trajectory) {
+    double abs_vel = std::fabs(tp.longitudinal_velocity_mps);
+    if (abs_vel > max_vel) {
+      max_vel = abs_vel;
+    }
+  }
+  return max_vel;
+}
+
+void applyMaximumVelocityLimit(
+  const size_t begin, const size_t end, const double max_vel, TrajectoryPoints & trajectory)
+{
+  for (size_t idx = begin; idx < end; ++idx) {
+    if (trajectory.at(idx).longitudinal_velocity_mps > max_vel) {
+      trajectory.at(idx).longitudinal_velocity_mps = max_vel;
+    }
+  }
+}
+
+boost::optional<TrajectoryPoints> applyLinearInterpolation(
+  const std::vector<double> & base_index, const TrajectoryPoints & base_trajectory,
+  const std::vector<double> & out_index, const bool use_spline_for_pose)
+{
+  std::vector<double> px, py, pz, pyaw, tlx, taz, alx;
+  for (const auto & p : base_trajectory) {
+    px.push_back(p.pose.position.x);
+    py.push_back(p.pose.position.y);
+    pz.push_back(p.pose.position.z);
+    pyaw.push_back(tf2::getYaw(p.pose.orientation));
+    tlx.push_back(p.longitudinal_velocity_mps);
+    taz.push_back(p.heading_rate_rps);
+    alx.push_back(p.acceleration_mps2);
+  }
+
+  convertEulerAngleToMonotonic(pyaw);
+
+  boost::optional<std::vector<double>> px_p, py_p, pz_p, pyaw_p;
+  if (use_spline_for_pose) {
+    px_p = interpolation::slerp(base_index, px, out_index);
+    py_p = interpolation::slerp(base_index, py, out_index);
+    pz_p = interpolation::slerp(base_index, pz, out_index);
+    pyaw_p = interpolation::slerp(base_index, pyaw, out_index);
+  } else {
+    px_p = linear_interpolation::interpolate(base_index, px, out_index);
+    py_p = linear_interpolation::interpolate(base_index, py, out_index);
+    pz_p = linear_interpolation::interpolate(base_index, pz, out_index);
+    pyaw_p = linear_interpolation::interpolate(base_index, pyaw, out_index);
+  }
+  const auto tlx_p = linear_interpolation::interpolate(base_index, tlx, out_index);
+  const auto taz_p = linear_interpolation::interpolate(base_index, taz, out_index);
+  const auto alx_p = linear_interpolation::interpolate(base_index, alx, out_index);
+
+  if (!px_p || !py_p || !pz_p || !pyaw_p || !tlx_p || !taz_p || !alx_p) {
+    RCLCPP_WARN(
+      rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"),
+      "interpolation error!!");
+    return {};
+  }
+
+  TrajectoryPoints out_trajectory;
+  TrajectoryPoint point;
+  for (unsigned int i = 0; i < out_index.size(); ++i) {
+    point.pose.position.x = px_p->at(i);
+    point.pose.position.y = py_p->at(i);
+    point.pose.position.z = pz_p->at(i);
+    point.pose.orientation = autoware_utils::createQuaternionFromYaw(pyaw_p->at(i));
+
+    point.longitudinal_velocity_mps = tlx_p->at(i);
+    point.heading_rate_rps = taz_p->at(i);
+    point.acceleration_mps2 = alx_p->at(i);
+    out_trajectory.push_back(point);
+  }
+  return boost::optional<TrajectoryPoints>(out_trajectory);
+}
+
+bool calcStopDistWithJerkConstraints(
+  const double v0, const double a0, const double jerk_acc, const double jerk_dec,
+  const double min_acc, const double target_vel, std::map<double, double> & jerk_profile,
+  double & stop_dist)
+{
+  auto calculateTime = [](const double a0, const double target_acc, const double jerk) {
+    return (target_acc - a0) / jerk;
+  };
+  enum class AccelerationType { TRAPEZOID = 0, TRIANGLE = 1, LINEAR = 2 } type;
+
+  jerk_profile.clear();
+
+  constexpr double t_threshold = 1e-06;  // Threshold for truncating value to 0
+  constexpr double a_target = 0.0;       // [m/s^2]
+
+  // Calculate time interval with constant acceleration (min_acc)
+  double t_min;
+  {
+    const double v_diff_jerk_dec = 0.5 * calculateTime(a0, 0.0, jerk_dec) * a0 +
+                                   0.5 * calculateTime(0.0, min_acc, jerk_dec) * min_acc;
+    const double v_diff_jerk_acc = 0.5 * calculateTime(min_acc, 0.0, jerk_acc) * min_acc;
+    const double v_error = target_vel - v0;
+    // From v_error = v_diff_jerk_dec + min_acc * t_min + v_diff_jerk_acc
+    t_min = (v_error - v_diff_jerk_dec - v_diff_jerk_acc) / min_acc;
+  }
+
+  if (t_min > 0) {
+    // Necessary to increase deceleration to min_acc
+    type = AccelerationType::TRAPEZOID;
+  } else {
+    const double v_error = target_vel - v0;
+    const double v_diff_decel = 0.5 * a0 * calculateTime(a0, 0.0, jerk_acc);
+    if (v_diff_decel > v_error || a0 > 0.0) {
+      // Necessary to increase deceleration case
+      type = AccelerationType::TRIANGLE;
+    } else {
+      type = AccelerationType::LINEAR;
+    }
+  }
+
+  double x, v, a, j;
+  std::tuple<double, double, double, double> state;
+
+  switch (type) {
+    case AccelerationType::TRAPEZOID: {
+      // Calculate time
+      double t1 = calculateTime(a0, min_acc, jerk_dec);
+      double t2 = t_min;
+      double t3 = calculateTime(min_acc, 0.0, jerk_acc);
+      if (t1 < t_threshold) {
+        t1 = 0;
+      }
+      if (t2 < t_threshold) {
+        t2 = 0;
+      }
+      if (t3 < t_threshold) {
+        t3 = 0;
+      }
+
+      // Set jerk profile
+      jerk_profile.insert(std::make_pair(jerk_dec, t1));
+      jerk_profile.insert(std::make_pair(0.0, t2));
+      jerk_profile.insert(std::make_pair(jerk_acc, t3));
+
+      // Calculate state = (x, v, a, j) at t = t1 + t2 + t3
+      const auto state = updateStateWithJerkConstraint(v0, a0, jerk_profile, t1 + t2 + t3);
+      if (!state) {
+        return false;
+      }
+      std::tie(x, v, a, j) = *state;
+      break;
+    }
+
+    case AccelerationType::TRIANGLE: {
+      /*
+          v_error = v_diff_from_a0_to_zero + v_diff_from_zero_to_a1 + v_diff_from_a1_to_zero,
+          where v_diff_from_zero_to_a1 = 1/2 * ((a1 - 0.0) / jerk_dec) * a1, v_diff_from_a1_to_zero
+         = 1/2 * ((0.0 - a1) / jerk_acc) * a1. Thus a1^2 = (v_error - v_diff_from_a0_to_zero) * 2
+         * 1.0 / (1.0 / jerk_dec - 1.0 / jerk_acc).
+        */
+      const double v_error = target_vel - v0;
+      const double v_diff_from_a0_to_zero = 0.5 * calculateTime(a0, 0.0, jerk_dec) * a0;
+      const double a1_square =
+        (v_error - v_diff_from_a0_to_zero) * 2 * 1.0 / (1.0 / jerk_dec - 1.0 / jerk_acc);
+      const double a1 = -std::sqrt(a1_square);
+
+      // Calculate time
+      double t1 = calculateTime(a0, a1, jerk_dec);
+      double t2 = calculateTime(a1, 0.0, jerk_acc);
+      if (t1 < t_threshold) {
+        t1 = 0;
+      }
+      if (t2 < t_threshold) {
+        t2 = 0;
+      }
+
+      // Set jerk profile
+      jerk_profile.insert(std::make_pair(jerk_dec, t1));
+      jerk_profile.insert(std::make_pair(jerk_acc, t2));
+
+      // Calculate state = (x, v, a, j) at t = t1 + t2
+      const auto state = updateStateWithJerkConstraint(v0, a0, jerk_profile, t1 + t2);
+      if (!state) {
+        return false;
+      }
+      std::tie(x, v, a, j) = *state;
+      break;
+    }
+
+    case AccelerationType::LINEAR: {
+      // Calculate time
+      const double jerk = (0.0 - a0 * a0) / (2 * (target_vel - v0));
+      double t1 = calculateTime(a0, 0.0, jerk);
+      if (t1 < 0) {
+        RCLCPP_WARN(
+          rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"),
+          "t1 < 0. unexpected condition.");
+        return false;
+      } else if (t1 < t_threshold) {
+        t1 = 0;
+      }
+
+      // Set jerk profile
+      jerk_profile.insert(std::make_pair(jerk, t1));
+
+      // Calculate state = (x, v, a, j) at t = t1 + t2
+      const auto state = updateStateWithJerkConstraint(v0, a0, jerk_profile, t1);
+      if (!state) {
+        return false;
+      }
+      std::tie(x, v, a, j) = *state;
+      break;
+    }
+  }
+
+  constexpr double v_margin = 0.3;  // [m/s]
+  constexpr double a_margin = 0.1;  // [m/s^2]
+  stop_dist = x;
+  if (!isValidStopDist(v, a, target_vel, a_target, v_margin, a_margin)) {
+    RCLCPP_WARN_STREAM(
+      rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"),
+      "valid error. type = " << static_cast<size_t>(type));
+    return false;
+  }
+  return true;
+}
+
+bool isValidStopDist(
+  const double v_end, const double a_end, const double v_target, const double a_target,
+  const double v_margin, const double a_margin)
+{
+  const double v_min = v_target - std::abs(v_margin);
+  const double v_max = v_target + std::abs(v_margin);
+  const double a_min = a_target - std::abs(a_margin);
+  const double a_max = a_target + std::abs(a_margin);
+  if (v_end < v_min || v_max < v_end) {
+    RCLCPP_WARN_STREAM(
+      rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"),
+      "valid check error! v_target = " << v_target << ", v_end = " << v_end);
+    return false;
+  }
+  if (a_end < a_min || a_max < a_end) {
+    RCLCPP_WARN_STREAM(
+      rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"),
+      "valid check error! a_target = " << a_target << ", a_end = " << a_end);
+    return false;
+  }
+  return true;
+}
+
+boost::optional<TrajectoryPoints> applyDecelFilterWithJerkConstraint(
+  const TrajectoryPoints & input, const size_t start_index, const double v0, const double a0,
+  const double min_acc, const double decel_target_vel,
+  const std::map<double, double> & jerk_profile)
+{
+  double t_total{0.0};
+  for (auto & it : jerk_profile) {
+    t_total += it.second;
+  }
+
+  std::vector<double> ts;
+  std::vector<double> xs;
+  std::vector<double> vs;
+  std::vector<double> as;
+  std::vector<double> js;
+  const double dt{0.1};
+  double x{0.0};
+  double v{0.0};
+  double a{0.0};
+  double j{0.0};
+
+  for (double t = 0.0; t < t_total; t += dt) {
+    // Calculate state = (x, v, a, j) at t
+    const auto state = updateStateWithJerkConstraint(v0, a0, jerk_profile, t_total);
+    if (!state) {
+      return {};
+    }
+    std::tie(x, v, a, j) = *state;
+    if (v > 0.0) {
+      a = std::max(a, min_acc);
+      ts.push_back(t);
+      xs.push_back(x);
+      vs.push_back(v);
+      as.push_back(a);
+      js.push_back(j);
+    }
+  }
+  // Calculate state = (x, v, a, j) at t_total
+  const auto state = updateStateWithJerkConstraint(v0, a0, jerk_profile, t_total);
+  if (!state) {
+    return {};
+  }
+  std::tie(x, v, a, j) = *state;
+  if (v > 0.0 && !xs.empty() && xs.back() < x) {
+    a = std::max(a, min_acc);
+    ts.push_back(t_total);
+    xs.push_back(x);
+    vs.push_back(v);
+    as.push_back(a);
+    js.push_back(j);
+  }
+
+  const double a_target{0.0};
+  const double v_margin{0.3};
+  const double a_margin{0.1};
+  if (!isValidStopDist(v, a, decel_target_vel, a_target, v_margin, a_margin)) {
+    RCLCPP_WARN_STREAM(
+      rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"),
+      "validation check error");
+    return {};
+  }
+
+  TrajectoryPoints output_trajectory{input};
+
+  if (xs.empty()) {
+    output_trajectory.at(start_index).longitudinal_velocity_mps = decel_target_vel;
+    output_trajectory.at(start_index).acceleration_mps2 = 0.0;
+    for (unsigned int i = start_index + 1; i < output_trajectory.size(); ++i) {
+      output_trajectory.at(i).longitudinal_velocity_mps = decel_target_vel;
+      output_trajectory.at(i).acceleration_mps2 = 0.0;
+    }
+    return output_trajectory;
+  }
+
+  const std::vector<double> distance_all = calcArclengthArray(output_trajectory);
+  const auto it_end = std::find_if(
+    distance_all.begin(), distance_all.end(), [&xs](double x) { return x > xs.back(); });
+  const std::vector<double> distance{distance_all.begin() + start_index, it_end};
+
+  const auto vel_at_wp = linear_interpolation::interpolate(xs, vs, distance);
+  const auto acc_at_wp = linear_interpolation::interpolate(xs, as, distance);
+
+  if (!vel_at_wp || !acc_at_wp) {
+    RCLCPP_WARN_STREAM(
+      rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"),
+      "interpolation error");
+    return {};
+  }
+
+  for (unsigned int i = 0; i < vel_at_wp->size(); ++i) {
+    output_trajectory.at(start_index + i).longitudinal_velocity_mps = vel_at_wp->at(i);
+    output_trajectory.at(start_index + i).acceleration_mps2 = acc_at_wp->at(i);
+  }
+  for (unsigned int i = start_index + vel_at_wp->size(); i < output_trajectory.size(); ++i) {
+    output_trajectory.at(i).longitudinal_velocity_mps = decel_target_vel;
+    output_trajectory.at(i).acceleration_mps2 = 0.0;
+  }
+
+  return output_trajectory;
+}
+
+boost::optional<std::tuple<double, double, double, double>> updateStateWithJerkConstraint(
+  const double v0, const double a0, const std::map<double, double> & jerk_profile, const double t)
+{
+  // constexpr double eps = 1.0E-05;
+  double j{0.0};
+  double a{a0};
+  double v{v0};
+  double x{0.0};
+  double t_sum{0.0};
+
+  for (auto & it : jerk_profile) {
+    j = it.first;
+    t_sum += it.second;
+    if (t > t_sum) {
+      x = integ_x(x, v, a, j, it.second);
+      v = integ_v(v, a, j, it.second);
+      a = integ_a(a, j, it.second);
+    } else {
+      const double dt = t - (t_sum - it.second);
+      x = integ_x(x, v, a, j, dt);
+      v = integ_v(v, a, j, dt);
+      a = integ_a(a, j, dt);
+      const auto state = std::make_tuple(x, v, a, j);
+      return boost::optional<std::tuple<double, double, double, double>>(state);
+    }
+  }
+
+  RCLCPP_WARN_STREAM(
+    rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"),
+    "Invalid jerk profile");
+  return {};
+}
+
+}  // namespace trajectory_utils
+}  // namespace motion_velocity_smoother