pr2_mechanism_model: pr2_belt_transmission.h Source File

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/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