Change std::vector to Eigen::Vector in wam_node_test.

diff --git a/wam_node_test/src/wam_node_test.cpp b/wam_node_test/src/wam_node_test.cpp
index 3566463..5050f19 100644 (file)
--- a/wam_node_test/src/wam_node_test.cpp
+++ b/wam_node_test/src/wam_node_test.cpp
@@ -18,10 +18,14 @@
         
 *******************************************************************************/
 
-#include <atomic>
 #include <thread>
 
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
 #include <ros/ros.h>
+#include <eigen_conversions/eigen_msg.h>
+
 #include <geometry_msgs/PoseStamped.h>
 #include <sensor_msgs/JointState.h>
 #include <wam_msgs/RTJointPos.h>
@@ -37,15 +41,21 @@
 #include <wam_srvs/OrtnMove.h>
 #include <wam_srvs/PoseMove.h>
 
-#define sqr(x) ((x)*(x))
-
 class WamNodeOp
 {
        public:
+       static const size_t DOF=7;
+       typedef Eigen::Matrix<double,DOF,1> jt_type;
+       typedef Eigen::Matrix<double,DOF,1> jp_type;
+       typedef Eigen::Matrix<double,DOF,1> jv_type;
+       typedef Eigen::Matrix<double,3,1> cp_type;
+       typedef Eigen::Matrix<double,3,1> cv_type;
+       typedef std::tuple<cp_type,Eigen::Quaterniond> pose_type;
+       
        WamNodeOp(const ros::NodeHandle &node,double updateRate,double jointTolerance,double jointRateLimit,double cartTolerance,double cartRateLimit);
        ~WamNodeOp(void);
-       void jointPos(const std::vector<float> &jpCmd,bool block=true);
-       void jointWait(const std::vector<float> &jpCmd) const;
+       void jointPos(const jp_type &jpCmd,bool block=true);
+       void jointWait(const jp_type &jpCmd) const;
        void fingerPos(double f1,double f2,double f3,bool block=true);
        void graspPos(double pos,bool block=true);
        void fingerWait(double f1,double f2,double f3) const;
@@ -59,14 +69,14 @@ class WamNodeOp
        void holdCartPos(bool hold);
        void holdJointPos(bool hold);
        void holdOrtnPos(bool hold);
-       void jointMove(const std::vector<float> &jpCmd,bool block=true);
+       void jointMove(const jp_type &jpCmd,bool block=true);
        void trackJointPos(bool track);
-       void cartPosMove(const std::vector<float> &cpCmd,bool block=true);
-       void cartPosWait(const std::vector<float> &cpCmd) const;
-       void ortnMove(const std::vector<float> &ortnCmd,bool block=true);
-       void ortnWait(const std::vector<float> &ortnCmd) const;
-       void poseMove(const std::vector<float> &cpCmd,const std::vector<float> &ortnCmd,bool block=true);
-       void cartPos(const std::vector<float> &cpCmd,bool block=true);
+       void cartPosMove(const cp_type &cpCmd,bool block=true);
+       void cartPosWait(const cp_type &cpCmd) const;
+       void ortnMove(const Eigen::Quaterniond &ortnCmd,bool block=true);
+       void ortnWait(const Eigen::Quaterniond &ortnCmd) const;
+       void poseMove(const pose_type &poseCmd,bool block=true);
+       void cartPos(const cp_type &cpCmd,bool block=true);
        void trackCartPos(bool track);
 
        private:
@@ -95,12 +105,12 @@ class WamNodeOp
        ros::ServiceClient ortnMoveCli_;
        ros::ServiceClient poseMoveCli_;
 
-       std::vector< std::atomic<float> > jpCmd_;
-       std::vector< std::atomic<float> > cpCmd_;
-       std::vector< std::atomic<float> > wamJp_;
-       std::vector< std::atomic<float> > bhandJp_;
-       std::vector< std::atomic<float> > wamCp_;
-       std::vector< std::atomic<float> > wamOrtn_;
+       jp_type jpCmd_;
+       cp_type cpCmd_;
+       jp_type wamJp_;
+       cp_type wamCp_;
+       Eigen::Quaterniond wamOrtn_;
+       Eigen::Vector4d bhandJp_;
 
        std::thread update_;
        bool runUpdate_;
@@ -113,10 +123,13 @@ class WamNodeOp
        double cartTolerance_;
        double cartRateLimit_;
        const double msgTimeout;
+       
+       public:
+       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 WamNodeOp::WamNodeOp(const ros::NodeHandle &node,double updateRate,double jointTolerance,double jointRateLimit,double cartTolerance,double cartRateLimit):
-       jpCmd_(7), cpCmd_(3), wamJp_(7), bhandJp_(4), wamCp_(3), wamOrtn_(4), runUpdate_(true), trackJointPos_(false), trackCartPos_(false), msgTimeout(0.4)
+       runUpdate_(true),trackJointPos_(false),trackCartPos_(false),msgTimeout(0.4)
 {
        node_=node;
        jointPosPub_=node_.advertise<wam_msgs::RTJointPos>("wam/jnt_pos_cmd",10);
@@ -159,9 +172,9 @@ WamNodeOp::~WamNodeOp(void)
        poseSub_.shutdown();
 }
 
-void WamNodeOp::jointPos(const std::vector<float> &jpCmd,bool block)
+void WamNodeOp::jointPos(const jp_type &jpCmd,bool block)
 {
-       for(int i=0;i < jpCmd_.size();i++) jpCmd_[i]=jpCmd[i];
+       jpCmd_=jpCmd;
        trackJointPos(true);
        if(block) jointWait(jpCmd);
 }
@@ -172,18 +185,10 @@ void WamNodeOp::trackJointPos(bool track)
        ros::Duration(msgTimeout).sleep();
 }
 
