From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Wed, 28 Nov 2018 22:05:58 +0000 (-0200)
Subject: Change wam_controllers for robot independent controller packages.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=c2a37eef024de043a818a6619ac4acef3894f277;p=ufrgs_wam.git

Change wam_controllers for robot independent controller packages.

Add wam_simulation to hold the Barrett WAM specific controller configuration.
---

diff --git a/wam_controllers/config/computed_torque_controller.yaml b/wam_controllers/config/computed_torque_controller.yaml
deleted file mode 100644
index d9f19ab..0000000
--- a/wam_controllers/config/computed_torque_controller.yaml
+++ /dev/null
@@ -1,31 +0,0 @@
-wam:
-    joint_state_controller:
-      type: joint_state_controller/JointStateController
-      publish_rate: 100
-
-    computed_torque_controller:
-      type: effort_controllers/ComputedTorqueController
-      joints:
-        - wam_joint_1
-        - wam_joint_2
-        - wam_joint_3
-        - wam_joint_4
-        - wam_joint_5
-        - wam_joint_6
-        - wam_joint_7
-      Kp: [25.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 
-          0.0, 25.0, 0.0, 0.0, 0.0, 0.0, 0.0, 
-          0.0, 0.0, 25.0, 0.0, 0.0, 0.0, 0.0, 
-          0.0, 0.0, 0.0, 25.0, 0.0, 0.0, 0.0, 
-          0.0, 0.0, 0.0, 0.0, 25.0, 0.0, 0.0, 
-          0.0, 0.0, 0.0, 0.0, 0.0, 25.0, 0.0, 
-          0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 25.0]
-      Kd: [10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 
-          0.0, 10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 
-          0.0, 0.0, 10.0, 0.0, 0.0, 0.0, 0.0, 
-          0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0, 
-          0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 
-          0.0, 0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 
-          0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0]
-      gravity: {x: 0.0, y: 0.0, z: -9.8}
-      chain: {root: "wam_origin", tip: "wam_tool_plate"}
diff --git a/wam_controllers/config/pid_plus_gravity_controller.yaml b/wam_controllers/config/pid_plus_gravity_controller.yaml
deleted file mode 100644
index b7b7d59..0000000
--- a/wam_controllers/config/pid_plus_gravity_controller.yaml
+++ /dev/null
@@ -1,32 +0,0 @@
-wam:
-    joint_state_controller:
-      type: joint_state_controller/JointStateController
-      publish_rate: 250
-
-    pid_plus_gravity_controller:
-      type: effort_controllers/PidPlusGravityController
-      joints:
-        - wam_joint_1
-        - wam_joint_2
-        - wam_joint_3
-        - wam_joint_4
-        - wam_joint_5
-        - wam_joint_6
-        - wam_joint_7
-# PID parameters from /etc/barrett/wam7w.conf
-      wam_joint_1:
-        pid: {p: 900.0, i: 2.5, d: 10.0, i_clamp: 25.0}
-      wam_joint_2:
-        pid: {p: 2500.0, i: 5.0, d: 20.0, i_clamp: 20.0}
-      wam_joint_3:
-        pid: {p: 600.0, i: 2.0, d: 5.0, i_clamp: 15.0}
-      wam_joint_4:
-        pid: {p: 500.0, i: 0.5, d: 2.0, i_clamp: 15.0}
-      wam_joint_5:
-        pid: {p: 50.0, i: 0.5, d: 0.5, i_clamp: 5.0}
-      wam_joint_6:
-        pid: {p: 50.0, i: 0.5, d: 0.5, i_clamp: 5.0}
-      wam_joint_7:
-        pid: {p: 8.0, i: 0.1, d: 0.05, i_clamp: 5.0}
-      gravity: {x: 0.0, y: 0.0, z: -9.8}
-      chain: {root: "wam_origin", tip: "wam_tool_plate"}
diff --git a/wam_controllers/doc/urdf2kdl.txt b/wam_controllers/doc/urdf2kdl.txt
deleted file mode 100644
index 0f5af05..0000000
--- a/wam_controllers/doc/urdf2kdl.txt
+++ /dev/null
@@ -1,9 +0,0 @@
-KDL specifies the inertia in the reference frame of the link, the URDF
- specifies the inertia in the inertia reference frame.
-
-A KDL segment is an ideal rigid body to which one single Joint is connected
-and one single tip frame.  It contains:
-
-- a Joint located at the root frame of the Segment.
-- a Frame describing the pose between the end of the Joint and the tip
-frame of the Segment.
diff --git a/wam_controllers/include/wam_controllers/computed_torque_controller.h b/wam_controllers/include/wam_controllers/computed_torque_controller.h
deleted file mode 100644
index 15fe88a..0000000
--- a/wam_controllers/include/wam_controllers/computed_torque_controller.h
+++ /dev/null
@@ -1,84 +0,0 @@
-/******************************************************************************
-			   ROS wam_controllers Package
-                           Computed Torque  Controller
-          Copyright (C) 2013..2017 Walter Fetter Lages <w.fetter@ieee.org>
-
-        This program is free software: you can redistribute it and/or modify
-        it under the terms of the GNU General Public License as published by
-        the Free Software Foundation, either version 3 of the License, or
-        (at your option) any later version.
-
-        This program is distributed in the hope that it will be useful, but
-        WITHOUT ANY WARRANTY; without even the implied warranty of
-        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
-        General Public License for more details.
-
-        You should have received a copy of the GNU General Public License
-        along with this program.  If not, see
-        <http://www.gnu.org/licenses/>.
-        
-*******************************************************************************/
-
-#ifndef WAM_CONTROLLERS_COMPUTED_TORQUE_CONTROLLER_H
-#define WAM_CONTROLLERS_COMPUTED_TORQUE_CONTROLLER_H
-
-#include <cstddef>
-#include <vector>
-#include <string>
-
-#include <ros/node_handle.h>
-#include <hardware_interface/joint_command_interface.h>
-#include <controller_interface/controller.h>
-#include <trajectory_msgs/JointTrajectoryPoint.h>
-#include <sensor_msgs/JointState.h>
-
-#include <Eigen/Core>
-
-#include <kdl/frames.hpp>
-#include <kdl_parser/kdl_parser.hpp>
-#include <kdl/chainidsolver_recursive_newton_euler.hpp>
-
-namespace effort_controllers
-{
-	class ComputedTorqueController: public controller_interface::
-		Controller<hardware_interface::EffortJointInterface>
-	{
-		ros::NodeHandle node_;
-
-		hardware_interface::EffortJointInterface *hw_;
-		std::vector<hardware_interface::JointHandle> joints_;
-		int nJoints_;
-
-		ros::Subscriber sub_command_;
-			
-		KDL::ChainIdSolver_RNE *idsolver;
-				
-		KDL::JntArray q;
-		KDL::JntArray dq;
-		KDL::JntArray v;
-			
-		KDL::JntArray qr;
-		KDL::JntArray dqr;
-		KDL::JntArray ddqr;
-			
-		KDL::JntArray torque;
-		
-		KDL::Wrenches fext;
-			
-		Eigen::MatrixXd Kp;
-		Eigen::MatrixXd Kd;
-		
-		void commandCB(const trajectory_msgs::JointTrajectoryPoint::ConstPtr 
-			&referencePoint);
-		
-		public:
-		ComputedTorqueController(void);
-		~ComputedTorqueController(void);
-		
-		bool init(hardware_interface::EffortJointInterface *hw,
-			ros::NodeHandle &n);
-		void starting(const ros::Time& time);
-		void update(const ros::Time& time,const ros::Duration& duration);
-	};
-}
-#endif
diff --git a/wam_controllers/include/wam_controllers/pid_plus_gravity_controller.h b/wam_controllers/include/wam_controllers/pid_plus_gravity_controller.h
deleted file mode 100644
index 2fcfe45..0000000
--- a/wam_controllers/include/wam_controllers/pid_plus_gravity_controller.h
+++ /dev/null
@@ -1,75 +0,0 @@
-/******************************************************************************
-			    ROS wam_controllers Package
-                        PID+Gravity Compensation Controller
-          Copyright (C) 2017 Walter Fetter Lages <w.fetter@ieee.org>
-
-        This program is free software: you can redistribute it and/or modify
-        it under the terms of the GNU General Public License as published by
-        the Free Software Foundation, either version 3 of the License, or
-        (at your option) any later version.
-
-        This program is distributed in the hope that it will be useful, but
-        WITHOUT ANY WARRANTY; without even the implied warranty of
-        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
-        General Public License for more details.
-
-        You should have received a copy of the GNU General Public License
-        along with this program.  If not, see
-        <http://www.gnu.org/licenses/>.
-        
-*******************************************************************************/
-
-#ifndef WAM_CONTROLLERS_PID_PLUS_GRAVITY_CONTROLLER_H
-#define WAM_CONTROLLERS_PID_PLUS_GRAVITY_CONTROLLER_H
-
-#include <cstddef>
-#include <vector>
-#include <string>
-
-#include <ros/node_handle.h>
-#include <hardware_interface/joint_command_interface.h>
-#include <control_toolbox/pid.h>
-#include <controller_interface/controller.h>
-#include <trajectory_msgs/JointTrajectoryPoint.h>
-#include <sensor_msgs/JointState.h>
-
-#include <Eigen/Core>
-
-#include <kdl/frames.hpp>
-#include <kdl_parser/kdl_parser.hpp>
-#include <kdl/chaindynparam.hpp>
-
-namespace effort_controllers
-{
-	class PidPlusGravityController: public controller_interface::
-		Controller<hardware_interface::EffortJointInterface>
-	{
-		ros::NodeHandle node_;
-
-		hardware_interface::EffortJointInterface *hw_;
-		std::vector<hardware_interface::JointHandle> joints_;
-
-		ros::Subscriber sub_command_;
-			
-		KDL::ChainDynParam *chainParam_;
-				
-		KDL::JntArray q_;
-		KDL::JntArray qr_;
-		KDL::JntArray gravity_;
-		
-		std::vector<control_toolbox::Pid> pid_;
-		
-		void commandCB(const trajectory_msgs::JointTrajectoryPoint::ConstPtr 
-			&referencePoint);
-		
-		public:
-		PidPlusGravityController(void);
-		~PidPlusGravityController(void);
-		
-		bool init(hardware_interface::EffortJointInterface *hw,
-			ros::NodeHandle &n);
-		void starting(const ros::Time& time);
-		void update(const ros::Time& time,const ros::Duration& duration);
-	};
-}
-#endif
diff --git a/wam_controllers/launch/computed_torque.launch b/wam_controllers/launch/computed_torque.launch
deleted file mode 100644
index c452826..0000000
--- a/wam_controllers/launch/computed_torque.launch
+++ /dev/null
@@ -1,9 +0,0 @@
-<launch>
-	<rosparam file="$(find wam_controllers)/config/computed_torque_controller.yaml"
-		command="load"/>
-
-	<node name="controller_spawner" pkg="controller_manager" type="spawner"
-		respawn="false" output="screen" ns="wam"
-		args="joint_state_controller computed_torque_controller" >
-	</node>
-</launch>
diff --git a/wam_controllers/launch/display.launch b/wam_controllers/launch/display.launch
deleted file mode 100644
index e9c9698..0000000
--- a/wam_controllers/launch/display.launch
+++ /dev/null
@@ -1,19 +0,0 @@
-<launch>
-	<arg name="table" default="true"/>
-	<arg name="bhand" default="true"/>
-	<arg name="use_gui" default="false"/>
-
-	<remap from="/joint_states" to="wam/joint_states" />
-
-	<include file="$(find wam_description)/launch/display.launch" >
-		<arg name="table" value="$(arg table)"/>
-		<arg name="bhand" value="$(arg bhand)"/>
-		<arg name="use_gui" value="$(arg use_gui)" />
-
-	</include>
-
-	<!-- For this to work, a controller manager should be loaded -->
-
-	<!--include file="$(find wam_controllers)/launch/computed_torque.launch" /-->
-
-</launch>
diff --git a/wam_controllers/launch/gazebo_pid_plus_gravity.launch b/wam_controllers/launch/gazebo_pid_plus_gravity.launch
deleted file mode 100644
index 9d63288..0000000
--- a/wam_controllers/launch/gazebo_pid_plus_gravity.launch
+++ /dev/null
@@ -1,19 +0,0 @@
-<launch>
-	<arg name="paused" default="true"/>
-	<arg name="headless" default="false"/>
-	<arg name="use_sim_time" default="true"/>
-	<arg name="table" default="true"/>
-	<arg name="bhand" default="false"/>
-	
-
-	<include file="$(find wam_description)/launch/gazebo.launch" >
-		<arg name="paused" value="$(arg paused)"/>
-		<arg name="headless" value="$(arg headless)"/>
-		<arg name="use_sim_time" value="$(arg use_sim_time)"/>
-		<arg name="table" value="$(arg table)"/>
-		<arg name="bhand" value="$(arg bhand)"/>
-	</include>
-
-	<include file="$(find wam_controllers)/launch/pid_plus_gravity.launch" />
-
-</launch>
diff --git a/wam_controllers/launch/pid_plus_gravity.launch b/wam_controllers/launch/pid_plus_gravity.launch
deleted file mode 100644
index c0005a0..0000000
--- a/wam_controllers/launch/pid_plus_gravity.launch
+++ /dev/null
@@ -1,9 +0,0 @@
-<launch>
-	<rosparam file="$(find wam_controllers)/config/pid_plus_gravity_controller.yaml"
-		command="load"/>
-
-	<node name="controller_spawner" pkg="controller_manager" type="spawner"
-		respawn="false" output="screen" ns="wam"
-		args="joint_state_controller pid_plus_gravity_controller" >
-	</node>
-</launch>
diff --git a/wam_controllers/src/computed_torque_controller.cpp b/wam_controllers/src/computed_torque_controller.cpp
deleted file mode 100644
index 5165310..0000000
--- a/wam_controllers/src/computed_torque_controller.cpp
+++ /dev/null
@@ -1,201 +0,0 @@
-/******************************************************************************
-			   ROS wam_controllers Package
-                           Computed Torque  Controller
-          Copyright (C) 2013..2017 Walter Fetter Lages <w.fetter@ieee.org>
-
-        This program is free software: you can redistribute it and/or modify
-        it under the terms of the GNU General Public License as published by
-        the Free Software Foundation, either version 3 of the License, or
-        (at your option) any later version.
-
-        This program is distributed in the hope that it will be useful, but
-        WITHOUT ANY WARRANTY; without even the implied warranty of
-        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
-        General Public License for more details.
-
-        You should have received a copy of the GNU General Public License
-        along with this program.  If not, see
-        <http://www.gnu.org/licenses/>.
-        
-*******************************************************************************/
-
-#include <sys/mman.h>
-
-#include <wam_controllers/computed_torque_controller.h>
-#include <pluginlib/class_list_macros.h>
-
-namespace effort_controllers
-{	
-	ComputedTorqueController::ComputedTorqueController(void):
-		q(0),dq(0),v(0),qr(0),dqr(0),ddqr(0),torque(0),fext(0)
-	{
-	}
-	
-	ComputedTorqueController::~ComputedTorqueController(void)
-	{
-		sub_command_.shutdown();
-	}
-		
-	bool ComputedTorqueController::
-	        init(hardware_interface::EffortJointInterface *hw,ros::NodeHandle &n)
-	{
-		node_=n;
-		hw_=hw;
-		
-		std::vector<std::string> joint_names;
-		if(!node_.getParam("joints",joint_names))
-		{
-			ROS_ERROR("No 'joints' in controller. (namespace: %s)",
-			        node_.getNamespace().c_str());
-			return false;
-		}
-		
-		nJoints_=joint_names.size();		
-		
-		for(int i=0; i < nJoints_;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;
-			}
-		}
-		sub_command_=node_.subscribe("command",1,
-		        &ComputedTorqueController::commandCB, this);
-		
-		std::string robot_desc_string;
-		if(!node_.getParam("/robot_description",robot_desc_string))
-                {
-			ROS_ERROR("Could not find '/robot_description'.");
-			return false;
-		}
-		
-		KDL::Tree tree;
-		if (!kdl_parser::treeFromString(robot_desc_string,tree))
-		{
-			ROS_ERROR("Failed to construct KDL tree.");
-			return false;
-		}
-		
-		std::string chainRoot;
-		if(!node_.getParam("chain/root",chainRoot))
-                {
-			ROS_ERROR("Could not find 'chain_root' parameter.");
-			return false;
-		}
-		
-		std::string chainTip;
-		if(!node_.getParam("chain/tip",chainTip))
-                {
-			ROS_ERROR("Could not find 'chain/tip' parameter.");
-			return false;
-		}
-		
-	        KDL::Chain chain;
-		if (!tree.getChain(chainRoot,chainTip,chain)) 
-		{
-			ROS_ERROR("Failed to get chain from KDL tree.");
-			return false;
-		}
-		
-		KDL::Vector g;
-		node_.param("gravity/x",g[0],0.0);
-		node_.param("gravity/y",g[1],0.0);
-		node_.param("gravity/z",g[2],-9.8);
-
-		if((idsolver=new KDL::ChainIdSolver_RNE(chain,g)) == NULL)
-		{
-			ROS_ERROR("Failed to create ChainIDSolver_RNE.");
-			return false;
-		}
-
-		q.resize(nJoints_);
-		dq.resize(nJoints_);
-		v.resize(nJoints_);
-		qr.resize(nJoints_);
-		dqr.resize(nJoints_);
-		ddqr.resize(nJoints_);
-		torque.resize(nJoints_);
-
-		fext.resize(chain.getNrOfSegments());
-		
-		Kp.resize(nJoints_,nJoints_);
-		Kd.resize(nJoints_,nJoints_);
-		
-		std::vector<double> KpVec;
-		if(!node_.getParam("Kp",KpVec))
-		{
-			ROS_ERROR("No 'Kp' in controller %s.",node_.getNamespace().c_str());
-		        return false;
-		}
-		Kp=Eigen::Map<Eigen::MatrixXd>(KpVec.data(),nJoints_,nJoints_);
-		
-		std::vector<double> KdVec;
-		if(!node_.getParam("Kd",KdVec))
-		{
-			ROS_ERROR("No 'Kd' in controller %s.",node_.getNamespace().c_str());
-		        return false;
-		}
-		Kd=Eigen::Map<Eigen::MatrixXd>(KdVec.data(),nJoints_,nJoints_);
-		
-		return true;
-	}
-	
-	void ComputedTorqueController::starting(const ros::Time& time)
-	{
-		for(unsigned int i=0;i < nJoints_;i++)
-		{
-			q(i)=joints_[i].getPosition();
-			dq(i)=joints_[i].getVelocity();
-		}
-                qr=q;
-                dqr=dq;
-		SetToZero(ddqr);
-		
-                struct sched_param param;
-                param.sched_priority=sched_get_priority_max(SCHED_FIFO);
-                if(sched_setscheduler(0,SCHED_FIFO,&param) == -1)
-                {
-                        ROS_WARN("Failed to set real-time scheduler.");
-                        return;
-                }
-                if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1)
-                        ROS_WARN("Failed to lock memory.");
-	}
-	
-	void ComputedTorqueController::update(const ros::Time& time,
-	        const ros::Duration& duration)
-	{
-		for(unsigned int i=0;i < nJoints_;i++)
-		{
-			q(i)=joints_[i].getPosition();
-			dq(i)=joints_[i].getVelocity();
-		}
-		for(unsigned int i=0;i < fext.size();i++) fext[i].Zero();
-		
-		v.data=ddqr.data+Kp*(qr.data-q.data)+Kd*(dqr.data-dq.data);
-		if(idsolver->CartToJnt(q,dq,v,fext,torque) < 0)
-		        ROS_ERROR("KDL inverse dynamics solver failed.");
-		
-		for(unsigned int i=0;i < nJoints_;i++)
-		        joints_[i].setCommand(torque(i));
-	}
-	
-	void ComputedTorqueController::commandCB(const trajectory_msgs::
-	        JointTrajectoryPoint::ConstPtr &referencePoint)
-	{
-		for(unsigned int i=0;i < nJoints_;i++)
-		{
-			qr(i)=referencePoint->positions[i];
-			dqr(i)=referencePoint->velocities[i];
-			ddqr(i)=referencePoint->accelerations[i];
-		}
-	}
-}
-
-PLUGINLIB_EXPORT_CLASS(effort_controllers::ComputedTorqueController,
-        controller_interface::ControllerBase)
diff --git a/wam_controllers/src/pid_plus_gravity_controller.cpp b/wam_controllers/src/pid_plus_gravity_controller.cpp
deleted file mode 100644
index e144ba1..0000000
--- a/wam_controllers/src/pid_plus_gravity_controller.cpp
+++ /dev/null
@@ -1,173 +0,0 @@
-/******************************************************************************
-			    ROS wam_controllers Package
-                        PID+Gravity Compensation Controller
-          Copyright (C) 2017 Walter Fetter Lages <w.fetter@ieee.org>
-
-        This program is free software: you can redistribute it and/or modify
-        it under the terms of the GNU General Public License as published by
-        the Free Software Foundation, either version 3 of the License, or
-        (at your option) any later version.
-
-        This program is distributed in the hope that it will be useful, but
-        WITHOUT ANY WARRANTY; without even the implied warranty of
-        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
-        General Public License for more details.
-
-        You should have received a copy of the GNU General Public License
-        along with this program.  If not, see
-        <http://www.gnu.org/licenses/>.
-        
-*******************************************************************************/
-
-#include <sys/mman.h>
-
-#include <wam_controllers/pid_plus_gravity_controller.h>
-#include <pluginlib/class_list_macros.h>
-
-namespace effort_controllers
-{	
-	PidPlusGravityController::PidPlusGravityController(void):
-		q_(0),qr_(0),gravity_(0)
-	{
-	}
-	
-	PidPlusGravityController::~PidPlusGravityController(void)
-	{
-		sub_command_.shutdown();
-	}
-		
-	bool PidPlusGravityController::
-	        init(hardware_interface::EffortJointInterface *hw,ros::NodeHandle &n)
-	{
-		node_=n;
-		hw_=hw;
-		
-		std::vector<std::string> joint_names;
-		if(!node_.getParam("joints",joint_names))
-		{
-			ROS_ERROR("No 'joints' in controller. (namespace: %s)",
-			        node_.getNamespace().c_str());
-			return false;
-		}
-
-		pid_.resize(joint_names.size());
-
-		for(int i=0; i < joint_names.size();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;
-			}
-			if(!pid_[i].init(ros::NodeHandle(node_,joint_names[i]+"/pid")))
-			{
-				ROS_ERROR_STREAM("Failed to load PID parameters from " << joint_names[i] + "/pid");
-				return false;
-			}
-		}
-
-		sub_command_=node_.subscribe("command",1,
-		        &PidPlusGravityController::commandCB, this);
-		
-		std::string robot_desc_string;
-		if(!node_.getParam("/robot_description",robot_desc_string))
-                {
-			ROS_ERROR("Could not find '/robot_description'.");
-			return false;
-		}
-		
-		KDL::Tree tree;
-		if (!kdl_parser::treeFromString(robot_desc_string,tree))
-		{
-			ROS_ERROR("Failed to construct KDL tree.");
-			return false;
-		}
-		
-		std::string chainRoot;
-		if(!node_.getParam("chain/root",chainRoot))
-                {
-			ROS_ERROR("Could not find 'chain_root' parameter.");
-			return false;
-		}
-		
-		std::string chainTip;
-		if(!node_.getParam("chain/tip",chainTip))
-                {
-			ROS_ERROR("Could not find 'chain/tip' parameter.");
-			return false;
-		}
-		
-	        KDL::Chain chain;
-		if (!tree.getChain(chainRoot,chainTip,chain)) 
-		{
-			ROS_ERROR("Failed to get chain from KDL tree.");
-			return false;
-		}
-		
-		KDL::Vector g;
-		node_.param("gravity/x",g[0],0.0);
-		node_.param("gravity/y",g[1],0.0);
-		node_.param("gravity/z",g[2],-9.8);
-		
-		if((chainParam_=new KDL::ChainDynParam(chain,g)) == NULL)
-		{
-			ROS_ERROR("Failed to create ChainDynParam.");
-			return false;
-		}
-
-		q_.resize(chain.getNrOfJoints());
-		qr_.resize(chain.getNrOfJoints());
-		gravity_.resize(chain.getNrOfJoints());
-		
-		return true;
-	}
-	
-	void PidPlusGravityController::starting(const ros::Time& time)
-	{
-		for(unsigned int i=0;i < joints_.size();i++)
-			q_(i)=joints_[i].getPosition();
-                qr_=q_;
-		
-                struct sched_param param;
-                param.sched_priority=sched_get_priority_max(SCHED_FIFO);
-                if(sched_setscheduler(0,SCHED_FIFO,&param) == -1)
-                {
-                        ROS_WARN("Failed to set real-time scheduler.");
-                        return;
-                }
-                if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1)
-                        ROS_WARN("Failed to lock memory.");
-	}
-	
-	void PidPlusGravityController::update(const ros::Time& time,
-	        const ros::Duration& duration)
-	{
-		for(unsigned int i=0;i < joints_.size();i++)
-			q_(i)=joints_[i].getPosition();
-			
-		if(chainParam_->JntToGravity(q_,gravity_) < 0)
-		        ROS_ERROR("KDL dynamics solver failed.");
-		
-		for(unsigned int i=0;i < joints_.size();i++)
-		        joints_[i].setCommand(gravity_(i)+pid_[i].computeCommand(qr_(i)-q_(i),duration));
-	}
-	
-	void PidPlusGravityController::commandCB(const trajectory_msgs::
-	        JointTrajectoryPoint::ConstPtr &referencePoint)
-	{
-		if(referencePoint->positions.size()!=qr_.rows())
-		{ 
-			ROS_ERROR_STREAM("Dimension of command (" << referencePoint->positions.size() << ") does not match number of joints (" << qr_.rows() << ")!");
-			return; 
-		}
-		for(unsigned int i=0;i < qr_.rows();i++)
-			qr_(i)=referencePoint->positions[i];
-	}
-}
-
-PLUGINLIB_EXPORT_CLASS(effort_controllers::PidPlusGravityController,
-        controller_interface::ControllerBase)
diff --git a/wam_controllers/wam_controllers_plugins.xml b/wam_controllers/wam_controllers_plugins.xml
deleted file mode 100644
index aac31df..0000000
--- a/wam_controllers/wam_controllers_plugins.xml
+++ /dev/null
@@ -1,27 +0,0 @@
-<library path="lib/libwam_controllers">
-
-  <class name="effort_controllers/ComputedTorqueController"
-    type="effort_controllers::ComputedTorqueController"
-    base_class_type="controller_interface::ControllerBase">
-    <description>
-      The ComputedTorqueController implements a computed torque controller
-      in joint space for the Barrett WAM dynamic model.  The reference
-      inputs (command in the ROS nomenclature) are joint positions,
-      velocities and accelerations.  This type of controller expects an
-      EffortJointInterface type of hardware interface.
-    </description>
-  </class>
-  
-    <class name="effort_controllers/PidPlusGravityController"
-    type="effort_controllers::PidPlusGravityController"
-    base_class_type="controller_interface::ControllerBase">
-    <description>
-      The PidPlusGravityController implements a PID controller with gravity
-      compensation in joint space for the Barrett WAM dynamic model.  The
-      reference inputs (command in the ROS nomenclature) are joint
-      positions.  This type of controller expects an EffortJointInterface
-      type of hardware interface.
-    </description>
-  </class>
-
-</library>
diff --git a/wam_controllers/CMakeLists.txt b/wam_simulation/CMakeLists.txt
similarity index 79%
rename from wam_controllers/CMakeLists.txt
rename to wam_simulation/CMakeLists.txt
index bf0ae5b..1734d6d 100644
--- a/wam_controllers/CMakeLists.txt
+++ b/wam_simulation/CMakeLists.txt
@@ -1,21 +1,14 @@
 cmake_minimum_required(VERSION 2.8.3)
