Add wam_simulation to hold the Barrett WAM specific controller configuration.
+++ /dev/null
-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"}
+++ /dev/null
-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"}
+++ /dev/null
-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.
+++ /dev/null
-/******************************************************************************
- 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
+++ /dev/null
-/******************************************************************************
- 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
+++ /dev/null
-<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>
+++ /dev/null
-<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>
+++ /dev/null
-<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>
+++ /dev/null
-<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>
+++ /dev/null
-/******************************************************************************
- 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,¶m) == -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)
+++ /dev/null
-/******************************************************************************
- 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,¶m) == -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)
+++ /dev/null
-<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>
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
## 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
)
###########
## 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 ##
# )
## 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
#############
## 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()
--- /dev/null
+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
--- /dev/null
+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
+
<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)"/>
<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>
<?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: -->
<!-- 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>
<!-- <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>
<!-- <metapackage/> -->
<!-- Other tools can request additional information be placed here -->
- <controller_interface plugin="${prefix}/wam_controllers_plugins.xml"/>
</export>
</package>
#!/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]" \
#!/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]" \
#!/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]" \
"['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]" \
'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]" \
projects / ufrgs_wam.git / commitdiff