From: Walter Fetter Lages Date: Wed, 28 Nov 2018 22:32:38 +0000 (-0200) Subject: Remove wam_gazebo_ros_control. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=087aac726ea687cc69a7289772951b89ba4368b7;p=ufrgs_wam.git Remove wam_gazebo_ros_control. --- diff --git a/wam_gazebo_ros_control/CMakeLists.txt b/wam_gazebo_ros_control/CMakeLists.txt deleted file mode 100644 index f8d4956..0000000 --- a/wam_gazebo_ros_control/CMakeLists.txt +++ /dev/null @@ -1,161 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(wam_gazebo_ros_control) - -## 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) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) -find_package(gazebo REQUIRED) - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependencies might have been -## pulled in transitively but can be declared for certainty nonetheless: -## * add a build_depend tag for "message_generation" -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES wam_gazebo_ros_control - CATKIN_DEPENDS - controller_manager - pluginlib - gazebo_ros_control -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories(include - ${GAZEBO_INCLUDE_DIRS} -) - -## Declare a cpp library -add_library(wam_gazebo_ros_control - src/wam_robot_hw_sim.cpp -) - -## Declare a cpp executable -# add_executable(wam_gazebo_ros_control_node src/wam_gazebo_ros_control_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_gazebo_ros_control_node wam_gazebo_ros_control_generate_messages_cpp) - -## Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} -) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## 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} - ) - -## Mark cpp header files for installation -# 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 wam_gazebo_ros_control_plugins.xml -# # myfile1 -# # myfile2 - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_wam_gazebo_ros_control.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/wam_gazebo_ros_control/package.xml b/wam_gazebo_ros_control/package.xml deleted file mode 100644 index ac4211e..0000000 --- a/wam_gazebo_ros_control/package.xml +++ /dev/null @@ -1,72 +0,0 @@ - - - wam_gazebo_ros_control - 2.0.0 - The wam_gazebo_ros_control package - - - - - Walter Fetter Lages - - - - - - GPLv3 - - - - - - - http://www.ece.ufrgs.br/~fetter/ufrgs_wam - - - - - - - Walter Fetter Lages - - - - - - - - - - - - - - catkin - - controller_manager - pluginlib - gazebo_ros_control - - wam_description - gazebo - - roscpp - - - controller_manager - pluginlib - - - gazebo_ros_control - - - - - - - - - - - - diff --git a/wam_gazebo_ros_control/src/default_robot_hw_sim.cpp b/wam_gazebo_ros_control/src/default_robot_hw_sim.cpp deleted file mode 100644 index 1a45eb9..0000000 --- a/wam_gazebo_ros_control/src/default_robot_hw_sim.cpp +++ /dev/null @@ -1,509 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, Open Source Robotics Foundation - * Copyright (c) 2013, The Johns Hopkins University - * 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 Open Source Robotics Foundation - * 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: Dave Coleman, Johnathan Bohren - Desc: Hardware Interface for any simulated robot in Gazebo -*/ - -#ifndef _GAZEBO_ROS_CONTROL___DEFAULT_ROBOT_HW_SIM_H_ -#define _GAZEBO_ROS_CONTROL___DEFAULT_ROBOT_HW_SIM_H_ - -// ros_control -#include -#include -#include -#include -#include -#include -#include - -// Gazebo -#include -#include -#include - -// ROS -#include -#include -#include - -// gazebo_ros_control -#include - -// URDF -#include - -namespace -{ - -double clamp(const double val, const double min_val, const double max_val) -{ - return std::min(std::max(val, min_val), max_val); -} - -} - -namespace gazebo_ros_control -{ - -class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim -{ -public: - - bool initSim( - const std::string& robot_namespace, - ros::NodeHandle model_nh, - gazebo::physics::ModelPtr parent_model, - const urdf::Model *const urdf_model, - std::vector transmissions) - { - // getJointLimits() searches joint_limit_nh for joint limit parameters. The format of each - // parameter's name is "joint_limits/". An example is "joint_limits/axle_joint". - const ros::NodeHandle joint_limit_nh(model_nh, robot_namespace); - - // Resize vectors to our DOF - n_dof_ = transmissions.size(); - joint_names_.resize(n_dof_); - joint_types_.resize(n_dof_); - joint_lower_limits_.resize(n_dof_); - joint_upper_limits_.resize(n_dof_); - joint_effort_limits_.resize(n_dof_); - joint_control_methods_.resize(n_dof_); - pid_controllers_.resize(n_dof_); - joint_position_.resize(n_dof_); - joint_velocity_.resize(n_dof_); - joint_effort_.resize(n_dof_); - joint_effort_command_.resize(n_dof_); - joint_position_command_.resize(n_dof_); - joint_velocity_command_.resize(n_dof_); - - // Initialize values - for(unsigned int j=0; j < n_dof_; j++) - { - // Check that this transmission has one joint - if(transmissions[j].joints_.size() == 0) - { - ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_ - << " has no associated joints."); - continue; - } - else if(transmissions[j].joints_.size() > 1) - { - ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_ - << " has more than one joint. Currently the default robot hardware simulation " - << " interface only supports one."); - continue; - } - - // Check that this transmission has one actuator - if(transmissions[j].actuators_.size() == 0) - { - ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_ - << " has no associated actuators."); - continue; - } - else if(transmissions[j].actuators_.size() > 1) - { - ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_ - << " has more than one actuator. Currently the default robot hardware simulation " - << " interface only supports one."); - continue; - } - - // Add data from transmission - joint_names_[j] = transmissions[j].joints_[0].name_; - joint_position_[j] = 1.0; - joint_velocity_[j] = 0.0; - joint_effort_[j] = 1.0; // N/m for continuous joints - joint_effort_command_[j] = 0.0; - joint_position_command_[j] = 0.0; - joint_velocity_command_[j] = 0.0; - -#if ROS_VERSION_MINOR > 10 || ROS_VERSION_MAJOR > 1 - const std::string &hardware_interface = - transmissions[j].actuators_[0].hardware_interfaces_[0]; -#else - const std::string &hardware_interface = - transmissions[j].actuators_[0].hardware_interface_; -#endif - - // Debug - ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim","Loading joint '" << joint_names_[j] - << "' of type '" << hardware_interface << "'"); - - // Create joint state interface for all joints - js_interface_.registerHandle(hardware_interface::JointStateHandle( - joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j])); - - // Decide what kind of command interface this actuator/joint has - hardware_interface::JointHandle joint_handle; - if(hardware_interface == "EffortJointInterface") - { - // Create effort joint interface - joint_control_methods_[j] = EFFORT; - joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), - &joint_effort_command_[j]); - ej_interface_.registerHandle(joint_handle); - } - else if(hardware_interface == "PositionJointInterface") - { - // Create position joint interface - joint_control_methods_[j] = POSITION; - joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), - &joint_position_command_[j]); - pj_interface_.registerHandle(joint_handle); - } - else if(hardware_interface == "VelocityJointInterface") - { - // Create velocity joint interface - joint_control_methods_[j] = VELOCITY; - joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), - &joint_velocity_command_[j]); - vj_interface_.registerHandle(joint_handle); - } - else - { - ROS_FATAL_STREAM_NAMED("default_robot_hw_sim","No matching hardware interface found for '" - << hardware_interface ); - return false; - } - - // Get the gazebo joint that corresponds to the robot joint. - //ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim", "Getting pointer to gazebo joint: " - // << joint_names_[j]); - gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]); - if (!joint) - { - ROS_ERROR_STREAM("This robot has a joint named \"" << joint_names_[j] - << "\" which is not in the gazebo model."); - return false; - } - sim_joints_.push_back(joint); - - registerJointLimits(joint_names_[j], joint_handle, joint_control_methods_[j], - joint_limit_nh, urdf_model, - &joint_types_[j], &joint_lower_limits_[j], &joint_upper_limits_[j], - &joint_effort_limits_[j]); - if (joint_control_methods_[j] != EFFORT) - { - // Initialize the PID controller. If no PID gain values are found, use joint->SetAngle() or - // joint->SetVelocity() to control the joint. - const ros::NodeHandle nh(model_nh, robot_namespace + "/gazebo_ros_control/pid_gains/" + - joint_names_[j]); - if (pid_controllers_[j].init(nh)) - { - switch (joint_control_methods_[j]) - { - case POSITION: - joint_control_methods_[j] = POSITION_PID; - break; - case VELOCITY: - joint_control_methods_[j] = VELOCITY_PID; - break; - } - } - else - { - // joint->SetMaxForce() must be called if joint->SetAngle() or joint->SetVelocity() are - // going to be called. joint->SetMaxForce() must *not* be called if joint->SetForce() is - // going to be called. - joint->SetMaxForce(0, joint_effort_limits_[j]); - } - } - } - - // Register interfaces - registerInterface(&js_interface_); - registerInterface(&ej_interface_); - registerInterface(&pj_interface_); - registerInterface(&vj_interface_); - - return true; - } - - void readSim(ros::Time time, ros::Duration period) - { - for(unsigned int j=0; j < n_dof_; j++) - { - // Gazebo has an interesting API... - if (joint_types_[j] == urdf::Joint::PRISMATIC) - { - joint_position_[j] = sim_joints_[j]->GetAngle(0).Radian(); - } - else - { - joint_position_[j] += angles::shortest_angular_distance(joint_position_[j], - sim_joints_[j]->GetAngle(0).Radian()); - } - joint_velocity_[j] = sim_joints_[j]->GetVelocity(0); - joint_effort_[j] = sim_joints_[j]->GetForce((unsigned int)(0)); - } - } - - void writeSim(ros::Time time, ros::Duration period) - { - ej_sat_interface_.enforceLimits(period); - ej_limits_interface_.enforceLimits(period); - pj_sat_interface_.enforceLimits(period); - pj_limits_interface_.enforceLimits(period); - vj_sat_interface_.enforceLimits(period); - vj_limits_interface_.enforceLimits(period); - - for(unsigned int j=0; j < n_dof_; j++) - { - switch (joint_control_methods_[j]) - { - case EFFORT: - { - const double effort = joint_effort_command_[j]; - sim_joints_[j]->SetForce(0, effort); - } - break; - - case POSITION: -#if GAZEBO_MAJOR_VERSION >= 4 - sim_joints_[j]->SetPosition(0, joint_position_command_[j]); -#else - sim_joints_[j]->SetAngle(0, joint_position_command_[j]); -#endif - break; - - case POSITION_PID: - { - double error; - switch (joint_types_[j]) - { - case urdf::Joint::REVOLUTE: - angles::shortest_angular_distance_with_limits(joint_position_[j], - joint_position_command_[j], - joint_lower_limits_[j], - joint_upper_limits_[j], - error); - break; - case urdf::Joint::CONTINUOUS: - error = angles::shortest_angular_distance(joint_position_[j], - joint_position_command_[j]); - break; - default: - error = joint_position_command_[j] - joint_position_[j]; - } - - const double effort_limit = joint_effort_limits_[j]; - const double effort = clamp(pid_controllers_[j].computeCommand(error, period), - -effort_limit, effort_limit); - sim_joints_[j]->SetForce(0, effort); - } - break; - - case VELOCITY: - sim_joints_[j]->SetVelocity(0, joint_velocity_command_[j]); - break; - - case VELOCITY_PID: - const double error = joint_velocity_command_[j] - joint_velocity_[j]; - const double effort_limit = joint_effort_limits_[j]; - const double effort = clamp(pid_controllers_[j].computeCommand(error, period), - -effort_limit, effort_limit); - sim_joints_[j]->SetForce(0, effort); - break; - } - } - } - -private: - // Methods used to control a joint. - enum ControlMethod {EFFORT, POSITION, POSITION_PID, VELOCITY, VELOCITY_PID}; - - // Register the limits of the joint specified by joint_name and joint_handle. The limits are - // retrieved from joint_limit_nh. If urdf_model is not NULL, limits are retrieved from it also. - // Return the joint's type, lower position limit, upper position limit, and effort limit. - void registerJointLimits(const std::string& joint_name, - const hardware_interface::JointHandle& joint_handle, - const ControlMethod ctrl_method, - const ros::NodeHandle& joint_limit_nh, - const urdf::Model *const urdf_model, - int *const joint_type, double *const lower_limit, - double *const upper_limit, double *const effort_limit) - { - *joint_type = urdf::Joint::UNKNOWN; - *lower_limit = -std::numeric_limits::max(); - *upper_limit = std::numeric_limits::max(); - *effort_limit = std::numeric_limits::max(); - - joint_limits_interface::JointLimits limits; - bool has_limits = false; - joint_limits_interface::SoftJointLimits soft_limits; - bool has_soft_limits = false; - - if (urdf_model != NULL) - { - const boost::shared_ptr urdf_joint = urdf_model->getJoint(joint_name); - if (urdf_joint != NULL) - { - *joint_type = urdf_joint->type; - // Get limits from the URDF file. - if (joint_limits_interface::getJointLimits(urdf_joint, limits)) - has_limits = true; - if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits)) - has_soft_limits = true; - } - } - // Get limits from the parameter server. - if (joint_limits_interface::getJointLimits(joint_name, joint_limit_nh, limits)) - has_limits = true; - - if (!has_limits) - return; - - if (*joint_type == urdf::Joint::UNKNOWN) - { - // Infer the joint type. - - if (limits.has_position_limits) - { - *joint_type = urdf::Joint::REVOLUTE; - } - else - { - if (limits.angle_wraparound) - *joint_type = urdf::Joint::CONTINUOUS; - else - *joint_type = urdf::Joint::PRISMATIC; - } - } - - if (limits.has_position_limits) - { - *lower_limit = limits.min_position; - *upper_limit = limits.max_position; - } - if (limits.has_effort_limits) - *effort_limit = limits.max_effort; - - if (has_soft_limits) - { - switch (ctrl_method) - { - case EFFORT: - { - const joint_limits_interface::EffortJointSoftLimitsHandle - limits_handle(joint_handle, limits, soft_limits); - ej_limits_interface_.registerHandle(limits_handle); - } - break; - case POSITION: - { - const joint_limits_interface::PositionJointSoftLimitsHandle - limits_handle(joint_handle, limits, soft_limits); - pj_limits_interface_.registerHandle(limits_handle); - } - break; - case VELOCITY: - { - const joint_limits_interface::VelocityJointSoftLimitsHandle - limits_handle(joint_handle, limits, soft_limits); - vj_limits_interface_.registerHandle(limits_handle); - } - break; - } - } - else - { - switch (ctrl_method) - { - case EFFORT: - { - const joint_limits_interface::EffortJointSaturationHandle - sat_handle(joint_handle, limits); - ej_sat_interface_.registerHandle(sat_handle); - } - break; - case POSITION: - { - const joint_limits_interface::PositionJointSaturationHandle - sat_handle(joint_handle, limits); - pj_sat_interface_.registerHandle(sat_handle); - } - break; - case VELOCITY: - { - const joint_limits_interface::VelocityJointSaturationHandle - sat_handle(joint_handle, limits); - vj_sat_interface_.registerHandle(sat_handle); - } - break; - } - } - } - - unsigned int n_dof_; - - hardware_interface::JointStateInterface js_interface_; - hardware_interface::EffortJointInterface ej_interface_; - hardware_interface::PositionJointInterface pj_interface_; - hardware_interface::VelocityJointInterface vj_interface_; - - joint_limits_interface::EffortJointSaturationInterface ej_sat_interface_; - joint_limits_interface::EffortJointSoftLimitsInterface ej_limits_interface_; - joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_; - joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_; - joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_; - joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_; - - std::vector joint_names_; - std::vector joint_types_; - std::vector joint_lower_limits_; - std::vector joint_upper_limits_; - std::vector joint_effort_limits_; - std::vector joint_control_methods_; - std::vector pid_controllers_; - std::vector joint_position_; - std::vector joint_velocity_; - std::vector joint_effort_; - std::vector joint_effort_command_; - std::vector joint_position_command_; - std::vector joint_velocity_command_; - - std::vector sim_joints_; -}; - -typedef boost::shared_ptr DefaultRobotHWSimPtr; - -} - -PLUGINLIB_EXPORT_CLASS(gazebo_ros_control::DefaultRobotHWSim, gazebo_ros_control::RobotHWSim) - -#endif // #ifndef __GAZEBO_ROS_CONTROL_PLUGIN_DEFAULT_ROBOT_HW_SIM_H_ diff --git a/wam_gazebo_ros_control/src/wam_robot_hw_sim.cpp b/wam_gazebo_ros_control/src/wam_robot_hw_sim.cpp deleted file mode 100644 index d9655d0..0000000 --- a/wam_gazebo_ros_control/src/wam_robot_hw_sim.cpp +++ /dev/null @@ -1,469 +0,0 @@ -#ifndef _GAZEBO_ROS_CONTROL___WAM_ROBOT_HW_SIM_H_ -#define _GAZEBO_ROS_CONTROL___WAM_ROBOT_HW_SIM_H_ - -// ros_control -#include -#include -#include -#include -#include -#include -#include - -// Gazebo -#include -#include -#include - -// ROS -#include -#include -#include - -// gazebo_ros_control -#include - -// URDF -#include - -namespace -{ - -double clamp(const double val, const double min_val, const double max_val) -{ - return std::min(std::max(val, min_val), max_val); -} - -} - -namespace gazebo_ros_control -{ - -class WamRobotHWSim : public gazebo_ros_control::RobotHWSim -{ -public: - - bool initSim( - const std::string& robot_namespace, - ros::NodeHandle model_nh, - gazebo::physics::ModelPtr parent_model, - const urdf::Model *const urdf_model, - std::vector transmissions) - { - // getJointLimits() searches joint_limit_nh for joint limit parameters. The format of each - // parameter's name is "joint_limits/". An example is "joint_limits/axle_joint". - const ros::NodeHandle joint_limit_nh(model_nh, robot_namespace); - - // Resize vectors to our DOF - n_dof_ = transmissions.size(); - joint_names_.resize(n_dof_); - joint_types_.resize(n_dof_); - joint_lower_limits_.resize(n_dof_); - joint_upper_limits_.resize(n_dof_); - joint_effort_limits_.resize(n_dof_); - joint_control_methods_.resize(n_dof_); - pid_controllers_.resize(n_dof_); - joint_position_.resize(n_dof_); - joint_velocity_.resize(n_dof_); - joint_effort_.resize(n_dof_); - joint_effort_command_.resize(n_dof_); - joint_position_command_.resize(n_dof_); - joint_velocity_command_.resize(n_dof_); - - // Initialize values - for(unsigned int j=0; j < n_dof_; j++) - { - // Check that this transmission has one joint - if(transmissions[j].joints_.size() == 0) - { - ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_ - << " has no associated joints."); - continue; - } - else if(transmissions[j].joints_.size() > 1) - { - ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_ - << " has more than one joint. Currently the default robot hardware simulation " - << " interface only supports one."); - continue; - } - - // Check that this transmission has one actuator - if(transmissions[j].actuators_.size() == 0) - { - ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_ - << " has no associated actuators."); - continue; - } - else if(transmissions[j].actuators_.size() > 1) - { - ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_ - << " has more than one actuator. Currently the default robot hardware simulation " - << " interface only supports one."); - continue; - } - - // Add data from transmission - joint_names_[j] = transmissions[j].joints_[0].name_; - joint_position_[j] = 1.0; - joint_velocity_[j] = 0.0; - joint_effort_[j] = 1.0; // N/m for continuous joints - joint_effort_command_[j] = 0.0; - joint_position_command_[j] = 0.0; - joint_velocity_command_[j] = 0.0; - -#if ROS_VERSION_MINOR > 10 || ROS_VERSION_MAJOR > 1 - const std::string &hardware_interface = - transmissions[j].actuators_[0].hardware_interfaces_[0]; -#else - const std::string &hardware_interface = - transmissions[j].actuators_[0].hardware_interface_; -#endif - - // Debug - ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim","Loading joint '" << joint_names_[j] - << "' of type '" << hardware_interface << "'"); - - // Create joint state interface for all joints - js_interface_.registerHandle(hardware_interface::JointStateHandle( - joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j])); - - // Decide what kind of command interface this actuator/joint has - hardware_interface::JointHandle joint_handle; - if(hardware_interface == "EffortJointInterface") - { - // Create effort joint interface - joint_control_methods_[j] = EFFORT; - joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), - &joint_effort_command_[j]); - ej_interface_.registerHandle(joint_handle); - } - else if(hardware_interface == "PositionJointInterface") - { - // Create position joint interface - joint_control_methods_[j] = POSITION; - joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), - &joint_position_command_[j]); - pj_interface_.registerHandle(joint_handle); - } - else if(hardware_interface == "VelocityJointInterface") - { - // Create velocity joint interface - joint_control_methods_[j] = VELOCITY; - joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), - &joint_velocity_command_[j]); - vj_interface_.registerHandle(joint_handle); - } - else - { - ROS_FATAL_STREAM_NAMED("default_robot_hw_sim","No matching hardware interface found for '" - << hardware_interface ); - return false; - } - - // Get the gazebo joint that corresponds to the robot joint. - //ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim", "Getting pointer to gazebo joint: " - // << joint_names_[j]); - gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]); - if (!joint) - { - ROS_ERROR_STREAM("This robot has a joint named \"" << joint_names_[j] - << "\" which is not in the gazebo model."); - return false; - } - sim_joints_.push_back(joint); - - registerJointLimits(joint_names_[j], joint_handle, joint_control_methods_[j], - joint_limit_nh, urdf_model, - &joint_types_[j], &joint_lower_limits_[j], &joint_upper_limits_[j], - &joint_effort_limits_[j]); - if (joint_control_methods_[j] != EFFORT) - { - // Initialize the PID controller. If no PID gain values are found, use joint->SetAngle() or - // joint->SetVelocity() to control the joint. - const ros::NodeHandle nh(model_nh, robot_namespace + "/gazebo_ros_control/pid_gains/" + - joint_names_[j]); - if (pid_controllers_[j].init(nh)) - { - switch (joint_control_methods_[j]) - { - case POSITION: - joint_control_methods_[j] = POSITION_PID; - break; - case VELOCITY: - joint_control_methods_[j] = VELOCITY_PID; - break; - } - } - else - { - // joint->SetMaxForce() must be called if joint->SetAngle() or joint->SetVelocity() are - // going to be called. joint->SetMaxForce() must *not* be called if joint->SetForce() is - // going to be called. - joint->SetMaxForce(0, joint_effort_limits_[j]); - } - } - } - - // Register interfaces - registerInterface(&js_interface_); - registerInterface(&ej_interface_); - registerInterface(&pj_interface_); - registerInterface(&vj_interface_); - - return true; - } - - void readSim(ros::Time time, ros::Duration period) - { - for(unsigned int j=0; j < n_dof_; j++) - { - // Gazebo has an interesting API... - if (joint_types_[j] == urdf::Joint::PRISMATIC) - { - joint_position_[j] = sim_joints_[j]->GetAngle(0).Radian(); - } - else - { - joint_position_[j] += angles::shortest_angular_distance(joint_position_[j], - sim_joints_[j]->GetAngle(0).Radian()); - } - joint_velocity_[j] = sim_joints_[j]->GetVelocity(0); - joint_effort_[j] = sim_joints_[j]->GetForce((unsigned int)(0)); - } - } - - void writeSim(ros::Time time, ros::Duration period) - { - ej_sat_interface_.enforceLimits(period); - ej_limits_interface_.enforceLimits(period); - pj_sat_interface_.enforceLimits(period); - pj_limits_interface_.enforceLimits(period); - vj_sat_interface_.enforceLimits(period); - vj_limits_interface_.enforceLimits(period); - - for(unsigned int j=0; j < n_dof_; j++) - { - switch (joint_control_methods_[j]) - { - case EFFORT: - { - const double effort = joint_effort_command_[j]; - sim_joints_[j]->SetForce(0, effort); - } - break; - - case POSITION: -#if GAZEBO_MAJOR_VERSION >= 4 - sim_joints_[j]->SetPosition(0, joint_position_command_[j]); -#else - sim_joints_[j]->SetAngle(0, joint_position_command_[j]); -#endif - break; - - case POSITION_PID: - { - double error; - switch (joint_types_[j]) - { - case urdf::Joint::REVOLUTE: - angles::shortest_angular_distance_with_limits(joint_position_[j], - joint_position_command_[j], - joint_lower_limits_[j], - joint_upper_limits_[j], - error); - break; - case urdf::Joint::CONTINUOUS: - error = angles::shortest_angular_distance(joint_position_[j], - joint_position_command_[j]); - break; - default: - error = joint_position_command_[j] - joint_position_[j]; - } - - const double effort_limit = joint_effort_limits_[j]; - const double effort = clamp(pid_controllers_[j].computeCommand(error, period), - -effort_limit, effort_limit); - sim_joints_[j]->SetForce(0, effort); - } - break; - - case VELOCITY: - sim_joints_[j]->SetVelocity(0, joint_velocity_command_[j]); - break; - - case VELOCITY_PID: - const double error = joint_velocity_command_[j] - joint_velocity_[j]; - const double effort_limit = joint_effort_limits_[j]; - const double effort = clamp(pid_controllers_[j].computeCommand(error, period), - -effort_limit, effort_limit); - sim_joints_[j]->SetForce(0, effort); - break; - } - } - } - -private: - // Methods used to control a joint. - enum ControlMethod {EFFORT, POSITION, POSITION_PID, VELOCITY, VELOCITY_PID}; - - // Register the limits of the joint specified by joint_name and joint_handle. The limits are - // retrieved from joint_limit_nh. If urdf_model is not NULL, limits are retrieved from it also. - // Return the joint's type, lower position limit, upper position limit, and effort limit. - void registerJointLimits(const std::string& joint_name, - const hardware_interface::JointHandle& joint_handle, - const ControlMethod ctrl_method, - const ros::NodeHandle& joint_limit_nh, - const urdf::Model *const urdf_model, - int *const joint_type, double *const lower_limit, - double *const upper_limit, double *const effort_limit) - { - *joint_type = urdf::Joint::UNKNOWN; - *lower_limit = -std::numeric_limits::max(); - *upper_limit = std::numeric_limits::max(); - *effort_limit = std::numeric_limits::max(); - - joint_limits_interface::JointLimits limits; - bool has_limits = false; - joint_limits_interface::SoftJointLimits soft_limits; - bool has_soft_limits = false; - - if (urdf_model != NULL) - { - const boost::shared_ptr urdf_joint = urdf_model->getJoint(joint_name); - if (urdf_joint != NULL) - { - *joint_type = urdf_joint->type; - // Get limits from the URDF file. - if (joint_limits_interface::getJointLimits(urdf_joint, limits)) - has_limits = true; - if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits)) - has_soft_limits = true; - } - } - // Get limits from the parameter server. - if (joint_limits_interface::getJointLimits(joint_name, joint_limit_nh, limits)) - has_limits = true; - - if (!has_limits) - return; - - if (*joint_type == urdf::Joint::UNKNOWN) - { - // Infer the joint type. - - if (limits.has_position_limits) - { - *joint_type = urdf::Joint::REVOLUTE; - } - else - { - if (limits.angle_wraparound) - *joint_type = urdf::Joint::CONTINUOUS; - else - *joint_type = urdf::Joint::PRISMATIC; - } - } - - if (limits.has_position_limits) - { - *lower_limit = limits.min_position; - *upper_limit = limits.max_position; - } - if (limits.has_effort_limits) - *effort_limit = limits.max_effort; - - if (has_soft_limits) - { - switch (ctrl_method) - { - case EFFORT: - { - const joint_limits_interface::EffortJointSoftLimitsHandle - limits_handle(joint_handle, limits, soft_limits); - ej_limits_interface_.registerHandle(limits_handle); - } - break; - case POSITION: - { - const joint_limits_interface::PositionJointSoftLimitsHandle - limits_handle(joint_handle, limits, soft_limits); - pj_limits_interface_.registerHandle(limits_handle); - } - break; - case VELOCITY: - { - const joint_limits_interface::VelocityJointSoftLimitsHandle - limits_handle(joint_handle, limits, soft_limits); - vj_limits_interface_.registerHandle(limits_handle); - } - break; - } - } - else - { - switch (ctrl_method) - { - case EFFORT: - { - const joint_limits_interface::EffortJointSaturationHandle - sat_handle(joint_handle, limits); - ej_sat_interface_.registerHandle(sat_handle); - } - break; - case POSITION: - { - const joint_limits_interface::PositionJointSaturationHandle - sat_handle(joint_handle, limits); - pj_sat_interface_.registerHandle(sat_handle); - } - break; - case VELOCITY: - { - const joint_limits_interface::VelocityJointSaturationHandle - sat_handle(joint_handle, limits); - vj_sat_interface_.registerHandle(sat_handle); - } - break; - } - } - } - - unsigned int n_dof_; - - hardware_interface::JointStateInterface js_interface_; - hardware_interface::EffortJointInterface ej_interface_; - hardware_interface::PositionJointInterface pj_interface_; - hardware_interface::VelocityJointInterface vj_interface_; - - joint_limits_interface::EffortJointSaturationInterface ej_sat_interface_; - joint_limits_interface::EffortJointSoftLimitsInterface ej_limits_interface_; - joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_; - joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_; - joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_; - joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_; - - std::vector joint_names_; - std::vector joint_types_; - std::vector joint_lower_limits_; - std::vector joint_upper_limits_; - std::vector joint_effort_limits_; - std::vector joint_control_methods_; - std::vector pid_controllers_; - std::vector joint_position_; - std::vector joint_velocity_; - std::vector joint_effort_; - std::vector joint_effort_command_; - std::vector joint_position_command_; - std::vector joint_velocity_command_; - - std::vector sim_joints_; -}; - -typedef boost::shared_ptr WamRobotHWSimPtr; - -} - -PLUGINLIB_EXPORT_CLASS(gazebo_ros_control::WamRobotHWSim, gazebo_ros_control::RobotHWSim) - -#endif // #ifndef __GAZEBO_ROS_CONTROL_PLUGIN_WAM_ROBOT_HW_SIM_H_ diff --git a/wam_gazebo_ros_control/src/wam_robot_hw_sim.cpp.orig b/wam_gazebo_ros_control/src/wam_robot_hw_sim.cpp.orig deleted file mode 100644 index fd8ee43..0000000 --- a/wam_gazebo_ros_control/src/wam_robot_hw_sim.cpp.orig +++ /dev/null @@ -1,101 +0,0 @@ -#include - -#include -#include - -#include - -#include - -#include -#include -#include - -namespace wam_gazebo_ros_control -{ - class WamRobotHWSim:public gazebo_ros_control::RobotHWSim - { - unsigned int n_dof_; - - hardware_interface::JointStateInterface js_interface_; - hardware_interface::EffortJointInterface ej_interface_; - - std::vector joint_name_; - std::vector joint_position_; - std::vector joint_velocity_; - std::vector joint_effort_; - std::vector joint_effort_command_; - - std::vector sim_joints_; - - public: - - WamRobotHWSim(void):n_dof_(7),joint_name_(n_dof_),joint_position_(n_dof_), - joint_velocity_(n_dof_),joint_effort_(n_dof_),joint_effort_command_(n_dof_) - { - joint_name_[0]="wam_joint_1"; - joint_name_[1]="wam_joint_2"; - joint_name_[2]="wam_joint_3"; - joint_name_[3]="wam_joint_4"; - joint_name_[4]="wam_joint_5"; - joint_name_[5]="wam_joint_6"; - joint_name_[6]="wam_joint_7"; - - for(unsigned int j=0;j < n_dof_;j++) - { - joint_position_[j]=0.0; - joint_velocity_[j]=0.0; - joint_effort_[j]=0.0; - - js_interface_.registerHandle(hardware_interface::JointStateHandle(joint_name_[j],&joint_position_[j], - &joint_velocity_[j],&joint_effort_[j])); - - joint_effort_command_[j] = 0.0; - ej_interface_.registerHandle(hardware_interface::JointHandle(js_interface_.getHandle(joint_name_[j]), - &joint_effort_command_[j])); - } - - registerInterface(&js_interface_); - registerInterface(&ej_interface_); - } - - bool initSim(const std::string& robot_namespace, - ros::NodeHandle nh,gazebo::physics::ModelPtr model, - const urdf::Model *const urdf_model, - std::vector transmissions) - { - for(unsigned int j=0;j < n_dof_;j++) - { - ROS_INFO_STREAM("Getting pointer to gazebo joint: " << joint_name_[j]); - gazebo::physics::JointPtr joint=model->GetJoint(joint_name_[j]); - if(joint) sim_joints_.push_back(joint); - else - { - ROS_ERROR_STREAM("This robot has a joint named \"" << joint_name_[j] - <<"\" which is not in the gazebo model."); - return false; - } - } - return true; - } - - void readSim(ros::Time time,ros::Duration period) - { - for(unsigned int j=0; j < n_dof_;j++) - { - joint_position_[j]=sim_joints_[j]->GetAngle(0).Radian(); - joint_velocity_[j]=sim_joints_[j]->GetVelocity(0); - joint_effort_[j]=joint_effort_command_[j]; - } - } - - void writeSim(ros::Time time,ros::Duration period) - { - for(unsigned int j=0;j < n_dof_;j++) - sim_joints_[j]->SetForce(0,joint_effort_command_[j]); - } - - }; -} - -PLUGINLIB_EXPORT_CLASS(wam_gazebo_ros_control::WamRobotHWSim,gazebo_ros_control::RobotHWSim) diff --git a/wam_gazebo_ros_control/wam_gazebo_ros_control_plugins.xml b/wam_gazebo_ros_control/wam_gazebo_ros_control_plugins.xml deleted file mode 100644 index fb5ee2b..0000000 --- a/wam_gazebo_ros_control/wam_gazebo_ros_control_plugins.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - A ROS/Gazebo interface for Barrett WAM, exporting a - joint_state_interface and a joint_effort_interface. - - - -