From: Walter Fetter Lages Date: Thu, 10 Oct 2019 20:06:08 +0000 (-0300) Subject: Fix problem with persistense of chain_ in inverse kinematics. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=a044890067d9a7820beec600393d27fe932bc872;p=q2d.git 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 --- 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 + Copyright (c) 2018,2019 Walter Fetter Lages 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 #include -#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 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 --- 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 + Copyright (c) 2018,2019 Walter Fetter Lages 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 #include -#define sqr(x) (x*x) - -//DEBUG -#include -#include +#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 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;