$search
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 #include <gtest/gtest.h> 00031 #include <vector> 00032 #include "pr2_mechanism_model/wrist_transmission.h" 00033 #include "pr2_hardware_interface/hardware_interface.h" 00034 00035 using namespace pr2_mechanism_model; 00036 using namespace pr2_hardware_interface; 00037 using namespace std; 00038 00039 static const double eps = 1e-6; 00040 00041 class BaseFixture : public testing::Test 00042 { 00043 public: 00044 WristTransmission wrist_; 00045 std::vector<Actuator*> actuators_; 00046 std::vector<JointState*> joint_states_; 00047 00048 protected: 00049 //virtual void SetUp() 00050 BaseFixture() 00051 { 00052 actuators_.resize(2); 00053 actuators_[0] = new Actuator(); 00054 actuators_[1] = new Actuator(); 00055 00056 joint_states_.resize(2); 00057 joint_states_[0] = new JointState(); 00058 joint_states_[1] = new JointState(); 00059 00060 boost::shared_ptr<const urdf::Joint> joint(new urdf::Joint()); 00061 joint_states_[0]->joint_ = joint; 00062 joint_states_[1]->joint_ = joint; 00063 00064 } 00065 00066 virtual void TearDown() 00067 { 00068 delete actuators_[0]; 00069 delete actuators_[1]; 00070 delete joint_states_[0]; 00071 delete joint_states_[1]; 00072 } 00073 }; 00074 00075 // ******* Sets all the gearings to 1. This is a very basic test ******* 00076 class EasyNoGearingTest : public BaseFixture 00077 { 00078 virtual void SetUp() 00079 { 00080 vector<double> ar(2); 00081 ar[0] = 1.0; 00082 ar[1] = 1.0; 00083 00084 vector<double> jr(2); 00085 jr[0] = 1.0; 00086 jr[1] = 1.0; 00087 00088 wrist_.setReductions(ar, jr); 00089 } 00090 }; 00091 00092 TEST_F(EasyNoGearingTest, ForwardPositionTest1) 00093 { 00094 actuators_[0]->state_.position_ = 1.0; 00095 actuators_[1]->state_.position_ = 1.0; 00096 wrist_.propagatePosition(actuators_, joint_states_); 00097 EXPECT_NEAR(joint_states_[0]->position_, 0.0, eps); 00098 EXPECT_NEAR(joint_states_[1]->position_, -1.0, eps); 00099 } 00100 00101 TEST_F(EasyNoGearingTest, ForwardVelocityTest1) 00102 { 00103 actuators_[0]->state_.velocity_ = 1.0; 00104 actuators_[1]->state_.velocity_ =-1.0; 00105 wrist_.propagatePosition(actuators_, joint_states_); 00106 EXPECT_NEAR(joint_states_[0]->velocity_, 1.0, eps); 00107 EXPECT_NEAR(joint_states_[1]->velocity_, 0.0, eps); 00108 } 00109 00110 TEST_F(EasyNoGearingTest, ForwardMeasuredEffortTest1) 00111 { 00112 actuators_[0]->state_.last_measured_effort_ = 1.0; 00113 actuators_[1]->state_.last_measured_effort_ = -1.0; 00114 wrist_.propagatePosition(actuators_, joint_states_); 00115 EXPECT_NEAR(joint_states_[0]->measured_effort_, 2.0, eps); 00116 EXPECT_NEAR(joint_states_[1]->measured_effort_, 0.0, eps); 00117 } 00118 00119 // ******* Check joint gearing ******* 00120 class EasyJointGearingTest : public BaseFixture 00121 { 00122 virtual void SetUp() 00123 { 00124 vector<double> ar(2); 00125 ar[0] = 1.0; 00126 ar[1] = 1.0; 00127 00128 vector<double> jr(2); 00129 jr[0] = 10.0; 00130 jr[1] = 100.0; 00131 00132 wrist_.setReductions(ar, jr); 00133 } 00134 }; 00135 00136 TEST_F(EasyJointGearingTest, ForwardPositionTest1) 00137 { 00138 actuators_[0]->state_.position_ = 3.0; 00139 actuators_[1]->state_.position_ = 1.0; 00140 wrist_.propagatePosition(actuators_, joint_states_); 00141 EXPECT_NEAR(joint_states_[0]->position_, 0.10, eps); 00142 EXPECT_NEAR(joint_states_[1]->position_, -0.02, eps); 00143 } 00144 00145 // ******* Check motor gearing ******* 00146 class EasyMotorGearingTest : public BaseFixture 00147 { 00148 virtual void SetUp() 00149 { 00150 vector<double> ar(2); 00151 ar[0] = 10.0; 00152 ar[1] = 100.0; 00153 00154 vector<double> jr(2); 00155 jr[0] = 1.0; 00156 jr[1] = 1.0; 00157 00158 wrist_.setReductions(ar, jr); 00159 } 00160 }; 00161 00162 TEST_F(EasyMotorGearingTest, ForwardPositionTest1) 00163 { 00164 actuators_[0]->state_.position_ = 100.0; 00165 actuators_[1]->state_.position_ = 0.0; 00166 wrist_.propagatePosition(actuators_, joint_states_); 00167 EXPECT_NEAR(joint_states_[0]->position_, 5, eps); 00168 EXPECT_NEAR(joint_states_[1]->position_, -5, eps); 00169 } 00170 00171 TEST_F(EasyMotorGearingTest, ForwardPositionTest2) 00172 { 00173 actuators_[0]->state_.position_ = 0.0; 00174 actuators_[1]->state_.position_ = 100.0; 00175 wrist_.propagatePosition(actuators_, joint_states_); 00176 EXPECT_NEAR(joint_states_[0]->position_, -0.5, eps); 00177 EXPECT_NEAR(joint_states_[1]->position_, -0.5, eps); 00178 } 00179 00180 // ******* Check both directions ******* 00181 class PropagateSanityCheck : public BaseFixture 00182 { 00183 public: 00184 std::vector<Actuator*> actuators2_; 00185 00186 PropagateSanityCheck() 00187 { 00188 actuators2_.resize(2); 00189 actuators2_[0] = new Actuator(); 00190 actuators2_[1] = new Actuator(); 00191 } 00192 00193 ~PropagateSanityCheck() 00194 { 00195 delete actuators2_[0]; 00196 delete actuators2_[1]; 00197 } 00198 00199 virtual void SetUp() 00200 { 00201 vector<double> ar(2); 00202 ar[0] = 10.0; 00203 ar[1] = 100.0; 00204 00205 vector<double> jr(2); 00206 jr[0] = 20.0; 00207 jr[1] = 40.0; 00208 00209 wrist_.setReductions(ar, jr); 00210 00211 actuators_[0]->state_.velocity_ = 3.0; 00212 actuators_[1]->state_.velocity_ = 4.0; 00213 actuators_[0]->state_.last_measured_effort_ = 5.0; 00214 actuators_[1]->state_.last_measured_effort_ = 6.0; 00215 } 00216 }; 00217 00218 TEST_F(PropagateSanityCheck, Position) 00219 { 00220 actuators_[0]->state_.position_ = 1.0; 00221 actuators_[1]->state_.position_ = 2.0; 00222 wrist_.propagatePosition(actuators_, joint_states_); 00223 wrist_.propagatePositionBackwards(joint_states_, actuators2_); 00224 EXPECT_NEAR(actuators2_[0]->state_.position_, actuators_[0]->state_.position_, eps); 00225 EXPECT_NEAR(actuators2_[1]->state_.position_, actuators_[1]->state_.position_, eps); 00226 } 00227 00228 TEST_F(PropagateSanityCheck, Velocity) 00229 { 00230 actuators_[0]->state_.velocity_ = 1.0; 00231 actuators_[1]->state_.velocity_ = 2.0; 00232 wrist_.propagatePosition(actuators_, joint_states_); 00233 wrist_.propagatePositionBackwards(joint_states_, actuators2_); 00234 EXPECT_NEAR(actuators2_[0]->state_.velocity_, actuators_[0]->state_.velocity_, eps); 00235 EXPECT_NEAR(actuators2_[1]->state_.velocity_, actuators_[1]->state_.velocity_, eps); 00236 } 00237 00238 TEST_F(PropagateSanityCheck, MeasuredEffort) 00239 { 00240 actuators_[0]->state_.last_measured_effort_ = 1.0; 00241 actuators_[1]->state_.last_measured_effort_ = 2.0; 00242 wrist_.propagatePosition(actuators_, joint_states_); 00243 wrist_.propagatePositionBackwards(joint_states_, actuators2_); 00244 EXPECT_NEAR(actuators2_[0]->state_.last_measured_effort_, actuators_[0]->state_.last_measured_effort_, eps); 00245 EXPECT_NEAR(actuators2_[1]->state_.last_measured_effort_, actuators_[1]->state_.last_measured_effort_, eps); 00246 } 00247 00248 TEST_F(PropagateSanityCheck, CommandEffort) 00249 { 00250 actuators_[0]->command_.effort_ = 1.0; 00251 actuators_[1]->command_.effort_ = 2.0; 00252 wrist_.propagateEffortBackwards(actuators_, joint_states_); 00253 wrist_.propagateEffort(joint_states_, actuators2_); 00254 EXPECT_NEAR(actuators2_[0]->command_.effort_, actuators_[0]->command_.effort_, eps); 00255 EXPECT_NEAR(actuators2_[1]->command_.effort_, actuators_[1]->command_.effort_, eps); 00256 } 00257 00258 00259 int main(int argc, char **argv) 00260 { 00261 ros::Time::init(); 00262 testing::InitGoogleTest(&argc, argv); 00263 return RUN_ALL_TESTS(); 00264 }