From: Walter Fetter Lages Date: Sun, 23 Dec 2018 05:19:03 +0000 (-0200) Subject: Add joint_move service. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=0d8ae8217021048fa4cbc1a9c64f357b95cb8c5c;p=ufrgs_wam.git Add joint_move service. --- 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 5b231e1..2b2743c 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 @@ -39,12 +39,8 @@ #ifndef WAM_NODE_SIM_WAM_IMPL_H_ #define WAM_NODE_SIM_WAM_IMPL_H_ -//#include // usleep #include - -//#include -//#include -//#include +#include //#define EIGEN_USE_NEW_STDVECTOR //#include @@ -55,6 +51,9 @@ #include #include +#include + +//#include namespace barrett { @@ -81,6 +80,10 @@ Wam::Wam(Hand *hand,const std::string& sysName) ROS_ERROR("Failed to get chain from KDL tree."); fwdKinSolverPos_=new KDL::ChainFkSolverPos_recursive(chain_); + + jointPosition_.setZero(); + jointVelocity_.setZero(); + jointEffort_.setZero(); } template @@ -106,7 +109,6 @@ void Wam::trackReferenceSignal(jp_type &referenceSignal) wam_cmd_pub.publish(jointRef); } - template inline const typename Wam::jp_type& Wam::getHomePosition() const { @@ -118,24 +120,28 @@ inline const typename Wam::jp_type& Wam::getHomePosition() const template typename Wam::jt_type Wam::getJointTorques() const { + ros::spinOnce(); return jointEffort_; } template inline typename Wam::jp_type Wam::getJointPositions() const { + ros::spinOnce(); return jointPosition_; } template inline typename Wam::jv_type Wam::getJointVelocities() const { + ros::spinOnce(); return jointVelocity_; } template typename Wam::cp_type Wam::getToolPosition() const { + ros::spinOnce(); KDL::JntArray jointPosition(DOF); for(int i;i < DOF;i++) jointPosition(i)=jointPosition_[i]; @@ -157,7 +163,8 @@ typename Wam::cv_type Wam::getToolVelocity() const template Eigen::Quaterniond Wam::getToolOrientation() const -{ +{ + ros::spinOnce(); KDL::JntArray jointPosition(DOF); for(int i;i < DOF;i++) jointPosition(i)=jointPosition_[i]; @@ -228,19 +235,19 @@ template inline void Wam::moveTo(const jp_type& destination, bool blocking, double velocity, double acceleration) { // moveTo(currentPosHelper(getJointPositions()), getJointVelocities(), destination, blocking, velocity, acceleration); -// moveTo(currentPosHelper(getJointPositions()), /*jv_type(0.0),*/ destination, blocking, velocity, acceleration); + moveTo(currentPosHelper(getJointPositions()), /*jv_type(0.0),*/ destination, blocking, velocity, acceleration); } template inline void Wam::moveTo(const cp_type& destination, bool blocking, double velocity, double acceleration) { -// moveTo(currentPosHelper(getToolPosition()), /*cv_type(0.0),*/ destination, blocking, velocity, acceleration); + moveTo(currentPosHelper(getToolPosition()), /*cv_type(0.0),*/ destination, blocking, velocity, acceleration); } template inline void Wam::moveTo(const Eigen::Quaterniond& destination, bool blocking, double velocity, double acceleration) { -// moveTo(currentPosHelper(getToolOrientation()), destination, blocking, velocity, acceleration); + moveTo(currentPosHelper(getToolOrientation()), destination, blocking, velocity, acceleration); } /* @@ -255,24 +262,31 @@ template template void Wam::moveTo(const T& currentPos, /*const typename T::unitless_type& currentVel,*/ const T& destination, bool blocking, double velocity, double acceleration) { - bool started = false; + std::atomic started; + started = false; // boost::promise threadPtrPromise; // boost::shared_future threadPtrFuture(threadPtrPromise.get_future()); // boost::thread* threadPtr = new boost::thread(&Wam::moveToThread, this, boost::ref(currentPos), /*currentVel,*/ boost::ref(destination), velocity, acceleration, &started, threadPtrFuture); // mtThreadGroup.add_thread(threadPtr); // threadPtrPromise.set_value(threadPtr); + std::thread thread; +// if(std::is_same::value) +// thread=std::thread(&Wam::moveToThread,this,currentPos,destination,velocity,acceleration,&started); +// else + thread=std::thread(&Wam::moveToThread,this,currentPos,destination,velocity,acceleration,&started); // wait until move starts while ( !started ) { -// btsleep(0.001); + ros::Duration(0.001).sleep(); } - + if (blocking) { - while (!moveIsDone()) { -// btsleep(0.01); - } - } +// while (!moveIsDone()) { +// ros::Duration(0.01).sleep(); +// } + thread.join(); + } else thread.detach(); } template @@ -288,14 +302,140 @@ void Wam::idle() } -//template -//template -//void Wam::moveToThread(const T& currentPos, /*const typename T::unitless_type& currentVel,*/ const T& destination, double velocity, double acceleration, bool* started, boost::shared_future threadPtrFuture) -//{ +template +template +T Wam::currentPosHelper(const T& currentPos) +{ +// System::Input* input = NULL; +// supervisoryController.getInput(&input); +// if (input != NULL && input->valueDefined()) { + // If we're already using the controller we're about to use in the + // moveTo() call, it's best to treat the current *set point* as the + // "current position". That way the reference signal is continuous as it + // transitions from whatever is happening now to the moveTo() Spline. +// return input->getValue(); +// } + + // If we're not using the appropriate controller already, then use our + // *actual* current position. + return currentPos; +} +template +template +void Wam::moveToThread(const T ¤tPos,const T &destination,double velocity,double acceleration,std::atomic *started) +{ + if(!std::is_same::value) + { + ROS_WARN_STREAM("moveToThread template does not support this type."); + return; + } + static const int PUBLISH_FREQ=250.0; + int longest; + (currentPos-destination).cwiseAbs().maxCoeff(&longest); + + std::vector trajectory(DOF); -//} + trajectory[longest].SetMax(velocity,acceleration); + trajectory[longest].SetProfile(currentPos[longest],destination[longest]); + double tf=trajectory[longest].Duration(); + + if(tf < 1.0/PUBLISH_FREQ) tf=1.0/PUBLISH_FREQ; + for(int i=0;i < DOF;i++) + { + trajectory[i].SetMax(velocity,acceleration); + trajectory[i].SetProfileDuration(currentPos[i],destination[i],tf); + } + + doneMoving=false; + *started=true; + + jp_type jpLimit=jp_type::Constant(2.0); + wam_node_sim::RateLimiter jpRateLimiter(jpLimit,1.0/PUBLISH_FREQ); + jp_type jp; + for(int i=0;i < DOF;i++) jp[i]=currentPos[i]; + jpRateLimiter.setCurVal(jp); + + 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) + { + for(size_t i=0; i < DOF;i++) jp[i]=trajectory[i].Pos(t); + trackReferenceSignal(jpRateLimiter.getLimit(jp)); + + loop.sleep(); + } + for(int i=0;i < DOF;i++) jp[i]=destination[i]; + trackReferenceSignal(jpRateLimiter.getLimit(jp)); + doneMoving=true; +} + +/* +template +template +*/ +//void Wam::moveToThread(const T& currentPos, /*const typename T::unitless_type& currentVel,*/ const T& destination, double velocity, double acceleration, bool* started, boost::shared_future threadPtrFuture) +/* +void Wam::moveToThread(const T& currentPos, const T& destination, double velocity, double acceleration, std::atomic* started) +{ + + // Only remove this thread from mtThreadGroup on orderly exit. (Don't remove on exception.) +// bool removeThread = false; + +// try { +// std::vector > vec; +// vec.push_back(currentPos); +// vec.push_back(destination); + + // TODO(dc): Use currentVel. Requires changes to math::spline > specialization. + //math::Spline spline(vec, currentVel); + //math::TrapezoidalVelocityProfile profile(velocity, acceleration, currentVel.norm(), spline.changeInS()); +// math::Spline spline(vec); +// math::TrapezoidalVelocityProfile profile(velocity, acceleration, 0.0, spline.changeInS()); + +// Ramp time(NULL, 1.0); +// Callback trajectory(boost::bind(boost::ref(spline), boost::bind(boost::ref(profile), _1))); + +// connect(time.output, trajectory.input); +// trackReferenceSignal(trajectory.output); +// time.start(); + + doneMoving = false; + *started = true; + + // The value of trajectory.input will be undefined until the next execution cycle. + // It may become undefined again if trajectory.output is disconnected and this chain + // of Systems loses its ExecutionManager. We must check that the value is defined + // before calling getValue(). +// while ( !trajectory.input.valueDefined() || trajectory.input.getValue() < profile.finalT()) { + ROS_WARN_STREAM("moveToThread template not implemented."); + // if the move is interrupted, clean up and end the thread +// if ( !trajectory.output.isConnected() || boost::this_thread::interruption_requested() ) { +// removeThread = true; +// break; +// } +// ros::Duration(0.01).sleep(); // Interruption point. May throw boost::thread_interrupted +// } + +// if ( !removeThread ) { + doneMoving = true; + + // Wait until the trajectory is no longer referenced by supervisoryController +// while (trajectory.hasExecutionManager() && !boost::this_thread::interruption_requested()) { +// ros::Duration(0.01).sleep(); // Interruption point. May throw boost::thread_interrupted +// } +// removeThread = true; +// } +// } catch (const std::exception &e) {ROS_WARN_STREAM("wam-impl: " << e.what());} + +// if (removeThread) { +// mtThreadGroup.remove_thread(threadPtrFuture.get()); +// delete threadPtrFuture.get(); +// } +} +*/ template void Wam::jointStatesCB(const sensor_msgs::JointState &jointStates) { diff --git a/wam_node_sim/include/wam_node_sim/wam.h b/wam_node_sim/include/wam_node_sim/wam.h index 3a40716..ee6b9b2 100644 --- a/wam_node_sim/include/wam_node_sim/wam.h +++ b/wam_node_sim/include/wam_node_sim/wam.h @@ -43,6 +43,7 @@ //#include +#include //#include #include @@ -154,7 +155,7 @@ public: void moveTo(const cp_type& destination, bool blocking = true, double velocity = 0.1, double acceleration = 0.2); void moveTo(const Eigen::Quaterniond& destination, bool blocking = true, double velocity = 0.5, double acceleration = 0.5); // void moveTo(const pose_type& destination, bool blocking = true, double velocity = 0.1, double acceleration = 0.2); - template void moveTo(const T& currentPos, /*const typename T::unitless_type& currentVel,*/ const T& destination, bool blocking, double velocity, double acceleration); + template void moveTo(const T& currentPos, const T& destination, bool blocking, double velocity, double acceleration); /** moveIsDone() method returns false while the trajectory controller for the most recent moveTo() command is still active. * @@ -174,8 +175,12 @@ public: private: + template T currentPosHelper(const T& currentPos); +// template void moveToThread(const T& currentPos, const T& destination, double velocity, double acceleration, bool* started, boost::shared_future threadPtrFuture); + template void moveToThread(const T& currentPos, const T& destination, double velocity, double acceleration, std::atomic* started); +// void moveToThread(const jp_type ¤tPos,const jp_type &destination,double velocity,double acceleration,std::atomic *started); - bool doneMoving; + std::atomic doneMoving; // boost::thread_group mtThreadGroup; // Used to calculate TP and TO if the values aren't already being calculated in the control loop. diff --git a/wam_node_sim/scripts/go_home.sh b/wam_node_sim/scripts/go_home.sh new file mode 100755 index 0000000..c188249 --- /dev/null +++ b/wam_node_sim/scripts/go_home.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +rosservice call /wam/go_home diff --git a/wam_node_sim/scripts/joint_move.sh b/wam_node_sim/scripts/joint_move.sh new file mode 100755 index 0000000..bde929e --- /dev/null +++ b/wam_node_sim/scripts/joint_move.sh @@ -0,0 +1,7 @@ +#!/bin/bash + +if [ "$#" -ne 7 ]; then + echo "Error: There should be 7 parameters." + exit -1; +fi; +rosservice call /wam/joint_move "[$1, $2, $3, $4, $5, $6, $7]" diff --git a/wam_node_sim/src/hand.cpp b/wam_node_sim/src/hand.cpp index ccad25a..f8292e2 100644 --- a/wam_node_sim/src/hand.cpp +++ b/wam_node_sim/src/hand.cpp @@ -193,11 +193,12 @@ void Hand::close(unsigned int whichDigits, bool blocking) const { /** trapezoidalMove Method */ void Hand::trapezoidalMove(const jp_type& jp, unsigned int whichDigits, bool blocking) const { + ros::spinOnce(); // necessary to update jointPosition_ jp_type finalJp; for(int i=0;i < DOF;i++) finalJp[i]=digitsInclude(whichDigits,i)? jp[i]:jointPosition_[i]; int longest; - (jp-finalJp).cwiseAbs().maxCoeff(&longest); + (jointPosition_-finalJp).cwiseAbs().maxCoeff(&longest); std::vector trajectory(DOF); @@ -211,7 +212,6 @@ void Hand::trapezoidalMove(const jp_type& jp, unsigned int whichDigits, bool blo if(i == longest) continue; if(i!=SPREAD_INDEX) trajectory[i].SetMax(J2_MAX_VEL,J2_MAX_ACCEL); else trajectory[i].SetMax(SPREAD_MAX_VEL,SPREAD_MAX_ACCEL); - trajectory[i].SetMax(0,0); trajectory[i].SetProfileDuration(jointPosition_[i],finalJp[i],tf); } @@ -221,19 +221,19 @@ void Hand::trapezoidalMove(const jp_type& jp, unsigned int whichDigits, bool blo commandMsg.layout.dim[0].size=DOF; commandMsg.layout.dim[0].stride=DOF; commandMsg.layout.data_offset=0; + commandMsg.data.resize(DOF); ros::Rate loop(BHAND_PUBLISH_FREQ); double t=0.0; ros::Time t0=ros::Time::now(); while(ros::ok() && (t=(ros::Time::now()-t0).toSec()) < tf) { - for(size_t i=0; i < DOF;i++) commandMsg.data.push_back(trajectory[i].Pos(t)); + for(size_t i=0; i < DOF;i++) commandMsg.data[i]=trajectory[i].Pos(t); commandPub_.publish(commandMsg); - - ros::spinOnce(); + loop.sleep(); } - for(size_t i=0; i < DOF;i++) commandMsg.data.push_back(finalJp[i]); + for(size_t i=0; i < DOF;i++) commandMsg.data[i]=finalJp[i]; commandPub_.publish(commandMsg); blockIf(blocking, whichDigits); diff --git a/wam_node_sim/src/wam_node_sim.cpp b/wam_node_sim/src/wam_node_sim.cpp index b6a3d70..312b5d1 100644 --- a/wam_node_sim/src/wam_node_sim.cpp +++ b/wam_node_sim/src/wam_node_sim.cpp @@ -265,11 +265,11 @@ template //Advertising the following rosservices // gravity_srv = n_.advertiseService("gravity_comp", &WamNode::gravity, this); // wam/gravity_comp -// go_home_srv = n_.advertiseService("go_home", &WamNode::goHome, this); // wam/go_home + go_home_srv = n_.advertiseService("go_home", &WamNode::goHome, this); // wam/go_home hold_jpos_srv = n_.advertiseService("hold_joint_pos", &WamNode::holdJPos, this); // wam/hold_joint_pos hold_cpos_srv = n_.advertiseService("hold_cart_pos", &WamNode::holdCPos, this); // wam/hold_cart_pos hold_ortn_srv = n_.advertiseService("hold_ortn", &WamNode::holdOrtn, this); // wam/hold_ortn -// joint_move_srv = n_.advertiseService("joint_move", &WamNode::jointMove, this); // wam/joint_move + joint_move_srv = n_.advertiseService("joint_move", &WamNode::jointMove, this); // wam/joint_move // pose_move_srv = n_.advertiseService("pose_move", &WamNode::poseMove, this); // wam/pose_move // cart_move_srv = n_.advertiseService("cart_move", &WamNode::cartMove, this); // wam/cart_pos_move // ortn_move_srv = n_.advertiseService("ortn_move", &WamNode::ortnMove, this); // wam/ortn_move @@ -827,7 +827,8 @@ int main(int argc,char *argv[]) wam_node.updateRT(); loop.sleep(); } - - handPubThread.join(); + + if(useHand) handPubThread.join(); + return 0; } diff --git a/wam_node_test/src/wam_node_test.cpp b/wam_node_test/src/wam_node_test.cpp index 37d9aea..5d70e87 100644 --- a/wam_node_test/src/wam_node_test.cpp +++ b/wam_node_test/src/wam_node_test.cpp @@ -215,6 +215,13 @@ int main(int argc,char* argv[]) WamNodeOp wamCmd(node,50.0,0.01,0.5); std::vector jpCmd(7,0); + jpCmd[1]=-2.0; + jpCmd[3]=3.1; + wamCmd.wamMove(jpCmd); + wamCmd.wamWait(jpCmd); + + jpCmd[0]=M_PI_2; + jpCmd[1]=-M_PI_2; jpCmd[3]=M_PI_2; wamCmd.wamMove(jpCmd); wamCmd.wamWait(jpCmd);