diff --git a/CMakeLists.txt b/CMakeLists.txt
index b258306..9933bca 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -67,6 +67,9 @@ add_library("${PROJECT_NAME}" SHARED
   src/MoveUntilTouchTopicController.cpp
   src/FTThresholdClient.cpp
   src/FTThresholdServer.cpp
+  src/joint_group_position_controller.cpp
+  src/joint_group_velocity_controller.cpp
+  src/joint_group_effort_controller.cpp
 )
 target_link_libraries("${PROJECT_NAME}"
   ${DART_LIBRARIES}
diff --git a/include/rewd_controllers/detail/forward_joint_group_command_controller_impl.h b/include/rewd_controllers/detail/forward_joint_group_command_controller_impl.h
new file mode 100644
index 0000000..73cebd5
--- /dev/null
+++ b/include/rewd_controllers/detail/forward_joint_group_command_controller_impl.h
@@ -0,0 +1,309 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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;
+    }
+    
+    // Clear joints_ first in case this is called twice
+    joints_.clear();
+    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");
+  pr_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 = pr_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 = pr_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 = pr_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/include/rewd_controllers/forward_joint_group_command_controller.h b/include/rewd_controllers/forward_joint_group_command_controller.h
new file mode 100644
index 0000000..1c8e949
--- /dev/null
+++ b/include/rewd_controllers/forward_joint_group_command_controller.h
@@ -0,0 +1,150 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  Copyright (c) 2012, hiDOF, Inc.
+ *  Copyright (c) 2013, PAL Robotics, S.L.
+ *  Copyright (c) 2014, Fraunhofer IPA
+ *  All rights reserved.
+ *
+ *  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 the Willow Garage 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.
+ *********************************************************************/
+
+#pragma once
+
+
+#include <vector>
+#include <string>
+
+#include <ros/node_handle.h>
+#include <realtime_tools/realtime_buffer.h>
+
+// actionlib
+#include <actionlib/server/action_server.h>
+
+// ROS messages
+#include <pr_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>
+
+
+namespace forward_command_controller
+{
+
+/**
+ * \brief Forward command controller for a set of joints.
+ *
+ * This class forwards the command signal down to a set of joints.
+ * Command signal and joint hardware interface are of the same type, e.g. effort commands for an effort-controlled
+ * joint.
+ *
+ * \tparam T Type implementing the JointCommandInterface.
+ *
+ * \section ROS interface
+ *
+ * \param type hardware interface type.
+ * \param joints Names of the joints to control.
+ *
+ * Subscribes to:
+ * - \b command (std_msgs::Float64MultiArray) : The joint commands to apply.
+ */
+template <class HardwareInterface>
+class ForwardJointGroupCommandController: public controller_interface::Controller<HardwareInterface>
+{
+public:
+  ForwardJointGroupCommandController() {}
+  ~ForwardJointGroupCommandController() {sub_command_.shutdown();}
+
+  bool init(HardwareInterface* hw, ros::NodeHandle &n);
+
+  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]); }
+    setActionFeedback(time);
+  }
+
+protected:
+  typedef actionlib::ActionServer<pr_control_msgs::JointGroupCommandAction>                   ActionServer;
+  typedef std::shared_ptr<ActionServer>                                                       ActionServerPtr;
+  typedef ActionServer::GoalHandle                                                            GoalHandle;
+  typedef realtime_tools::RealtimeServerGoalHandle<pr_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:
+  /**
+   * @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 <rewd_controllers/detail/forward_joint_group_command_controller_impl.h>
diff --git a/include/rewd_controllers/joint_group_effort_controller.h b/include/rewd_controllers/joint_group_effort_controller.h
new file mode 100644
index 0000000..f9a0435
--- /dev/null
+++ b/include/rewd_controllers/joint_group_effort_controller.h
@@ -0,0 +1,61 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  Copyright (c) 2012, hiDOF, Inc.
+ *  Copyright (c) 2013, PAL Robotics, S.L.
+ *  Copyright (c) 2014, Fraunhofer IPA
+ *  All rights reserved.
+ *
+ *  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 the Willow Garage 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.
+ *********************************************************************/
+
+#pragma once
+
+
+#include <rewd_controllers/forward_joint_group_command_controller.h>
+
+namespace effort_controllers
+{
+
+/**
+ * \brief Forward command controller for a set of effort controlled joints (torque or force).
+ *
+ * This class forwards the commanded efforts down to a set of joints.
+ *
+ * \section ROS interface
+ *
+ * \param type Must be "JointGroupEffortController".
+ * \param joints List of names of the joints to control.
+ *
+ * Subscribes to:
+ * - \b command (std_msgs::Float64MultiArray) : The joint efforts to apply
+ */
+typedef forward_command_controller::ForwardJointGroupCommandController<hardware_interface::EffortJointInterface>
+        JointGroupEffortController;
+}
diff --git a/include/rewd_controllers/joint_group_position_controller.h b/include/rewd_controllers/joint_group_position_controller.h
new file mode 100644
index 0000000..b1609cf
--- /dev/null
+++ b/include/rewd_controllers/joint_group_position_controller.h
@@ -0,0 +1,62 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  Copyright (c) 2012, hiDOF, Inc.
+ *  Copyright (c) 2013, PAL Robotics, S.L.
+ *  Copyright (c) 2014, Fraunhofer IPA
+ *  All rights reserved.
+ *
+ *  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 the Willow Garage 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.
+ *********************************************************************/
+
+#pragma once
+
+
+#include <rewd_controllers/forward_joint_group_command_controller.h>
+#include <hardware_interface/joint_command_interface.h>
+
+namespace position_controllers
+{
+
+/**
+ * \brief Forward command controller for a set of position controlled joints (linear or angular).
+ *
+ * This class forwards the commanded positions down to a set of joints.
+ *
+ * \section ROS interface
+ *
+ * \param type Must be "JointGroupPositionController".
+ * \param joints List of names of the joints to control.
+ *
+ * Subscribes to:
+ * - \b command (std_msgs::Float64MultiArray) : The joint positions to apply
+ */
+typedef forward_command_controller::ForwardJointGroupCommandController<hardware_interface::PositionJointInterface>
+        JointGroupPositionController;
+}
diff --git a/include/rewd_controllers/joint_group_velocity_controller.h b/include/rewd_controllers/joint_group_velocity_controller.h
new file mode 100644
index 0000000..e01ad40
--- /dev/null
+++ b/include/rewd_controllers/joint_group_velocity_controller.h
@@ -0,0 +1,63 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  Copyright (c) 2012, hiDOF, Inc.
+ *  Copyright (c) 2013, PAL Robotics, S.L.
+ *  Copyright (c) 2014, Fraunhofer IPA
+ *  All rights reserved.
+ *
+ *  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 the Willow Garage 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.
+ *********************************************************************/
+
+#pragma once
+
+
+#include <rewd_controllers/forward_joint_group_command_controller.h>
+#include <hardware_interface/joint_command_interface.h>
+
+namespace velocity_controllers
+{
+
+/**
+ * \brief Forward command controller for a set of velocity controlled joints (linear or angular).
+ *
+ * This class forwards the commanded velocities down to a set of joints.
+ *
+ * \section ROS interface
+ *
+ * \param type Must be "JointGroupVelocityController".
+ * \param joints List of names of the joints to control.
+ *
+ * Subscribes to:
+ * - \b command (std_msgs::Float64MultiArray) : The joint velocities to apply
+ */
+typedef forward_command_controller::ForwardJointGroupCommandController<hardware_interface::VelocityJointInterface>
+        JointGroupVelocityController;
+
+}
diff --git a/package.xml b/package.xml
index 524d4f6..072e86e 100644
--- a/package.xml
+++ b/package.xml
@@ -3,7 +3,7 @@
   <name>rewd_controllers</name>
   <version>0.0.0</version>
   <description>The ROS Effort With Dynamics Controllers package</description>
