+++ /dev/null
-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)
+++ /dev/null
-/*********************************************************************
- * 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 <control_toolbox/pid.h>
-#include <hardware_interface/joint_command_interface.h>
-#include <hardware_interface/robot_hw.h>
-#include <joint_limits_interface/joint_limits.h>
-#include <joint_limits_interface/joint_limits_interface.h>
-#include <joint_limits_interface/joint_limits_rosparam.h>
-#include <joint_limits_interface/joint_limits_urdf.h>
-
-// Gazebo
-#include <gazebo/common/common.hh>
-#include <gazebo/physics/physics.hh>
-#include <gazebo/gazebo.hh>
-
-// ROS
-#include <ros/ros.h>
-#include <angles/angles.h>
-#include <pluginlib/class_list_macros.h>
-
-// gazebo_ros_control
-#include <gazebo_ros_control/robot_hw_sim.h>
-
-// URDF
-#include <urdf/model.h>
-
-
-
-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<transmission_interface::TransmissionInfo> 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<std::string> joint_names_;
- std::vector<int> joint_types_;
- std::vector<double> joint_lower_limits_;
- std::vector<double> joint_upper_limits_;
- std::vector<double> joint_effort_limits_;
- std::vector<ControlMethod> joint_control_methods_;
- std::vector<control_toolbox::Pid> pid_controllers_;
- std::vector<double> joint_position_;
- std::vector<double> joint_velocity_;
- std::vector<double> joint_effort_;
- std::vector<double> joint_effort_command_;
- std::vector<double> joint_position_command_;
- std::vector<double> last_joint_position_command_;
- std::vector<double> joint_velocity_command_;
-
- std::vector<gazebo::physics::JointPtr> 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<TwilRobotHWSim> TwilRobotHWSimPtr;
-
-}
-
-#endif // #ifndef __GAZEBO_ROS_CONTROL_PLUGIN_DEFAULT_ROBOT_HW_SIM_H_
+++ /dev/null
-<?xml version="1.0"?>
-<package format="2">
- <name>twil_gazebo_ros_control</name>
- <version>4.1.0</version>
- <description>The twil_gazebo_ros_control package</description>
-
- <!-- One maintainer tag required, multiple allowed, one person per tag -->
- <!-- Example: -->
- <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
- <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
-
-
- <!-- One license tag required, multiple allowed, one license per tag -->
- <!-- Commonly used license strings: -->
- <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
- <license>GPLv3</license>
-
-
- <!-- Url tags are optional, but multiple are allowed, one per tag -->
- <!-- Optional attribute type can be: website, bugtracker, or repository -->
- <!-- Example: -->
- <!-- <url type="website">http://wiki.ros.org/twil_gazebo_ros_control</url> -->
-
-
- <!-- Author tags are optional, multiple are allowed, one per tag -->
- <!-- Authors do not have to be maintainers, but could be -->
- <!-- Example: -->
- <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
- <author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
-
-
- <!-- The *depend tags are used to specify dependencies -->
- <!-- Dependencies can be catkin packages or system dependencies -->
- <!-- Examples: -->
- <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
- <!-- <depend>roscpp</depend> -->
- <!-- Note that this is equivalent to the following: -->
- <!-- <build_depend>roscpp</build_depend> -->
- <!-- <exec_depend>roscpp</exec_depend> -->
- <!-- Use build_depend for packages you need at compile time: -->
- <!-- <build_depend>message_generation</build_depend> -->
- <!-- Use build_export_depend for packages you need in order to build against this package: -->
- <!-- <build_export_depend>message_generation</build_export_depend> -->
- <!-- Use buildtool_depend for build tool packages: -->
- <!-- <buildtool_depend>catkin</buildtool_depend> -->
- <!-- Use exec_depend for packages you need at runtime: -->
- <!-- <exec_depend>message_runtime</exec_depend> -->
- <!-- Use test_depend for packages you need only for testing: -->
- <!-- <test_depend>gtest</test_depend> -->
- <!-- Use doc_depend for packages you need only for building documentation: -->
- <!-- <doc_depend>doxygen</doc_depend> -->
- <buildtool_depend>catkin</buildtool_depend>
- <build_depend>control_toolbox</build_depend>
- <build_depend>controller_manager</build_depend>
- <build_depend>gazebo</build_depend>
- <build_depend>gazebo_ros_control</build_depend>
- <build_depend>pluginlib</build_depend>
- <build_depend>roscpp</build_depend>
- <build_depend>twil_description</build_depend>
- <build_export_depend>control_toolbox</build_export_depend>
- <build_export_depend>controller_manager</build_export_depend>
- <build_export_depend>gazebo</build_export_depend>
- <build_export_depend>gazebo_ros_control</build_export_depend>
- <build_export_depend>pluginlib</build_export_depend>
- <build_export_depend>roscpp</build_export_depend>
- <build_export_depend>twil_description</build_export_depend>
- <exec_depend>control_toolbox</exec_depend>
- <exec_depend>controller_manager</exec_depend>
- <exec_depend>gazebo</exec_depend>
- <exec_depend>gazebo_ros_control</exec_depend>
- <exec_depend>pluginlib</exec_depend>
- <exec_depend>roscpp</exec_depend>
- <exec_depend>twil_description</exec_depend>
-
-
- <!-- The export tag contains other, unspecified, tags -->
- <export>
- <!-- Other tools can request additional information be placed here -->
-
- <gazebo_ros_control plugin="${prefix}/twil_gazebo_ros_control_plugins.xml" />
-
- </export>
-</package>
+++ /dev/null
-/*********************************************************************
- * 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 <twil_gazebo_ros_control/twil_robot_hw_sim.h>
-
-
-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<transmission_interface::TransmissionInfo> transmissions)
-{
- // getJointLimits() searches joint_limit_nh for joint limit parameters. The format of each
- // parameter's name is "joint_limits/<joint name>". 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<std::string> 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 <hardware_interface> element of tranmission " <<
- transmissions[j].name_ << " should be nested inside the <joint> element, not <actuator>. " <<
- "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<double>::max();
- *upper_limit = std::numeric_limits<double>::max();
- *effort_limit = std::numeric_limits<double>::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<const urdf::Joint> 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)
+++ /dev/null
-<library path="lib/libtwil_gazebo_ros_control">
-
- <class
- name="twil_gazebo_ros_control/TwilRobotHWSim"
- type="twil_gazebo_ros_control::TwilRobotHWSim"
- base_class_type="gazebo_ros_control::RobotHWSim">
- <description>
- A ROS/Gazebo interface for Twil, exporting a joint_state_interface and a
- joint_effort_interface.
- </description>
- </class>
-</library>
projects / twil.git / commitdiff