/opt/ros/diamondback/stacks/wg_robots_gazebo/pr2_bringup_gazebo_demo/include/pr2_bringup_gazebo_demo/pr2_gripper_transmission.h
Go to the documentation of this file.00001 /* 00002 * Copyright (c) 2008, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 /* 00031 * <transmission type="PR2GripperTransmissionCal" name="gripper_l_transmission"> 00032 * <actuator name="l_gripper_motor" /> 00033 * <gap_joint name="l_gripper_joint" mechanical_reduction="1.0" A="0.05" B="1.0" C="0.0" /> 00034 * <passive_joint name="l_gripper_l_finger_joint" /> 00035 * <passive_joint name="l_gripper_r_finger_joint" /> 00036 * <passive_joint name="l_gripper_r_finger_tip_joint" /> 00037 * <passive_joint name="l_gripper_l_finger_tip_joint" /> 00038 * </transmission> 00039 * 00040 * Author: John Hsu 00041 */ 00042 00043 #ifndef GRIPPER_TRANSMISSION_H 00044 #define GRIPPER_TRANSMISSION_H 00045 00046 #include <vector> 00047 #include "tinyxml/tinyxml.h" 00048 #include "pr2_mechanism_model/transmission.h" 00049 #include "pr2_mechanism_model/robot.h" 00050 //#include <fstream> 00051 #include "pr2_bringup_gazebo_demo/joint_calibration_simulator.h" 00052 00053 namespace pr2_bringup_gazebo_demo { 00054 00055 class PR2GripperTransmissionCal : public pr2_mechanism_model::Transmission 00056 { 00057 public: 00058 PR2GripperTransmissionCal() {default_passive_joint_index_from_sim=1;use_simulated_gripper_joint=false;} 00059 virtual ~PR2GripperTransmissionCal() {/*myfile.close();*/} 00060 00061 bool initXml(TiXmlElement *config, pr2_mechanism_model::Robot *robot); 00062 00063 void propagatePosition(std::vector<pr2_hardware_interface::Actuator*>&, 00064 std::vector<pr2_mechanism_model::JointState*>&); 00065 void propagatePositionBackwards(std::vector<pr2_mechanism_model::JointState*>&, 00066 std::vector<pr2_hardware_interface::Actuator*>&); 00067 void propagateEffort(std::vector<pr2_mechanism_model::JointState*>&, 00068 std::vector<pr2_hardware_interface::Actuator*>&); 00069 void propagateEffortBackwards(std::vector<pr2_hardware_interface::Actuator*>&, 00070 std::vector<pr2_mechanism_model::JointState*>&); 00071 std::string gap_joint_; 00072 // 00073 // per Functions Engineering doc, 090224_link_data.xml, 00074 // here, gap_mechanical_reduction_ transforms FROM ENCODER VALUE TO MOTOR REVOLUTIONS 00075 // 00076 double gap_mechanical_reduction_; 00077 00078 // The joint_names_ variable is inherited from Transmission. In 00079 // joint_names_, the gap joint is first, followed by all the passive 00080 // joints. 00081 00082 // store name for passive joints. This matches elements 1 to N of joint_names_. 00083 std::vector<std::string> passive_joints_; 00084 00085 private: 00087 bool use_simulated_gripper_joint; 00088 00090 void computeGapStates(double MR,double MR_dot,double MT, 00091 double &theta,double &dtheta_dMR,double &dt_dtheta,double &dt_dMR,double &gap_size,double &gap_velocity,double &gap_effort); 00092 void inverseGapStates(double theta,double &MR, double &dMR_dtheta,double &dtheta_dt,double &dMR_dt); 00093 //std::ofstream myfile; 00094 void getRateFromMaxRateJoint(std::vector<pr2_mechanism_model::JointState*>& js, 00095 std::vector<pr2_hardware_interface::Actuator*>& as, 00096 int &maxRateJointIndex,double &rate); 00097 00098 // use the same passive joint for determining gipper position, so forward/backward are consistent 00099 int default_passive_joint_index_from_sim; 00100 00101 // 00102 // SOME CONSTANTS 00103 // the default theta0 when gap size is 0 is needed to assign passive joint angles 00104 // 00105 double screw_reduction_; // default for alpha2 = 2.0/1000.0; //in meters/revolution 00106 double gear_ratio_ ; // default for alpha2 = 29.16; // is a ratio 00107 double theta0_ ; // default for alpha2 = 2.97571*M_PI/180.0; // convert to radians 00108 double phi0_ ; // default for alpha2 = 29.98717*M_PI/180.0; // convert to radians 00109 double t0_ ; // default for alpha2 = 0; //-0.19543/1000.0; // convert to meters 00110 double L0_ ; // default for alpha2 = 34.70821/1000.0; // convert to meters 00111 double h_ ; // default for alpha2 = 5.200/1000.0; // convert to meters 00112 double a_ ; // default for alpha2 = 67.56801/1000.0; // convert to meters 00113 double b_ ; // default for alpha2 = 48.97193/1000.0; // convert to meters 00114 double r_ ; // default for alpha2 = 91.50000/1000.0; // convert to meters 00115 00116 #define RAD2MR (1.0/(2.0*M_PI)) // convert radians to motor revolutions 00117 #define TOL 0.00001 // limit for denominators 00118 00119 JointCalibrationSimulator joint_calibration_simulator_; 00120 }; 00121 00122 } // namespace pr2_mechanism_model 00123 00124 #endif