diff --git a/rrrobot_src/src/arm_control/include/arm_control/arm.h b/rrrobot_src/src/arm_control/include/arm_control/arm.h index 54b2e20..5b2cad9 100644 --- a/rrrobot_src/src/arm_control/include/arm_control/arm.h +++ b/rrrobot_src/src/arm_control/include/arm_control/arm.h @@ -6,7 +6,9 @@ #include #include #include +#include #include +#include #include #include @@ -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 fksolver; + std::unique_ptr iksolver; + std::unique_ptr idsolver; /* * i_com: the inertia about the center of mass of this link diff --git a/rrrobot_src/src/arm_control/src/arm.cpp b/rrrobot_src/src/arm_control/src/arm.cpp index 74b8278..a7b9422 100644 --- a/rrrobot_src/src/arm_control/src/arm.cpp +++ b/rrrobot_src/src/arm_control/src/arm.cpp @@ -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; diff --git a/rrrobot_src/src/arm_control/src/arm_controller.cpp b/rrrobot_src/src/arm_control/src/arm_controller.cpp index bc5e7cb..ff25c89 100644 --- a/rrrobot_src/src/arm_control/src/arm_controller.cpp +++ b/rrrobot_src/src/arm_control/src/arm_controller.cpp @@ -10,6 +10,7 @@ #include "ros/ros.h" #include #include +#include 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("/arm_node/arm_commands", 1000); + publisher = nh.advertise("/arm_node/arm_commands", 1000); ros::Subscriber sub = nh.subscribe("/arm_node/arm_positions", 1000, angle_callback); cout << "0 positions for each segment" << endl; diff --git a/rrrobot_src/src/arm_control/test/test.cpp b/rrrobot_src/src/arm_control/test/test.cpp index e71248c..142235b 100644 --- a/rrrobot_src/src/arm_control/test/test.cpp +++ b/rrrobot_src/src/arm_control/test/test.cpp @@ -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; + } } \ No newline at end of file