From: Walter Fetter Lages Date: Wed, 9 Jan 2019 15:58:29 +0000 (-0200) Subject: Change std::vector to Eigen::Vector in wam_node_test. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=c37d6f9c90ae67f07dde9cf8c0e25253e3fb89dc;p=ufrgs_wam.git 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 --- a/wam_node_test/src/wam_node_test.cpp +++ b/wam_node_test/src/wam_node_test.cpp @@ -18,10 +18,14 @@ *******************************************************************************/ -#include #include +#include +#include + #include +#include + #include #include #include @@ -37,15 +41,21 @@ #include #include -#define sqr(x) ((x)*(x)) - class WamNodeOp { public: + static const size_t DOF=7; + typedef Eigen::Matrix jt_type; + typedef Eigen::Matrix jp_type; + typedef Eigen::Matrix jv_type; + typedef Eigen::Matrix cp_type; + typedef Eigen::Matrix cv_type; + typedef std::tuple pose_type; + WamNodeOp(const ros::NodeHandle &node,double updateRate,double jointTolerance,double jointRateLimit,double cartTolerance,double cartRateLimit); ~WamNodeOp(void); - void jointPos(const std::vector &jpCmd,bool block=true); - void jointWait(const std::vector &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 &jpCmd,bool block=true); + void jointMove(const jp_type &jpCmd,bool block=true); void trackJointPos(bool track); - void cartPosMove(const std::vector &cpCmd,bool block=true); - void cartPosWait(const std::vector &cpCmd) const; - void ortnMove(const std::vector &ortnCmd,bool block=true); - void ortnWait(const std::vector &ortnCmd) const; - void poseMove(const std::vector &cpCmd,const std::vector &ortnCmd,bool block=true); - void cartPos(const std::vector &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 > jpCmd_; - std::vector< std::atomic > cpCmd_; - std::vector< std::atomic > wamJp_; - std::vector< std::atomic > bhandJp_; - std::vector< std::atomic > wamCp_; - std::vector< std::atomic > 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/jnt_pos_cmd",10); @@ -159,9 +172,9 @@ WamNodeOp::~WamNodeOp(void) poseSub_.shutdown(); } -void WamNodeOp::jointPos(const std::vector &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 &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 &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 &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 &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 &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 &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 &cpCmd,const std::vector &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 &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 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 cpCmd(3); + WamNodeOp::cp_type cpCmd; cpCmd[0]=0.0; cpCmd[1]=-0.6; cpCmd[2]=1.3; wam.cartPosMove(cpCmd); - std::vector 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);