From: Walter Fetter Lages Date: Thu, 13 Jul 2023 04:18:08 +0000 (-0300) Subject: Remove twil_gazebo_ros_control. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=b4b5196e02a3c9547f80b47be63e86ebacfbf188;p=twil.git Remove twil_gazebo_ros_control. --- diff --git a/twil_gazebo_ros_control/CMakeLists.txt b/twil_gazebo_ros_control/CMakeLists.txt deleted file mode 100644 index 082aa92..0000000 --- a/twil_gazebo_ros_control/CMakeLists.txt +++ /dev/null @@ -1,210 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(twil_gazebo_ros_control) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## 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 - control_toolbox - controller_manager - gazebo_ros - gazebo_ros_control - pluginlib - roscpp - twil_description -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## 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 tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_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 -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## 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 your 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 twil_gazebo_ros_control -# CATKIN_DEPENDS control_toolbox controller_manager gazebo gazebo_ros_control pluginlib roscpp twil_description -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/twil_gazebo_ros_control.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/twil_gazebo_ros_control_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${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 -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_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 -# # 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_twil_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/twil_gazebo_ros_control/include/twil_gazebo_ros_control/twil_robot_hw_sim.h b/twil_gazebo_ros_control/include/twil_gazebo_ros_control/twil_robot_hw_sim.h deleted file mode 100644 index a9f15da..0000000 --- a/twil_gazebo_ros_control/include/twil_gazebo_ros_control/twil_robot_hw_sim.h +++ /dev/null @@ -1,145 +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, Jonathan Bohren - Desc: Hardware Interface for any simulated robot in Gazebo -*/ - -#ifndef _TWIL_GAZEBO_ROS_CONTROL___TWIL_ROBOT_HW_SIM_H_ -#define _TWIL_GAZEBO_ROS_CONTROL___TWIL_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 twil_gazebo_ros_control -{ - -class TwilRobotHWSim : public gazebo_ros_control::RobotHWSim -{ -public: - - virtual 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); - - virtual void readSim(ros::Time time, ros::Duration period); - - virtual void writeSim(ros::Time time, ros::Duration period); - - virtual void eStopActive(const bool active); - -protected: - // 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); - - 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 last_joint_position_command_; - std::vector joint_velocity_command_; - - std::vector sim_joints_; - - // e_stop_active_ is true if the emergency stop is active. - bool e_stop_active_, last_e_stop_active_; -}; - -typedef boost::shared_ptr TwilRobotHWSimPtr; - -} - -#endif // #ifndef __GAZEBO_ROS_CONTROL_PLUGIN_DEFAULT_ROBOT_HW_SIM_H_ diff --git a/twil_gazebo_ros_control/package.xml b/twil_gazebo_ros_control/package.xml deleted file mode 100644 index 4e646d9..0000000 --- a/twil_gazebo_ros_control/package.xml +++ /dev/null @@ -1,83 +0,0 @@ - - - twil_gazebo_ros_control - 4.1.0 - The twil_gazebo_ros_control package - - - - - Walter Fetter Lages - - - - - - GPLv3 - - - - - - - - - - - - - Walter Fetter Lages - - - - - - - - - - - - - - - - - - - - - - - catkin - control_toolbox - controller_manager - gazebo - gazebo_ros_control - pluginlib - roscpp - twil_description - control_toolbox - controller_manager - gazebo - gazebo_ros_control - pluginlib - roscpp - twil_description - control_toolbox - controller_manager - gazebo - gazebo_ros_control - pluginlib - roscpp - twil_description - - - - - - - - - - diff --git a/twil_gazebo_ros_control/src/twil_robot_hw_sim.cpp b/twil_gazebo_ros_control/src/twil_robot_hw_sim.cpp deleted file mode 100644 index 0c373d8..0000000 --- a/twil_gazebo_ros_control/src/twil_robot_hw_sim.cpp +++ /dev/null @@ -1,485 +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, Jonathan Bohren - Desc: Hardware Interface for any simulated robot in Gazebo -*/ - - -#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 twil_gazebo_ros_control -{ - - -bool TwilRobotHWSim::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); - - // 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("twil_robot_hw_sim","Transmission " << transmissions[j].name_ - << " has no associated joints."); - continue; - } - else if(transmissions[j].joints_.size() > 1) - { - ROS_WARN_STREAM_NAMED("twil_robot_hw_sim","Transmission " << transmissions[j].name_ - << " has more than one joint. Currently the twil robot hardware simulation " - << " interface only supports one."); - continue; - } - - std::vector joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_; - if (joint_interfaces.empty() && - !(transmissions[j].actuators_.empty()) && - !(transmissions[j].actuators_[0].hardware_interfaces_.empty())) - { - // TODO: Deprecate HW interface specification in actuators in ROS J - joint_interfaces = transmissions[j].actuators_[0].hardware_interfaces_; - ROS_WARN_STREAM_NAMED("twil_robot_hw_sim", "The element of tranmission " << - transmissions[j].name_ << " should be nested inside the element, not . " << - "The transmission will be properly loaded, but please update " << - "your robot model to remain compatible with future versions of the plugin."); - } - if (joint_interfaces.empty()) - { - ROS_WARN_STREAM_NAMED("twil_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ << - " of transmission " << transmissions[j].name_ << " does not specify any hardware interface. " << - "Not adding it to the robot hardware simulation."); - continue; - } - else if (joint_interfaces.size() > 1) - { - ROS_WARN_STREAM_NAMED("twil_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ << - " of transmission " << transmissions[j].name_ << " specifies multiple hardware interfaces. " << - "Currently the twil robot hardware simulation interface only supports one. Using the first entry!"); - //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; - - const std::string& hardware_interface = joint_interfaces.front(); - - // Debug - ROS_DEBUG_STREAM_NAMED("twil_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("twil_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("twil_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->SetParam("vel") to control the joint. - const ros::NodeHandle nh(model_nh, "/gazebo_ros_control/pid_gains/" + - joint_names_[j]); - if (pid_controllers_[j].init(nh, true)) - { - 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->SetParam("fmax") must be called if joint->SetAngle() or joint->SetParam("vel") are - // going to be called. joint->SetParam("fmax") must *not* be called if joint->SetForce() is - // going to be called. -#if GAZEBO_MAJOR_VERSION > 2 - joint->SetParam("fmax", 0, joint_effort_limits_[j]); -#else - joint->SetMaxForce(0, joint_effort_limits_[j]); -#endif - } - } - } - - // Register interfaces - registerInterface(&js_interface_); - registerInterface(&ej_interface_); - registerInterface(&pj_interface_); - registerInterface(&vj_interface_); - - // Initialize the emergency stop code. - e_stop_active_ = false; - last_e_stop_active_ = false; - - return true; -} - -void TwilRobotHWSim::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 TwilRobotHWSim::writeSim(ros::Time time, ros::Duration period) -{ - // If the E-stop is active, joints controlled by position commands will maintain their positions. - if (e_stop_active_) - { - if (!last_e_stop_active_) - { - last_joint_position_command_ = joint_position_; - last_e_stop_active_ = true; - } - joint_position_command_ = last_joint_position_command_; - } - else - { - last_e_stop_active_ = false; - } - - 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 = e_stop_active_ ? 0 : 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: -#if GAZEBO_MAJOR_VERSION > 2 - sim_joints_[j]->SetParam("vel", 0, e_stop_active_ ? 0 : joint_velocity_command_[j]); -#else - sim_joints_[j]->SetVelocity(0, e_stop_active_ ? 0 : joint_velocity_command_[j]); -#endif - break; - - case VELOCITY_PID: - double error; - if (e_stop_active_) - error = -joint_velocity_[j]; - else - 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; - } - } -} - -void TwilRobotHWSim::eStopActive(const bool active) -{ - e_stop_active_ = active; -} - -// 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 TwilRobotHWSim::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; - } - } -} - -} - -PLUGINLIB_EXPORT_CLASS(twil_gazebo_ros_control::TwilRobotHWSim, gazebo_ros_control::RobotHWSim) diff --git a/twil_gazebo_ros_control/twil_gazebo_ros_control_plugins.xml b/twil_gazebo_ros_control/twil_gazebo_ros_control_plugins.xml deleted file mode 100644 index 5862ba2..0000000 --- a/twil_gazebo_ros_control/twil_gazebo_ros_control_plugins.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - A ROS/Gazebo interface for Twil, exporting a joint_state_interface and a - joint_effort_interface. - - -