pr2_examples_gazebo: pr2_examples_gazebo: spring

00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
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 Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 /*
00035  * Author: Stuart Glaser
00036  */
00037 #include <math.h>
00038 #include <pluginlib/class_list_macros.h>
00039 #include "pr2_mechanism_model/robot.h"
00040 #include "pr2_examples_gazebo/spring_transmission.h"
00041 #include <angles/angles.h>
00042 
00043 using namespace pr2_examples_gazebo;
00044 using namespace pr2_hardware_interface;
00045 
00046 PLUGINLIB_DECLARE_CLASS(pr2_examples_gazebo, SpringTransmission, 
00047                         pr2_examples_gazebo::SpringTransmission, 
00048                         pr2_mechanism_model::Transmission)
00049 
00050 
00051 bool SpringTransmission::initXml(TiXmlElement *elt, pr2_mechanism_model::Robot *robot)
00052 {
00053   const char *name = elt->Attribute("name");
00054   name_ = name ? name : "";
00055 
00056   TiXmlElement *jel = elt->FirstChildElement("joint");
00057   const char *joint_name = jel ? jel->Attribute("name") : NULL;
00058   if (!joint_name)
00059   {
00060     ROS_ERROR("SpringTransmission did not specify joint name");
00061     return false;
00062   }
00063 
00064   const boost::shared_ptr<const urdf::Joint> joint = robot->robot_model_.getJoint(joint_name);
00065   if (!joint)
00066   {
00067     ROS_ERROR("SpringTransmission could not find joint named \"%s\"", joint_name);
00068     return false;
00069   }
00070   joint_names_.push_back(joint_name);
00071 
00072 
00073 
00074   TiXmlElement *ael = elt->FirstChildElement("actuator");
00075   const char *actuator_name = ael ? ael->Attribute("name") : NULL;
00076   Actuator *a;
00077   if (!actuator_name || (a = robot->getActuator(actuator_name)) == NULL )
00078   {
00079     ROS_ERROR("SpringTransmission could not find actuator named \"%s\"", actuator_name);
00080     return false;
00081   }
00082   a->command_.enable_ = true;
00083   actuator_names_.push_back(actuator_name);
00084 
00085 
00086 
00087 
00088   spring_stiffness_ = atof(elt->FirstChildElement("springStiffness")->GetText());
00089 
00090   return true;
00091 }
00092 
00093 void SpringTransmission::propagatePosition(
00094    std::vector<Actuator*>& as, std::vector<pr2_mechanism_model::JointState*>& js)
00095 {
00096   // propagate position to joint
00097   assert(as.size() == 1);
00098   assert(js.size() == 1);
00099   js[0]->position_ = as[0]->state_.position_;
00100   js[0]->velocity_ = as[0]->state_.velocity_;
00101   js[0]->measured_effort_ = as[0]->state_.last_measured_effort_;
00102 }
00103 
00104 void SpringTransmission::propagatePositionBackwards(
00105   std::vector<pr2_mechanism_model::JointState*>& js, std::vector<Actuator*>& as)
00106 {
00107   assert(as.size() == 1);
00108   assert(js.size() == 1);
00109   as[0]->state_.position_ = js[0]->position_;
00110   as[0]->state_.velocity_ = js[0]->velocity_;
00111   as[0]->state_.last_measured_effort_ = js[0]->measured_effort_;
00112 }
00113 
00114 void SpringTransmission::propagateEffort(
00115    std::vector<pr2_mechanism_model::JointState*>& js, std::vector<Actuator*>& as)
00116 {
00117   assert(as.size() == 1);
00118   assert(js.size() == 1);
00119   as[0]->command_.effort_ = -spring_stiffness_ * js[0]->position_;
00120 }
00121 
00122 void SpringTransmission::propagateEffortBackwards(
00123   std::vector<Actuator*>& as, std::vector<pr2_mechanism_model::JointState*>& js)
00124 {
00125   assert(as.size() == 1);
00126   assert(js.size() == 1);
00127   js[0]->commanded_effort_ = as[0]->command_.effort_;
00128 }
00129
spring_transmission.cpp
