diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp
index 6c495797c..1d4d5cc8e 100644
--- a/effort_controllers/src/joint_group_effort_controller.cpp
+++ b/effort_controllers/src/joint_group_effort_controller.cpp
@@ -45,5 +45,21 @@ void forward_command_controller::ForwardJointGroupCommandController<T>::starting
   commands_buffer_.readFromRT()->assign(n_joints_, 0.0);
 }
 
+template <class T>
+void forward_command_controller::ForwardJointGroupCommandController<T>::updateDefaultCommand()
+{
+  // Set defaults to 0
+  for(unsigned int i=0; i<joints_.size(); i++)
+  {
+    default_commands_[i] = 0.0;
+  }
+}
+
+template <class T>
+void forward_command_controller::ForwardJointGroupCommandController<T>::goalCB(GoalHandle gh)
+{
+  // Set as position command
+  setGoal(gh, gh.getGoal()->command.effort);
+}
 
 PLUGINLIB_EXPORT_CLASS(effort_controllers::JointGroupEffortController,controller_interface::ControllerBase)
diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt
index 4b364a959..179b8b759 100644
--- a/forward_command_controller/CMakeLists.txt
+++ b/forward_command_controller/CMakeLists.txt
@@ -8,6 +8,7 @@ endif()
 
 # Load catkin and all dependencies required for this package
 find_package(catkin REQUIRED COMPONENTS
+  actionlib
   controller_interface
   hardware_interface
   realtime_tools
@@ -20,6 +21,7 @@ catkin_package(
   INCLUDE_DIRS
     include
   CATKIN_DEPENDS
+    actionlib
     controller_interface
     hardware_interface
     realtime_tools
diff --git a/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h b/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h
index a0f9e166a..c9061e606 100644
--- a/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h
+++ b/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h
@@ -42,10 +42,20 @@
 #include <string>
 
 #include <ros/node_handle.h>
+#include <realtime_tools/realtime_buffer.h>
+
+// actionlib
+#include <actionlib/server/action_server.h>
+
+// ROS messages
+#include <control_msgs/JointGroupCommandAction.h>
+#include <trajectory_msgs/JointTrajectoryPoint.h>
+#include <std_msgs/Float64MultiArray.h>
+
+// ros_controls
+#include <realtime_tools/realtime_server_goal_handle.h>
 #include <hardware_interface/joint_command_interface.h>
 #include <controller_interface/controller.h>
-#include <std_msgs/Float64MultiArray.h>
-#include <realtime_tools/realtime_buffer.h>
 
 
 namespace forward_command_controller
@@ -68,71 +78,73 @@ namespace forward_command_controller
  * Subscribes to:
  * - \b command (std_msgs::Float64MultiArray) : The joint commands to apply.
  */
-template <class T>
-class ForwardJointGroupCommandController: public controller_interface::Controller<T>
+template <class HardwareInterface>
+class ForwardJointGroupCommandController: public controller_interface::Controller<HardwareInterface>
 {
 public:
   ForwardJointGroupCommandController() {}
   ~ForwardJointGroupCommandController() {sub_command_.shutdown();}
 
-  bool init(T* hw, ros::NodeHandle &n)
-  {
-    // List of controlled joints
-    std::string param_name = "joints";
-    if(!n.getParam(param_name, joint_names_))
-    {
-      ROS_ERROR_STREAM("Failed to getParam '" << param_name << "' (namespace: " << n.getNamespace() << ").");
-      return false;
-    }
-    n_joints_ = joint_names_.size();
-
-    if(n_joints_ == 0){
-      ROS_ERROR_STREAM("List of joint names is empty.");
-      return false;
-    }
-    for(unsigned int i=0; i<n_joints_; i++)
-    {
-      try
-      {
-        joints_.push_back(hw->getHandle(joint_names_[i]));
-      }
-      catch (const hardware_interface::HardwareInterfaceException& e)
-      {
-        ROS_ERROR_STREAM("Exception thrown: " << e.what());
-        return false;
-      }
-    }
-
-    commands_buffer_.writeFromNonRT(std::vector<double>(n_joints_, 0.0));
-
-    sub_command_ = n.subscribe<std_msgs::Float64MultiArray>("command", 1, &ForwardJointGroupCommandController::commandCB, this);
-    return true;
-  }
+  bool init(HardwareInterface* hw, ros::NodeHandle &n);
 
-  void starting(const ros::Time& time);
-  void update(const ros::Time& /*time*/, const ros::Duration& /*period*/)
+  void starting(const ros::Time& /*time*/);
+  void stopping(const ros::Time& /*time*/) {preemptActiveGoal();}
+  void update(const ros::Time& time, const ros::Duration& /*period*/)
   {
     std::vector<double> & commands = *commands_buffer_.readFromRT();
     for(unsigned int i=0; i<n_joints_; i++)
-    {  joints_[i].setCommand(commands[i]);  }
+    {  joints_[i].setCommand(commands[i]); }
+    setActionFeedback(time);
   }
 
-  std::vector< std::string > joint_names_;
-  std::vector< hardware_interface::JointHandle > joints_;
+protected:
+  typedef actionlib::ActionServer<control_msgs::JointGroupCommandAction>                      ActionServer;
+  typedef std::shared_ptr<ActionServer>                                                       ActionServerPtr;
+  typedef ActionServer::GoalHandle                                                            GoalHandle;
+  typedef realtime_tools::RealtimeServerGoalHandle<control_msgs::JointGroupCommandAction>     RealtimeGoalHandle;
+  typedef boost::shared_ptr<RealtimeGoalHandle>                                               RealtimeGoalHandlePtr;
+
   realtime_tools::RealtimeBuffer<std::vector<double> > commands_buffer_;
+  std::vector<double> default_commands_; // Defaults to 0 on init, can override in starting()
   unsigned int n_joints_;
 
+  std::vector< std::string >                     joint_names_;         ///< Controlled joint names.
+  std::vector< hardware_interface::JointHandle > joints_;              ///< Handle to controlled joint.
+  std::string                                    name_;               ///< Controller name.
+  RealtimeGoalHandlePtr                          rt_active_goal_;     ///< Currently active action goal, if any.
+
+  // ROS API
+  ros::NodeHandle    controller_nh_;
+  ros::Subscriber    sub_command_;
+  ActionServerPtr    action_server_;
+  ros::Timer         goal_handle_timer_;
+  ros::Timer         goal_duration_timer_;
+  ros::Duration      action_monitor_period_;
+
+  // Callback to call setGoal. Override with specific command
+  void goalCB(GoalHandle gh);
+  void setGoal(GoalHandle gh, std::vector<double> command);
+
+  // General callbacks
+  void cancelCB(GoalHandle gh);
+  void timeoutCB(const ros::TimerEvent& event);
+  void preemptActiveGoal();
+  void commandCB(const std_msgs::Float64MultiArrayConstPtr& msg);
+
+  // Defaults to no change in default command
+  void updateDefaultCommand();
+
 private:
-  ros::Subscriber sub_command_;
-  void commandCB(const std_msgs::Float64MultiArrayConstPtr& msg)
-  {
-    if(msg->data.size()!=n_joints_)
-    {
-      ROS_ERROR_STREAM("Dimension of command (" << msg->data.size() << ") does not match number of joints (" << n_joints_ << ")! Not executing!");
-      return;
-    }
-    commands_buffer_.writeFromNonRT(msg->data);
-  }
+  /**
+   * @brief Updates the pre-allocated feedback of the current active goal (if any)
+   * based on the current state values.
+   *
+   * @note This function is NOT thread safe but intended to be used in the
+   * update-function.
+   */
+  void setActionFeedback(const ros::Time& time);
 };
 
-}
+} // namespace
+
+#include <forward_command_controller/forward_joint_group_command_controller_impl.h>
diff --git a/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller_impl.h b/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller_impl.h
new file mode 100644
index 000000000..f32446007
--- /dev/null
+++ b/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller_impl.h
@@ -0,0 +1,306 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2021, Personal Robotics Lab
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//   * Redistributions of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//   * Redistributions in binary form must reproduce the above copyright
+//     notice, this list of conditions and the following disclaimer in the
+//     documentation and/or other materials provided with the distribution.
+//   * Neither the name of Personal Robotics Lab nor the names of its
+//     contributors may be used to endorse or promote products derived from
+//     this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+/// \author Ethan Kroll Gordon
+
+#pragma once
+
+
+namespace forward_command_controller
+{
+
+namespace internal
+{
+// TODO: create a utils file?
+/**
+ * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \t2 indices.
+ * If \p t1 is <tt>"{C, B}"</tt> and \p t2 is <tt>"{A, B, C, D}"</tt>, the associated mapping vector is
+ * <tt>"{2, 1}"</tt>.
+ */
+template <class T>
+inline std::vector<unsigned int> mapping(const T& t1, const T& t2)
+{
+  typedef unsigned int SizeType;
+
+  // t1 must be a subset of t2
+  if (t1.size() > t2.size()) {return std::vector<SizeType>();}
+
+  std::vector<SizeType> mapping_vector(t1.size()); // Return value
+  for (typename T::const_iterator t1_it = t1.begin(); t1_it != t1.end(); ++t1_it)
+  {
+    typename T::const_iterator t2_it = std::find(t2.begin(), t2.end(), *t1_it);
+    if (t2.end() == t2_it) {return std::vector<SizeType>();}
+    else
+    {
+      const SizeType t1_dist = std::distance(t1.begin(), t1_it);
+      const SizeType t2_dist = std::distance(t2.begin(), t2_it);
+      mapping_vector[t1_dist] = t2_dist;
+    }
+  }
+  return mapping_vector;
+}
+
+inline std::string getLeafNamespace(const ros::NodeHandle& nh)
+{
+  const std::string complete_ns = nh.getNamespace();
+  std::size_t id   = complete_ns.find_last_of("/");
+  return complete_ns.substr(id + 1);
+}
+
+} // namespace
+
+template <class HardwareInterface>
+bool ForwardJointGroupCommandController<HardwareInterface>::init(HardwareInterface* hw,
+                                                                    ros::NodeHandle&   n)
+{
+    // Cache controller node handle
+    controller_nh_ = n;
+
+    // Controller name
+    name_ = internal::getLeafNamespace(controller_nh_);
+
+    // Action status checking update rate
+    double action_monitor_rate = 20.0;
+    controller_nh_.getParam("action_monitor_rate", action_monitor_rate);
+    action_monitor_period_ = ros::Duration(1.0 / action_monitor_rate);
+    ROS_DEBUG_STREAM_NAMED(name_, "Action status changes will be monitored at " << action_monitor_rate << "Hz.");
+
+    // Initialize controlled joints
+    std::string param_name = "joints";
+    if(!n.getParam(param_name, joint_names_))
+    {
+      ROS_ERROR_STREAM("Failed to getParam '" << param_name << "' (namespace: " << n.getNamespace() << ").");
+      return false;
+    }
+    n_joints_ = joint_names_.size();
+
+    if(n_joints_ == 0){
+      ROS_ERROR_STREAM("List of joint names is empty.");
+      return false;
+    }
+    for(unsigned int i=0; i<n_joints_; i++)
+    {
+      try
+      {
+        joints_.push_back(hw->getHandle(joint_names_[i]));
+      }
+      catch (const hardware_interface::HardwareInterfaceException& e)
+      {
+        ROS_ERROR_STREAM("Exception thrown: " << e.what());
+        return false;
+      }
+    }
+
+    commands_buffer_.writeFromNonRT(std::vector<double>(n_joints_, 0.0));
+    default_commands_.resize(n_joints_);
+
+    // ROS API: Subscribed topics
+    sub_command_ = n.subscribe<std_msgs::Float64MultiArray>("command", 1, &ForwardJointGroupCommandController::commandCB, this);
+
+    // ROS API: Action interface
+    action_server_.reset(new ActionServer(controller_nh_, "joint_group_command",
+                                        boost::bind(&ForwardJointGroupCommandController::goalCB,   this, _1),
+                                        boost::bind(&ForwardJointGroupCommandController::cancelCB, this, _1),
+                                        false));
+    action_server_->start();
+    return true;
+}
+
+template <class HardwareInterface>
+void ForwardJointGroupCommandController<HardwareInterface>::preemptActiveGoal()
+{
+    RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);
+
+    // Cancel any goal timeout
+    goal_duration_timer_.stop();
+
+    // Cancels the currently active goal
+    if (current_active_goal)
+    {
+      // Marks the current goal as canceled
+      rt_active_goal_.reset();
+      current_active_goal->gh_.setCanceled();
+    }
+}
+
+template <class HardwareInterface>
+void ForwardJointGroupCommandController<HardwareInterface>::commandCB(const std_msgs::Float64MultiArrayConstPtr& msg)
+{
+  // Preconditions
+  if (!this->isRunning())
+  {
+    ROS_ERROR_STREAM_NAMED(name_, "Can't accept new commands. Controller is not running.");
+    return;
+  }
+
+  if (!msg)
+  {
+    ROS_WARN_STREAM_NAMED(name_, "Received null-pointer message, skipping.");
+    return;
+  }
+
+  if(msg->data.size()!=n_joints_)
+  {
+    ROS_ERROR_STREAM_NAMED(name_, "Dimension of command (" << msg->data.size() << ") does not match number of joints (" << n_joints_ << ")! Not executing!");
+    return;
+  }
+
+  commands_buffer_.writeFromNonRT(msg->data);
+  preemptActiveGoal();
+}
+
+template <class HardwareInterface>
+void ForwardJointGroupCommandController<HardwareInterface>::setGoal(GoalHandle gh,
+  std::vector<double> command)
+{
+  ROS_DEBUG_STREAM_NAMED(name_,"Received new action goal");
+  control_msgs::JointGroupCommandResult result;
+
+  // Preconditions
+  if (!this->isRunning())
+  {
+    result.error_string = "Can't accept new action goals. Controller is not running.";
+    ROS_ERROR_STREAM_NAMED(name_, result.error_string);
+    result.error_code = control_msgs::JointGroupCommandResult::INVALID_GOAL;
+    gh.setRejected(result);
+    return;
+  }
+
+
+  if (gh.getGoal()->joint_names.size() != command.size()) {
+    result.error_string = "Size of command must match size of joint_names.";
+    ROS_ERROR_STREAM_NAMED(name_, result.error_string);
+    result.error_code = control_msgs::JointGroupCommandResult::INVALID_GOAL;
+    gh.setRejected(result);
+    return;
+  }
+
+  // Goal should specify valid controller joints (they can be ordered differently). Reject if this is not the case
+  using internal::mapping;
+  std::vector<unsigned int> mapping_vector = mapping(gh.getGoal()->joint_names, joint_names_);
+
+  if (mapping_vector.size() != gh.getGoal()->joint_names.size())
+  {
+    result.error_string = "Joints on incoming goal don't match the controller joints.";
+    ROS_ERROR_STREAM_NAMED(name_, result.error_string);
+    result.error_code = control_msgs::JointGroupCommandResult::INVALID_JOINTS;
+    gh.setRejected(result);
+    return;
+  }
+
+  // update new command
+  RealtimeGoalHandlePtr rt_goal(new RealtimeGoalHandle(gh));
+  std::vector< double > new_commands = default_commands_;
+  for(int i = 0; i < mapping_vector.size(); i++) {
+    new_commands[mapping_vector[i]] = command[i];
+  }
+  rt_goal->preallocated_feedback_->joint_names = joint_names_;
+  commands_buffer_.writeFromNonRT(new_commands);
+
+  // Accept new goal
+  preemptActiveGoal();
+  gh.setAccepted();
+  rt_active_goal_ = rt_goal;
+
+  // Setup goal status checking timer
+  goal_handle_timer_ = controller_nh_.createTimer(action_monitor_period_,
+                                                    &RealtimeGoalHandle::runNonRealtime,
+                                                    rt_goal);
+  goal_handle_timer_.start();
+
+  // Setup goal timeout
+  if (gh.getGoal()->command.time_from_start > ros::Duration()) {
+    goal_duration_timer_ = controller_nh_.createTimer(gh.getGoal()->command.time_from_start,
+                                                    &ForwardJointGroupCommandController::timeoutCB,
+                                                    this,
+                                                    true);
+    goal_duration_timer_.start();
+  }
+}
+
+template <class HardwareInterface>
+void ForwardJointGroupCommandController<HardwareInterface>::timeoutCB(const ros::TimerEvent& event)
+{
+  RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);
+
+  // Check that timeout refers to currently active goal (if any)
+  if (current_active_goal) {
+    ROS_DEBUG_NAMED(name_, "Active action goal reached requested timeout.");
+
+    // Give sub-classes option to update default_commands_
+    updateDefaultCommand();
+    commands_buffer_.writeFromNonRT(default_commands_);
+
+    // Marks the current goal as succeeded
+    rt_active_goal_.reset();
+    current_active_goal->gh_.setSucceeded();
+  }
+}
+
+template <class HardwareInterface>
+void ForwardJointGroupCommandController<HardwareInterface>::cancelCB(GoalHandle gh)
+{
+  RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);
+
+  // Check that cancel request refers to currently active goal
+  if (current_active_goal && current_active_goal->gh_ == gh)
+  {
+    ROS_DEBUG_NAMED(name_, "Canceling active action goal because cancel callback recieved from actionlib.");
+
+    // Give sub-classes option to update default_commands_
+    updateDefaultCommand();
+    commands_buffer_.writeFromNonRT(default_commands_);
+
+    preemptActiveGoal();
+  }
+}
+
+template <class HardwareInterface>
+void ForwardJointGroupCommandController<HardwareInterface>::setActionFeedback(const ros::Time& time)
+{
+  RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);
+  if (!current_active_goal)
+  {
+    return;
+  }
+
+  current_active_goal->preallocated_feedback_->header.stamp = time;
+  current_active_goal->preallocated_feedback_->desired = current_active_goal->gh_.getGoal()->command;
+  current_active_goal->preallocated_feedback_->actual.positions.clear();
+  current_active_goal->preallocated_feedback_->actual.velocities.clear();
+  current_active_goal->preallocated_feedback_->actual.effort.clear();
+  for (auto j : joints_)
+  {
+    current_active_goal->preallocated_feedback_->actual.positions.push_back(j.getPosition());
+    current_active_goal->preallocated_feedback_->actual.velocities.push_back(j.getVelocity());
+    current_active_goal->preallocated_feedback_->actual.effort.push_back(j.getEffort());
+  }
+
+  current_active_goal->setFeedback( current_active_goal->preallocated_feedback_ );
+}
+
+} // namespace
diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml
index 916460241..0d2d72d18 100644
--- a/forward_command_controller/package.xml
+++ b/forward_command_controller/package.xml
@@ -22,6 +22,8 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
+  <depend>actionlib</depend>
+
   <build_depend>controller_interface</build_depend>
   <build_depend>hardware_interface</build_depend>
   <build_depend>realtime_tools</build_depend>
