00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
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
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
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
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
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 }