Skip to content

Hack/force sensor #686

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 17 commits into
base: master
Choose a base branch
from
Open

Hack/force sensor #686

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
17 commits
Select commit Hold shift + click to select a range
afd0b52
Merge branch 'feature/pmd_depth_cam'
karenbodie Dec 2, 2019
c49270c
add force sensor plugin
karenbodie Dec 3, 2019
966062b
Remove extra bracket
karenbodie Dec 4, 2019
ed1e6c5
better d gains in tiltrotor
karenbodie Dec 8, 2019
df955c4
big hack for force sensor testing
karenbodie Dec 8, 2019
247292b
Add measurement divisor to IMU plugin.
karenbodie Jan 22, 2020
01344b1
Parametrize pid tilt gains.
karenbodie Jan 23, 2020
547415a
Mini-Voliro simulation rear rotor changes
May 20, 2021
46e55ef
Builds with OpenCV4
May 21, 2020
d2eba6a
Merge pull request #681 from ethz-asl/fix/hack/force_sensor/opencv
rikba Oct 11, 2021
728407a
Revert "Mini-Voliro simulation rear rotor changes". Take out this com…
maxb91 Nov 18, 2021
67694ce
Merge branch 'master' into hack/force_sensor
maxb91 Nov 18, 2021
c9e0b23
update hack, according to the current ft sensor pose wrt. body
grzemal Mar 11, 2022
76cb7b7
delete obsolete, enable running the simulation
grzemal Mar 11, 2022
cf08489
Removed hard coded tilt-motor number for single-tilt motors
clanegge Oct 9, 2022
1a7b888
realistic mav mass
mikeallenspach Dec 14, 2022
5ad9319
Merge branch 'hack/force_sensor' of github.com:ethz-asl/rotors_simula…
mikeallenspach Dec 14, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
big hack for force sensor testing
  • Loading branch information
@karenbodie
karenbodie committed Dec 8, 2019
commit df955c4305033c947b3ac05faa023eb6e01a01cb
3 changes: 3 additions & 0 deletions rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_force_sensor_plugin.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,7 @@
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";
Expand Down Expand Up @@ -120,6 +121,7 @@ class GazeboForceSensorPlugin : public ModelPlugin {
GazeboForceSensorPlugin()
: ModelPlugin(),
random_generator_(random_device_()),
publish_frame_id_(kDefaultPublishFrameId),
parent_frame_id_(kDefaultParentFrameId),
reference_frame_id_(kDefaultReferenceFrameId),
link_name_(kDefaultLinkName),
Expand Down Expand Up @@ -158,6 +160,7 @@ class GazeboForceSensorPlugin : public ModelPlugin {
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_;
Expand Down
13 changes: 7 additions & 6 deletions rotors_gazebo_plugins/src/gazebo_force_sensor_plugin.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,7 @@ void GazeboForceSensorPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sd
getSdfParam<std::string>(_sdf, "forceSensorTopic", force_sensor_pub_topic_, force_sensor_pub_topic_);
getSdfParam<std::string>(_sdf, "forceSensorTruthTopic", force_sensor_truth_pub_topic_, force_sensor_truth_pub_topic_);
getSdfParam<std::string>(_sdf, "wrenchVectorTopic", wrench_vector_pub_topic_, wrench_vector_pub_topic_);
getSdfParam<std::string>(_sdf, "publishFrameId", publish_frame_id_, publish_frame_id_);
getSdfParam<std::string>(_sdf, "parentFrameId", parent_frame_id_, parent_frame_id_);
getSdfParam<std::string>(_sdf, "referenceFrameId", reference_frame_id_, reference_frame_id_);
getSdfParam<ignition::math::Vector3d>(_sdf, "noiseNormalLinearForce", noise_normal_linear_force, zeros3);
Expand Down Expand Up @@ -159,7 +160,6 @@ void GazeboForceSensorPlugin::OnUpdate(const common::UpdateInfo& _info) {
ignition::math::Pose3d W_pose_W_R = ignition::math::Pose3d::Zero;

if (parent_frame_id_ != kDefaultParentFrameId) {
// W_pose_W_P = parent_link_->WorldCoGPose();
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;
Expand Down Expand Up @@ -207,17 +207,18 @@ void GazeboForceSensorPlugin::OnUpdate(const common::UpdateInfo& _info) {

if (gazebo_sequence_ % measurement_divisor_ == 0) {
// Copy data into wrench message.
wrench_msg_.header.frame_id = parent_frame_id_;
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;

wrench_msg_.wrench.force.x = force[0];
// HUUUUUUGE HACK.
wrench_msg_.wrench.force.x = -force[2];
wrench_msg_.wrench.force.y = force[1];
wrench_msg_.wrench.force.z = force[2];
wrench_msg_.wrench.torque.x = torque[0];
wrench_msg_.wrench.force.z = force[0];
wrench_msg_.wrench.torque.x = -torque[2];
wrench_msg_.wrench.torque.y = torque[1];
wrench_msg_.wrench.torque.z = 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_));
Expand Down