-void WamNodeOp::jointWait(const std::vector<float> &jpCmd) const
+void WamNodeOp::jointWait(const jp_type &jpCmd) const
 {
-       float error;
-       do
-       {
+       while((wamJp_-jpCmd).norm() > wamJp_.size()*jointTolerance_)
                ros::Duration(0.01).sleep();
-               error=0.0;      
-               for(int i=0;i < wamJp_.size() && i < jpCmd.size();i++)
-                       error+=sqr(wamJp_[i]-jpCmd[i]);
-               error=sqrt(error);
-       }
-       while(error > 7*jointTolerance_);
 }
 
 void WamNodeOp::fingerPos(double f1,double f2,double f3,bool block)
@@ -200,13 +205,10 @@ void WamNodeOp::fingerPos(double f1,double f2,double f3,bool block)
 
 void WamNodeOp::fingerWait(double f1,double f2,double f3) const
 {
-       float error;
-       do
-       {
+       Eigen::Vector3d jp(f1,f2,f3);
+       
+       while((jp-bhandJp_.head(3)).norm() > 3*jointTolerance_)
                ros::Duration(0.01).sleep();
-               error=sqrt(sqr(f1-bhandJp_[0])+sqr(f2-bhandJp_[1])+sqr(f3-bhandJp_[2]));
-       }
-       while(error > 3*jointTolerance_);
 }
 
 void WamNodeOp::graspPos(double pos,bool block)
@@ -231,13 +233,8 @@ void WamNodeOp::spreadPos(double spread,bool block)
 
 void WamNodeOp::spreadWait(double spread) const
 {
-       float error;
-       do
-       {
+       while(fabs(spread-bhandJp_[3]) > jointTolerance_)
                ros::Duration(0.01).sleep();
-               error=fabs(spread-bhandJp_[3]);
-       }
-       while(error > jointTolerance_);
 }
 
 void WamNodeOp::closeGrasp(bool block)
@@ -311,94 +308,73 @@ void WamNodeOp::holdOrtnPos(bool hold)
        else ROS_ERROR_STREAM("Error holding orientation.");
 }
 
-void WamNodeOp::jointMove(const std::vector<float> &jpCmd,bool block)
+void WamNodeOp::jointMove(const jp_type &jpCmd,bool block)
 {
        wam_srvs::JointMove jointMoveSrv;
        
-       jointMoveSrv.request.joints=jpCmd;
-       if(jointMoveCli_.call(jointMoveSrv)) ROS_INFO_STREAM("Joints moved to "
-               << jpCmd[0] << " " << jpCmd [1] << " " << jpCmd[2] << " "
-               << jpCmd[3] << " " << jpCmd[4] << " " << jpCmd[5] << " "
-               << jpCmd[6] << " " << ".");
+       for(int i=0;i < jpCmd.size();i++) jointMoveSrv.request.joints.push_back(jpCmd[i]);
+       if(jointMoveCli_.call(jointMoveSrv))
+               ROS_INFO_STREAM("Joints moved to " << jpCmd.transpose() << ".");
        else ROS_ERROR_STREAM("Error moving joints.");
        if(block) jointWait(jpCmd);
 }
 
