00001 /* 00002 * Copyright (c) 2009, 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 #ifndef PR2_BELT_COMPENSATOR_TRANSMISSION_H 00031 #define PR2_BELT_COMPENSATOR_TRANSMISSION_H 00032 00033 #include <tinyxml.h> 00034 00035 #include <pr2_hardware_interface/hardware_interface.h> 00036 #include <pr2_mechanism_model/joint.h> 00037 #include <pr2_mechanism_model/robot.h> 00038 #include <pr2_mechanism_model/transmission.h> 00039 #include "pr2_mechanism_model/joint_calibration_simulator.h" 00040 00041 namespace pr2_mechanism_model { 00042 00047 class PR2BeltCompensatorTransmission : public Transmission 00048 { 00049 public: 00050 PR2BeltCompensatorTransmission() {} 00051 ~PR2BeltCompensatorTransmission() {} 00052 00053 bool initXml(TiXmlElement *config, Robot *robot); 00054 bool initXml(TiXmlElement *config); 00055 00056 void propagatePosition(std::vector<pr2_hardware_interface::Actuator*>&, 00057 std::vector<pr2_mechanism_model::JointState*>&); 00058 void propagatePositionBackwards(std::vector<pr2_mechanism_model::JointState*>&, 00059 std::vector<pr2_hardware_interface::Actuator*>&); 00060 void propagateEffort(std::vector<pr2_mechanism_model::JointState*>&, 00061 std::vector<pr2_hardware_interface::Actuator*>&); 00062 void propagateEffortBackwards(std::vector<pr2_hardware_interface::Actuator*>&, 00063 std::vector<pr2_mechanism_model::JointState*>&); 00064 00065 private: 00066 ros::Duration last_timestamp_; 00067 double dt; 00068 00069 // Transmission parameters 00070 double mechanical_reduction_; 00071 double trans_compl_; // Transmission compliance 00072 double trans_tau_; // Transmission time constant 00073 double Kd_motor_; // Motor damping gain 00074 double lambda_motor_; // Damping cutoff bandwidth 00075 double lambda_joint_; // Joint estimate rolloff bandwidth 00076 double lambda_combo_; // Estimate combination crossover bandwidth 00077 00078 double last_motor_pos_; 00079 double last_motor_vel_; 00080 00081 double last_jnt1_pos_; 00082 double last_jnt1_vel_; 00083 double last_jnt1_acc_; 00084 00085 double last_defl_pos_; 00086 double last_defl_vel_; 00087 double last_defl_acc_; 00088 00089 double last_joint_pos_; 00090 double last_joint_vel_; 00091 00092 double delta_motor_vel_; 00093 double last_motor_damping_force_; 00094 00095 00096 // Backward transmission states 00097 ros::Duration last_timestamp_backwards_; 00098 double halfdt_backwards_; 00099 00100 double motor_force_backwards_; 00101 00102 double last_motor_pos_backwards_; 00103 double last_motor_vel_backwards_; 00104 double last_motor_acc_backwards_; 00105 00106 double last_joint_pos_backwards_; 00107 double last_joint_vel_backwards_; 00108 00109 int simulated_actuator_timestamp_initialized_; 00110 ros::Time simulated_actuator_start_time_; 00111 00112 00113 JointCalibrationSimulator joint_calibration_simulator_; 00114 }; 00115 00116 } // namespace 00117 00118 #endif