forked from xtdrone/XTDrone
add a gazebo plugin
This commit is contained in:
parent
bd42919ac7
commit
527175761b
|
|
@ -0,0 +1,41 @@
|
|||
cmake_minimum_required(VERSION 2.8.6 FATAL_ERROR)
|
||||
project(roboticsgroup_gazebo_plugins)
|
||||
|
||||
# Load catkin and all dependencies required for this package
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
gazebo_ros
|
||||
control_toolbox
|
||||
)
|
||||
|
||||
# Depend on system install of Gazebo
|
||||
find_package(gazebo REQUIRED)
|
||||
# Gazebo cxx flags should have all the required C++ flags
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
|
||||
|
||||
find_package(Boost REQUIRED)
|
||||
|
||||
catkin_package(
|
||||
DEPENDS
|
||||
roscpp
|
||||
gazebo_ros
|
||||
control_toolbox
|
||||
)
|
||||
|
||||
link_directories(${GAZEBO_LIBRARY_DIRS})
|
||||
include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} include)
|
||||
|
||||
add_library(roboticsgroup_gazebo_mimic_joint_plugin src/mimic_joint_plugin.cpp)
|
||||
target_link_libraries(roboticsgroup_gazebo_mimic_joint_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
|
||||
|
||||
add_library(roboticsgroup_gazebo_disable_link_plugin src/disable_link_plugin.cpp)
|
||||
target_link_libraries(roboticsgroup_gazebo_disable_link_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
|
||||
|
||||
install(TARGETS roboticsgroup_gazebo_mimic_joint_plugin roboticsgroup_gazebo_disable_link_plugin
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
install(DIRECTORY include/${PROJECT_NAME}
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
)
|
||||
|
|
@ -0,0 +1,79 @@
|
|||
roboticsgroup_gazebo_plugins
|
||||
================
|
||||
|
||||
Collection of small gazebo plugins
|
||||
----------------------------------
|
||||
|
||||
MimicJointPlugin
|
||||
----------------
|
||||
|
||||
A simple (Model) plugin for Gazebo in order to add to Gazebo the mimic joint functionality that exists in URDF (ROS). Inspired by code of Goncalo Cabrita.
|
||||
|
||||
- *XML Parameters*
|
||||
|
||||
- joint (Required)
|
||||
|
||||
A **string** specifying the name of the joint to be mimic-ed.
|
||||
|
||||
- mimicJoint (Required)
|
||||
|
||||
A **string** specifying the name of the mimic joint.
|
||||
|
||||
- multiplier
|
||||
|
||||
A **double** specifying the multiplier parameter of the mimic joint. Defaults to 1.0.
|
||||
|
||||
- offset
|
||||
|
||||
A **double** specifying the offset parameter of the mimic joint. Defaults to 0.0.
|
||||
|
||||
- maxEffort
|
||||
|
||||
A **double** specifying the max effort the mimic joint can generate. Defaults to 1.0.
|
||||
|
||||
- sensitiveness
|
||||
|
||||
A **double** specifying the sensitiveness of the mimic joint. Defaults to 0.0. It basically is the threshold of the difference between the 2 angles (joint's and mimic's) before applying the "mimicness".
|
||||
|
||||
- robotNamespace
|
||||
|
||||
A **string** specifying the namespace the robot is under. Defaults to '/'.
|
||||
|
||||
- hasPID
|
||||
|
||||
Determines whether the joint has PID in order to be controlled via PID position/effort controller. Takes no value: *\<hasPID/\>* means that the mimic joint is controlled via PID. Ommit it so that the mimic joint is controlled via setAngle.
|
||||
|
||||
DisableLinkPlugin
|
||||
-----------------
|
||||
|
||||
A simple (Model) plugin for Gazebo that allows you to disable a link in Gazebo's physics engine.
|
||||
|
||||
- *XML Parameters*
|
||||
|
||||
- link (Required)
|
||||
|
||||
A **string** specifying the name of the link to be disabled. It should be a valid sdf (not urdf) link.
|
||||
|
||||
### Hoping to add more plugins....
|
||||
|
||||
Usage
|
||||
------
|
||||
|
||||
Standard Gazebo plugin import inside xacro/urdf. Use **libroboticsgroup_gazebo_** prefix. E.g. if you want to import MimicJointPlugin:
|
||||
|
||||
```
|
||||
libroboticsgroup_gazebo_mimic_joint_plugin.so
|
||||
```
|
||||
|
||||
Notes
|
||||
------
|
||||
|
||||
If there is a need, please make an issue and I'll see what I can do to add that functionality/plugin.
|
||||
|
||||
License
|
||||
----
|
||||
|
||||
BSD
|
||||
|
||||
|
||||
Copyright (c) 2014, **Konstantinos Chatzilygeroudis**
|
||||
|
|
@ -0,0 +1,64 @@
|
|||
/**
|
||||
Copyright (c) 2014, Konstantinos Chatzilygeroudis
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
|
||||
|
||||
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
|
||||
|
||||
2. 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.
|
||||
|
||||
3. Neither the name of the copyright holder 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 HOLDER 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.
|
||||
**/
|
||||
|
||||
#ifndef ROBOTICSGROUP_GAZEBO_PLUGINS_DISABLE_LINK_PLUGIN
|
||||
#define ROBOTICSGROUP_GAZEBO_PLUGINS_DISABLE_LINK_PLUGIN
|
||||
|
||||
// ROS includes
|
||||
#include <ros/ros.h>
|
||||
|
||||
// Boost includes
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
// Gazebo includes
|
||||
#include <gazebo/common/Plugin.hh>
|
||||
#include <gazebo/gazebo.hh>
|
||||
#include <gazebo/physics/physics.hh>
|
||||
#include <gazebo/common/common.hh>
|
||||
|
||||
namespace gazebo {
|
||||
class DisableLinkPlugin : public ModelPlugin {
|
||||
public:
|
||||
DisableLinkPlugin();
|
||||
~DisableLinkPlugin();
|
||||
|
||||
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
|
||||
void UpdateChild();
|
||||
|
||||
private:
|
||||
// Parameters
|
||||
std::string link_name_;
|
||||
|
||||
bool kill_sim;
|
||||
|
||||
// Pointers to the joints
|
||||
physics::LinkPtr link_;
|
||||
|
||||
// Pointer to the model
|
||||
physics::ModelPtr model_;
|
||||
|
||||
// Pointer to the world
|
||||
physics::WorldPtr world_;
|
||||
};
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,73 @@
|
|||
/**
|
||||
Copyright (c) 2014, Konstantinos Chatzilygeroudis
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
|
||||
|
||||
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
|
||||
|
||||
2. 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.
|
||||
|
||||
3. Neither the name of the copyright holder 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 HOLDER 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.
|
||||
**/
|
||||
|
||||
#ifndef ROBOTICSGROUP_GAZEBO_PLUGINS_MIMIC_JOINT_PLUGIN
|
||||
#define ROBOTICSGROUP_GAZEBO_PLUGINS_MIMIC_JOINT_PLUGIN
|
||||
|
||||
// ROS includes
|
||||
#include <ros/ros.h>
|
||||
|
||||
// ros_control
|
||||
#include <control_toolbox/pid.h>
|
||||
|
||||
// Boost includes
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
// Gazebo includes
|
||||
#include <gazebo/common/Plugin.hh>
|
||||
#include <gazebo/gazebo.hh>
|
||||
#include <gazebo/physics/physics.hh>
|
||||
#include <gazebo/common/common.hh>
|
||||
|
||||
namespace gazebo {
|
||||
class MimicJointPlugin : public ModelPlugin {
|
||||
public:
|
||||
MimicJointPlugin();
|
||||
~MimicJointPlugin();
|
||||
|
||||
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
|
||||
void UpdateChild();
|
||||
|
||||
private:
|
||||
// Parameters
|
||||
std::string joint_name_, mimic_joint_name_, robot_namespace_;
|
||||
double multiplier_, offset_, sensitiveness_, max_effort_;
|
||||
bool has_pid_;
|
||||
|
||||
// PID controller if needed
|
||||
control_toolbox::Pid pid_;
|
||||
|
||||
// Pointers to the joints
|
||||
physics::JointPtr joint_, mimic_joint_;
|
||||
|
||||
// Pointer to the model
|
||||
physics::ModelPtr model_;
|
||||
|
||||
// Pointer to the world
|
||||
physics::WorldPtr world_;
|
||||
|
||||
// Pointer to the update event connection
|
||||
event::ConnectionPtr updateConnection;
|
||||
};
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,51 @@
|
|||
<?xml version="1.0"?>
|
||||
<!-- Copyright (c) 2014, Konstantinos Chatzilygeroudis
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
|
||||
|
||||
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
|
||||
|
||||
2. 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.
|
||||
|
||||
3. Neither the name of the copyright holder 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 HOLDER 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. -->
|
||||
<package>
|
||||
<name>roboticsgroup_gazebo_plugins</name>
|
||||
<version>0.0.1</version>
|
||||
<description>Collection of small gazebo plugins</description>
|
||||
|
||||
<maintainer email="costashatz@gmail.com">Konstantinos Chatzilygeroudis</maintainer>
|
||||
|
||||
<license>BSD</license>
|
||||
|
||||
<url type="repository">https://github.com/roboticsgroup/roboticsgroup_gazebo_plugins</url>
|
||||
<url type="bugtracker">https://github.com/roboticsgroup/roboticsgroup_gazebo_plugins/issues</url>
|
||||
|
||||
<author email="costashatz@gmail.com">Konstantinos Chatzilygeroudis</author>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
|
||||
<build_depend>gazebo_ros</build_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>control_toolbox</build_depend>
|
||||
|
||||
|
||||
<run_depend>gazebo_ros</run_depend>
|
||||
<run_depend>roscpp</run_depend>
|
||||
<run_depend>control_toolbox</run_depend>
|
||||
|
||||
|
||||
<export>
|
||||
<gazebo_ros plugin_path="${prefix}/lib" gazebo_media_path="${prefix}" />
|
||||
</export>
|
||||
</package>
|
||||
|
|
@ -0,0 +1,64 @@
|
|||
/**
|
||||
Copyright (c) 2014, Konstantinos Chatzilygeroudis
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
|
||||
|
||||
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
|
||||
|
||||
2. 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.
|
||||
|
||||
3. Neither the name of the copyright holder 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 HOLDER 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.
|
||||
**/
|
||||
|
||||
#include <roboticsgroup_gazebo_plugins/disable_link_plugin.h>
|
||||
|
||||
namespace gazebo {
|
||||
|
||||
DisableLinkPlugin::DisableLinkPlugin()
|
||||
{
|
||||
kill_sim = false;
|
||||
|
||||
link_.reset();
|
||||
}
|
||||
|
||||
DisableLinkPlugin::~DisableLinkPlugin()
|
||||
{
|
||||
kill_sim = true;
|
||||
}
|
||||
|
||||
void DisableLinkPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
|
||||
{
|
||||
model_ = _parent;
|
||||
world_ = model_->GetWorld();
|
||||
|
||||
// Check for link element
|
||||
if (!_sdf->HasElement("link")) {
|
||||
ROS_ERROR("No link element present. DisableLinkPlugin could not be loaded.");
|
||||
return;
|
||||
}
|
||||
|
||||
link_name_ = _sdf->GetElement("link")->Get<std::string>();
|
||||
|
||||
// Get pointers to joints
|
||||
link_ = model_->GetLink(link_name_);
|
||||
if (link_) {
|
||||
link_->SetEnabled(false);
|
||||
// Output some confirmation
|
||||
ROS_INFO_STREAM("DisableLinkPlugin loaded! Link: \"" << link_name_);
|
||||
}
|
||||
else
|
||||
ROS_ERROR_STREAM("Link" << link_name_ << " not found! DisableLinkPlugin could not be loaded.");
|
||||
}
|
||||
|
||||
GZ_REGISTER_MODEL_PLUGIN(DisableLinkPlugin);
|
||||
}
|
||||
|
|
@ -0,0 +1,188 @@
|
|||
/**
|
||||
Copyright (c) 2014, Konstantinos Chatzilygeroudis
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
|
||||
|
||||
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
|
||||
|
||||
2. 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.
|
||||
|
||||
3. Neither the name of the copyright holder 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 HOLDER 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.
|
||||
**/
|
||||
|
||||
#include <roboticsgroup_gazebo_plugins/mimic_joint_plugin.h>
|
||||
|
||||
#if GAZEBO_MAJOR_VERSION >= 8
|
||||
namespace math = ignition::math;
|
||||
#else
|
||||
namespace math = gazebo::math;
|
||||
#endif
|
||||
|
||||
namespace gazebo {
|
||||
|
||||
MimicJointPlugin::MimicJointPlugin()
|
||||
{
|
||||
joint_.reset();
|
||||
mimic_joint_.reset();
|
||||
}
|
||||
|
||||
MimicJointPlugin::~MimicJointPlugin()
|
||||
{
|
||||
this->updateConnection.reset();
|
||||
}
|
||||
|
||||
void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
|
||||
{
|
||||
ros::NodeHandle model_nh;
|
||||
model_ = _parent;
|
||||
world_ = model_->GetWorld();
|
||||
|
||||
// Error message if the model couldn't be found
|
||||
if (!model_) {
|
||||
ROS_ERROR("Parent model is NULL! GazeboNaoqiControlPlugin could not be loaded.");
|
||||
return;
|
||||
}
|
||||
|
||||
// Check that ROS has been initialized
|
||||
if (!ros::isInitialized()) {
|
||||
ROS_ERROR("A ROS node for Gazebo has not been initialized, unable to load plugin.");
|
||||
return;
|
||||
}
|
||||
|
||||
// Check for robot namespace
|
||||
robot_namespace_ = "/";
|
||||
if (_sdf->HasElement("robotNamespace")) {
|
||||
robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
|
||||
}
|
||||
|
||||
// Check for joint element
|
||||
if (!_sdf->HasElement("joint")) {
|
||||
ROS_ERROR("No joint element present. MimicJointPlugin could not be loaded.");
|
||||
return;
|
||||
}
|
||||
|
||||
joint_name_ = _sdf->GetElement("joint")->Get<std::string>();
|
||||
|
||||
// Check for mimicJoint element
|
||||
if (!_sdf->HasElement("mimicJoint")) {
|
||||
ROS_ERROR("No mimicJoint element present. MimicJointPlugin could not be loaded.");
|
||||
return;
|
||||
}
|
||||
|
||||
mimic_joint_name_ = _sdf->GetElement("mimicJoint")->Get<std::string>();
|
||||
|
||||
has_pid_ = false;
|
||||
// Check if PID controller wanted
|
||||
if (_sdf->HasElement("hasPID")) {
|
||||
has_pid_ = true;
|
||||
|
||||
const ros::NodeHandle nh(model_nh, std::string(robot_namespace_ + "/gazebo_ros_control/pid_gains/") + mimic_joint_name_);
|
||||
pid_.init(nh);
|
||||
}
|
||||
|
||||
// Check for multiplier element
|
||||
multiplier_ = 1.0;
|
||||
if (_sdf->HasElement("multiplier"))
|
||||
multiplier_ = _sdf->GetElement("multiplier")->Get<double>();
|
||||
|
||||
// Check for offset element
|
||||
offset_ = 0.0;
|
||||
if (_sdf->HasElement("offset"))
|
||||
offset_ = _sdf->GetElement("offset")->Get<double>();
|
||||
|
||||
// Check for sensitiveness element
|
||||
sensitiveness_ = 0.0;
|
||||
if (_sdf->HasElement("sensitiveness"))
|
||||
sensitiveness_ = _sdf->GetElement("sensitiveness")->Get<double>();
|
||||
|
||||
// Check for max effort
|
||||
max_effort_ = 1.0;
|
||||
if (_sdf->HasElement("maxEffort")) {
|
||||
max_effort_ = _sdf->GetElement("maxEffort")->Get<double>();
|
||||
}
|
||||
|
||||
// Get pointers to joints
|
||||
joint_ = model_->GetJoint(joint_name_);
|
||||
if (!joint_) {
|
||||
ROS_ERROR_STREAM("No joint named \"" << joint_name_ << "\". MimicJointPlugin could not be loaded.");
|
||||
return;
|
||||
}
|
||||
mimic_joint_ = model_->GetJoint(mimic_joint_name_);
|
||||
if (!mimic_joint_) {
|
||||
ROS_ERROR_STREAM("No (mimic) joint named \"" << mimic_joint_name_ << "\". MimicJointPlugin could not be loaded.");
|
||||
return;
|
||||
}
|
||||
|
||||
// Set max effort
|
||||
if (!has_pid_) {
|
||||
#if GAZEBO_MAJOR_VERSION > 2
|
||||
mimic_joint_->SetEffortLimit(0, max_effort_);
|
||||
#else
|
||||
mimic_joint_->SetMaxForce(0, max_effort_);
|
||||
#endif
|
||||
}
|
||||
|
||||
// Listen to the update event. This event is broadcast every
|
||||
// simulation iteration.
|
||||
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
|
||||
boost::bind(&MimicJointPlugin::UpdateChild, this));
|
||||
|
||||
// Output some confirmation
|
||||
ROS_INFO_STREAM("MimicJointPlugin loaded! Joint: \"" << joint_name_ << "\", Mimic joint: \"" << mimic_joint_name_ << "\""
|
||||
<< ", Multiplier: " << multiplier_ << ", Offset: " << offset_
|
||||
<< ", MaxEffort: " << max_effort_ << ", Sensitiveness: " << sensitiveness_);
|
||||
}
|
||||
|
||||
void MimicJointPlugin::UpdateChild()
|
||||
{
|
||||
#if GAZEBO_MAJOR_VERSION >= 8
|
||||
static ros::Duration period(world_->Physics()->GetMaxStepSize());
|
||||
#else
|
||||
static ros::Duration period(world_->GetPhysicsEngine()->GetMaxStepSize());
|
||||
#endif
|
||||
|
||||
// Set mimic joint's angle based on joint's angle
|
||||
#if GAZEBO_MAJOR_VERSION >= 8
|
||||
double angle = joint_->Position(0) * multiplier_ + offset_;
|
||||
double a = mimic_joint_->Position(0);
|
||||
#else
|
||||
double angle = joint_->GetAngle(0).Radian() * multiplier_ + offset_;
|
||||
double a = mimic_joint_->GetAngle(0).Radian();
|
||||
#endif
|
||||
|
||||
if (abs(angle - a) >= sensitiveness_) {
|
||||
if (has_pid_) {
|
||||
if (a != a)
|
||||
a = angle;
|
||||
double error = angle - a;
|
||||
double effort = math::clamp(pid_.computeCommand(error, period), -max_effort_, max_effort_);
|
||||
mimic_joint_->SetForce(0, effort);
|
||||
}
|
||||
else {
|
||||
#if GAZEBO_MAJOR_VERSION >= 9
|
||||
mimic_joint_->SetPosition(0, angle, true);
|
||||
#elif GAZEBO_MAJOR_VERSION > 2
|
||||
ROS_WARN_ONCE("The mimic_joint plugin is using the Joint::SetPosition method without preserving the link velocity.");
|
||||
ROS_WARN_ONCE("As a result, gravity will not be simulated correctly for your model.");
|
||||
ROS_WARN_ONCE("Please set gazebo_pid parameters or upgrade to Gazebo 9.");
|
||||
ROS_WARN_ONCE("For details, see https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612");
|
||||
mimic_joint_->SetPosition(0, angle);
|
||||
#else
|
||||
mimic_joint_->SetAngle(0, math::Angle(angle));
|
||||
#endif
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
GZ_REGISTER_MODEL_PLUGIN(MimicJointPlugin);
|
||||
}
|
||||
Loading…
Reference in New Issue