test_wrist_transmission.cpp
Go to the documentation of this file.
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 }


pr2_mechanism_model
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Mon Dec 2 2013 13:13:02