Fix problem with persistense of chain_ in inverse kinematics.

diff --git a/q2d_teleop/src/q2d_teleop_rviz.cpp b/q2d_teleop/src/q2d_teleop_rviz.cpp
index 025fdd1..59d17a6 100644 (file)
--- a/q2d_teleop/src/q2d_teleop_rviz.cpp
+++ b/q2d_teleop/src/q2d_teleop_rviz.cpp
@@ -1,7 +1,7 @@
 /*
   q2d_teleop_rviz: A ROS node to teloperate the Quanser 2DSFJE robot.
   
-  Copyright (c) 2018 Walter Fetter Lages <w.fetter@ieee.org>
+  Copyright (c) 2018,2019 Walter Fetter Lages <w.fetter@ieee.org>
 
     This program is free software; you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
@@ -28,7 +28,7 @@
 #include <kdl/chainiksolverpos_lma.hpp>
 #include <kdl_parser/kdl_parser.hpp>
 
-#define sqr(x) (x*x)
+#define sqr(x) ((x)*(x))
 
 class Q2dTeleop
 {
@@ -45,6 +45,7 @@ class Q2dTeleop
        ros::Publisher  elbowCmdPub_;
        
        KDL::Frame goal_;
+       KDL::Chain chain_;
        KDL::ChainIkSolverPos_LMA *ikSolverPos_;
        KDL::JntArray q_;
                
@@ -67,11 +68,10 @@ Q2dTeleop::Q2dTeleop(ros::NodeHandle node): q_(2)
        if (!kdl_parser::treeFromString(robotDescription,tree))
                ROS_ERROR_STREAM("Failed to construct KDL tree.");
                
-       KDL::Chain chain;
-       if (!tree.getChain("origin_link","tool_link",chain)) 
+       if (!tree.getChain("origin_link","tool_link",chain_))
                ROS_ERROR_STREAM("Failed to get chain from KDL tree.");
 
-       q_.resize(chain.getNrOfJoints());
+       q_.resize(chain_.getNrOfJoints());
 
        goal_.Identity();
        goal_.p.x(0.61);
@@ -79,22 +79,10 @@ Q2dTeleop::Q2dTeleop(ros::NodeHandle node): q_(2)
        
         Eigen::Matrix<double,6,1> L;
         L << 1.0 , 1.0 , 1.0, 0.01, 0.01, 0.01;
-        
-       ikSolverPos_=new KDL::ChainIkSolverPos_LMA(chain,L);
-       ikSolverPos_->display_information=true;
-       
-/*      
-        std::cout << "q_=" << q_ << std::endl;
-       std::cout << "goal=" << goal_ << std::endl;
-       
-       KDL::JntArray q_out=q_;
-       int error=0;
-       if((error=ikSolverPos_->CartToJnt(q_,goal_,q_out)) < 0)
-               ROS_ERROR_STREAM("Failed to compute invere kinematics: (" << error << ") " 
-                       << ikSolverPos_->strError(error));
-        q_=q_out;
-        std::cout << "q_=" << q_ << std::endl;
-*/
+
+       // A copy of chain_ is not created inside!
+       ikSolverPos_=new KDL::ChainIkSolverPos_LMA(chain_,L);
+       ikSolverPos_->display_information=false;
 }
 
 Q2dTeleop::~Q2dTeleop(void)
@@ -115,21 +103,18 @@ void Q2dTeleop::clickCB(const geometry_msgs::PointStamped &click)
        publish();
 }
 
+#define KDL_IK
 void Q2dTeleop::publish(void)
 {
-/*
-        std::cout << "q_=" << q_ << std::endl;
-       std::cout << "goal=" << goal_ << std::endl;
-
+#ifdef KDL_IK
        KDL::JntArray q_out=q_;
        int error=0;
        if((error=ikSolverPos_->CartToJnt(q_,goal_,q_out)) < 0)
                ROS_ERROR_STREAM("Failed to compute invere kinematics: (" << error << ") " 
                        << ikSolverPos_->strError(error));
         q_=q_out;
-        std::cout << "q_=" << q_ << std::endl;
-*/
-
+#else
+       // Algebric inverse kinematics
         const double l1=0.343;
         const double l2=0.267;
         double c1=(sqr(goal_.p.x())+sqr(goal_.p.y())-sqr(l1)-sqr(l2))/2/l1/l2;
@@ -154,7 +139,8 @@ void Q2dTeleop::publish(void)
                         q_(1)=q1b;
                 }
         }
-        
+#endif
+
        std_msgs::Float64 shoulderCmd;
        std_msgs::Float64 elbowCmd;
        

