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

rrrobot_src/src/arm_control/include/arm_control/arm.h

@@ -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

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;

rrrobot_src/src/arm_control/src/arm_controller.cpp

@@ -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 << nj << endl;
-        cout << "shoulder_pivot: " << jointpositions(0) << endl;
+        // cout << "shoulder_pivot: " << jointpositions(0) << endl;
-        cout << "shoulder_joint: " << jointpositions(1) << endl;
+        // cout << "shoulder_joint: " << jointpositions(1) << endl;
-        cout << "elbow_joint: " << jointpositions(2) << endl;
+        // cout << "elbow_joint: " << jointpositions(2) << endl;
-        cout << "wrist_pivot: " << jointpositions(3) << endl;
+        // cout << "wrist_pivot: " << jointpositions(3) << endl;
-        cout << "wrist_joint: " << jointpositions(4) << endl;
+        // cout << "wrist_joint: " << jointpositions(4) << endl;
-        cout << "end_effector_pivot: " << jointpositions(5) << 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;

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;
+        jointpositions(which_joint) = shoulder_pivot;
-        kinematics_status = fksolver.JntToCart(jointpositions, cartpos);
+        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;
+    }
 }