From: Walter Fetter Lages Date: Tue, 8 Jan 2019 07:02:37 +0000 (-0200) Subject: Add tests for all services and topics available in wam_node. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=3fde3a908f0a268cca3438529e826bcc935b352e;p=ufrgs_wam.git Add tests for all services and topics available in wam_node. --- diff --git a/wam_bringup/launch/display.launch b/wam_bringup/launch/display.launch index f32d143..5d56fa1 100644 --- a/wam_bringup/launch/display.launch +++ b/wam_bringup/launch/display.launch @@ -9,7 +9,7 @@ - - + + diff --git a/wam_bringup/launch/wam_node.launch b/wam_bringup/launch/wam_node.launch new file mode 100644 index 0000000..aa93bd9 --- /dev/null +++ b/wam_bringup/launch/wam_node.launch @@ -0,0 +1,16 @@ + + + + + + + + + ["/wam/joint_states","/bhand/joint_states"] + + + + + + + diff --git a/wam_bringup/launch/wam_node_sim.launch b/wam_bringup/launch/wam_node_sim.launch new file mode 100644 index 0000000..76f9c7b --- /dev/null +++ b/wam_bringup/launch/wam_node_sim.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/wam_node_sim/include/wam_node_sim/detail/wam-impl.h b/wam_node_sim/include/wam_node_sim/detail/wam-impl.h index 47a5390..024cd6c 100644 --- a/wam_node_sim/include/wam_node_sim/detail/wam-impl.h +++ b/wam_node_sim/include/wam_node_sim/detail/wam-impl.h @@ -53,11 +53,14 @@ #include #include #include -#include +#include +#include #include #include +#include + namespace barrett { @@ -82,14 +85,30 @@ Wam::Wam(Hand *hand,const std::string& sysName) ROS_ERROR("Failed to get chain from KDL tree."); fkSolverPos_=new KDL::ChainFkSolverPos_recursive(chain_); - Eigen::Matrix L; - L << 1.0, 1.0, 1.0, 0.0, 0.0, 0.0; - ikSolverPos_=new KDL::ChainIkSolverPos_LMA(chain_,L); - ikSolverPose_=new KDL::ChainIkSolverPos_LMA(chain_); + KDL::JntArray qMin(DOF); + qMin(0)=-2.6; + qMin(1)=-2.0; + qMin(2)=-2.8; + qMin(3)=-0.9; + qMin(4)=-4.76; + qMin(5)=-M_PI_2; + qMin(6)=-3.0; + KDL::JntArray qMax(DOF); + qMax(0)=2.6; + qMax(1)=2.0; + qMax(2)=2.8; + qMax(3)=3.1; + qMax(4)=1.24; + qMax(5)=M_PI_2; + qMax(6)=3.0; + + ikSolverVel_=new KDL::ChainIkSolverVel_wdls(chain_,1000); + ikSolverPos_=new KDL::ChainIkSolverPos_NR_JL(chain_,qMin,qMax,*fkSolverPos_,*ikSolverVel_,1000); jointPosition_.setZero(); jointVelocity_.setZero(); jointEffort_.setZero(); + doneMoving=true; } template @@ -99,6 +118,7 @@ Wam::~Wam() jointStatesSubscriber_.shutdown(); delete fkSolverPos_; delete ikSolverPos_; + delete ikSolverVel_; } template @@ -121,16 +141,37 @@ void Wam::trackReferenceSignal(cp_type &referenceSignal) { KDL::JntArray q0(DOF); q0.data=jointPosition_; + // try to induce convergence to an out-elbow pose - q0(3)=3.1; - q0(4)=0.0; - q0(5)=0.0; - q0(6)=0.0; + q0(3)=2.0; + Eigen::MatrixXd Mq(DOF,DOF); + Mq.setIdentity(); + Mq(3,3)=0.7; + static_cast(ikSolverVel_)->setWeightJS(Mq); + + // ignore orientation + Eigen::MatrixXd Mx(6,6); + Mx.setIdentity(); + Mx(3,3)=0.0001; + Mx(4,4)=0.0001; + Mx(5,5)=0.0001; + static_cast(ikSolverVel_)->setWeightTS(Mx); + KDL::Frame frame(KDL::Vector(referenceSignal[0],referenceSignal[1],referenceSignal[2])); KDL::JntArray q(DOF); - int error; - if((error=ikSolverPos_->CartToJnt(q0,frame,q)) < 0) - ROS_ERROR_STREAM("Failed to compute inverse kinematics: " << ikSolverPos_->strError(error)); + + int error=ikSolverPos_->CartToJnt(q0,frame,q); + + Mq.setIdentity(); + static_cast(ikSolverVel_)->setWeightJS(Mq); + Mx.setIdentity(); + static_cast(ikSolverVel_)->setWeightTS(Mx); + + if(error < 0) + { + ROS_ERROR_STREAM("Failed to compute inverse kinematics while tracking Cartesian position: " << ikSolverPos_->strError(error)); + return; + } jp_type jp; jp=q.data; trackReferenceSignal(jp); @@ -146,12 +187,13 @@ void Wam::trackReferenceSignal(Eigen::Quaterniond &referenceSignal) tf::transformEigenToKDL(t,frame); KDL::JntArray q0(DOF); q0.data=jointPosition_; - // try to induce convergence to an out-elbow pose - q0(3)=3.1; KDL::JntArray q(DOF); int error; - if((error=ikSolverPose_->CartToJnt(q0,frame,q)) < 0) - ROS_ERROR_STREAM("Failed to compute inverse kinematics: " << ikSolverPos_->strError(error)); + if((error=ikSolverPos_->CartToJnt(q0,frame,q)) < 0) + { + ROS_ERROR_STREAM("Failed to compute inverse kinematics while tracking orientation: " << ikSolverPos_->strError(error)); + return; + } jp_type jp; jp=q.data; trackReferenceSignal(jp); @@ -167,12 +209,13 @@ void Wam::trackReferenceSignal(pose_type &referenceSignal) tf::transformEigenToKDL(t,frame); KDL::JntArray q0(DOF); q0.data=jointPosition_; - // try to induce convergence to an out-elbow pose - q0(3)=3.1; KDL::JntArray q(DOF); int error; - if((error=ikSolverPose_->CartToJnt(q0,frame,q)) < 0) - ROS_ERROR_STREAM("Failed to compute inverse kinematics: " << ikSolverPos_->strError(error)); + if((error=ikSolverPos_->CartToJnt(q0,frame,q)) < 0) + { + ROS_ERROR_STREAM("Failed to compute inverse kinematics while tracking Cartesian pose: " << ikSolverPos_->strError(error)); + return; + } jp_type jp; jp=q.data; trackReferenceSignal(jp); @@ -389,6 +432,11 @@ template template void Wam::moveToThread(const T ¤tPos,const T &destination,double velocity,double acceleration,std::atomic *started) { + // avoids more than one move at the same time + // Is it possible more than one controller to be active in the real WAM + // supervisory controller? + std::lock_guard lock(moveToMutex); + wam_node_sim::Spline spline(currentPos,destination); KDL::VelocityProfile_Trap profile; @@ -398,24 +446,17 @@ void Wam::moveToThread(const T ¤tPos,const T &destination,double velo doneMoving=false; *started=true; - -// T limit=T::Constant(2.0); static const int PUBLISH_FREQ=250.0; -// wam_node_sim::RateLimiter rateLimiter(limit,1.0/PUBLISH_FREQ); -// rateLimiter.setCurVal(currentPos); - ros::Rate loop(PUBLISH_FREQ); double t=0.0; ros::Time t0=ros::Time::now(); while(ros::ok() && (t=(ros::Time::now()-t0).toSec()) < tf) { T pos=spline.eval(profile.Pos(t)); -// trackReferenceSignal(rateLimiter.getLimit(pos)); trackReferenceSignal(pos); loop.sleep(); } -// trackReferenceSignal(rateLimiter.getLimit(destination)); T pos=destination; trackReferenceSignal(pos); doneMoving=true; diff --git a/wam_node_sim/include/wam_node_sim/wam.h b/wam_node_sim/include/wam_node_sim/wam.h index 89dcbf7..e021812 100644 --- a/wam_node_sim/include/wam_node_sim/wam.h +++ b/wam_node_sim/include/wam_node_sim/wam.h @@ -42,6 +42,7 @@ #define WAM_NODE_SIM_WAM_H_ #include +#include #include #include @@ -176,15 +177,15 @@ private: template void moveToThread(const T& currentPos, const T& destination, double velocity, double acceleration, std::atomic* started); std::atomic doneMoving; + std::mutex moveToMutex; // Used to calculate TP and TO if the values aren't already being calculated in the control loop. KDL::Tree tree_; KDL::Chain chain_; KDL::ChainFkSolverPos *fkSolverPos_; KDL::ChainIkSolverPos *ikSolverPos_; - KDL::ChainIkSolverPos *ikSolverPose_; + KDL::ChainIkSolverVel *ikSolverVel_; - ros::Publisher wam_cmd_pub; // controller command ros::Subscriber jointStatesSubscriber_; jp_type jointPosition_; diff --git a/wam_node_test/launch/gazebo.launch b/wam_node_test/launch/gazebo.launch new file mode 100644 index 0000000..70a7bbc --- /dev/null +++ b/wam_node_test/launch/gazebo.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/wam_node_test/src/wam_node_test.cpp b/wam_node_test/src/wam_node_test.cpp index 5d70e87..bd0a908 100644 --- a/wam_node_test/src/wam_node_test.cpp +++ b/wam_node_test/src/wam_node_test.cpp @@ -1,6 +1,6 @@ /****************************************************************************** Node to test the wam_node - 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 @@ -22,63 +22,129 @@ #include #include +#include #include - #include +#include + +#include #include #include +#include +#include +#include +#include +#include +#include #define sqr(x) ((x)*(x)) class WamNodeOp { public: - WamNodeOp(const ros::NodeHandle &node,double updateRate,double tolerance,double jointRateLimit); + WamNodeOp(const ros::NodeHandle &node,double updateRate,double jointTolerance,double jointRateLimit,double cartTolerance,double cartRateLimit); ~WamNodeOp(void); - void wamMove(const std::vector &jpCmd); - void wamWait(const std::vector &jpCmd) const; - void fingerMove(double f1,double f2,double f3); - void fingerMove(double finger); + void jointPos(const std::vector &jpCmd,bool block=true); + void jointWait(const std::vector &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; - void fingerWait(double finger) const; - void spreadMove(double spread); + void spreadPos(double spread,bool block=true); void spreadWait(double spread) const; + void closeGrasp(bool block=true); + void closeSpread(bool block=true); + void openGrasp(bool block=true); + void openSpread(bool block=true); + void goHome(void); + void holdCartPos(bool hold); + void holdJointPos(bool hold); + void holdOrtnPos(bool hold); + void jointMove(const std::vector &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 trackCartPos(bool track); private: - void jointStatesCB(const sensor_msgs::JointState &jointStates); + void poseCB(const geometry_msgs::PoseStamped &pose); void update(void) const; ros::NodeHandle node_; ros::Publisher jointPosPub_; + ros::Publisher cartPosPub_; ros::Subscriber jointStatesSub_; + ros::Subscriber poseSub_; ros::ServiceClient fingerPosCli_; ros::ServiceClient spreadPosCli_; + ros::ServiceClient graspPosCli_; + ros::ServiceClient closeGraspCli_; + ros::ServiceClient closeSpreadCli_; + ros::ServiceClient openGraspCli_; + ros::ServiceClient openSpreadCli_; + ros::ServiceClient goHomeCli_; + ros::ServiceClient holdCartPosCli_; + ros::ServiceClient holdJointPosCli_; + ros::ServiceClient holdOrtnCli_; + ros::ServiceClient jointMoveCli_; + ros::ServiceClient cartPosMoveCli_; + 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_; std::thread update_; bool runUpdate_; + bool trackJointPos_; + bool trackCartPos_; double updateRate_; - double tolerance_; + double jointTolerance_; double jointRateLimit_; + double cartTolerance_; + double cartRateLimit_; + const double msgTimeout; }; -WamNodeOp::WamNodeOp(const ros::NodeHandle &node,double updateRate,double tolerance,double jointRateLimit): - jpCmd_(7), wamJp_(7), bhandJp_(4), runUpdate_(true) +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) { node_=node; jointPosPub_=node_.advertise("wam/jnt_pos_cmd",10); + cartPosPub_=node_.advertise("wam/cart_pos_cmd",10); + closeGraspCli_=node_.serviceClient("bhand/close_grasp"); + closeSpreadCli_=node_.serviceClient("bhand/close_spread"); + openGraspCli_=node_.serviceClient("bhand/open_grasp"); + openSpreadCli_=node_.serviceClient("bhand/open_spread"); fingerPosCli_=node_.serviceClient("bhand/finger_pos"); spreadPosCli_=node_.serviceClient("bhand/spread_pos"); + graspPosCli_=node_.serviceClient("bhand/grasp_pos"); + cartPosMoveCli_=node_.serviceClient("wam/cart_move"); + goHomeCli_=node_.serviceClient("wam/go_home"); + holdCartPosCli_=node_.serviceClient("wam/hold_cart_pos"); + holdJointPosCli_=node_.serviceClient("wam/hold_joint_pos"); + holdOrtnCli_=node_.serviceClient("wam/hold_ortn"); + jointMoveCli_=node_.serviceClient("wam/joint_move"); + ortnMoveCli_=node_.serviceClient("wam/ortn_move"); + poseMoveCli_=node_.serviceClient("wam/pose_move"); +// gravityCompCli_=node_.serviceClient("wam/gravity_comp"); jointStatesSub_=node_.subscribe("/joint_states",10,&WamNodeOp::jointStatesCB,this); + poseSub_=node_.subscribe("wam/pose",10,&WamNodeOp::poseCB,this); - tolerance_=tolerance; + jointTolerance_=jointTolerance; updateRate_=updateRate; jointRateLimit_=jointRateLimit; + cartTolerance_=cartTolerance; + cartRateLimit_=cartRateLimit; update_=std::thread(&WamNodeOp::update,this); } @@ -88,15 +154,25 @@ WamNodeOp::~WamNodeOp(void) runUpdate_=false; update_.join(); jointPosPub_.shutdown(); + cartPosPub_.shutdown(); jointStatesSub_.shutdown(); + poseSub_.shutdown(); } -void WamNodeOp::wamMove(const std::vector &jpCmd) +void WamNodeOp::jointPos(const std::vector &jpCmd,bool block) { - for(int i=0;i < jpCmd_.size() && i < jpCmd.size();i++) jpCmd_[i]=jpCmd[i]; + for(int i=0;i < jpCmd_.size();i++) jpCmd_[i]=jpCmd[i]; + trackJointPos(true); + if(block) jointWait(jpCmd); } -void WamNodeOp::wamWait(const std::vector &jpCmd) const +void WamNodeOp::trackJointPos(bool track) +{ + trackJointPos_=track; + ros::Duration(msgTimeout).sleep(); +} + +void WamNodeOp::jointWait(const std::vector &jpCmd) const { float error; do @@ -107,18 +183,19 @@ void WamNodeOp::wamWait(const std::vector &jpCmd) const error+=sqr(wamJp_[i]-jpCmd[i]); error=sqrt(error); } - while(error > 7*tolerance_); + while(error > 7*jointTolerance_); } -void WamNodeOp::fingerMove(double f1,double f2,double f3) +void WamNodeOp::fingerPos(double f1,double f2,double f3,bool block) { wam_srvs::BHandFingerPos fingerPosSrv; - fingerPosSrv.request.radians[0]=f1;; + fingerPosSrv.request.radians[0]=f1; fingerPosSrv.request.radians[1]=f2; fingerPosSrv.request.radians[2]=f3; - if(fingerPosCli_.call(fingerPosSrv)) ROS_INFO_STREAM("Finger move to " << f1 << " " << f2 << " " << f3); - else ROS_ERROR_STREAM("Error in finger move."); + if(fingerPosCli_.call(fingerPosSrv)) ROS_INFO_STREAM("Finger moved to " << f1 << " " << f2 << " " << f3 << "."); + else ROS_ERROR_STREAM("Error moving finger."); + if(block) fingerWait(f1,f2,f3); } void WamNodeOp::fingerWait(double f1,double f2,double f3) const @@ -129,27 +206,27 @@ void WamNodeOp::fingerWait(double f1,double f2,double f3) const ros::Duration(0.01).sleep(); error=sqrt(sqr(f1-bhandJp_[0])+sqr(f2-bhandJp_[1])+sqr(f3-bhandJp_[2])); } - while(error > 3*tolerance_); -} - -void WamNodeOp::fingerWait(double finger) const -{ - fingerWait(finger,finger,finger); + while(error > 3*jointTolerance_); } - -void WamNodeOp::fingerMove(double finger) +void WamNodeOp::graspPos(double pos,bool block) { - fingerMove(finger,finger,finger); + wam_srvs::BHandGraspPos graspPosSrv; + + graspPosSrv.request.radians=pos; + if(graspPosCli_.call(graspPosSrv)) ROS_INFO_STREAM("Grasp moved to " << pos << "."); + else ROS_ERROR_STREAM("Error moving grasp."); + if(block) fingerWait(pos,pos,pos); } -void WamNodeOp::spreadMove(double spread) +void WamNodeOp::spreadPos(double spread,bool block) { wam_srvs::BHandSpreadPos spreadPosSrv; spreadPosSrv.request.radians=spread; - if(spreadPosCli_.call(spreadPosSrv)) ROS_INFO_STREAM("Spread move to " << spread); - else ROS_ERROR_STREAM("Error in spread move."); + if(spreadPosCli_.call(spreadPosSrv)) ROS_INFO_STREAM("Spread moved to " << spread << "."); + else ROS_ERROR_STREAM("Error moving spread."); + if(block) spreadWait(spread); } void WamNodeOp::spreadWait(double spread) const @@ -160,7 +237,180 @@ void WamNodeOp::spreadWait(double spread) const ros::Duration(0.01).sleep(); error=fabs(spread-bhandJp_[3]); } - while(error > tolerance_); + while(error > jointTolerance_); +} + +void WamNodeOp::closeGrasp(bool block) +{ + std_srvs::Empty emptySrv; + + if(closeGraspCli_.call(emptySrv)) ROS_INFO_STREAM("Grasp closed."); + else ROS_ERROR_STREAM("Error closing grasp."); + if(block) fingerWait(2.44,2.44,2.44); +} + +void WamNodeOp::closeSpread(bool block) +{ + std_srvs::Empty emptySrv; + + if(closeSpreadCli_.call(emptySrv)) ROS_INFO_STREAM("Spread closed."); + else ROS_ERROR_STREAM("Error closing spread."); + if(block) spreadWait(0); +} + +void WamNodeOp::openGrasp(bool block) +{ + std_srvs::Empty emptySrv; + + if(openGraspCli_.call(emptySrv)) ROS_INFO_STREAM("Grasp open."); + else ROS_ERROR_STREAM("Error opening grasp."); + if(block) fingerWait(0,0,0); +} + +void WamNodeOp::openSpread(bool block) +{ + std_srvs::Empty emptySrv; + + if(openSpreadCli_.call(emptySrv)) ROS_INFO_STREAM("Spread open."); + else ROS_ERROR_STREAM("Error opening spread."); + if(block) spreadWait(M_PI); +} + +void WamNodeOp::goHome(void) +{ + std_srvs::Empty emptySrv; + + if(goHomeCli_.call(emptySrv)) ROS_INFO_STREAM("Returned home position."); + else ROS_ERROR_STREAM("Error returning home position."); +} + +void WamNodeOp::holdCartPos(bool hold) +{ + wam_srvs::Hold holdSrv; + + holdSrv.request.hold=hold; + if(holdCartPosCli_.call(holdSrv)) ROS_INFO_STREAM("Cartesian position held."); + else ROS_ERROR_STREAM("Error holding Cartesian position."); +} + +void WamNodeOp::holdJointPos(bool hold) +{ + wam_srvs::Hold holdSrv; + + holdSrv.request.hold=hold; + if(holdJointPosCli_.call(holdSrv)) ROS_INFO_STREAM("Joint position held."); + else ROS_ERROR_STREAM("Error holding joint position."); +} + +void WamNodeOp::holdOrtnPos(bool hold) +{ + wam_srvs::Hold holdSrv; + + holdSrv.request.hold=hold; + if(holdOrtnCli_.call(holdSrv)) ROS_INFO_STREAM("Orientation held."); + else ROS_ERROR_STREAM("Error holding orientation."); +} + +void WamNodeOp::jointMove(const std::vector &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] << " " << "."); + else ROS_ERROR_STREAM("Error moving joints."); + if(block) jointWait(jpCmd); +} + +void WamNodeOp::cartPosMove(const std::vector &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] << "."); + else ROS_ERROR_STREAM("Error moving to Cartesian position."); + if(block) cartPosWait(cpCmd); +} + +void WamNodeOp::cartPosWait(const std::vector &cpCmd) const +{ + float error; + do + { + 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) +{ + wam_srvs::OrtnMove ortnMoveSrv; + + for(int i=0;i < 4;i++) ortnMoveSrv.request.orientation[i]=ortnCmd[i]; + if(ortnMoveCli_.call(ortnMoveSrv)) ROS_INFO_STREAM("Tool orientation moved to " + << ortnCmd[0] << " " << ortnCmd [1] << " " << ortnCmd[2] << " " << ortnCmd[3] << "."); + else ROS_ERROR_STREAM("Error moving tool orientation."); + if(block) ortnWait(ortnCmd); +} + +void WamNodeOp::ortnWait(const std::vector &ortnCmd) const +{ + float error; + do + { + 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) +{ + 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]; + + 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] << "."); + else ROS_ERROR_STREAM("Error moving to pose."); + if(block) + { + cartPosWait(cpCmd); + ortnWait(ortnCmd); + } +} + +void WamNodeOp::cartPos(const std::vector &cpCmd,bool block) +{ + for(int i=0;i < cpCmd_.size();i++) cpCmd_[i]=cpCmd[i]; + + trackCartPos(true); + + if(block) cartPosWait(cpCmd); +} + +void WamNodeOp::trackCartPos(bool track) +{ + trackCartPos_=track; + ros::Duration(msgTimeout).sleep(); } void WamNodeOp::update(void) const @@ -168,11 +418,26 @@ void WamNodeOp::update(void) const ros::Rate loop(updateRate_); while(ros::ok() && runUpdate_) { - wam_msgs::RTJointPos jointPosMsg; + if(trackJointPos_) + { + wam_msgs::RTJointPos jointPosMsg; + + for(int i=0;i < jpCmd_.size();i++) jointPosMsg.joints.push_back(jpCmd_[i]); + jointPosMsg.rate_limits.resize(jpCmd_.size(),jointRateLimit_); + jointPosPub_.publish(jointPosMsg); + } + + if(trackCartPos_) + { + wam_msgs::RTCartPos cartPosMsg; - for(int i=0;i < jpCmd_.size();i++) jointPosMsg.joints.push_back(jpCmd_[i]); - jointPosMsg.rate_limits.resize(jpCmd_.size(),jointRateLimit_); - jointPosPub_.publish(jointPosMsg); + for(int i=0;i < cpCmd_.size();i++) + { + cartPosMsg.position[i]=cpCmd_[i]; + cartPosMsg.rate_limits[i]=cartRateLimit_; + } + cartPosPub_.publish(cartPosMsg); + } ros::spinOnce(); loop.sleep(); @@ -207,57 +472,100 @@ 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; +} + int main(int argc,char* argv[]) { ros::init(argc,argv,"wam_node_test"); ros::NodeHandle node; - - WamNodeOp wamCmd(node,50.0,0.01,0.5); + + WamNodeOp wam(node,50.0,0.02,0.5,0.005,0.1); std::vector jpCmd(7,0); jpCmd[1]=-2.0; jpCmd[3]=3.1; - wamCmd.wamMove(jpCmd); - wamCmd.wamWait(jpCmd); + wam.jointPos(jpCmd); jpCmd[0]=M_PI_2; jpCmd[1]=-M_PI_2; jpCmd[3]=M_PI_2; - wamCmd.wamMove(jpCmd); - wamCmd.wamWait(jpCmd); + wam.jointPos(jpCmd); + wam.trackJointPos(false); - wamCmd.spreadMove(M_PI); - wamCmd.spreadWait(M_PI); + wam.openSpread(); + + wam.graspPos(M_PI_2); - wamCmd.fingerMove(M_PI_2); - wamCmd.fingerWait(M_PI_2); + wam.openGrasp(); - wamCmd.fingerMove(2.44); - wamCmd.fingerWait(2.44); + wam.closeGrasp(); - wamCmd.fingerMove(0.0); - wamCmd.fingerWait(0.0); + wam.openGrasp(); - wamCmd.spreadMove(0); - wamCmd.spreadWait(0); + wam.closeSpread(); - wamCmd.fingerMove(2.44,2.44,0.0); - wamCmd.fingerWait(2.44,2.44,0.0); + wam.fingerPos(2.44,2.44,0.0); - wamCmd.fingerMove(2.44); - wamCmd.fingerWait(2.44); + wam.closeGrasp(); - wamCmd.fingerMove(0.0); - wamCmd.fingerWait(0.0); + wam.openGrasp(); - wamCmd.spreadMove(0.0); - wamCmd.spreadWait(0.0); + wam.closeSpread(); + wam.goHome(); + + std::vector cpCmd(3); + 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; + 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); + + cpCmd[0]=-0.6; + cpCmd[1]=0.0; + cpCmd[2]=1.5; + wam.cartPos(cpCmd); + + cpCmd[0]=0.0; + cpCmd[1]=0.0; + cpCmd[2]=1.5; + wam.cartPos(cpCmd); + + cpCmd[0]=0.0; + cpCmd[1]=0.0; + cpCmd[2]=1.76; + wam.cartPos(cpCmd); + wam.trackCartPos(false); + std::fill(jpCmd.begin(),jpCmd.end(),0.0); jpCmd[1]=-2.0; jpCmd[3]=3.1; - wamCmd.wamMove(jpCmd); - wamCmd.wamWait(jpCmd); + wam.jointMove(jpCmd); return 0; }