diff --git a/position_controllers/src/joint_group_position_controller.cpp b/position_controllers/src/joint_group_position_controller.cpp
index 95bb7945d..f70a34a7c 100644
--- a/position_controllers/src/joint_group_position_controller.cpp
+++ b/position_controllers/src/joint_group_position_controller.cpp
@@ -46,8 +46,25 @@ void forward_command_controller::ForwardJointGroupCommandController<T>::starting
   for(unsigned int i=0; i<joints_.size(); i++)
   {
     commands[i]=joints_[i].getPosition();
+    default_commands_[i] = commands[i];
   }
 }
 
+template <class T>
+void forward_command_controller::ForwardJointGroupCommandController<T>::updateDefaultCommand()
+{
+  // Set default to current position
+  for(unsigned int i=0; i<joints_.size(); i++)
+  {
+    default_commands_[i] = joints_[i].getPosition();
+  }
+}
+
+template <class T>
+void forward_command_controller::ForwardJointGroupCommandController<T>::goalCB(GoalHandle gh)
+{
+  // Set as position command
+  setGoal(gh, gh.getGoal()->command.positions);
+}
 
 PLUGINLIB_EXPORT_CLASS(position_controllers::JointGroupPositionController,controller_interface::ControllerBase)
diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp
index d6e64068f..8f7ebaaca 100644
--- a/velocity_controllers/src/joint_group_velocity_controller.cpp
+++ b/velocity_controllers/src/joint_group_velocity_controller.cpp
@@ -45,5 +45,21 @@ void forward_command_controller::ForwardJointGroupCommandController<T>::starting
   commands_buffer_.readFromRT()->assign(n_joints_, 0.0);
 }
 
+template <class T>
+void forward_command_controller::ForwardJointGroupCommandController<T>::updateDefaultCommand()
+{
+  // Set default to 0
+  for(unsigned int i=0; i<joints_.size(); i++)
+  {
+    default_commands_[i] = 0.0;
+  }
+}
+
+template <class T>
+void forward_command_controller::ForwardJointGroupCommandController<T>::goalCB(GoalHandle gh)
+{
+  // Set as position command
+  setGoal(gh, gh.getGoal()->command.velocities);
+}
 
 PLUGINLIB_EXPORT_CLASS(velocity_controllers::JointGroupVelocityController,controller_interface::ControllerBase)