--- /dev/null
+cmake_minimum_required(VERSION 2.8.3)
+project(gazebo_ros_electrical)
+
+## 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
+ angles
+ control_toolbox
+ gazebo
+ gazebo_ros_control
+ hardware_interface
+ joint_limits_interface
+ pluginlib
+ roscpp
+ transmission_interface
+ urdf
+)
+
+## 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 run_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 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
+# )
+
+################################################
+## 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 run_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 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 gazebo_ros_electrical
+# CATKIN_DEPENDS angles control_toolbox gazebo hardware_interface joint_limits_interface pluginlib roscpp transmission_interface urdf
+# 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}
+ ${GAZEBO_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+add_library(${PROJECT_NAME}
+ src/dcmotor_robot_hw_sim.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/gazebo_ros_electrical_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}
+ ${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
+# # 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_gazebo_ros_electrical.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
+ * Copyright (c) 2018, Walter Fetter Lages
+ * 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.
+ *********************************************************************/
+
+/*
+ default_robot_hw_sim.cpp
+ Author: Dave Coleman, Jonathan Bohren
+ Desc: Hardware Interface for any simulated robot in Gazebo
+
+ dcmotor_robot_hw_sim.cpp
+ Author: Walter Fetter Lages
+ Desc: Hardware Interface simulating the dynamics of a D.C. motor,
+ developed based on default_robot_hw_sim.h
+*/
+
+#ifndef _GAZEBO_ROS_ELECTRICAL_DCMOTOR_ROBOT_HW_SIM_H_
+#define _GAZEBO_ROS_ELECTRICAL_DCMOTOR_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>
+
+#ifndef HARDWARE_INTERFACE_JOINT_COMMAND_EXTENDED_INTERFACE_H
+#define HARDWARE_INTERFACE_JOINT_COMMAND_EXTENDED_INTERFACE_H
+
+#include "hardware_interface/joint_command_interface.h"
+
+namespace twil_hardware_interfaces {
+
+/// \ref JointCommandInterface for commanding voltage-based joints.
+class VoltageJointInterface : public hardware_interface::JointCommandInterface {};
+
+}
+
+#endif
+
+namespace hardware_interface
+{
+class VoltageJointInterface : public twil_hardware_interfaces::VoltageJointInterface {};
+}
+
+namespace gazebo_ros_electrical
+{
+
+class DCmotorRobotHWSim : 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, VOLTAGE};
+
+ // 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_;
+ hardware_interface::VoltageJointInterface uj_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<double> joint_voltage_command_;
+ std::vector<double> joint_current_;
+ std::vector<double> joint_resistance_;
+ std::vector<double> joint_inductance_;
+ std::vector<double> joint_torque_constant_;
+
+
+ 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<DCmotorRobotHWSim> DCmotorRobotHWSimPtr;
+
+}
+
+#endif // #ifndef _GAZEBO_ROS_ELECTRICAL_DCMOTOR_ROBOT_HW_SIM_H_
--- /dev/null
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2013, Open Source Robotics Foundation
+ * Copyright (c) 2013, The Johns Hopkins University
+ * Copyright (c) 2018, Walter Fetter Lages
+ * 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.
+ *********************************************************************/
+
+/*
+ default_robot_hw_sim.cpp
+ Author: Dave Coleman, Jonathan Bohren
+ Desc: Hardware Interface for any simulated robot in Gazebo
+
+ dcmotor_robot_hw_sim.cpp
+ Author: Walter Fetter Lages
+ Desc: Hardware Interface simulating the dynamics of a D.C. motor,
+ developed based on default_robot_hw_sim.cpp
+*/
+
+
+#include <gazebo_ros_electrical/dcmotor_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 gazebo_ros_electrical
+{
+
+
+bool DCmotorRobotHWSim::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_);
+ joint_voltage_command_.resize(n_dof_);
+ joint_current_.resize(n_dof_);
+ joint_resistance_.resize(n_dof_);
+ joint_inductance_.resize(n_dof_);
+ joint_torque_constant_.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("dcmotor_robot_hw_sim","Transmission " << transmissions[j].name_
+ << " has no associated joints.");
+ continue;
+ }
+ else if(transmissions[j].joints_.size() > 1)
+ {
+ ROS_WARN_STREAM_NAMED("dcmotor_robot_hw_sim","Transmission " << transmissions[j].name_
+ << " has more than one joint. Currently the D.C motor 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("dcmotor_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("dcmotor_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("dcmotor_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ <<
+ " of transmission " << transmissions[j].name_ << " specifies multiple hardware interfaces. " <<
+ "Currently the D.C. motor 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;
+ joint_voltage_command_[j]=0.0;
+ joint_current_[j]=0.0;
+ joint_resistance_[j]=0.0;
+ joint_inductance_[j]=0.0;
+ joint_torque_constant_[j]=0.0;
+
+ const std::string& hardware_interface = joint_interfaces.front();
+
+ // Debug
+ ROS_DEBUG_STREAM_NAMED("dcmotor_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);
+ }
+ if(hardware_interface == "VoltageJointInterface")
+ {
+ // Create voltage joint interface
+ joint_control_methods_[j] = VOLTAGE;
+ joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
+ &joint_voltage_command_[j]);
+ uj_interface_.registerHandle(joint_handle);
+ }
+ else
+ {
+ ROS_FATAL_STREAM_NAMED("dcmotor_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("dcmotor_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 && joint_control_methods_[j] != VOLTAGE)
+ {
+ // 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
+ }
+ }
+ if (joint_control_methods_[j] == VOLTAGE)
+ {
+ const ros::NodeHandle nh(model_nh,"/gazebo_ros_electrical/dcmotor/"+joint_names_[j]);
+ if (!nh.getParam("Ra",joint_resistance_[j]))
+ {
+ ROS_ERROR("No Ra specified for D.C motor. Namespace: %s", nh.getNamespace().c_str());
+ }
+ if (!nh.getParam("La",joint_inductance_[j])) joint_inductance_[j]=0.0;
+ if (!nh.getParam("Kt",joint_torque_constant_[j]))
+ {
+ ROS_ERROR("No Kt specified for D.C motor. Namespace: %s", nh.getNamespace().c_str());
+ }
+ }
+ }
+
+ // Register interfaces
+ registerInterface(&js_interface_);
+ registerInterface(&ej_interface_);
+ registerInterface(&pj_interface_);
+ registerInterface(&vj_interface_);
+ registerInterface(&uj_interface_);
+
+ // Initialize the emergency stop code.
+ e_stop_active_ = false;
+ last_e_stop_active_ = false;
+
+ return true;
+}
+
+void DCmotorRobotHWSim::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 DCmotorRobotHWSim::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;
+ }
+
+ case VOLTAGE:
+ {
+ const double voltage = e_stop_active_ ? 0 : joint_voltage_command_[j];
+
+ if(joint_inductance_[j] > 0.0)
+ {
+ joint_current_[j]+=(voltage-joint_current_[j]*joint_resistance_[j]-joint_torque_constant_[j]*joint_velocity_[j])/joint_inductance_[j]/period.toSec();
+ }
+ else joint_current_[j]=(voltage-joint_torque_constant_[j]*joint_velocity_[j])/joint_resistance_[j];
+ const double effort=joint_torque_constant_[j]*joint_current_[j];
+ sim_joints_[j]->SetForce(0, effort);
+ break;
+ }
+
+ }
+ }
+}
+
+void DCmotorRobotHWSim::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 DCmotorRobotHWSim::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(gazebo_ros_electrical::DCmotorRobotHWSim, gazebo_ros_control::RobotHWSim)
projects / gazebo_ros_electrical.git / commitdiff