-void WamNodeOp::cartPosMove(const std::vector<float> &cpCmd,bool block)
+void WamNodeOp::cartPosMove(const cp_type &cpCmd,bool block)
 {
        wam_srvs::CartPosMove cartPosMoveSrv;
        
        for(int i=0;i < 3;i++) cartPosMoveSrv.request.position[i]=cpCmd[i];
-       if(cartPosMoveCli_.call(cartPosMoveSrv)) ROS_INFO_STREAM("Cartesian posiiton moved to "
-               << cpCmd[0] << " " << cpCmd [1] << " " << cpCmd[2] << ".");
+       if(cartPosMoveCli_.call(cartPosMoveSrv))
+               ROS_INFO_STREAM("Cartesian posiiton moved to " << cpCmd.transpose() << ".");
        else ROS_ERROR_STREAM("Error moving to Cartesian position.");
        if(block) cartPosWait(cpCmd);
 }
 
-void WamNodeOp::cartPosWait(const std::vector<float> &cpCmd) const
+void WamNodeOp::cartPosWait(const cp_type &cpCmd) const
 {
-       float error;
-       do
-       {
+       while((wamCp_-cpCmd).norm() > cartTolerance_)
                ros::Duration(0.01).sleep();
-               error=0.0;      
-               for(int i=0;i < wamCp_.size() && i < cpCmd.size();i++)
-                       error+=sqr(wamCp_[i]-cpCmd[i]);
-               error=sqrt(error);
-       }
-       while(error > cartTolerance_);
 }
 
-void WamNodeOp::ortnMove(const std::vector<float> &ortnCmd,bool block)
+void WamNodeOp::ortnMove(const Eigen::Quaterniond &ortnCmd,bool block)
 {
        wam_srvs::OrtnMove ortnMoveSrv;
        
-       for(int i=0;i < 4;i++) ortnMoveSrv.request.orientation[i]=ortnCmd[i];
+       ortnMoveSrv.request.orientation[0]=ortnCmd.x();
+       ortnMoveSrv.request.orientation[1]=ortnCmd.y();
+       ortnMoveSrv.request.orientation[2]=ortnCmd.z();
+       ortnMoveSrv.request.orientation[3]=ortnCmd.w();
        if(ortnMoveCli_.call(ortnMoveSrv)) ROS_INFO_STREAM("Tool orientation moved to "
-               << ortnCmd[0] << " " << ortnCmd [1] << " " << ortnCmd[2] << " " << ortnCmd[3] << ".");
+               << ortnCmd.x() << " " << ortnCmd.y() << " " << ortnCmd.z() << " " << ortnCmd.w() << ".");
        else ROS_ERROR_STREAM("Error moving tool orientation.");
        if(block) ortnWait(ortnCmd);
 }
 
-void WamNodeOp::ortnWait(const std::vector<float> &ortnCmd) const
+void WamNodeOp::ortnWait(const Eigen::Quaterniond &ortnCmd) const
 {
-       float error;
-       do
-       {
+       while(fabs(ortnCmd.angularDistance(wamOrtn_)) > 8*jointTolerance_);
                ros::Duration(0.01).sleep();
-               error=0.0;      
-               for(int i=0;i < wamOrtn_.size() && i < ortnCmd.size();i++)
-                       error+=sqr(wamOrtn_[i]-ortnCmd[i]);
-               error=sqrt(error);
-       }
-       while(error > 8*jointTolerance_);
 }
 
-void WamNodeOp::poseMove(const std::vector<float> &cpCmd,const std::vector<float> &ortnCmd,bool block)
+void WamNodeOp::poseMove(const pose_type &poseCmd,bool block)
 {
        wam_srvs::PoseMove poseMoveSrv;
        
-       poseMoveSrv.request.pose.position.x=cpCmd[0];
-       poseMoveSrv.request.pose.position.y=cpCmd[1];
-       poseMoveSrv.request.pose.position.z=cpCmd[2];
+       tf::pointEigenToMsg(std::get<0>(poseCmd),poseMoveSrv.request.pose.position);
+       tf::quaternionEigenToMsg(std::get<1>(poseCmd),poseMoveSrv.request.pose.orientation);
 
-       poseMoveSrv.request.pose.orientation.x=ortnCmd[0];
-       poseMoveSrv.request.pose.orientation.y=ortnCmd[1];
-       poseMoveSrv.request.pose.orientation.z=ortnCmd[2];
-       poseMoveSrv.request.pose.orientation.w=ortnCmd[3];
-       
-       if(poseMoveCli_.call(poseMoveSrv)) ROS_INFO_STREAM("Pose moved to "
-               << cpCmd[0] << " " << cpCmd [1] << " " << cpCmd[2] << " "
-               << ortnCmd[0] << " " << ortnCmd [1] << " " << ortnCmd[2] << " " << ortnCmd[3] << ".");
+       if(poseMoveCli_.call(poseMoveSrv))
+               ROS_INFO_STREAM("Pose moved to " << std::get<0>(poseCmd).transpose() << " "
+               << std::get<1>(poseCmd).x() << " " << std::get<1>(poseCmd).y() << " " << std::get<1>(poseCmd).z() << " " << std::get<1>(poseCmd).w() << ".");
        else ROS_ERROR_STREAM("Error moving to pose.");
        if(block)
        {
-               cartPosWait(cpCmd);
-               ortnWait(ortnCmd);
+               cartPosWait(std::get<0>(poseCmd));
+               ortnWait(std::get<1>(poseCmd));
        }
 }
 
