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.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() {use_simulated_actuated_joint_=false;has_simulated_passive_actuated_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 // if a screw_joint is specified, apply torque based on simulated_reduction_ 00080 double simulated_reduction_; 00081 bool use_simulated_actuated_joint_; 00082 bool has_simulated_passive_actuated_joint_; 00083 00084 // The joint_names_ variable is inherited from Transmission. In 00085 // joint_names_, the gap joint is first, followed by all the passive 00086 // joints. 00087 00088 // store name for passive joints. This matches elements 1 to N of joint_names_. 00089 std::vector<std::string> passive_joints_; 00090 00091 private: 00093 void computeGapStates(double MR,double MR_dot,double MT, 00094 double &theta,double &dtheta_dMR,double &dt_dtheta,double &dt_dMR,double &gap_size,double &gap_velocity,double &gap_effort); 00095 void inverseGapStates(double gap_size,double &MR, double &dMR_dtheta,double &dtheta_dt,double &dMR_dt); 00096 void inverseGapStates1(double theta,double &MR, double &dMR_dtheta,double &dtheta_dt,double &dMR_dt); 00097 00098 // 00099 // SOME CONSTANTS 00100 // the default theta0 when gap size is 0 is needed to assign passive joint angles 00101 // 00102 double screw_reduction_; // default for alpha2 = 2.0/1000.0; //in meters/revolution 00103 double gear_ratio_ ; // default for alpha2 = 29.16; // is a ratio 00104 double theta0_ ; // default for alpha2 = 2.97571*M_PI/180.0; // convert to radians 00105 double phi0_ ; // default for alpha2 = 29.98717*M_PI/180.0; // convert to radians 00106 double t0_ ; // default for alpha2 = 0; //-0.19543/1000.0; // convert to meters 00107 double L0_ ; // default for alpha2 = 34.70821/1000.0; // convert to meters 00108 double h_ ; // default for alpha2 = 5.200/1000.0; // convert to meters 00109 double a_ ; // default for alpha2 = 67.56801/1000.0; // convert to meters 00110 double b_ ; // default for alpha2 = 48.97193/1000.0; // convert to meters 00111 double r_ ; // default for alpha2 = 91.50000/1000.0; // convert to meters 00112 00113 #define RAD2MR (1.0/(2.0*M_PI)) // convert radians to motor revolutions 00114 #define TOL 0.00001 // limit for denominators 00115 00116 int simulated_actuator_timestamp_initialized_; 00117 ros::Time simulated_actuator_start_time_; 00118 00119 JointCalibrationSimulator joint_calibration_simulator_; 00120 }; 00121 00122 } // namespace pr2_mechanism_model 00123 00124 #endif