-  <maintainer email="cliddick@cmu.edu">Clint Liddick</maintainer>
+  <maintainer email="ekgordon@cs.uw.edu">Ethan K. Gordon</maintainer>
   <license>BSD</license>
   <url type="repository">https://github.com/personalrobotics/rewd_controllers</url>
   <author email="cliddick@cmu.edu">Clint Liddick</author>
diff --git a/src/joint_group_effort_controller.cpp b/src/joint_group_effort_controller.cpp
new file mode 100644
index 0000000..5661418
--- /dev/null
+++ b/src/joint_group_effort_controller.cpp
@@ -0,0 +1,65 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  Copyright (c) 2012, hiDOF, Inc.
+ *  Copyright (c) 2013, PAL Robotics, S.L.
+ *  Copyright (c) 2014, Fraunhofer IPA
+ *  All rights reserved.
+ *
+ *  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 the Willow Garage 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.
+ *********************************************************************/
+
+#include <rewd_controllers/joint_group_effort_controller.h>
+#include <pluginlib/class_list_macros.hpp>
+
+template <class T>
+void forward_command_controller::ForwardJointGroupCommandController<T>::starting(const ros::Time& time)
+{
+  // Start controller with 0.0 efforts
+  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/src/joint_group_position_controller.cpp b/src/joint_group_position_controller.cpp
new file mode 100644
index 0000000..371ddcb
--- /dev/null
+++ b/src/joint_group_position_controller.cpp
@@ -0,0 +1,70 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  Copyright (c) 2012, hiDOF, Inc.
+ *  Copyright (c) 2013, PAL Robotics, S.L.
+ *  Copyright (c) 2014, Fraunhofer IPA
+ *  All rights reserved.
+ *
+ *  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 the Willow Garage 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.
+ *********************************************************************/
+
+#include <rewd_controllers/joint_group_position_controller.h>
+#include <pluginlib/class_list_macros.hpp>
+
+template <class T>
+void forward_command_controller::ForwardJointGroupCommandController<T>::starting(const ros::Time& time)
+{
+  // Start controller with current joint positions
+  std::vector<double> & commands = *commands_buffer_.readFromRT();
+  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/src/joint_group_velocity_controller.cpp b/src/joint_group_velocity_controller.cpp
new file mode 100644
index 0000000..a625e5f
--- /dev/null
+++ b/src/joint_group_velocity_controller.cpp
@@ -0,0 +1,65 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  Copyright (c) 2012, hiDOF, Inc.
+ *  Copyright (c) 2013, PAL Robotics, S.L.
+ *  Copyright (c) 2014, Fraunhofer IPA
+ *  All rights reserved.
+ *
+ *  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 the Willow Garage 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.
+ *********************************************************************/
+
+#include <rewd_controllers/joint_group_velocity_controller.h>
+#include <pluginlib/class_list_macros.hpp>
+
+template <class T>
+void forward_command_controller::ForwardJointGroupCommandController<T>::starting(const ros::Time& time)
+{
+  // Start controller with 0.0 velocities
+  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)