test_wrist_transmission.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <gtest/gtest.h>
31 #include <vector>
34 
35 using namespace pr2_mechanism_model;
36 using namespace pr2_hardware_interface;
37 using namespace std;
38 
39 static const double eps = 1e-6;
40 
41 class BaseFixture : public testing::Test
42 {
43 public:
45  std::vector<Actuator*> actuators_;
46  std::vector<JointState*> joint_states_;
47 
48 protected:
49  //virtual void SetUp()
51  {
52  actuators_.resize(2);
53  actuators_[0] = new Actuator();
54  actuators_[1] = new Actuator();
55 
56  joint_states_.resize(2);
57  joint_states_[0] = new JointState();
58  joint_states_[1] = new JointState();
59 
60  boost::shared_ptr<const urdf::Joint> joint(new urdf::Joint());
61  joint_states_[0]->joint_ = joint;
62  joint_states_[1]->joint_ = joint;
63 
64  }
65 
66  virtual void TearDown()
67  {
68  delete actuators_[0];
69  delete actuators_[1];
70  delete joint_states_[0];
71  delete joint_states_[1];
72  }
73 };
74 
75 // ******* Sets all the gearings to 1. This is a very basic test *******
77 {
78  virtual void SetUp()
79  {
80  vector<double> ar(2);
81  ar[0] = 1.0;
82  ar[1] = 1.0;
83 
84  vector<double> jr(2);
85  jr[0] = 1.0;
86  jr[1] = 1.0;
87 
88  wrist_.setReductions(ar, jr);
89  }
90 };
91 
92 TEST_F(EasyNoGearingTest, ForwardPositionTest1)
93 {
94  actuators_[0]->state_.position_ = 1.0;
95  actuators_[1]->state_.position_ = 1.0;
96  wrist_.propagatePosition(actuators_, joint_states_);
97  EXPECT_NEAR(joint_states_[0]->position_, 0.0, eps);
98  EXPECT_NEAR(joint_states_[1]->position_, -1.0, eps);
99 }
100 
101 TEST_F(EasyNoGearingTest, ForwardVelocityTest1)
102 {
103  actuators_[0]->state_.velocity_ = 1.0;
104  actuators_[1]->state_.velocity_ =-1.0;
105  wrist_.propagatePosition(actuators_, joint_states_);
106  EXPECT_NEAR(joint_states_[0]->velocity_, 1.0, eps);
107  EXPECT_NEAR(joint_states_[1]->velocity_, 0.0, eps);
108 }
109 
110 TEST_F(EasyNoGearingTest, ForwardMeasuredEffortTest1)
111 {
112  actuators_[0]->state_.last_measured_effort_ = 1.0;
113  actuators_[1]->state_.last_measured_effort_ = -1.0;
114  wrist_.propagatePosition(actuators_, joint_states_);
115  EXPECT_NEAR(joint_states_[0]->measured_effort_, 2.0, eps);
116  EXPECT_NEAR(joint_states_[1]->measured_effort_, 0.0, eps);
117 }
118 
119 // ******* Check joint gearing *******
121 {
122  virtual void SetUp()
123  {
124  vector<double> ar(2);
125  ar[0] = 1.0;
126  ar[1] = 1.0;
127 
128  vector<double> jr(2);
129  jr[0] = 10.0;
130  jr[1] = 100.0;
131 
132  wrist_.setReductions(ar, jr);
133  }
134 };
135 
136 TEST_F(EasyJointGearingTest, ForwardPositionTest1)
137 {
138  actuators_[0]->state_.position_ = 3.0;
139  actuators_[1]->state_.position_ = 1.0;
140  wrist_.propagatePosition(actuators_, joint_states_);
141  EXPECT_NEAR(joint_states_[0]->position_, 0.10, eps);
142  EXPECT_NEAR(joint_states_[1]->position_, -0.02, eps);
143 }
144 
145 // ******* Check motor gearing *******
147 {
148  virtual void SetUp()
149  {
150  vector<double> ar(2);
151  ar[0] = 10.0;
152  ar[1] = 100.0;
153 
154  vector<double> jr(2);
155  jr[0] = 1.0;
156  jr[1] = 1.0;
157 
158  wrist_.setReductions(ar, jr);
159  }
160 };
161 
162 TEST_F(EasyMotorGearingTest, ForwardPositionTest1)
163 {
164  actuators_[0]->state_.position_ = 100.0;
165  actuators_[1]->state_.position_ = 0.0;
166  wrist_.propagatePosition(actuators_, joint_states_);
167  EXPECT_NEAR(joint_states_[0]->position_, 5, eps);
168  EXPECT_NEAR(joint_states_[1]->position_, -5, eps);
169 }
170 
171 TEST_F(EasyMotorGearingTest, ForwardPositionTest2)
172 {
173  actuators_[0]->state_.position_ = 0.0;
174  actuators_[1]->state_.position_ = 100.0;
175  wrist_.propagatePosition(actuators_, joint_states_);
176  EXPECT_NEAR(joint_states_[0]->position_, -0.5, eps);
177  EXPECT_NEAR(joint_states_[1]->position_, -0.5, eps);
178 }
179 
180 // ******* Check both directions *******
182 {
183 public:
184  std::vector<Actuator*> actuators2_;
185 
187  {
188  actuators2_.resize(2);
189  actuators2_[0] = new Actuator();
190  actuators2_[1] = new Actuator();
191  }
192 
194  {
195  delete actuators2_[0];
196  delete actuators2_[1];
197  }
198 
199  virtual void SetUp()
200  {
201  vector<double> ar(2);
202  ar[0] = 10.0;
203  ar[1] = 100.0;
204 
205  vector<double> jr(2);
206  jr[0] = 20.0;
207  jr[1] = 40.0;
208 
209  wrist_.setReductions(ar, jr);
210 
211  actuators_[0]->state_.velocity_ = 3.0;
212  actuators_[1]->state_.velocity_ = 4.0;
213  actuators_[0]->state_.last_measured_effort_ = 5.0;
214  actuators_[1]->state_.last_measured_effort_ = 6.0;
215  }
216 };
217 
219 {
220  actuators_[0]->state_.position_ = 1.0;
221  actuators_[1]->state_.position_ = 2.0;
222  wrist_.propagatePosition(actuators_, joint_states_);
223  wrist_.propagatePositionBackwards(joint_states_, actuators2_);
224  EXPECT_NEAR(actuators2_[0]->state_.position_, actuators_[0]->state_.position_, eps);
225  EXPECT_NEAR(actuators2_[1]->state_.position_, actuators_[1]->state_.position_, eps);
226 }
227 
229 {
230  actuators_[0]->state_.velocity_ = 1.0;
231  actuators_[1]->state_.velocity_ = 2.0;
232  wrist_.propagatePosition(actuators_, joint_states_);
233  wrist_.propagatePositionBackwards(joint_states_, actuators2_);
234  EXPECT_NEAR(actuators2_[0]->state_.velocity_, actuators_[0]->state_.velocity_, eps);
235  EXPECT_NEAR(actuators2_[1]->state_.velocity_, actuators_[1]->state_.velocity_, eps);
236 }
237 
238 TEST_F(PropagateSanityCheck, MeasuredEffort)
239 {
240  actuators_[0]->state_.last_measured_effort_ = 1.0;
241  actuators_[1]->state_.last_measured_effort_ = 2.0;
242  wrist_.propagatePosition(actuators_, joint_states_);
243  wrist_.propagatePositionBackwards(joint_states_, actuators2_);
244  EXPECT_NEAR(actuators2_[0]->state_.last_measured_effort_, actuators_[0]->state_.last_measured_effort_, eps);
245  EXPECT_NEAR(actuators2_[1]->state_.last_measured_effort_, actuators_[1]->state_.last_measured_effort_, eps);
246 }
247 
249 {
250  actuators_[0]->command_.effort_ = 1.0;
251  actuators_[1]->command_.effort_ = 2.0;
252  wrist_.propagateEffortBackwards(actuators_, joint_states_);
253  wrist_.propagateEffort(joint_states_, actuators2_);
254  EXPECT_NEAR(actuators2_[0]->command_.effort_, actuators_[0]->command_.effort_, eps);
255  EXPECT_NEAR(actuators2_[1]->command_.effort_, actuators_[1]->command_.effort_, eps);
256 }
257 
258 
259 int main(int argc, char **argv)
260 {
261  ros::Time::init();
262  testing::InitGoogleTest(&argc, argv);
263  return RUN_ALL_TESTS();
264 }
TEST_F(EasyNoGearingTest, ForwardPositionTest1)
static const double eps
WristTransmission wrist_
int main(int argc, char **argv)
std::vector< Actuator * > actuators2_
std::vector< JointState * > joint_states_
static void init()
std::vector< Actuator * > actuators_
virtual void TearDown()


pr2_mechanism_model
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Mon Jun 10 2019 14:19:04