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="PR2GripperTransmission" 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_mechanism_model/joint_calibration_simulator.h" 00052 00053 namespace pr2_mechanism_model { 00054 00055 class PR2GripperTransmission : public Transmission 00056 { 00057 public: 00058 PR2GripperTransmission() {default_passive_joint_index_from_sim=1;use_simulated_gripper_joint=false;} 00059 virtual ~PR2GripperTransmission() {/*myfile.close();*/} 00060 00061 bool initXml(TiXmlElement *config, Robot *robot); 00062 bool initXml(TiXmlElement *config); 00063 00064 void propagatePosition(std::vector<pr2_hardware_interface::Actuator*>&, 00065 std::vector<pr2_mechanism_model::JointState*>&); 00066 void propagatePositionBackwards(std::vector<pr2_mechanism_model::JointState*>&, 00067 std::vector<pr2_hardware_interface::Actuator*>&); 00068 void propagateEffort(std::vector<pr2_mechanism_model::JointState*>&, 00069 std::vector<pr2_hardware_interface::Actuator*>&); 00070 void propagateEffortBackwards(std::vector<pr2_hardware_interface::Actuator*>&, 00071 std::vector<pr2_mechanism_model::JointState*>&); 00072 std::string gap_joint_; 00073 // 00074 // per Functions Engineering doc, 090224_link_data.xml, 00075 // here, gap_mechanical_reduction_ transforms FROM ENCODER VALUE TO MOTOR REVOLUTIONS 00076 // 00077 double gap_mechanical_reduction_; 00078 00079 // The joint_names_ variable is inherited from Transmission. In 00080 // joint_names_, the gap joint is first, followed by all the passive 00081 // joints. 00082 00083 // store name for passive joints. This matches elements 1 to N of joint_names_. 00084 std::vector<std::string> passive_joints_; 00085 00086 private: 00088 bool use_simulated_gripper_joint; 00089 00091 void computeGapStates(double MR,double MR_dot,double MT, 00092 double &theta,double &dtheta_dMR,double &dt_dtheta,double &dt_dMR,double &gap_size,double &gap_velocity,double &gap_effort); 00093 void inverseGapStates(double theta,double &MR, double &dMR_dtheta,double &dtheta_dt,double &dMR_dt); 00094 //std::ofstream myfile; 00095 void getRateFromMaxRateJoint(std::vector<pr2_mechanism_model::JointState*>& js, 00096 std::vector<pr2_hardware_interface::Actuator*>& as, 00097 int &maxRateJointIndex,double &rate); 00098 00099 // use the same passive joint for determining gipper position, so forward/backward are consistent 00100 int default_passive_joint_index_from_sim; 00101 00102 // 00103 // SOME CONSTANTS 00104 // the default theta0 when gap size is 0 is needed to assign passive joint angles 00105 // 00106 double screw_reduction_; // default for alpha2 = 2.0/1000.0; //in meters/revolution 00107 double gear_ratio_ ; // default for alpha2 = 29.16; // is a ratio 00108 double theta0_ ; // default for alpha2 = 2.97571*M_PI/180.0; // convert to radians 00109 double phi0_ ; // default for alpha2 = 29.98717*M_PI/180.0; // convert to radians 00110 double t0_ ; // default for alpha2 = 0; //-0.19543/1000.0; // convert to meters 00111 double L0_ ; // default for alpha2 = 34.70821/1000.0; // convert to meters 00112 double h_ ; // default for alpha2 = 5.200/1000.0; // convert to meters 00113 double a_ ; // default for alpha2 = 67.56801/1000.0; // convert to meters 00114 double b_ ; // default for alpha2 = 48.97193/1000.0; // convert to meters 00115 double r_ ; // default for alpha2 = 91.50000/1000.0; // convert to meters 00116 00117 #define RAD2MR (1.0/(2.0*M_PI)) // convert radians to motor revolutions 00118 #define TOL 0.00001 // limit for denominators 00119 00120 int simulated_actuator_timestamp_initialized_; 00121 ros::Time simulated_actuator_start_time_; 00122 00123 JointCalibrationSimulator joint_calibration_simulator_; 00124 }; 00125 00126 } // namespace pr2_mechanism_model 00127 00128 #endif