gazebo_ros_hand_of_god.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Open Source Robotics Foundation
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Open Source Robotics Foundation
00018  *     nor the names of its contributors may be
00019  *     used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00043 #include <gazebo_plugins/gazebo_ros_hand_of_god.h>
00044 #include <ros/ros.h>
00045 
00046 namespace gazebo
00047 {
00048   // Register this plugin with the simulator
00049   GZ_REGISTER_MODEL_PLUGIN(GazeboRosHandOfGod);
00050 
00052   // Constructor
00053   GazeboRosHandOfGod::GazeboRosHandOfGod() :
00054     ModelPlugin(),
00055     robot_namespace_(""),
00056     frame_id_("hog"),
00057     kl_(200),
00058     ka_(200)
00059   {
00060   }
00061 
00063   // Destructor
00064   GazeboRosHandOfGod::~GazeboRosHandOfGod()
00065   {
00066   }
00067 
00069   // Load the controller
00070   void GazeboRosHandOfGod::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf )
00071   {
00072     // Make sure the ROS node for Gazebo has already been initalized
00073     if (!ros::isInitialized()) {
00074       ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00075                        << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00076       return;
00077     }
00078 
00079     // Get sdf parameters
00080     if(_sdf->HasElement("robotNamespace")) {
00081       this->robot_namespace_ = _sdf->Get<std::string>("robotNamespace") + "/";
00082     }  
00083 
00084     if(_sdf->HasElement("frameId")) {
00085       this->frame_id_ = _sdf->Get<std::string>("frameId");
00086     }
00087 
00088     if(_sdf->HasElement("kl")) {
00089       this->kl_ = _sdf->Get<double>("kl");
00090     }
00091     if(_sdf->HasElement("ka")) {
00092       this->ka_ = _sdf->Get<double>("ka");
00093     }
00094 
00095     if(_sdf->HasElement("linkName")) {
00096       this->link_name_ = _sdf->Get<std::string>("linkName");
00097     } else {
00098       ROS_FATAL_STREAM("The hand-of-god plugin requires a `linkName` parameter tag");
00099       return;
00100     }
00101 
00102     // Store the model
00103     model_ = _parent;
00104 
00105     // Disable gravity for the hog
00106     model_->SetGravityMode(false);
00107 
00108     // Get the floating link
00109     floating_link_ = model_->GetLink(link_name_);
00110     if(!floating_link_) {
00111       ROS_ERROR("Floating link not found!");
00112       const std::vector<physics::LinkPtr> &links = model_->GetLinks();
00113       for(unsigned i=0; i < links.size(); i++) {
00114         ROS_ERROR_STREAM(" -- Link "<<i<<": "<<links[i]->GetName());
00115       }
00116       return;
00117     }
00118 
00119     cl_ = 2.0 * sqrt(kl_*floating_link_->GetInertial()->GetMass());
00120     ca_ = 2.0 * sqrt(ka_*floating_link_->GetInertial()->GetIXX());
00121 
00122     // Create the TF listener for the desired position of the hog
00123     tf_buffer_.reset(new tf2_ros::Buffer());
00124     tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_));
00125     tf_broadcaster_.reset(new tf2_ros::TransformBroadcaster());
00126 
00127     // Register update event handler
00128     this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
00129         boost::bind(&GazeboRosHandOfGod::GazeboUpdate, this));
00130   }
00131 
00133   // Update the controller
00134   void GazeboRosHandOfGod::GazeboUpdate()
00135   {
00136     // Get TF transform relative to the /world link
00137     geometry_msgs::TransformStamped hog_desired_tform;
00138     static bool errored = false;
00139     try{
00140       hog_desired_tform = tf_buffer_->lookupTransform("world", frame_id_+"_desired", ros::Time(0));
00141       errored = false;
00142     } catch (tf2::TransformException ex){
00143       if(!errored) {
00144         ROS_ERROR("%s",ex.what());
00145         errored = true;
00146       } 
00147       return;
00148     }
00149 
00150     // Convert TF transform to Gazebo Pose
00151     const geometry_msgs::Vector3 &p = hog_desired_tform.transform.translation;
00152     const geometry_msgs::Quaternion &q = hog_desired_tform.transform.rotation;
00153     gazebo::math::Pose hog_desired(
00154         gazebo::math::Vector3(p.x, p.y, p.z),
00155         gazebo::math::Quaternion(q.w, q.x, q.y, q.z));
00156 
00157     // Relative transform from actual to desired pose
00158     gazebo::math::Pose world_pose = floating_link_->GetDirtyPose();
00159     gazebo::math::Vector3 err_pos = hog_desired.pos - world_pose.pos;
00160     // Get exponential coordinates for rotation
00161     gazebo::math::Quaternion err_rot =  (world_pose.rot.GetAsMatrix4().Inverse() * hog_desired.rot.GetAsMatrix4()).GetRotation();
00162     gazebo::math::Quaternion not_a_quaternion = err_rot.GetLog();
00163 
00164     floating_link_->AddForce(
00165         kl_ * err_pos - cl_ * floating_link_->GetWorldLinearVel());
00166 
00167     floating_link_->AddRelativeTorque(
00168         ka_ * gazebo::math::Vector3(not_a_quaternion.x, not_a_quaternion.y, not_a_quaternion.z) - ca_ * floating_link_->GetRelativeAngularVel());
00169 
00170     // Convert actual pose to TransformStamped message
00171     geometry_msgs::TransformStamped hog_actual_tform;
00172 
00173     hog_actual_tform.header.frame_id = "world";
00174     hog_actual_tform.header.stamp = ros::Time::now();
00175 
00176     hog_actual_tform.child_frame_id = frame_id_ + "_actual";
00177 
00178     hog_actual_tform.transform.translation.x = world_pose.pos.x;
00179     hog_actual_tform.transform.translation.y = world_pose.pos.y;
00180     hog_actual_tform.transform.translation.z = world_pose.pos.z;
00181 
00182     hog_actual_tform.transform.rotation.w = world_pose.rot.w;
00183     hog_actual_tform.transform.rotation.x = world_pose.rot.x;
00184     hog_actual_tform.transform.rotation.y = world_pose.rot.y;
00185     hog_actual_tform.transform.rotation.z = world_pose.rot.z;
00186     
00187     tf_broadcaster_->sendTransform(hog_actual_tform);
00188   }
00189 
00190 }


gazebo_plugins
Author(s): John Hsu
autogenerated on Fri Aug 28 2015 10:47:25