diff --git a/q2d_teleop/src/q2d_teleop_tablet.cpp b/q2d_teleop/src/q2d_teleop_tablet.cpp
index 663f237..ab2de87 100644 (file)
--- a/q2d_teleop/src/q2d_teleop_tablet.cpp
+++ b/q2d_teleop/src/q2d_teleop_tablet.cpp
@@ -1,7 +1,7 @@
 /*
   q2d_teleop_tablet: A ROS node to teloperate the Quanser 2DSFJE robot.
   
-  Copyright (c) 2018 Walter Fetter Lages <w.fetter@ieee.org>
+  Copyright (c) 2018,2019 Walter Fetter Lages <w.fetter@ieee.org>
 
     This program is free software; you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
@@ -28,11 +28,7 @@
 #include <kdl/chainiksolverpos_lma.hpp>
 #include <kdl_parser/kdl_parser.hpp>
 
-#define sqr(x) (x*x)
-
-//DEBUG
-#include <kdl/frames_io.hpp>
-#include <kdl//kinfam_io.hpp>
+#define sqr(x) ((x)*(x))
 
 class Q2dTeleop
 {
@@ -49,6 +45,7 @@ class Q2dTeleop
        ros::Publisher  elbowCmdPub_;
        
        KDL::Frame goal_;
+       KDL::Chain chain_;
        KDL::ChainIkSolverPos_LMA *ikSolverPos_;
        KDL::JntArray q_;
                
@@ -71,11 +68,10 @@ Q2dTeleop::Q2dTeleop(ros::NodeHandle node): q_(2)
        if (!kdl_parser::treeFromString(robotDescription,tree))
                ROS_ERROR_STREAM("Failed to construct KDL tree.");
                
-       KDL::Chain chain;
-       if (!tree.getChain("origin_link","tool_link",chain)) 
+       if (!tree.getChain("origin_link","tool_link",chain_))
                ROS_ERROR_STREAM("Failed to get chain from KDL tree.");
 
-       q_.resize(chain.getNrOfJoints());
+       q_.resize(chain_.getNrOfJoints());
 
        goal_.Identity();
        goal_.p.x(0.61);
@@ -83,22 +79,10 @@ Q2dTeleop::Q2dTeleop(ros::NodeHandle node): q_(2)
        
         Eigen::Matrix<double,6,1> L;
         L << 1.0 , 1.0 , 1.0, 0.01, 0.01, 0.01;
-        
-       ikSolverPos_=new KDL::ChainIkSolverPos_LMA(chain,L);
-       ikSolverPos_->display_information=true;
-       
-/*      
-        std::cout << "q_=" << q_ << std::endl;
-       std::cout << "goal=" << goal_ << std::endl;
-       
-       KDL::JntArray q_out=q_;
-       int error=0;
-       if((error=ikSolverPos_->CartToJnt(q_,goal_,q_out)) < 0)
-               ROS_ERROR_STREAM("Failed to compute invere kinematics: (" << error << ") " 
-                       << ikSolverPos_->strError(error));
-        q_=q_out;
-        std::cout << "q_=" << q_ << std::endl;
-*/
+
+       // A copy of chain_ is not created inside!
+       ikSolverPos_=new KDL::ChainIkSolverPos_LMA(chain_,L);
+       ikSolverPos_->display_information=false;
 }
 
 Q2dTeleop::~Q2dTeleop(void)
@@ -121,21 +105,18 @@ void Q2dTeleop::joyCB(const sensor_msgs::Joy &joy)
        publish();
 }
 
+#define KDL_IK
 void Q2dTeleop::publish(void)
 {
-/*
-        std::cout << "q_=" << q_ << std::endl;
-       std::cout << "goal=" << goal_ << std::endl;
-
+#ifdef KDL_IK
        KDL::JntArray q_out=q_;
        int error=0;
        if((error=ikSolverPos_->CartToJnt(q_,goal_,q_out)) < 0)
                ROS_ERROR_STREAM("Failed to compute invere kinematics: (" << error << ") " 
                        << ikSolverPos_->strError(error));
         q_=q_out;
-        std::cout << "q_=" << q_ << std::endl;
-*/
-
+#else
+       // Algebric inverse kinematics
         const double l1=0.343;
         const double l2=0.267;
         double c1=(sqr(goal_.p.x())+sqr(goal_.p.y())-sqr(l1)-sqr(l2))/2/l1/l2;
@@ -160,7 +141,8 @@ void Q2dTeleop::publish(void)
                         q_(1)=q1b;
                 }
         }
-        
+#endif
+
        std_msgs::Float64 shoulderCmd;
        std_msgs::Float64 elbowCmd;