30 #include <gtest/gtest.h> 39 static const double eps = 1e-6;
56 joint_states_.resize(2);
61 joint_states_[0]->joint_ = joint;
62 joint_states_[1]->joint_ = joint;
70 delete joint_states_[0];
71 delete joint_states_[1];
88 wrist_.setReductions(ar, jr);
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);
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);
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);
124 vector<double> ar(2);
128 vector<double> jr(2);
132 wrist_.setReductions(ar, jr);
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);
150 vector<double> ar(2);
154 vector<double> jr(2);
158 wrist_.setReductions(ar, jr);
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);
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);
188 actuators2_.resize(2);
195 delete actuators2_[0];
196 delete actuators2_[1];
201 vector<double> ar(2);
205 vector<double> jr(2);
209 wrist_.setReductions(ar, jr);
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;
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);
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);
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);
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);
259 int main(
int argc,
char **argv)
262 testing::InitGoogleTest(&argc, argv);
263 return RUN_ALL_TESTS();
TEST_F(EasyNoGearingTest, ForwardPositionTest1)
int main(int argc, char **argv)
std::vector< Actuator * > actuators2_
std::vector< JointState * > joint_states_
std::vector< Actuator * > actuators_