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

Added forward kinematics calls into Arm class

Browse Source
This commit is contained in:
Derek Witcpalek
2020-04-09 11:02:13 -04:00
parent 6a73cd2371
commit 007a5aca1a
2 changed files with 22 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
rrrobot_src/src/arm_control/include/arm_control/arm.h
View File

@@ -5,6 +5,9 @@
#include <kdl/joint.hpp> #include <kdl/joint.hpp>
#include <kdl/frames.hpp> #include <kdl/frames.hpp>
#include <LinearMath/btTransform.h> #include <LinearMath/btTransform.h>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <memory>
#include <string> #include <string>
@@ -59,11 +62,19 @@ public:
*/ */
std::vector<float> getRequiredTorques(/*std::vector<float> theta_des, std::vector<float> theta_dot_des, */ std::vector<float> theta_double_dot_des); std::vector<float> getRequiredTorques(/*std::vector<float> theta_des, std::vector<float> theta_dot_des, */ std::vector<float> theta_double_dot_des);
/*
* Using the provided joint positions, this calculates the position of the end of the
* arm. Returns whether it was successful
*/
bool calculateForwardKinematics(KDL::JntArray joint_positions, KDL::Frame &end_effector_frame);
const KDL::Chain &getArm() const; const KDL::Chain &getArm() const;
private: private:
KDL::Chain arm; KDL::Chain arm;
KDL::Chain simple_arm;
// Create solver based on kinematic chain
std::unique_ptr<KDL::ChainFkSolverPos_recursive> fksolver;
/* /*
* i_com: the inertia about the center of mass of this link * i_com: the inertia about the center of mass of this link

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

@@ -302,16 +302,13 @@ Arm::Arm(const std::string &sdf_file)
//KDL::Segment to_add(cur.getName(), cur.getJoint(), KDL::Frame(new_rotation, new_position), cur.getInertia()); //KDL::Segment to_add(cur.getName(), cur.getJoint(), KDL::Frame(new_rotation, new_position), cur.getInertia());
arm.addSegment(to_add); arm.addSegment(to_add);
if (simple_arm.getNrOfSegments() < 4)
{
simple_arm.addSegment(to_add);
}
prev_frame = joint_in_world; prev_frame = joint_in_world;
// prev_frame_world_transform = cur_frame_world_transform; // prev_frame_world_transform = cur_frame_world_transform;
} }
cout << arm.getNrOfJoints() << '\t' << arm.getNrOfSegments() << endl; cout << arm.getNrOfJoints() << '\t' << arm.getNrOfSegments() << endl;
fksolver.reset(new KDL::ChainFkSolverPos_recursive(arm));
} }
/* /*
@@ -372,6 +369,15 @@ std::vector<float> Arm::getRequiredTorques(/*std::vector<float> theta_des, std::
{ {
} }
/*
* Using the provided joint positions, this calculates the pose of the end of the
* arm. Returns whether it was successful
*/
bool Arm::calculateForwardKinematics(KDL::JntArray joint_positions, KDL::Frame &end_effector_frame)
{
return fksolver->JntToCart(joint_positions, end_effector_frame);
}
const KDL::Chain &Arm::getArm() const const KDL::Chain &Arm::getArm() const
{ {
return arm; return arm;