gazebo_ros_hand_of_god.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Open Source Robotics Foundation
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Open Source Robotics Foundation
18  * nor the names of its contributors may be
19  * used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
44 #include <ros/ros.h>
45 
46 namespace gazebo
47 {
48  // Register this plugin with the simulator
49  GZ_REGISTER_MODEL_PLUGIN(GazeboRosHandOfGod);
50 
52  // Constructor
54  ModelPlugin(),
55  robot_namespace_(""),
56  frame_id_("hog"),
57  kl_(200),
58  ka_(200)
59  {
60  }
61 
63  // Destructor
65  {
66  }
67 
69  // Load the controller
70  void GazeboRosHandOfGod::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf )
71  {
72  // Make sure the ROS node for Gazebo has already been initalized
73  if (!ros::isInitialized()) {
74  ROS_FATAL_STREAM_NAMED("hand_of_god", "A ROS node for Gazebo has not been initialized, unable to load plugin. "
75  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
76  return;
77  }
78 
79  // Get sdf parameters
80  if(_sdf->HasElement("robotNamespace")) {
81  this->robot_namespace_ = _sdf->Get<std::string>("robotNamespace") + "/";
82  }
83 
84  if(_sdf->HasElement("frameId")) {
85  this->frame_id_ = _sdf->Get<std::string>("frameId");
86  }
87 
88  if(_sdf->HasElement("kl")) {
89  this->kl_ = _sdf->Get<double>("kl");
90  }
91  if(_sdf->HasElement("ka")) {
92  this->ka_ = _sdf->Get<double>("ka");
93  }
94 
95  if(_sdf->HasElement("linkName")) {
96  this->link_name_ = _sdf->Get<std::string>("linkName");
97  } else {
98  ROS_FATAL_STREAM_NAMED("hand_of_god", "The hand-of-god plugin requires a `linkName` parameter tag");
99  return;
100  }
101 
102  // Store the model
103  model_ = _parent;
104 
105  // Get the floating link
106  floating_link_ = model_->GetLink(link_name_);
107  // Disable gravity for the hog
108  floating_link_->SetGravityMode(false);
109  if(!floating_link_) {
110  ROS_ERROR_NAMED("hand_of_god", "Floating link not found");
111  const std::vector<physics::LinkPtr> &links = model_->GetLinks();
112  for(unsigned i=0; i < links.size(); i++) {
113  ROS_ERROR_STREAM_NAMED("hand_of_god", " -- Link "<<i<<": "<<links[i]->GetName());
114  }
115  return;
116  }
117 
118 #if GAZEBO_MAJOR_VERSION >= 8
119  cl_ = 2.0 * sqrt(kl_*floating_link_->GetInertial()->Mass());
120  ca_ = 2.0 * sqrt(ka_*floating_link_->GetInertial()->IXX());
121 #else
122  cl_ = 2.0 * sqrt(kl_*floating_link_->GetInertial()->GetMass());
123  ca_ = 2.0 * sqrt(ka_*floating_link_->GetInertial()->GetIXX());
124 #endif
125 
126  // Create the TF listener for the desired position of the hog
127  tf_buffer_.reset(new tf2_ros::Buffer());
130 
131  // Register update event handler
132  this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
133  boost::bind(&GazeboRosHandOfGod::GazeboUpdate, this));
134  }
135 
137  // Update the controller
139  {
140  // Get TF transform relative to the /world link
141  geometry_msgs::TransformStamped hog_desired_tform;
142  static bool errored = false;
143  try{
144  hog_desired_tform = tf_buffer_->lookupTransform("world", frame_id_+"_desired", ros::Time(0));
145  errored = false;
146  } catch (tf2::TransformException ex){
147  if(!errored) {
148  ROS_ERROR_NAMED("hand_of_god", "%s",ex.what());
149  errored = true;
150  }
151  return;
152  }
153 
154  // Convert TF transform to Gazebo Pose
155  const geometry_msgs::Vector3 &p = hog_desired_tform.transform.translation;
156  const geometry_msgs::Quaternion &q = hog_desired_tform.transform.rotation;
157  ignition::math::Pose3d hog_desired(
158  ignition::math::Vector3d(p.x, p.y, p.z),
159  ignition::math::Quaterniond(q.w, q.x, q.y, q.z));
160 
161  // Relative transform from actual to desired pose
162 #if GAZEBO_MAJOR_VERSION >= 8
163  ignition::math::Pose3d world_pose = floating_link_->DirtyPose();
164  ignition::math::Vector3d worldLinearVel = floating_link_->WorldLinearVel();
165  ignition::math::Vector3d relativeAngularVel = floating_link_->RelativeAngularVel();
166 #else
167  ignition::math::Pose3d world_pose = floating_link_->GetDirtyPose().Ign();
168  ignition::math::Vector3d worldLinearVel = floating_link_->GetWorldLinearVel().Ign();
169  ignition::math::Vector3d relativeAngularVel = floating_link_->GetRelativeAngularVel().Ign();
170 #endif
171  ignition::math::Vector3d err_pos = hog_desired.Pos() - world_pose.Pos();
172  // Get exponential coordinates for rotation
173  ignition::math::Quaterniond err_rot = (ignition::math::Matrix4d(world_pose.Rot()).Inverse()
174  * ignition::math::Matrix4d(hog_desired.Rot())).Rotation();
175  ignition::math::Quaterniond not_a_quaternion = err_rot.Log();
176 
177  floating_link_->AddForce(
178  kl_ * err_pos - cl_ * worldLinearVel);
179 
180  floating_link_->AddRelativeTorque(
181  ka_ * ignition::math::Vector3d(not_a_quaternion.X(), not_a_quaternion.Y(), not_a_quaternion.Z())
182  - ca_ * relativeAngularVel);
183 
184  // Convert actual pose to TransformStamped message
185  geometry_msgs::TransformStamped hog_actual_tform;
186 
187  hog_actual_tform.header.frame_id = "world";
188  hog_actual_tform.header.stamp = ros::Time::now();
189 
190  hog_actual_tform.child_frame_id = frame_id_ + "_actual";
191 
192  hog_actual_tform.transform.translation.x = world_pose.Pos().X();
193  hog_actual_tform.transform.translation.y = world_pose.Pos().Y();
194  hog_actual_tform.transform.translation.z = world_pose.Pos().Z();
195 
196  hog_actual_tform.transform.rotation.w = world_pose.Rot().W();
197  hog_actual_tform.transform.rotation.x = world_pose.Rot().X();
198  hog_actual_tform.transform.rotation.y = world_pose.Rot().Y();
199  hog_actual_tform.transform.rotation.z = world_pose.Rot().Z();
200 
201  tf_broadcaster_->sendTransform(hog_actual_tform);
202  }
203 
204 }
#define ROS_ERROR_STREAM_NAMED(name, args)
event::ConnectionPtr update_connection_
Pointer to the update event connection.
ROSCPP_DECL bool isInitialized()
GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D)
boost::shared_ptr< tf2_ros::Buffer > tf_buffer_
#define ROS_FATAL_STREAM_NAMED(name, args)
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
Load the controller.
virtual ~GazeboRosHandOfGod()
Destructor.
#define ROS_ERROR_NAMED(name,...)
static Time now()
boost::shared_ptr< tf2_ros::TransformBroadcaster > tf_broadcaster_
boost::shared_ptr< tf2_ros::TransformListener > tf_listener_
virtual void GazeboUpdate()
Update the controller.


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Apr 6 2021 02:19:39