sravan/RRRobot
1
0
Fork 0
mirror of https://github.com/EECS-467-W20-RRRobot-Project/RRRobot.git synced 2025-08-17 23:02:45 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added stronger test case to verify that all transformations work

Browse Source
This commit is contained in:
Derek Witcpalek
2020-04-09 17:24:50 -04:00
parent 6acb7158a0
commit 653a19e4b0
1 changed files with 30 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
rrrobot_src/src/arm_control/test/test_internal_arm_representation.cpp
View File

@@ -12,6 +12,11 @@ double getDistance(const KDL::Vector &a, const KDL::Vector &b)
return sqrt(pow(a.x() - b.x(), 2) + pow(a.y() - b.y(), 2) + pow(a.z() - b.z(), 2)); return sqrt(pow(a.x() - b.x(), 2) + pow(a.y() - b.y(), 2) + pow(a.z() - b.z(), 2));
} }
void EXPECT_ROTATION_EQ(const KDL::Rotation &a, const KDL::Rotation &b)
{
EXPECT_TRUE(KDL::Equal(a, b, 0.005));
}
TEST(ArmRepresentationTests, shoulder_pivot) TEST(ArmRepresentationTests, shoulder_pivot)
{ {
Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf"); Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
@@ -91,6 +96,31 @@ TEST(ArmRepresentationTests, end_effector_pivot)
} }
} }
TEST(ArmRepresentationTests, all_joints_rotated)
{
Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
// Create joint array
unsigned int nj = arm.getArm().getNrOfJoints();
KDL::JntArray jointpositions = KDL::JntArray(nj);
jointpositions(0) = -12.0027;
jointpositions(1) = -1.86177;
jointpositions(2) = -0.991728;
jointpositions(3) = 39.5032;
jointpositions(4) = -1.45472;
jointpositions(5) = -25.4681;
// Create the frame that will contain the results
KDL::Frame cartpos;
KDL::Vector correct(-0.876742, 1.85761, 0.482547);
KDL::Rotation correct_rot = KDL::Rotation::RPY(-1.75917, -0.0162929, -0.823634);
arm.calculateForwardKinematics(jointpositions, cartpos);
EXPECT_NEAR(getDistance(cartpos.p, correct), 0.0, ALLOWED_ERROR);
EXPECT_ROTATION_EQ(cartpos.M, correct_rot);
}
int main(int argc, char **argv) int main(int argc, char **argv)
{ {
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);