-void WamNodeOp::cartPos(const std::vector<float> &cpCmd,bool block)
+void WamNodeOp::cartPos(const cp_type &cpCmd,bool block)
 {
        for(int i=0;i < cpCmd_.size();i++) cpCmd_[i]=cpCmd[i];
        
@@ -474,14 +450,8 @@ void WamNodeOp::jointStatesCB(const sensor_msgs::JointState &jointStates)
 
 void WamNodeOp::poseCB(const geometry_msgs::PoseStamped &pose)
 {
-       wamCp_[0]=pose.pose.position.x;
-       wamCp_[1]=pose.pose.position.y;
-       wamCp_[2]=pose.pose.position.z;
-       
-       wamOrtn_[0]=pose.pose.orientation.x;
-       wamOrtn_[1]=pose.pose.orientation.y;
-       wamOrtn_[2]=pose.pose.orientation.z;
-       wamOrtn_[3]=pose.pose.orientation.w;
+       tf::pointMsgToEigen(pose.pose.position,wamCp_);
+       tf::quaternionMsgToEigen(pose.pose.orientation,wamOrtn_);
 }
 
 int main(int argc,char* argv[])
@@ -491,7 +461,8 @@ int main(int argc,char* argv[])
 
        WamNodeOp wam(node,50.0,0.02,0.5,0.005,0.1);
        
-       std::vector<float> jpCmd(7,0);
+       WamNodeOp::jp_type jpCmd;
+       jpCmd.setZero();
        jpCmd[1]=-2.0;
        jpCmd[3]=3.1-0.4;
        wam.jointPos(jpCmd);
@@ -529,27 +500,27 @@ int main(int argc,char* argv[])
 
        wam.goHome();
        
-       std::vector<float> cpCmd(3);
+       WamNodeOp::cp_type cpCmd;
        cpCmd[0]=0.0;
        cpCmd[1]=-0.6;
        cpCmd[2]=1.3;
        wam.cartPosMove(cpCmd);
        
-       std::vector<float> ortnCmd(4);
-       ortnCmd[0]=0.0;
-       ortnCmd[1]=0.0;
-       ortnCmd[2]=0.0;
-       ortnCmd[3]=1.0;
+       Eigen::Quaterniond ortnCmd;
+       ortnCmd.x()=0.0;
+       ortnCmd.y()=0.0;
+       ortnCmd.z()=0.0;
+       ortnCmd.w()=1.0;
        wam.ortnMove(ortnCmd);
        
        cpCmd[0]=0.0;
        cpCmd[1]=-0.6;
        cpCmd[2]=1.5;
-       ortnCmd[0]=0.0;
-       ortnCmd[1]=0.0;
-       ortnCmd[2]=0.0;
-       ortnCmd[3]=1.0;
-       wam.poseMove(cpCmd,ortnCmd);
+       ortnCmd.x()=0.0;
+       ortnCmd.y()=0.0;
+       ortnCmd.z()=0.0;
+       ortnCmd.w()=1.0;
+       wam.poseMove(std::make_tuple(cpCmd,ortnCmd));
        
        cpCmd[0]=-0.6;
        cpCmd[1]=0.0;
@@ -567,7 +538,7 @@ int main(int argc,char* argv[])
        wam.cartPos(cpCmd);
        wam.trackCartPos(false);
        
-       std::fill(jpCmd.begin(),jpCmd.end(),0.0);
+       jpCmd.setZero();
        jpCmd[1]=-2.0;
        jpCmd[3]=3.1;
        wam.jointMove(jpCmd);