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

Added inverse kinematics and inverse dynamics calculations for the arm. The controller makes a feeble attempt at reaching target locations.

Browse Source
This commit is contained in:
Derek Witcpalek
2020-04-09 20:03:19 -04:00
parent 653a19e4b0
commit 24ae0bd01d
4 changed files with 97 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -6,7 +6,9 @@
#include <kdl/frames.hpp>
#include <LinearMath/btTransform.h>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainiksolverpos_lma.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainidsolver_recursive_newton_euler.hpp>
#include <memory>
#include <string>
@@ -68,6 +70,10 @@ public:
*/
bool calculateForwardKinematics(KDL::JntArray joint_positions, KDL::Frame &end_effector_frame);
bool calculateInverseKinematics(const KDL::JntArray &cur_configuration, const KDL::Frame &desired_end_effector_frame, KDL::JntArray &final_configuration);
int calculateInverseDynamics(const KDL::JntArray &pos, const KDL::JntArray &vel, const KDL::JntArray &accel, const KDL::Wrenches &f_external, KDL::JntArray &required_torque);
const KDL::Chain &getArm() const;
void print() const;
@@ -77,6 +83,8 @@ private:
// Create solver based on kinematic chain
std::unique_ptr<KDL::ChainFkSolverPos_recursive> fksolver;
std::unique_ptr<KDL::ChainIkSolverPos_LMA> iksolver;
std::unique_ptr<KDL::ChainIdSolver_RNE> idsolver;
/*
* i_com: the inertia about the center of mass of this link

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

@@ -209,6 +209,8 @@ Arm::Arm(const std::string &sdf_file)
cout << "# of arm segments: " << arm.getNrOfSegments() << "\t# of arm joints: " << arm.getNrOfJoints() << endl;
fksolver.reset(new KDL::ChainFkSolverPos_recursive(arm));
iksolver.reset(new KDL::ChainIkSolverPos_LMA(arm));
idsolver.reset(new KDL::ChainIdSolver_RNE(arm, KDL::Vector(0, 0, -9.81)));
}
/*
@@ -278,6 +280,21 @@ bool Arm::calculateForwardKinematics(KDL::JntArray joint_positions, KDL::Frame &
return fksolver->JntToCart(joint_positions, end_effector_frame);
}
bool Arm::calculateInverseKinematics(const KDL::JntArray &cur_configuration, const KDL::Frame &desired_end_effector_frame, KDL::JntArray &final_configuration)
{
return iksolver->CartToJnt(cur_configuration, desired_end_effector_frame, final_configuration);
}
int Arm::calculateInverseDynamics(const KDL::JntArray &pos, const KDL::JntArray &vel, const KDL::JntArray &accel, const KDL::Wrenches &f_external, KDL::JntArray &required_torque)
{
// cout << pos.rows() << endl;
// cout << vel.rows() << endl;
// cout << accel.rows() << endl;
// cout << f_external.size() << endl;
// cout << required_torque.rows() << endl;
return idsolver->CartToJnt(pos, vel, accel, f_external, required_torque);
}
const KDL::Chain &Arm::getArm() const
{
return arm;

39
rrrobot_src/src/arm_control/src/arm_controller.cpp
View File

@@ -10,6 +10,7 @@
#include "ros/ros.h"
#include <simulation_env/arm_command.h>
#include <simulation_env/arm_angles.h>
#include <simulation_env/arm_command.h>
using std::cout;
using std::endl;
@@ -18,6 +19,7 @@ using namespace KDL;
Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
KDL::Chain chain = arm.getArm();
KDL::Chain simple_chain;
ros::Publisher publisher;
void angle_callback(const simulation_env::arm_angles &msg)
{
@@ -43,13 +45,13 @@ void angle_callback(const simulation_env::arm_angles &msg)
kinematics_status = fksolver.JntToCart(jointpositions, cartpos);
if (kinematics_status >= 0)
{
cout << nj << endl;
cout << "shoulder_pivot: " << jointpositions(0) << endl;
cout << "shoulder_joint: " << jointpositions(1) << endl;
cout << "elbow_joint: " << jointpositions(2) << endl;
cout << "wrist_pivot: " << jointpositions(3) << endl;
cout << "wrist_joint: " << jointpositions(4) << endl;
cout << "end_effector_pivot: " << jointpositions(5) << endl;
// cout << nj << endl;
// cout << "shoulder_pivot: " << jointpositions(0) << endl;
// cout << "shoulder_joint: " << jointpositions(1) << endl;
// cout << "elbow_joint: " << jointpositions(2) << endl;
// cout << "wrist_pivot: " << jointpositions(3) << endl;
// cout << "wrist_joint: " << jointpositions(4) << endl;
// cout << "end_effector_pivot: " << jointpositions(5) << endl;
std::cout << cartpos.p << std::endl;
double roll, pitch, yaw;
cartpos.M.GetRPY(roll, pitch, yaw);
@@ -64,6 +66,27 @@ void angle_callback(const simulation_env::arm_angles &msg)
{
//printf("%s \n", "Error: could not calculate forward kinematics :(");
}
// try to move arm to (1, 1, 1)
KDL::JntArray vel(arm.getArm().getNrOfJoints());
KDL::JntArray accel(arm.getArm().getNrOfJoints());
KDL::Wrenches ext_force(arm.getArm().getNrOfSegments());
KDL::JntArray required_force(arm.getArm().getNrOfJoints());
KDL::Frame desired(KDL::Vector(1, 1, 1));
KDL::JntArray required_positions(arm.getArm().getNrOfJoints());
bool failed = arm.calculateInverseKinematics(jointpositions, desired, required_positions);
int error_val = arm.calculateInverseDynamics(required_positions, vel, accel, ext_force, required_force);
simulation_env::arm_command cmd;
cmd.shoulder_pivot_force = required_force(0);
cmd.shoulder_joint_force = -required_force(1);
cmd.elbow_joint_force = required_force(2);
cmd.wrist_pivot_force = required_force(3);
cmd.wrist_joint_force = required_force(4);
cmd.end_effector_pivot_force = required_force(5);
publisher.publish(cmd);
ros::spinOnce();
}
int main(int argc, char **argv)
@@ -76,7 +99,7 @@ int main(int argc, char **argv)
// correct.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0, 0, 1))));
ros::NodeHandle nh;
// publisher = nh.advertise<simulation_env::arm_command>("/arm_node/arm_commands", 1000);
publisher = nh.advertise<simulation_env::arm_command>("/arm_node/arm_commands", 1000);
ros::Subscriber sub = nh.subscribe("/arm_node/arm_positions", 1000, angle_callback);
cout << "0 positions for each segment" << endl;

46
rrrobot_src/src/arm_control/test/test.cpp
View File

@@ -24,7 +24,8 @@ int main(int argc, char **argv)
{
//Definition of a kinematic chain & add segments to the chain
Arm arm(/*"/home/rrrobot/rrrobot_src/src/gazebo_models/basic_arm/model.sdf"); /(*/ "/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
KDL::Chain chain = arm.getArm();
//KDL::Chain chain = arm.getArm();
// chain.addSegment(Segment(Joint(Joint::RotZ), Frame(Vector(0.0, 0.0, 0.0))));
// chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0, 0, 1))));
// chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0, 0, 1))));
@@ -35,10 +36,10 @@ int main(int argc, char **argv)
// chain.addSegment(Segment(Joint(Joint::RotZ)));
// Create solver based on kinematic chain
ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(chain);
//ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(chain);
// Create joint array
unsigned int nj = chain.getNrOfJoints();
unsigned int nj = arm.getArm().getNrOfJoints();
KDL::JntArray jointpositions = JntArray(nj);
for (int pos = 0; pos < nj; ++pos)
{
@@ -88,6 +89,9 @@ int main(int argc, char **argv)
KDL::Frame cartpos;
ofstream f_raw("configurations.txt");
ofstream f_corrected("corrected_configurations.txt");
int which_joint;
cout << "Which joint should be moved (0: shoulder pivot, 5: end effector pivot): ";
cin >> which_joint;
for (double shoulder_pivot = 0.0; shoulder_pivot <= 6.28; shoulder_pivot += 0.1)
{
@@ -107,8 +111,8 @@ int main(int argc, char **argv)
// // printf("%s \n", "Error: could not calculate forward kinematics :(");
// }
// }
jointpositions(5) = shoulder_pivot;
kinematics_status = fksolver.JntToCart(jointpositions, cartpos);
jointpositions(which_joint) = shoulder_pivot;
kinematics_status = arm.calculateForwardKinematics(jointpositions, cartpos); //fksolver.JntToCart(jointpositions, cartpos);
cout << "Final position: " << endl;
cout << cartpos << endl;
@@ -119,4 +123,36 @@ int main(int argc, char **argv)
f_corrected << corrected.x() << "," << corrected.y() << "," << corrected.z() << "\n";
}
// }
KDL::Frame desired(KDL::Vector(0.000767, -1.87014, 2.0658));
KDL::JntArray final_config(arm.getArm().getNrOfJoints());
bool error = arm.calculateInverseKinematics(jointpositions, desired, final_config);
if (error == false)
{
for (int joint = 0; joint < arm.getArm().getNrOfJoints(); ++joint)
{
cout << final_config(joint) << endl;
}
}
else
{
cout << "Failed to find configuration" << endl;
}
KDL::JntArray vel(arm.getArm().getNrOfJoints());
KDL::JntArray accel(arm.getArm().getNrOfJoints());
KDL::Wrenches ext_force(arm.getArm().getNrOfSegments());
KDL::JntArray required_force(arm.getArm().getNrOfJoints());
int error_val = arm.calculateInverseDynamics(jointpositions, vel, accel, ext_force, required_force);
if (error_val == 0)
{
for (int joint = 0; joint < arm.getArm().getNrOfJoints(); ++joint)
{
cout << required_force(joint) << endl;
}
}
else
{
cout << "Failed to find required torques, error #" << error_val << endl;
}
}