sravan/RRRobot
1
0
Fork 0
mirror of https://github.com/EECS-467-W20-RRRobot-Project/RRRobot.git synced 2025-08-20 07:32:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

Working kinematic representation. One caveat: there is no link representation between the world frame and the robot base frame. Sometimes when the simulation starts, the arm slides slightly off of (0, 0, 0) which causes joint positions to be slightly off (by the same amount).

Browse Source
This commit is contained in:
Derek Witcpalek
2020-04-09 17:05:06 -04:00
parent 496a7351c2
commit 6acb7158a0
4 changed files with 148 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
rrrobot_src/src/arm_control/CMakeLists.txt
View File

@@ -224,7 +224,14 @@ install(DIRECTORY include/${PROJECT_NAME}/
############# #############
## Add gtest based cpp test target and link libraries ## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_arm_control.cpp) catkin_add_gtest(${PROJECT_NAME}-test test/test_internal_arm_representation.cpp src/arm.cpp)
target_link_libraries(${PROJECT_NAME}-test
${catkin_LIBRARIES}
${kdl_parser_LIBRARIES}
${orocos_kdl_LIBRARIES}
${SDF_LIBRARIES}
ignition-math4::ignition-math4
)
# if(TARGET ${PROJECT_NAME}-test) # if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif() # endif()

75
rrrobot_src/src/arm_control/src/arm.cpp
View File

@@ -119,7 +119,7 @@ Arm::Arm(const std::string &sdf_file)
else if (axis_num == 2) else if (axis_num == 2)
joint_type = KDL::Joint::JointType::RotZ; joint_type = KDL::Joint::JointType::RotZ;
links[child].joint = KDL::Joint(name, joint_type); links[child].joint = KDL::Joint(name, joint_type); // correct
stringstream pose_string(joint->GetElement("pose")->GetValue()->GetAsString()); stringstream pose_string(joint->GetElement("pose")->GetValue()->GetAsString());
float x, y, z; float x, y, z;
@@ -128,13 +128,14 @@ Arm::Arm(const std::string &sdf_file)
KDL::Rotation frame_rotation = KDL::Rotation::RPY(roll, pitch, yaw); KDL::Rotation frame_rotation = KDL::Rotation::RPY(roll, pitch, yaw);
KDL::Vector frame_location(x, y, z); KDL::Vector frame_location(x, y, z);
links[child].joint_frame = KDL::Frame(frame_rotation, frame_location); links[child].joint_frame = KDL::Frame(frame_rotation, frame_location); // incorrect --> this is actually the
joint = joint->GetNextElement("joint"); joint = joint->GetNextElement("joint");
} }
string cur_link_name = "world"; string cur_link_name = "world";
KDL::Frame prev_frame(KDL::Rotation::RPY(0, 0, 0), KDL::Vector(0, 0, 0)); KDL::Frame prev_frame(KDL::Rotation::RPY(0, 0, 0), KDL::Vector(0, 0, 0));
KDL::Joint::JointType to_set(KDL::Joint::JointType::None);
while (link_ordering.find(cur_link_name) != link_ordering.end()) while (link_ordering.find(cur_link_name) != link_ordering.end())
{ {
@@ -145,45 +146,47 @@ Arm::Arm(const std::string &sdf_file)
KDL::Frame prev_frame_inverse = prev_frame.Inverse(); KDL::Frame prev_frame_inverse = prev_frame.Inverse();
KDL::Frame joint_relative_to_prev = prev_frame.Inverse() * joint_in_world; KDL::Frame joint_relative_to_prev = prev_frame.Inverse() * joint_in_world;
// EXPERIMENTAL - update rotation axis // // EXPERIMENTAL - update rotation axis
KDL::Vector uncorrected_axis(0, 0, 0); // KDL::Vector uncorrected_axis(0, 0, 0);
if (links[cur_link_name].joint.getType() == KDL::Joint::JointType::RotX) // if (links[cur_link_name].joint.getType() == KDL::Joint::JointType::RotX)
{ // {
uncorrected_axis = KDL::Vector(1, 0, 0); // uncorrected_axis = KDL::Vector(1, 0, 0);
} // }
else if (links[cur_link_name].joint.getType() == KDL::Joint::JointType::RotY) // else if (links[cur_link_name].joint.getType() == KDL::Joint::JointType::RotY)
{ // {
uncorrected_axis = KDL::Vector(0, 1, 0); // uncorrected_axis = KDL::Vector(0, 1, 0);
} // }
else if (links[cur_link_name].joint.getType() == KDL::Joint::JointType::RotZ) // else if (links[cur_link_name].joint.getType() == KDL::Joint::JointType::RotZ)
{ // {
uncorrected_axis = KDL::Vector(0, 0, 1); // uncorrected_axis = KDL::Vector(0, 0, 1);
} // }
KDL::Vector corrected_axis = joint_relative_to_prev.M * uncorrected_axis; // KDL::Vector corrected_axis = joint_relative_to_prev.M * uncorrected_axis;
double x = fabs(corrected_axis.x()); // double x = fabs(corrected_axis.x());
double y = fabs(corrected_axis.y()); // double y = fabs(corrected_axis.y());
double z = fabs(corrected_axis.z()); // double z = fabs(corrected_axis.z());
const string &name(links[cur_link_name].joint.getName()); // const string &name(links[cur_link_name].joint.getName());
if (x > y and x > z) // if (x > y and x > z)
{ // {
links[cur_link_name].joint = KDL::Joint(name, KDL::Joint::JointType::RotX); // links[cur_link_name].joint = KDL::Joint(name, KDL::Joint::JointType::RotX);
} // }
else if (y > x and y > z) // else if (y > x and y > z)
{ // {
links[cur_link_name].joint = KDL::Joint(name, KDL::Joint::JointType::RotY); // links[cur_link_name].joint = KDL::Joint(name, KDL::Joint::JointType::RotY);
} // }
else if (z > x and z > y) // else if (z > x and z > y)
{ // {
links[cur_link_name].joint = KDL::Joint(name, KDL::Joint::JointType::RotZ); // links[cur_link_name].joint = KDL::Joint(name, KDL::Joint::JointType::RotZ);
} // }
// END EXPERIMENTAL // // END EXPERIMENTAL
KDL::Joint::JointType next(links[cur_link_name].joint.getType());
KDL::RigidBodyInertia inertia(links[cur_link_name].mass, links[cur_link_name].com_location, links[cur_link_name].rotational_inertia); KDL::RigidBodyInertia inertia(links[cur_link_name].mass, links[cur_link_name].com_location, links[cur_link_name].rotational_inertia);
KDL::Segment to_add(links[cur_link_name].link_name, links[cur_link_name].joint, joint_relative_to_prev, inertia); KDL::Segment to_add(links[cur_link_name].link_name, KDL::Joint(links[cur_link_name].joint.getName(), to_set), joint_relative_to_prev, inertia);
to_set = next;
// cout << cur_link_name << endl; // cout << cur_link_name << endl;
@@ -202,6 +205,8 @@ Arm::Arm(const std::string &sdf_file)
prev_frame = joint_in_world; prev_frame = joint_in_world;
} }
arm.addSegment(KDL::Segment(string("pseudo_link"), KDL::Joint("pseudo_joint", to_set), KDL::Frame::Identity(), KDL::RigidBodyInertia()));
cout << "# of arm segments: " << arm.getNrOfSegments() << "\t# of arm joints: " << arm.getNrOfJoints() << endl; cout << "# of arm segments: " << arm.getNrOfSegments() << "\t# of arm joints: " << arm.getNrOfJoints() << endl;
fksolver.reset(new KDL::ChainFkSolverPos_recursive(arm)); fksolver.reset(new KDL::ChainFkSolverPos_recursive(arm));
} }