-project(wam_controllers)
+project(wam_simulation)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
-	controller_interface
-	control_msgs
-	urdf
 )
-find_package(cmake_modules REQUIRED)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(Eigen REQUIRED)
-find_package(orocos_kdl REQUIRED)
-find_package(kdl_parser REQUIRED)
 
 
 ## Uncomment this if the package has a setup.py. This macro ensures
@@ -85,10 +78,9 @@ find_package(kdl_parser REQUIRED)
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
 #  INCLUDE_DIRS include
-   LIBRARIES ${PROJECT_NAME}
-   CATKIN_DEPENDS controller_interface control_msgs urdf
+#   LIBRARIES ${PROJECT_NAME}
+#   CATKIN_DEPENDS
 #  DEPENDS system_lib
-   DEPENDS eigen orocos_kdl kdl_parser
 )
 
 ###########
@@ -97,30 +89,24 @@ catkin_package(
 
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
-include_directories(include
-  ${catkin_INCLUDE_DIRS}
-  ${Eigen_INCLUDE_DIRS}
-)
+#include_directories(include
+#)
 
 ## Declare a cpp library
-add_library(wam_controllers
-  src/computed_torque_controller.cpp
-  src/pid_plus_gravity_controller.cpp
-)
+#add_library(wam_simulation
+#)
 
 ## Declare a cpp executable
-# add_executable(wam_controllers_node src/wam_controllers_node.cpp)
+# add_executable(wam_simulation_node src/wam_simulation_node.cpp)
 
 ## Add cmake target dependencies of the executable/library
 ## as an example, message headers may need to be generated before nodes
-# add_dependencies(wam_controllers_node wam_controllers_generate_messages_cpp)
+# add_dependencies(wam_simulation_node wam_simulation_generate_messages_cpp)
 
 ## Specify libraries to link a library or executable target against
-target_link_libraries(${PROJECT_NAME}
-  ${catkin_LIBRARIES}
-  ${orocos_kdl_LIBRARIES}
-  ${kdl_parser_LIBRARIES}
-)
+#target_link_libraries(${PROJECT_NAME}
+#  ${catkin_LIBRARIES}
+#)
 
 #############
 ## Install ##
@@ -137,18 +123,18 @@ target_link_libraries(${PROJECT_NAME}
 # )
 
 ## Mark executables and/or libraries for installation
-install(TARGETS ${PROJECT_NAME}
-   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
+#install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+#)
 
 ## Mark cpp header files for installation
-install(DIRECTORY include/${PROJECT_NAME}/
-   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
 #   FILES_MATCHING PATTERN "*.h"
 #   PATTERN ".svn" EXCLUDE
-)
+#)
 
 ## Mark other files for installation (e.g. launch and bag files, etc.)
 # install(FILES
@@ -162,7 +148,7 @@ install(DIRECTORY include/${PROJECT_NAME}/
 #############
 
 ## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_wam_controllers.cpp)
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_wam_simulation.cpp)
 # if(TARGET ${PROJECT_NAME}-test)
 #   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
 # endif()
diff --git a/wam_simulation/config/computed_torque_controller.yaml b/wam_simulation/config/computed_torque_controller.yaml
new file mode 100644
index 0000000..8854c16
--- /dev/null
+++ b/wam_simulation/config/computed_torque_controller.yaml
@@ -0,0 +1,31 @@
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 100
+
+computed_torque_controller:
+  type: effort_controllers/ComputedTorqueController
+  joints:
+    - wam_joint_1
+    - wam_joint_2
+    - wam_joint_3
+    - wam_joint_4
+    - wam_joint_5
+    - wam_joint_6
+    - wam_joint_7
+  Kp: [25.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 
+      0.0, 25.0, 0.0, 0.0, 0.0, 0.0, 0.0, 
+      0.0, 0.0, 25.0, 0.0, 0.0, 0.0, 0.0, 
+      0.0, 0.0, 0.0, 25.0, 0.0, 0.0, 0.0, 
+      0.0, 0.0, 0.0, 0.0, 25.0, 0.0, 0.0, 
+      0.0, 0.0, 0.0, 0.0, 0.0, 25.0, 0.0, 
+      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 25.0]
+  Kd: [10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 
+      0.0, 10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 
+      0.0, 0.0, 10.0, 0.0, 0.0, 0.0, 0.0, 
+      0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0, 
+      0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 
+      0.0, 0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 
+      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0]
+  gravity: {x: 0.0, y: 0.0, z: -9.8}
+  chain: {root: "wam_origin", tip: "wam_tool_plate"}
+  priority: 99
diff --git a/wam_simulation/config/pid_plus_gravity_controller.yaml b/wam_simulation/config/pid_plus_gravity_controller.yaml
new file mode 100644
index 0000000..8039807
--- /dev/null
+++ b/wam_simulation/config/pid_plus_gravity_controller.yaml
@@ -0,0 +1,26 @@
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 250
+
+pid_plus_gravity_controller:
+  type: effort_controllers/PidPlusGravityController
+  joints:
+    - wam_joint_1
+    - wam_joint_2
+    - wam_joint_3
+    - wam_joint_4
+    - wam_joint_5
+    - wam_joint_6
+    - wam_joint_7
+# PID parameters from /etc/barrett/wam7w.conf
+  wam_joint_1: {p: 900.0, i: 2.5, d: 10.0, i_clamp: 25.0}
+  wam_joint_2: {p: 2500.0, i: 5.0, d: 20.0, i_clamp: 20.0}
+  wam_joint_3: {p: 600.0, i: 2.0, d: 5.0, i_clamp: 15.0}
+  wam_joint_4: {p: 500.0, i: 0.5, d: 2.0, i_clamp: 15.0}
+  wam_joint_5: {p: 50.0, i: 0.5, d: 0.5, i_clamp: 5.0}
+  wam_joint_6: {p: 50.0, i: 0.5, d: 0.5, i_clamp: 5.0}
+  wam_joint_7: {p: 8.0, i: 0.1, d: 0.05, i_clamp: 5.0}
+  gravity: {x: 0.0, y: 0.0, z: -9.8}
+  chain: {root: "wam_origin", tip: "wam_tool_plate"}
+  priority: 99
+  
diff --git a/wam_controllers/launch/gazebo.launch b/wam_simulation/launch/gazebo.launch
similarity index 51%
rename from wam_controllers/launch/gazebo.launch
rename to wam_simulation/launch/gazebo.launch
index 0b3a777..69f26d6 100644
--- a/wam_controllers/launch/gazebo.launch
+++ b/wam_simulation/launch/gazebo.launch
@@ -4,7 +4,12 @@
 	<arg name="use_sim_time" default="true"/>
 	<arg name="table" default="true"/>
 	<arg name="bhand" default="false"/>
+
+	<!-- This is the default Barrett WAM Controller, used by libbarrett -->
+	<arg name="controller" default="pid_plus_gravity_controller"/>
+	<arg name="config" default="$(find wam_simulation)/config/$(arg controller).yaml"/>
 	
+	<remap from="$(arg controller)/command" to="controller/command" />
 
 	<include file="$(find wam_description)/launch/gazebo.launch" >
 		<arg name="paused" value="$(arg paused)"/>
@@ -14,6 +19,9 @@
 		<arg name="bhand" value="$(arg bhand)"/>
 	</include>
 
-	<include file="$(find wam_controllers)/launch/computed_torque.launch" />
+	<rosparam file="$(arg config)" command="load"/>
 
+	<node name="controller_spawner" pkg="controller_manager" type="spawner"
+		respawn="false" output="screen"
+		args="$(arg controller) joint_state_controller" />
 </launch>
diff --git a/wam_controllers/package.xml b/wam_simulation/package.xml
similarity index 74%
rename from wam_controllers/package.xml
rename to wam_simulation/package.xml
index 819d207..70eaee5 100644
--- a/wam_controllers/package.xml
+++ b/wam_simulation/package.xml
@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <package>
-  <name>wam_controllers</name>
-  <version>2.0.0</version>
-  <description>The wam_controllers package</description>
+  <name>wam_simulation</name>
+  <version>1.0.0</version>
+  <description>The wam_simulation package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
   <!-- Example:  -->
@@ -19,7 +19,7 @@
   <!-- Url tags are optional, but mutiple are allowed, one per tag -->
   <!-- Optional attribute type can be: website, bugtracker, or repository -->
   <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/wam_controllers</url> -->
+  <!-- <url type="website">http://wiki.ros.org/wam_simulation</url> -->
   <url type="website">http://www.ece.ufrgs.br/ufrgs_wam</url>
 
 
@@ -42,21 +42,13 @@
   <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   
-  <build_depend>wam_gazebo_ros_control</build_depend>
-  <build_depend>controller_interface</build_depend>
-  <build_depend>orocos_kdl</build_depend>
-  <build_depend>kdl_parser</build_depend>
-  <build_depend>trajectory_msgs</build_depend>
-  <build_depend>control_msgs</build_depend>
-  <build_depend>urdf</build_depend>
-  <build_depend>cmake_modules</build_depend>
-  
-  <run_depend>controller_interface</run_depend>
   <run_depend>controller_manager</run_depend>
   <run_depend>control_msgs</run_depend>
   <run_depend>joint_state_controller</run_depend>
   <run_depend>urdf</run_depend>
-  <run_depend>kdl_parser</run_depend>
+  <run_depend>computed_torque_controller</run_depend>
+  <run_depend>pid_plus_gravity_controller</run_depend>
+  <run_depend>wam_description</run_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
@@ -64,6 +56,5 @@
     <!-- <metapackage/> -->
 
     <!-- Other tools can request additional information be placed here -->
-    <controller_interface plugin="${prefix}/wam_controllers_plugins.xml"/>
   </export>
 </package>
diff --git a/wam_controllers/scripts/move_home.sh b/wam_simulation/scripts/move_home.sh
similarity index 80%
rename from wam_controllers/scripts/move_home.sh
rename to wam_simulation/scripts/move_home.sh
index 31eaaef..0796e9c 100755
--- a/wam_controllers/scripts/move_home.sh
+++ b/wam_simulation/scripts/move_home.sh
@@ -1,6 +1,6 @@
 #!/bin/bash
 
-rostopic pub /wam/computed_torque_controller/command \
+rostopic pub /controller/command \
 trajectory_msgs/JointTrajectoryPoint \
 "[0.0,-2.0,0.0,3.1,0.0,0.0,0.0]" \
 "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]" \
diff --git a/wam_controllers/scripts/move_initial.sh b/wam_simulation/scripts/move_initial.sh
similarity index 80%
rename from wam_controllers/scripts/move_initial.sh
rename to wam_simulation/scripts/move_initial.sh
index 48f1231..7dcea5a 100755
--- a/wam_controllers/scripts/move_initial.sh
+++ b/wam_simulation/scripts/move_initial.sh
@@ -1,6 +1,6 @@
 #!/bin/bash
 
-rostopic pub /wam/computed_torque_controller/command \
+rostopic pub /controller/command \
 trajectory_msgs/JointTrajectoryPoint \
 "[0.0,0.75,0.0,1.5,0.0,0.9,0.0]" \
 "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]" \
diff --git a/wam_controllers/scripts/move_zero.sh b/wam_simulation/scripts/move_zero.sh
similarity index 80%
rename from wam_controllers/scripts/move_zero.sh
rename to wam_simulation/scripts/move_zero.sh
index 80b1c63..8fcc947 100755
--- a/wam_controllers/scripts/move_zero.sh
+++ b/wam_simulation/scripts/move_zero.sh
@@ -1,6 +1,6 @@
 #!/bin/bash
 
-rostopic pub /wam/computed_torque_controller/command \
+rostopic pub /controller/command \
 trajectory_msgs/JointTrajectoryPoint \
 "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]" \
 "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]" \
diff --git a/wam_controllers/scripts/set_home.sh b/wam_simulation/scripts/set_home.sh
similarity index 88%
rename from wam_controllers/scripts/set_home.sh
rename to wam_simulation/scripts/set_home.sh
index 1ff2f52..4c4e80d 100755
--- a/wam_controllers/scripts/set_home.sh
+++ b/wam_simulation/scripts/set_home.sh
@@ -4,7 +4,7 @@ rosservice call /gazebo/set_model_configuration wam joint \
 "['wam_joint_1','wam_joint_2','wam_joint_3','wam_joint_4','wam_joint_5','wam_joint_6','wam_joint_7']" \
 "[0.0,-2.0,0.0,3.1,0.0,0.0,0.0]"
 
-rostopic pub /wam/computed_torque_controller/command \
+rostopic pub /wam/controller/command \
 trajectory_msgs/JointTrajectoryPoint \
 "[0.0,-2.0,0.0,3.1,0.0,0.0,0.0]" \
 "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]" \
diff --git a/wam_controllers/scripts/set_initial.sh b/wam_simulation/scripts/set_initial.sh
similarity index 88%
rename from wam_controllers/scripts/set_initial.sh
rename to wam_simulation/scripts/set_initial.sh
index 94556b1..57bf9d1 100755
--- a/wam_controllers/scripts/set_initial.sh
+++ b/wam_simulation/scripts/set_initial.sh
@@ -5,7 +5,7 @@ rosservice call /gazebo/set_model_configuration wam joint \
 'wam_joint_5','wam_joint_6','wam_joint_7']" \
 "[0.0,0.75,0.0,1.5,0.0,0.9,0.0]"
 
-rostopic pub /wam/computed_torque_controller/command \
+rostopic pub /wam/controller/command \
 trajectory_msgs/JointTrajectoryPoint \
 "[0.0,0.75,0.0,1.5,0.0,0.9,0.0]" \
 "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]" \