diff --git a/rotors_description/urdf/component_snippets.xacro b/rotors_description/urdf/component_snippets.xacro
index 3c285e53a..2890dedd4 100644
--- a/rotors_description/urdf/component_snippets.xacro
+++ b/rotors_description/urdf/component_snippets.xacro
@@ -261,18 +261,6 @@
-
-
-
-
@@ -311,8 +299,8 @@
mass_imu_sensor gyroscope_noise_density gyroscope_random_walk
gyroscope_bias_correlation_time gyroscope_turn_on_bias_sigma
accelerometer_noise_density accelerometer_random_walk
- accelerometer_bias_correlation_time accelerometer_turn_on_bias_sigma
- *inertia *origin">
+ accelerometer_bias_correlation_time accelerometer_turn_on_bias_sigma
+ measurement_divisor *inertia *origin">
@@ -343,6 +331,7 @@
${accelerometer_random_walk}
${accelerometer_bias_correlation_time}
${accelerometer_turn_on_bias_sigma}
+ ${measurement_divisor}
@@ -787,7 +776,8 @@
accelerometer_noise_density="0.004"
accelerometer_random_walk="0.006"
accelerometer_bias_correlation_time="300.0"
- accelerometer_turn_on_bias_sigma="0.1960">
+ accelerometer_turn_on_bias_sigma="0.1960"
+ measurement_divisor="1">
@@ -808,7 +798,8 @@
accelerometer_noise_density="0.0"
accelerometer_random_walk="0.0"
accelerometer_bias_correlation_time="300.0"
- accelerometer_turn_on_bias_sigma="0.0">
+ accelerometer_turn_on_bias_sigma="0.0"
+ measurement_divisor="1">
@@ -860,7 +851,8 @@
accelerometer_noise_density="0.004"
accelerometer_random_walk="0.006"
accelerometer_bias_correlation_time="300.0"
- accelerometer_turn_on_bias_sigma="0.1960">
+ accelerometer_turn_on_bias_sigma="0.1960"
+ measurement_divisor="1">
diff --git a/rotors_description/urdf/firefly.xacro b/rotors_description/urdf/firefly.xacro
index 9dcfaca71..6fa3e10fd 100644
--- a/rotors_description/urdf/firefly.xacro
+++ b/rotors_description/urdf/firefly.xacro
@@ -25,7 +25,7 @@
-
+
diff --git a/rotors_description/urdf/omav.xacro b/rotors_description/urdf/omav.xacro
index 49718ff2a..98f9eee41 100644
--- a/rotors_description/urdf/omav.xacro
+++ b/rotors_description/urdf/omav.xacro
@@ -56,6 +56,10 @@
+
+
+
+
@@ -111,7 +115,10 @@
rolling_moment_coefficient="${rolling_moment_coefficient}"
mesh="propeller"
mesh_tilt_unit="${mesh_file_tilt_unit}"
- color="Black">
+ color="Black"
+ p_gain="${tilt_p_gain}"
+ i_gain="${tilt_i_gain}"
+ d_gain="${tilt_d_gain}">
@@ -137,7 +144,10 @@
rolling_moment_coefficient="${rolling_moment_coefficient}"
mesh="propeller"
mesh_tilt_unit="${mesh_file_tilt_unit}"
- color="Black">
+ color="Black"
+ p_gain="${tilt_p_gain}"
+ i_gain="${tilt_i_gain}"
+ d_gain="${tilt_d_gain}">
@@ -163,7 +173,10 @@
rolling_moment_coefficient="${rolling_moment_coefficient}"
mesh="propeller"
mesh_tilt_unit="${mesh_file_tilt_unit}"
- color="Orange">
+ color="Orange"
+ p_gain="${tilt_p_gain}"
+ i_gain="${tilt_i_gain}"
+ d_gain="${tilt_d_gain}">
@@ -188,7 +201,10 @@
rolling_moment_coefficient="${rolling_moment_coefficient}"
mesh="propeller"
mesh_tilt_unit="${mesh_file_tilt_unit}"
- color="DarkGrey">
+ color="DarkGrey"
+ p_gain="${tilt_p_gain}"
+ i_gain="${tilt_i_gain}"
+ d_gain="${tilt_d_gain}">
@@ -213,7 +229,10 @@
rolling_moment_coefficient="${rolling_moment_coefficient}"
mesh="propeller"
mesh_tilt_unit="${mesh_file_tilt_unit}"
- color="Orange">
+ color="Orange"
+ p_gain="${tilt_p_gain}"
+ i_gain="${tilt_i_gain}"
+ d_gain="${tilt_d_gain}">
@@ -238,7 +257,10 @@
rolling_moment_coefficient="${rolling_moment_coefficient}"
mesh="propeller"
mesh_tilt_unit="${mesh_file_tilt_unit}"
- color="DarkGrey">
+ color="DarkGrey"
+ p_gain="${tilt_p_gain}"
+ i_gain="${tilt_i_gain}"
+ d_gain="${tilt_d_gain}">
diff --git a/rotors_description/urdf/overactuated_base.xacro b/rotors_description/urdf/overactuated_base.xacro
index 7cbf5b53b..1d69a4636 100644
--- a/rotors_description/urdf/overactuated_base.xacro
+++ b/rotors_description/urdf/overactuated_base.xacro
@@ -72,7 +72,7 @@
+ params="robot_namespace suffix direction motor_constant moment_constant parent mass_rotor mass_tilt_unit time_constant_up time_constant_down max_rot_velocity rotor_offset_top motor_number tilt_unit_motor_number rotor_drag_coefficient rolling_moment_coefficient color mesh mesh_tilt_unit p_gain i_gain d_gain *origin *inertia">
@@ -108,13 +108,13 @@
ccw
position
/gazebo/command/motor_speed
- ${motor_number+12}
+ ${tilt_unit_motor_number}
/tilt_motor_${motor_number}/velocity
/tilt_motor_${motor_number}/position
- 1.0
- 0.0
- 0.01
+ ${p_gain}
+ ${i_gain}
+ ${d_gain}
1.0
-1.0
2.0
@@ -166,6 +166,7 @@
${moment_constant}
/gazebo/command/motor_speed
${motor_number}
+ velocity
${rotor_drag_coefficient}
${rolling_moment_coefficient}
/motor_speed/${motor_number}
@@ -179,7 +180,7 @@
+ params="robot_namespace suffix direction_upper direction_lower motor_constant motor_constant_lower moment_constant moment_constant_lower parent mass_rotor mass_tilt_unit time_constant_up time_constant_down max_rot_velocity rotor_offset_top motor_number rotor_drag_coefficient rolling_moment_coefficient color mesh mesh_tilt_unit p_gain i_gain d_gain *origin *inertia">
@@ -219,9 +220,9 @@
/tilt_motor_${motor_number}/velocity
/tilt_motor_${motor_number}/position
- 1.0
- 0.0
- 0.01
+ ${p_gain}
+ ${i_gain}
+ ${d_gain}
1.0
-1.0
2.0
diff --git a/rotors_gazebo_plugins/CMakeLists.txt b/rotors_gazebo_plugins/CMakeLists.txt
index 4082f9c11..8e1190d06 100644
--- a/rotors_gazebo_plugins/CMakeLists.txt
+++ b/rotors_gazebo_plugins/CMakeLists.txt
@@ -4,6 +4,7 @@
# BUILD_MAVLINK_INTERFACE_PLUGIN bool Build mavlink_interface_plugin if mavlink found (requires mav dependency).
# BUILD_OCTOMAP_PLUGIN bool Build the optical map plugin (requires Octomap).
# BUILD_OPTICAL_FLOW_PLUGIN bool Build the optical flow plugin (requires OpenCV).
+# BUILD_AERIAL_MANIPULATION_PLUGINS bool Build plugins related to aerial manipulation
# MAVLINK_HEADER_DIR string Location of MAVLink header files. If not provided, this CMakeLists.txt file will
# search the default locations (e.g. ROS) for them. This variable is only required
# if BUILD_MAVLINK_INTERFACE_PLUGIN=TRUE.
@@ -28,6 +29,11 @@ if(NOT DEFINED BUILD_OCTOMAP_PLUGIN)
set(BUILD_OCTOMAP_PLUGIN FALSE)
endif()
+if(NOT DEFINED BUILD_AERIAL_MANIPULATION_PLUGINS)
+ message(STATUS "BUILD_AERIAL_MANIPULATION_PLUGINS variable not provided, setting to TRUE.")
+ set(BUILD_AERIAL_MANIPULATION_PLUGINS TRUE)
+endif()
+
if(NOT DEFINED BUILD_OPTICAL_FLOW_PLUGIN)
message(STATUS "BUILD_OPTICAL_FLOW_PLUGIN variable not provided, setting to FALSE.")
set(BUILD_OPTICAL_FLOW_PLUGIN FALSE)
@@ -84,6 +90,12 @@ else ()
message(STATUS "BUILD_OPTICAL_FLOW_PLUGIN = FALSE, NOT building gazebo_optical_flow_plugin.")
endif ()
+if(BUILD_AERIAL_MANIPULATION_PLUGINS)
+ message(STATUS "BUILD_AERIAL_MANIPULATION_PLUGINS = TRUE, building aerial manipulation plugins.")
+else ()
+ message(STATUS "BUILD_AERIAL_MANIPULATION_PLUGINS = FALSE, NOT building aerial manipulation plugins.")
+endif ()
+
if(NO_ROS)
message(STATUS "NO_ROS = TRUE, building rotors_gazebo_plugins WITHOUT any ROS dependancies.")
else()
@@ -456,6 +468,18 @@ if(BUILD_OCTOMAP_PLUGIN)
endif()
list(APPEND targets_to_install rotors_gazebo_octomap_plugin)
endif()
+#====================================== Aerial Manipulation PLUGINs ==========================================//
+
+if(BUILD_AERIAL_MANIPULATION_PLUGINS)
+
+ add_library(rotors_gazebo_force_sensor_plugin SHARED src/gazebo_force_sensor_plugin.cpp)
+ target_link_libraries(rotors_gazebo_force_sensor_plugin ${target_linking_LIBRARIES})
+ if (NOT NO_ROS)
+ add_dependencies(rotors_gazebo_force_sensor_plugin ${catkin_EXPORTED_TARGETS})
+ endif()
+ list(APPEND targets_to_install rotors_gazebo_force_sensor_plugin)
+
+endif()
#======================================= ODOMETRY PLUGIN ========================================//
if (NOT NO_ROS)
diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_force_sensor_plugin.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_force_sensor_plugin.h
new file mode 100644
index 000000000..38fa31086
--- /dev/null
+++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_force_sensor_plugin.h
@@ -0,0 +1,220 @@
+/*
+ * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Simone Comari, ASL, ETH Zurich, Switzerland
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef ROTORS_GAZEBO_PLUGINS_FORCE_SENSOR_PLUGIN_H
+#define ROTORS_GAZEBO_PLUGINS_FORCE_SENSOR_PLUGIN_H
+
+#include
+#include
+#include
+#include
+
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+
+#include
+
+
+namespace gazebo {
+// Default values
+//static const std::string kDefaultNamespace = "";
+static const std::string kDefaultPublishFrameId = "world";
+static const std::string kDefaultParentFrameId = "world";
+static const std::string kDefaultReferenceFrameId = "world";
+static const std::string kDefaultLinkName = "f2g_sensor_link";
+static const std::string kDefaultForceSensorPubTopic = "force_sensor";
+static const std::string kDefaultForceSensorTruthPubTopic = "force_sensor_truth";
+static const std::string kDefaultWrenchVectorPubTopic = "wrench_vector";
+
+static constexpr int kDefaultMeasurementDelay = 0;
+static constexpr int kDefaultMeasurementDivisor = 1;
+static constexpr int kDefaultGazeboSequence = 0;
+static constexpr int kDefaultWrenchSequence = 0;
+static constexpr double kDefaultUnknownDelay = 0.0;
+static constexpr double kDefaultCutOffFrequency = 10.0;
+static constexpr bool kDefaultLinforceMeasEnabled = true;
+static constexpr bool kDefaultTorqueMeasEnabled = true;
+static constexpr bool kDefaultDispWrenchVector = true;
+
+
+class FirstOrderFilterJointWrench {
+ public:
+ FirstOrderFilterJointWrench(double timeConstantUp, double timeConstantDown, physics::JointWrench initialState):
+ previousState_(initialState) {
+ // Initialize filters, one for each wrench element.
+ force_x_filter_.reset(new FirstOrderFilter(timeConstantUp, timeConstantDown, initialState.body1Force[0]));
+ force_y_filter_.reset(new FirstOrderFilter(timeConstantUp, timeConstantDown, initialState.body1Force[1]));
+ force_z_filter_.reset(new FirstOrderFilter(timeConstantUp, timeConstantDown, initialState.body1Force[2]));
+ torque_x_filter_.reset(new FirstOrderFilter(timeConstantUp, timeConstantDown, initialState.body1Torque[0]));
+ torque_y_filter_.reset(new FirstOrderFilter(timeConstantUp, timeConstantDown, initialState.body1Torque[1]));
+ torque_z_filter_.reset(new FirstOrderFilter(timeConstantUp, timeConstantDown, initialState.body1Torque[2]));
+ }
+
+ physics::JointWrench updateFilter(physics::JointWrench inputState, double samplingTime) {
+ /*
+ This method will apply a first order filter on each wrench element of the inputState.
+ */
+ physics::JointWrench outputState;
+
+ outputState.body1Force[0] = force_x_filter_->updateFilter(inputState.body1Force[0],samplingTime);
+ outputState.body1Force[1] = force_y_filter_->updateFilter(inputState.body1Force[1],samplingTime);
+ outputState.body1Force[2] = force_z_filter_->updateFilter(inputState.body1Force[2],samplingTime);
+ outputState.body1Torque[0] = torque_x_filter_->updateFilter(inputState.body1Torque[0],samplingTime);
+ outputState.body1Torque[1] = torque_y_filter_->updateFilter(inputState.body1Torque[1],samplingTime);
+ outputState.body1Torque[2] = torque_z_filter_->updateFilter(inputState.body1Torque[2],samplingTime);
+
+ previousState_ = outputState;
+ return outputState;
+ }
+
+ ~FirstOrderFilterJointWrench() {}
+
+ protected:
+ physics::JointWrench previousState_;
+ std::unique_ptr> force_x_filter_;
+ std::unique_ptr> force_y_filter_;
+ std::unique_ptr> force_z_filter_;
+ std::unique_ptr> torque_x_filter_;
+ std::unique_ptr> torque_y_filter_;
+ std::unique_ptr> torque_z_filter_;
+};
+
+
+class GazeboForceSensorPlugin : public ModelPlugin {
+
+ public:
+ typedef std::normal_distribution<> NormalDistribution;
+ typedef std::uniform_real_distribution<> UniformDistribution;
+ typedef std::deque > WrenchQueue;
+
+ GazeboForceSensorPlugin()
+ : ModelPlugin(),
+ random_generator_(random_device_()),
+ publish_frame_id_(kDefaultPublishFrameId),
+ parent_frame_id_(kDefaultParentFrameId),
+ reference_frame_id_(kDefaultReferenceFrameId),
+ link_name_(kDefaultLinkName),
+ force_sensor_pub_topic_(kDefaultForceSensorPubTopic),
+ force_sensor_truth_pub_topic_(kDefaultForceSensorTruthPubTopic),
+ wrench_vector_pub_topic_(kDefaultWrenchVectorPubTopic),
+ measurement_delay_(kDefaultMeasurementDelay),
+ measurement_divisor_(kDefaultMeasurementDivisor),
+ unknown_delay_(kDefaultUnknownDelay),
+ cutoff_frequency_(kDefaultCutOffFrequency),
+ lin_force_meas_enabled_(kDefaultLinforceMeasEnabled),
+ torque_meas_enabled_(kDefaultTorqueMeasEnabled),
+ disp_wrench_vector_(kDefaultDispWrenchVector),
+ gazebo_sequence_(kDefaultGazeboSequence),
+ wrench_sequence_(kDefaultWrenchSequence),
+ prev_sim_time_(0.0),
+ sampling_time_(0.001),
+ node_handle_(NULL) {}
+
+ ~GazeboForceSensorPlugin();
+
+ void InitializeParams();
+ void Publish();
+
+
+ protected:
+ void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+ void OnUpdate(const common::UpdateInfo& /*_info*/);
+ void PrintJointWrench(physics::JointWrench jw_);
+
+
+ private:
+ WrenchQueue wrench_queue_;
+
+ std::string namespace_;
+ std::string force_sensor_pub_topic_;
+ std::string force_sensor_truth_pub_topic_;
+ std::string wrench_vector_pub_topic_;
+ std::string publish_frame_id_;
+ std::string parent_frame_id_;
+ std::string reference_frame_id_;
+ std::string link_name_;
+
+ NormalDistribution linear_force_n_[3];
+ NormalDistribution torque_n_[3];
+ UniformDistribution linear_force_u_[3];
+ UniformDistribution torque_u_[3];
+
+ geometry_msgs::WrenchStamped wrench_msg_;
+
+ int measurement_delay_;
+ int measurement_divisor_;
+ int gazebo_sequence_;
+ int wrench_sequence_;
+ double prev_sim_time_;
+ double sampling_time_;
+ double unknown_delay_;
+ double cutoff_frequency_;
+ bool lin_force_meas_enabled_;
+ bool torque_meas_enabled_;
+ bool disp_wrench_vector_;
+
+ std::random_device random_device_;
+ std::mt19937 random_generator_;
+
+ ros::NodeHandle* node_handle_;
+ ros::Publisher lin_force_sensor_pub_;
+ ros::Publisher lin_force_sensor_truth_pub_;
+ ros::Publisher ang_force_sensor_pub_;
+ ros::Publisher ang_force_sensor_truth_pub_;
+ ros::Publisher wrench_vector_pub_;
+
+ tf::Transform tf_;
+ tf::TransformBroadcaster transform_broadcaster_;
+
+ physics::WorldPtr world_;
+ physics::ModelPtr model_;
+ physics::LinkPtr link_;
+ physics::LinkPtr parent_link_;
+ physics::LinkPtr reference_link_;
+ physics::JointPtr joint_;
+ physics::JointWrench joint_wrench_;
+
+ std::unique_ptr sensor_data_filter_;
+
+ /// \brief Pointer to the update event connection.
+ event::ConnectionPtr updateConnection_;
+
+ boost::thread callback_queue_thread_;
+
+ void QueueThread();
+};
+
+}
+
+#endif /* ROTORS_GAZEBO_PLUGINS_FORCE_SENSOR_PLUGIN_H */
diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_imu_plugin.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_imu_plugin.h
index 51c7b4a15..4dcd0955e 100644
--- a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_imu_plugin.h
+++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_imu_plugin.h
@@ -55,6 +55,7 @@ static constexpr double kDefaultAdisAccelerometerTurnOnBiasSigma =
20.0e-3 * 9.8;
// Earth's gravity in Zurich (lat=+47.3667degN, lon=+8.5500degE, h=+500m, WGS84)
static constexpr double kDefaultGravityMagnitude = 9.8068;
+static constexpr int kDefaultMeasurementDivisor = 1;
// A description of the parameters:
// https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model-and-Intrinsics
@@ -172,6 +173,9 @@ class GazeboImuPlugin : public ModelPlugin {
Eigen::Vector3d gyroscope_turn_on_bias_;
Eigen::Vector3d accelerometer_turn_on_bias_;
+ int measurement_divisor_;
+ int gazebo_sequence_;
+
ImuParameters imu_parameters_;
};
diff --git a/rotors_gazebo_plugins/src/gazebo_force_sensor_plugin.cpp b/rotors_gazebo_plugins/src/gazebo_force_sensor_plugin.cpp
new file mode 100644
index 000000000..7cbde1ef9
--- /dev/null
+++ b/rotors_gazebo_plugins/src/gazebo_force_sensor_plugin.cpp
@@ -0,0 +1,347 @@
+/*
+ * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Simone Comari, ASL, ETH Zurich, Switzerland
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "../include/rotors_gazebo_plugins/gazebo_force_sensor_plugin.h"
+
+#include
+#include
+
+
+namespace gazebo {
+
+GazeboForceSensorPlugin::~GazeboForceSensorPlugin() {
+ // event::Events::DisconnectWorldUpdateBegin(updateConnection_);
+ if (node_handle_) {
+ node_handle_->shutdown();
+ delete node_handle_;
+ }
+}
+
+// void GazeboForceSensorPlugin::InitializeParams() {};
+// void GazeboForceSensorPlugin::Publish() {};
+
+void GazeboForceSensorPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
+ // Store the pointer to the model
+ model_ = _model;
+ world_ = model_->GetWorld();
+
+ ignition::math::Vector3d noise_normal_linear_force;
+ ignition::math::Vector3d noise_normal_torque;
+ ignition::math::Vector3d noise_uniform_linear_force;
+ ignition::math::Vector3d noise_uniform_torque;
+ const ignition::math::Vector3d zeros3(0.0, 0.0, 0.0);
+
+ wrench_queue_.clear();
+
+ namespace_.clear();
+ if (_sdf->HasElement("robotNamespace"))
+ namespace_ = _sdf->GetElement("robotNamespace")->Get();
+ else
+ gzerr << "[gazebo_force_sensor_plugin] Please specify a robotNamespace.\n";
+
+ node_handle_ = new ros::NodeHandle(namespace_);
+
+ if (_sdf->HasElement("linkName"))
+ link_name_ = _sdf->GetElement("linkName")->Get();
+ else
+ gzerr << "[gazebo_force_sensor_plugin] Please specify a linkName.\n";
+
+ link_ = model_->GetLink(link_name_);
+ if (link_ == NULL)
+ gzthrow("[gazebo_force_sensor_plugin] Couldn't find specified link \"" << link_name_ << "\".");
+
+ joint_ = model_->GetJoint(link_name_);
+ if (joint_ == NULL)
+ gzthrow("[gazebo_force_sensor_plugin] Couldn't find specified joint \"" << link_name_ << "\".");
+
+ if (_sdf->HasElement("randomEngineSeed")) {
+ random_generator_.seed(_sdf->GetElement("randomEngineSeed")->Get());
+ }
+ else {
+ random_generator_.seed(std::chrono::system_clock::now().time_since_epoch().count());
+ }
+
+ getSdfParam(_sdf, "forceSensorTopic", force_sensor_pub_topic_, force_sensor_pub_topic_);
+ getSdfParam(_sdf, "forceSensorTruthTopic", force_sensor_truth_pub_topic_, force_sensor_truth_pub_topic_);
+ getSdfParam(_sdf, "wrenchVectorTopic", wrench_vector_pub_topic_, wrench_vector_pub_topic_);
+ getSdfParam(_sdf, "publishFrameId", publish_frame_id_, publish_frame_id_);
+ getSdfParam(_sdf, "parentFrameId", parent_frame_id_, parent_frame_id_);
+ getSdfParam(_sdf, "referenceFrameId", reference_frame_id_, reference_frame_id_);
+ getSdfParam(_sdf, "noiseNormalLinearForce", noise_normal_linear_force, zeros3);
+ getSdfParam(_sdf, "noiseNormalTorque", noise_normal_torque, zeros3);
+ getSdfParam(_sdf, "noiseUniformLinearForce", noise_uniform_linear_force, zeros3);
+ getSdfParam(_sdf, "noiseUniformTorque", noise_uniform_torque, zeros3);
+ getSdfParam(_sdf, "measurementDelay", measurement_delay_, measurement_delay_);
+ getSdfParam(_sdf, "measurementDivisor", measurement_divisor_, measurement_divisor_);
+ getSdfParam(_sdf, "unknownDelay", unknown_delay_, unknown_delay_);
+ getSdfParam(_sdf, "cutoffFrequency", cutoff_frequency_, cutoff_frequency_);
+ getSdfParam(_sdf, "linForceMeasEnabled", lin_force_meas_enabled_, lin_force_meas_enabled_);
+ getSdfParam(_sdf, "torqueMeasEnabled", torque_meas_enabled_, torque_meas_enabled_);
+ getSdfParam(_sdf, "dispWrenchVector", disp_wrench_vector_, disp_wrench_vector_);
+
+ parent_link_ = model_->GetLink(parent_frame_id_);
+ if (parent_link_ == NULL && parent_frame_id_ != kDefaultParentFrameId) {
+ gzthrow("[gazebo_force_sensor_plugin] Couldn't find specified parent link \"" << parent_frame_id_ << "\".");
+ }
+
+ reference_link_ = model_->GetLink(reference_frame_id_);
+ if (reference_link_ == NULL && reference_frame_id_ != kDefaultReferenceFrameId) {
+ gzthrow("[gazebo_force_sensor_plugin] Couldn't find specified reference frame \"" << reference_frame_id_ << "\".");
+ }
+
+ if (lin_force_meas_enabled_) {
+ linear_force_n_[0] = NormalDistribution(0, noise_normal_linear_force[0]);
+ linear_force_n_[1] = NormalDistribution(0, noise_normal_linear_force[1]);
+ linear_force_n_[2] = NormalDistribution(0, noise_normal_linear_force[2]);
+
+ linear_force_u_[0] = UniformDistribution(-noise_uniform_linear_force[0], noise_uniform_linear_force[0]);
+ linear_force_u_[1] = UniformDistribution(-noise_uniform_linear_force[1], noise_uniform_linear_force[1]);
+ linear_force_u_[2] = UniformDistribution(-noise_uniform_linear_force[2], noise_uniform_linear_force[2]);
+
+ // Linear forces publisher
+ lin_force_sensor_pub_ = node_handle_->advertise(force_sensor_pub_topic_+"/linear", 10);
+ lin_force_sensor_truth_pub_ = node_handle_->advertise(force_sensor_truth_pub_topic_+"/linear", 10);
+ }
+
+ if (torque_meas_enabled_) {
+ torque_n_[0] = NormalDistribution(0, noise_normal_torque[0]);
+ torque_n_[1] = NormalDistribution(0, noise_normal_torque[1]);
+ torque_n_[2] = NormalDistribution(0, noise_normal_torque[2]);
+
+ torque_u_[0] = UniformDistribution(-noise_uniform_torque[0], noise_uniform_torque[0]);
+ torque_u_[1] = UniformDistribution(-noise_uniform_torque[1], noise_uniform_torque[1]);
+ torque_u_[2] = UniformDistribution(-noise_uniform_torque[2], noise_uniform_torque[2]);
+
+ // Torques publisher
+ ang_force_sensor_pub_ = node_handle_->advertise(force_sensor_pub_topic_+"/angular", 10);
+ ang_force_sensor_truth_pub_ = node_handle_->advertise(force_sensor_truth_pub_topic_+"/angular", 10);
+ }
+
+ if (disp_wrench_vector_) {
+ // Wrench vector publisher for RViz visualization
+ wrench_vector_pub_ = node_handle_->advertise(wrench_vector_pub_topic_, 10);
+ }
+
+ // Listen to the update event. This event is broadcast every simulation iteration.
+ updateConnection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboForceSensorPlugin::OnUpdate, this, _1));
+
+}
+
+
+// This gets called by the world update start event.
+void GazeboForceSensorPlugin::OnUpdate(const common::UpdateInfo& _info) {
+ /* APPLICATION POINT COMPUTATION */
+ // C denotes child frame, P parent frame, R reference frame and W world frame.
+ // Further C_pose_W_P denotes pose of P wrt. W expressed in C.
+ ignition::math::Pose3d W_pose_W_C = link_->WorldCoGPose();
+// math::Pos()e W_pose_W_C = joint_->WorldPose();
+ ignition::math::Pose3d gazebo_pose = W_pose_W_C;
+ ignition::math::Pose3d gazebo_parent_pose = ignition::math::Pose3d::Zero;
+ ignition::math::Pose3d gazebo_reference_pose = ignition::math::Pose3d::Zero;
+ ignition::math::Pose3d W_pose_W_P = ignition::math::Pose3d::Zero;
+ ignition::math::Pose3d W_pose_W_R = ignition::math::Pose3d::Zero;
+
+ if (parent_frame_id_ != kDefaultParentFrameId) {
+ W_pose_W_P = model_->GetJoint(parent_frame_id_)->WorldPose();
+ ignition::math::Pose3d C_pose_P_C = W_pose_W_C - W_pose_W_P;
+ gazebo_pose = C_pose_P_C;
+ gazebo_parent_pose = W_pose_W_P;
+ }
+
+ if (reference_frame_id_ != kDefaultReferenceFrameId && reference_frame_id_ != parent_frame_id_) {
+ W_pose_W_R = reference_link_->WorldCoGPose();
+ ignition::math::Pose3d P_pose_R_P = W_pose_W_P - W_pose_W_R;
+ gazebo_parent_pose = P_pose_R_P;
+ gazebo_reference_pose = W_pose_W_R;
+ }
+
+ /* FORCE PARSING */
+ // The wrench vectors consist of two vectors representing the internal resultant force and torque applied on force
+ // sensor joint by child link (i.e. force sensor itself). The values of the two 3D vectors are the coordinates in
+ // parent frame coordinates system of the tip of an arrow starting at the origin of parent frame.
+
+ ignition::math::Vector3d torque;
+ ignition::math::Vector3d force;
+ bool publish_forces = true;
+
+ // Get internal forces and torques at force sensor joint.
+ joint_wrench_ = joint_->GetForceTorque(0u);
+
+ // First order filter
+ if (prev_sim_time_ == 0.0) {
+ // Initialize the first order filter.
+ double time_constant_ = 1.0 / (2 * M_PI * cutoff_frequency_);
+ sensor_data_filter_.reset(new FirstOrderFilterJointWrench(time_constant_, time_constant_, joint_wrench_));
+ }
+ else {
+ // Smooth row data by first order filter.
+ sampling_time_ = _info.simTime.Double() - prev_sim_time_;
+ joint_wrench_ = sensor_data_filter_->updateFilter(joint_wrench_,sampling_time_);
+ }
+ prev_sim_time_ = _info.simTime.Double();
+
+
+ // Get forces applied to force sensor joint from child link (i.e. force sensor) in parent frame coordinates.
+ force = -joint_wrench_.body1Force;
+
+ // Get torques applied to sensor joint from child link (i.e. force sensor) in parent frame coordinates.
+ torque = -joint_wrench_.body1Torque;
+
+ if (gazebo_sequence_ % measurement_divisor_ == 0) {
+ // Copy data into wrench message.
+ wrench_msg_.header.frame_id = publish_frame_id_;
+ wrench_msg_.header.seq = wrench_sequence_++;
+ wrench_msg_.header.stamp.sec = (world_->SimTime()).sec + ros::Duration(unknown_delay_).sec;
+ wrench_msg_.header.stamp.nsec = (world_->SimTime()).nsec + ros::Duration(unknown_delay_).nsec;
+
+ // HUUUUUUGE HACK.
+ wrench_msg_.wrench.force.x = force[1];
+ wrench_msg_.wrench.force.y = force[2];
+ wrench_msg_.wrench.force.z = force[0];
+ wrench_msg_.wrench.torque.x = torque[1];
+ wrench_msg_.wrench.torque.y = torque[2];
+ wrench_msg_.wrench.torque.z = torque[0];
+
+ if (publish_forces)
+ wrench_queue_.push_back(std::make_pair(gazebo_sequence_ + measurement_delay_, wrench_msg_));
+ }
+
+ // Is it time to publish the front element?
+ if (gazebo_sequence_ == wrench_queue_.front().first) {
+ // Init wrench vector for RViz visualization purpose
+ const geometry_msgs::Vector3 zeros3;
+ wrench_msg_.wrench.force = zeros3;
+ wrench_msg_.wrench.torque = zeros3;
+
+ if (lin_force_meas_enabled_) {
+ // Init true and noisy linear force message.
+ geometry_msgs::Vector3StampedPtr true_lin_forces(new geometry_msgs::Vector3Stamped);
+ geometry_msgs::Vector3StampedPtr noisy_lin_forces(new geometry_msgs::Vector3Stamped);
+
+ // Fill true force message with latest linear forces data.
+ true_lin_forces->vector = wrench_queue_.front().second.wrench.force;
+
+ // Update headers.
+ true_lin_forces->header = wrench_queue_.front().second.header;
+ noisy_lin_forces->header = true_lin_forces->header;
+
+ // Calculate linear force distortions.
+ Eigen::Vector3d linear_force_n;
+ linear_force_n << linear_force_n_[0](random_generator_) + linear_force_u_[0](random_generator_),
+ linear_force_n_[1](random_generator_) + linear_force_u_[1](random_generator_),
+ linear_force_n_[2](random_generator_) + linear_force_u_[2](random_generator_);
+ // Fill linear force fields.
+ noisy_lin_forces->vector.x = true_lin_forces->vector.x + linear_force_n[0];
+ noisy_lin_forces->vector.y = true_lin_forces->vector.y + linear_force_n[1];
+ noisy_lin_forces->vector.z = true_lin_forces->vector.z + linear_force_n[2];
+
+ // Publish all the topics, for which the topic name is specified.
+ if (lin_force_sensor_pub_.getNumSubscribers() > 0) {
+ lin_force_sensor_pub_.publish(noisy_lin_forces);
+ }
+ if (lin_force_sensor_truth_pub_.getNumSubscribers() > 0) {
+ lin_force_sensor_truth_pub_.publish(true_lin_forces);
+ }
+
+ // Update wrench vector forces for possible future use.
+ wrench_msg_.wrench.force = noisy_lin_forces->vector;
+ }
+
+ if (torque_meas_enabled_) {
+ // Init true and noisy torque message.
+ geometry_msgs::Vector3StampedPtr true_torques(new geometry_msgs::Vector3Stamped);
+ geometry_msgs::Vector3StampedPtr noisy_torques(new geometry_msgs::Vector3Stamped);
+
+ // Fill true force message with latest torques data.
+ true_torques->vector = wrench_queue_.front().second.wrench.torque;
+
+ // Update headers.
+ true_torques->header = wrench_queue_.front().second.header;
+ noisy_torques->header = true_torques->header;
+
+ // Calculate torque distortions.
+ Eigen::Vector3d torque_n;
+ torque_n << torque_n_[0](random_generator_) + torque_u_[0](random_generator_),
+ torque_n_[1](random_generator_) + torque_u_[1](random_generator_),
+ torque_n_[2](random_generator_) + torque_u_[2](random_generator_);
+ // Fill torque fields.
+ noisy_torques->vector.x = true_torques->vector.x + torque_n[0];
+ noisy_torques->vector.y = true_torques->vector.y + torque_n[1];
+ noisy_torques->vector.z = true_torques->vector.z + torque_n[2];
+
+ // Publish all the topics, for which the topic name is specified.
+ if (ang_force_sensor_pub_.getNumSubscribers() > 0) {
+ ang_force_sensor_pub_.publish(noisy_torques);
+ }
+ if (ang_force_sensor_truth_pub_.getNumSubscribers() > 0) {
+ ang_force_sensor_truth_pub_.publish(true_torques);
+ }
+
+ // Update wrench vector torques for possible future use.
+ wrench_msg_.wrench.torque = noisy_torques->vector;
+ }
+
+ if (disp_wrench_vector_) {
+ // Complete wrench message.
+ wrench_msg_.header = wrench_queue_.front().second.header;
+ // Publish all the topics, for which the topic name is specified.
+ if (wrench_vector_pub_.getNumSubscribers() > 0) {
+ wrench_vector_pub_.publish(wrench_msg_);
+ }
+ }
+
+ // Transformation between sensor link and parent link.
+ tf::Quaternion tf_q_P_C(gazebo_pose.Rot().X(), gazebo_pose.Rot().Y(), gazebo_pose.Rot().Z(), gazebo_pose.Rot().W());
+ tf::Vector3 tf_p_P_C(gazebo_pose.Pos().X(), gazebo_pose.Pos().Y(), gazebo_pose.Pos().Z());
+ tf_ = tf::Transform(tf_q_P_C, tf_p_P_C);
+ transform_broadcaster_.sendTransform(tf::StampedTransform(tf_, wrench_queue_.front().second.header.stamp, parent_frame_id_, link_name_));
+ if (parent_frame_id_ != kDefaultParentFrameId && reference_frame_id_ != parent_frame_id_) {
+ // Transformation between parent link and reference frame.
+ tf::Quaternion tf_q_R_P(gazebo_parent_pose.Rot().X(), gazebo_parent_pose.Rot().Y(), gazebo_parent_pose.Rot().Z(), gazebo_parent_pose.Rot().W());
+ tf::Vector3 tf_p_R_P(gazebo_parent_pose.Pos().X(), gazebo_parent_pose.Pos().Y(), gazebo_parent_pose.Pos().Z());
+ tf_.setOrigin(tf_p_R_P);
+ tf_.setRotation(tf_q_R_P);
+ transform_broadcaster_.sendTransform(tf::StampedTransform(tf_, wrench_queue_.front().second.header.stamp, reference_frame_id_, parent_frame_id_));
+ }
+ if (reference_frame_id_ != kDefaultReferenceFrameId) {
+ // Transformation between reference frame and world (default).
+ tf::Quaternion tf_q_W_R(gazebo_reference_pose.Rot().X(), gazebo_reference_pose.Rot().Y(), gazebo_reference_pose.Rot().Z(), gazebo_reference_pose.Rot().W());
+ tf::Vector3 tf_p_W_R(gazebo_reference_pose.Pos().X(), gazebo_reference_pose.Pos().Y(), gazebo_reference_pose.Pos().Z());
+ tf_.setOrigin(tf_p_W_R);
+ tf_.setRotation(tf_q_W_R);
+ transform_broadcaster_.sendTransform(tf::StampedTransform(tf_, wrench_queue_.front().second.header.stamp, "world", reference_frame_id_));
+ }
+
+ // Remove first element from wrench array (LIFO policy).
+ wrench_queue_.pop_front();
+
+ }
+ ++gazebo_sequence_;
+}
+
+void GazeboForceSensorPlugin::PrintJointWrench(physics::JointWrench jw_){
+ ROS_INFO("[gazebo_force_sensor_plugin]\nJoint Wrench Forces:\n Fx = %f Fy = %f Fz = %f\nJoint Wrench Torques:\n Tx = %f Ty = %f Tz = %f\n",
+ jw_.body1Force[0], jw_.body1Force[1], jw_.body1Force[2], jw_.body1Torque[0], jw_.body1Torque[1], jw_.body1Torque[2]);
+}
+
+
+GZ_REGISTER_MODEL_PLUGIN(GazeboForceSensorPlugin);
+}
diff --git a/rotors_gazebo_plugins/src/gazebo_imu_plugin.cpp b/rotors_gazebo_plugins/src/gazebo_imu_plugin.cpp
index 7b642fdb9..ace52fb0c 100644
--- a/rotors_gazebo_plugins/src/gazebo_imu_plugin.cpp
+++ b/rotors_gazebo_plugins/src/gazebo_imu_plugin.cpp
@@ -41,7 +41,9 @@ GazeboImuPlugin::GazeboImuPlugin()
: ModelPlugin(),
node_handle_(0),
velocity_prev_W_(0, 0, 0),
- pubs_and_subs_created_(false) {}
+ pubs_and_subs_created_(false),
+ measurement_divisor_(kDefaultMeasurementDivisor),
+ gazebo_sequence_(0) {}
GazeboImuPlugin::~GazeboImuPlugin() {
}
@@ -115,6 +117,8 @@ void GazeboImuPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
getSdfParam(_sdf, "accelerometerTurnOnBiasSigma",
imu_parameters_.accelerometer_turn_on_bias_sigma,
imu_parameters_.accelerometer_turn_on_bias_sigma);
+ getSdfParam(_sdf, "measurementDivisor", measurement_divisor_,
+ measurement_divisor_);
last_time_ = world_->SimTime();
@@ -273,68 +277,71 @@ void GazeboImuPlugin::OnUpdate(const common::UpdateInfo& _info) {
pubs_and_subs_created_ = true;
}
- common::Time current_time = world_->SimTime();
- double dt = (current_time - last_time_).Double();
- last_time_ = current_time;
- double t = current_time.Double();
-
- ignition::math::Pose3d T_W_I = link_->WorldPose(); // TODO(burrimi): Check tf.
- ignition::math::Quaterniond C_W_I = T_W_I.Rot();
-
- ignition::math::Vector3d acceleration_I =
- link_->RelativeLinearAccel() - C_W_I.RotateVectorReverse(gravity_W_);
-
- ignition::math::Vector3d angular_vel_I = link_->RelativeAngularVel();
-
- Eigen::Vector3d linear_acceleration_I(acceleration_I.X(), acceleration_I.Y(),
- acceleration_I.Z());
- Eigen::Vector3d angular_velocity_I(angular_vel_I.X(), angular_vel_I.Y(),
- angular_vel_I.Z());
-
- AddNoise(&linear_acceleration_I, &angular_velocity_I, dt);
-
- // Fill IMU message.
- // imu_message_.header.stamp.sec = current_time.sec;
- imu_message_.mutable_header()->mutable_stamp()->set_sec(current_time.sec);
-
- // imu_message_.header.stamp.nsec = current_time.nsec;
- imu_message_.mutable_header()->mutable_stamp()->set_nsec(current_time.nsec);
-
- /// \todo(burrimi): Add orientation estimator.
- // NOTE: rotors_simulator used to set the orientation to "0", since it is
- // not raw IMU data but rather a calculation (and could confuse users).
- // However, the orientation is now set as it is used by PX4.
- /*gazebo::msgs::Quaternion* orientation = new gazebo::msgs::Quaternion();
- orientation->set_x(0);
- orientation->set_y(0);
- orientation->set_z(0);
- orientation->set_w(1);
- imu_message_.set_allocated_orientation(orientation);*/
-
- /// \todo(burrimi): add noise.
- gazebo::msgs::Quaternion* orientation = new gazebo::msgs::Quaternion();
- orientation->set_w(C_W_I.W());
- orientation->set_x(C_W_I.X());
- orientation->set_y(C_W_I.Y());
- orientation->set_z(C_W_I.Z());
- imu_message_.set_allocated_orientation(orientation);
-
- gazebo::msgs::Vector3d* linear_acceleration = new gazebo::msgs::Vector3d();
- linear_acceleration->set_x(linear_acceleration_I[0]);
- linear_acceleration->set_y(linear_acceleration_I[1]);
- linear_acceleration->set_z(linear_acceleration_I[2]);
- imu_message_.set_allocated_linear_acceleration(linear_acceleration);
-
- gazebo::msgs::Vector3d* angular_velocity = new gazebo::msgs::Vector3d();
- angular_velocity->set_x(angular_velocity_I[0]);
- angular_velocity->set_y(angular_velocity_I[1]);
- angular_velocity->set_z(angular_velocity_I[2]);
- imu_message_.set_allocated_angular_velocity(angular_velocity);
-
- // Publish the IMU message
- imu_pub_->Publish(imu_message_);
-
+ if (gazebo_sequence_ % measurement_divisor_ == 0) {
+ gazebo_sequence_ = 0;
+ common::Time current_time = world_->SimTime();
+ double dt = (current_time - last_time_).Double();
+ last_time_ = current_time;
+ double t = current_time.Double();
+
+ ignition::math::Pose3d T_W_I = link_->WorldPose(); // TODO(burrimi): Check tf.
+ ignition::math::Quaterniond C_W_I = T_W_I.Rot();
+
+ ignition::math::Vector3d acceleration_I =
+ link_->RelativeLinearAccel() - C_W_I.RotateVectorReverse(gravity_W_);
+
+ ignition::math::Vector3d angular_vel_I = link_->RelativeAngularVel();
+
+ Eigen::Vector3d linear_acceleration_I(acceleration_I.X(), acceleration_I.Y(),
+ acceleration_I.Z());
+ Eigen::Vector3d angular_velocity_I(angular_vel_I.X(), angular_vel_I.Y(),
+ angular_vel_I.Z());
+
+ AddNoise(&linear_acceleration_I, &angular_velocity_I, dt);
+
+ // Fill IMU message.
+ // imu_message_.header.stamp.sec = current_time.sec;
+ imu_message_.mutable_header()->mutable_stamp()->set_sec(current_time.sec);
+
+ // imu_message_.header.stamp.nsec = current_time.nsec;
+ imu_message_.mutable_header()->mutable_stamp()->set_nsec(current_time.nsec);
+
+ /// \todo(burrimi): Add orientation estimator.
+ // NOTE: rotors_simulator used to set the orientation to "0", since it is
+ // not raw IMU data but rather a calculation (and could confuse users).
+ // However, the orientation is now set as it is used by PX4.
+ /*gazebo::msgs::Quaternion* orientation = new gazebo::msgs::Quaternion();
+ orientation->set_x(0);
+ orientation->set_y(0);
+ orientation->set_z(0);
+ orientation->set_w(1);
+ imu_message_.set_allocated_orientation(orientation);*/
+
+ /// \todo(burrimi): add noise.
+ gazebo::msgs::Quaternion* orientation = new gazebo::msgs::Quaternion();
+ orientation->set_w(C_W_I.W());
+ orientation->set_x(C_W_I.X());
+ orientation->set_y(C_W_I.Y());
+ orientation->set_z(C_W_I.Z());
+ imu_message_.set_allocated_orientation(orientation);
+
+ gazebo::msgs::Vector3d* linear_acceleration = new gazebo::msgs::Vector3d();
+ linear_acceleration->set_x(linear_acceleration_I[0]);
+ linear_acceleration->set_y(linear_acceleration_I[1]);
+ linear_acceleration->set_z(linear_acceleration_I[2]);
+ imu_message_.set_allocated_linear_acceleration(linear_acceleration);
+
+ gazebo::msgs::Vector3d* angular_velocity = new gazebo::msgs::Vector3d();
+ angular_velocity->set_x(angular_velocity_I[0]);
+ angular_velocity->set_y(angular_velocity_I[1]);
+ angular_velocity->set_z(angular_velocity_I[2]);
+ imu_message_.set_allocated_angular_velocity(angular_velocity);
+
+ // Publish the IMU message
+ imu_pub_->Publish(imu_message_);
+ }
// std::cout << "Published IMU message.\n";
+ ++gazebo_sequence_;
}
void GazeboImuPlugin::CreatePubsAndSubs() {