00001 00006 #ifndef JOINTCOMMANDWRIST_H 00007 #define JOINTCOMMANDWRIST_H 00008 00009 #include "robodyn_mechanisms/JointCommandInterface.h" 00010 #include "robot_instance/StringUtilities.h" 00011 #include "robodyn_mechanisms/WristMechanism.h" 00012 #include "robodyn_mechanisms/HallsToAngle.h" 00013 #include "robodyn_utilities/MultiLoopController.h" 00014 #include <memory> 00015 #include <boost/ptr_container/ptr_array.hpp> 00016 #include "EmbeddedSmoother.h" 00017 00018 /***************************************************************************/ 00024 class JointCommandWrist : public JointCommandInterface 00025 { 00026 public: 00027 JointCommandWrist(const std::string& mechanism, IoFunctions ioFunctions); 00028 virtual ~JointCommandWrist(); 00029 00030 sensor_msgs::JointState getSimpleMeasuredState(); 00031 sensor_msgs::JointState getCompleteMeasuredState(); 00032 r2_msgs::JointCommand getCommandedState(); 00033 void setCommand(r2_msgs::JointCommand msg, r2_msgs::JointControlData control); 00034 void updateMeasuredState(r2_msgs::JointControlData msg); 00035 void setFaultState(); 00036 00037 r2_msgs::JointCapability getCapability(); 00038 void loadCoeffs(); 00039 00040 protected: 00041 std::auto_ptr<WristMechanism> wrist; 00042 std::auto_ptr<WristMechanism> wristFault; 00043 std::auto_ptr<MultiLoopController> q1Controller; 00044 std::auto_ptr<MultiLoopController> q2Controller; 00045 00046 std::string angleRosJointName; 00047 std::string motorRosJointName; 00048 std::string encoderRosJointName; 00049 std::string sliderRosJointName; 00050 std::string hallsRosJointName; 00051 00052 std::string encoderLittlePositionStatusElement; 00053 std::string encoderThumbPositionStatusElement; 00054 std::string hallsPitchStatusElement; 00055 std::string hallsYawStatusElement; 00056 00057 double desiredQ1; 00058 double desiredQ2; 00059 double desiredPitch; 00060 double desiredYaw; 00061 double desiredPitchVel; 00062 double desiredYawVel; 00063 00064 Eigen::Vector2d desiredSlider; 00065 Eigen::Vector2d desiredAngle; 00066 00067 std::string desiredMotComLittleCommandElement; 00068 std::string desiredMotComThumbCommandElement; 00069 00070 double busVoltage; 00071 00072 Eigen::Vector2d sliderVel; 00073 Eigen::Vector2d ang; 00074 Eigen::Vector2d angVel; 00075 Eigen::Vector2d encoder; 00076 Eigen::Vector2d encoderVel; 00077 Eigen::Vector2d halls; 00078 Eigen::Vector2d hallsVel; 00079 00080 double sliderMaxPosition; 00081 double sliderMinPosition; 00082 double maxSliderDiff; 00083 00084 double sliderMaxPositionFault; 00085 double sliderMinPositionFault; 00086 double maxSliderDiffFault; 00087 double maxSensorErrorFault; 00088 00089 // filter values 00090 Eigen::Vector2d filteredSlider; 00091 Eigen::Vector2d filteredHalls; 00092 std::vector<double> prevPositionVals; 00093 double timestep; 00094 double positionAlpha; 00095 bool sliderFilterInitialized; 00096 bool hallFilterInitialized; 00097 bool prevPosInitialized; 00098 00099 Eigen::Vector2d manualCalSliderPos; 00100 Eigen::Vector2d manualCalAng; 00101 00102 std::string namePitchLimit; 00103 std::string nameYawLimit; 00104 std::string nameLittlesideSliderLimit; 00105 std::string nameThumbsideSliderLimit; 00106 std::string nameSensorError; 00107 std::string nameSliderDiffError; 00108 std::string nameCalibrationMode; 00109 std::string nameCoeffState; 00110 00111 std::string namePitchMaxValue; 00112 std::string namePitchMinValue; 00113 std::string nameYawMaxValue; 00114 std::string nameYawMinValue; 00115 std::string namePitchHallsValue; 00116 std::string namePitchCalcValue; 00117 std::string nameYawHallsValue; 00118 std::string nameYawCalcValue; 00119 00120 // parameters 00121 std::string nameUseHalls; 00122 std::string nameManualCalThumbSlider; 00123 std::string nameManualCalLittleSlider; 00124 std::string nameManualCalPitch; 00125 std::string nameManualCalYaw; 00126 00127 bool useHalls; 00128 00129 std::vector<HallsToAngle> hta; 00130 00131 bool fwdKinFault; 00132 00133 // util 00134 std::vector<std::string>::iterator strVecIt; 00135 bool calFailureReported; 00136 bool calCalled; 00137 r2_msgs::JointControlCalibrationMode calibrationState; 00138 00139 std::vector<double> positionVals; 00140 std::vector<double> velocityVals; 00141 00142 std::auto_ptr<EmbeddedSmoother> smootherPitch; 00143 std::auto_ptr<EmbeddedSmoother> smootherYaw; 00144 double smoothedPitch; 00145 double smoothedYaw; 00146 Eigen::Vector2d sliderVelCommand; 00147 00148 unsigned int completeMessageSize; 00149 }; 00150 00151 #endif // JOINTCOMMANDWRIST_H