98
rrrobot_src/src/arm_control/test/test_internal_arm_representation.cpp Normal file
View File

@@ -0,0 +1,98 @@
#include <arm.h>
#include "gtest/gtest.h"
#include <kdl/frames_io.hpp>
#include <kdl/chain.hpp>
#include <cmath>
const double ALLOWED_ERROR = 0.05;
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));
}
TEST(ArmRepresentationTests, shoulder_pivot)
{
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);
for (int pos = 0; pos < nj; ++pos)
{
jointpositions(pos) = 0;
}
// Create the frame that will contain the results
KDL::Frame cartpos;
KDL::Vector correct(0.000767, -1.87014, 2.0658);
jointpositions(0) = 0.0;
arm.calculateForwardKinematics(jointpositions, cartpos);
EXPECT_NEAR(getDistance(cartpos.p, correct), 0.0, ALLOWED_ERROR);
jointpositions(0) = 2 * M_PI;
arm.calculateForwardKinematics(jointpositions, cartpos);
EXPECT_NEAR(getDistance(cartpos.p, correct), 0.0, ALLOWED_ERROR);
jointpositions(0) = M_PI;
arm.calculateForwardKinematics(jointpositions, cartpos);
correct = KDL::Vector(0.000767, 1.87014, 2.0658);
EXPECT_NEAR(getDistance(cartpos.p, correct), 0.0, ALLOWED_ERROR);
}
TEST(ArmRepresentationTests, elbow_pivot)
{
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);
for (int pos = 0; pos < nj; ++pos)
{
jointpositions(pos) = 0;
}
// Create the frame that will contain the results
KDL::Frame cartpos;
KDL::Vector correct(0.000767, -1.87014, 2.0658);
for (double pos = 0.0; pos <= M_2_PI; pos += 0.1)
{
jointpositions(3) = pos;
arm.calculateForwardKinematics(jointpositions, cartpos);
EXPECT_NEAR(getDistance(cartpos.p, correct), 0.0, ALLOWED_ERROR);
}
}
TEST(ArmRepresentationTests, end_effector_pivot)
{
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);
for (int pos = 0; pos < nj; ++pos)
{
jointpositions(pos) = 0;
}
// Create the frame that will contain the results
KDL::Frame cartpos;
KDL::Vector correct(0.000767, -1.87014, 2.0658);
for (double pos = 0.0; pos <= M_2_PI; pos += 0.1)
{
jointpositions(5) = pos;
arm.calculateForwardKinematics(jointpositions, cartpos);
EXPECT_NEAR(getDistance(cartpos.p, correct), 0.0, ALLOWED_ERROR);
}
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

2
rrrobot_src/test.sh Executable file
View File

@@ -0,0 +1,2 @@
source build.sh
catkin_make run_tests