#ifndef WAM_NODE_SIM_WAM_IMPL_H_
#define WAM_NODE_SIM_WAM_IMPL_H_
-//#include <unistd.h> // usleep
#include <string>
-
-//#include <boost/ref.hpp>
-//#include <boost/bind.hpp>
-//#include <boost/thread.hpp>
+#include <thread>
//#define EIGEN_USE_NEW_STDVECTOR
//#include <Eigen/StdVector>
#include <kdl/frames.hpp>
#include <kdl_parser/kdl_parser.hpp>
+#include <kdl/velocityprofile_trap.hpp>
+
+//#include <wam_node_sim/rate_limiter.h>
namespace barrett
{
ROS_ERROR("Failed to get chain from KDL tree.");
fwdKinSolverPos_=new KDL::ChainFkSolverPos_recursive(chain_);
+
+ jointPosition_.setZero();
+ jointVelocity_.setZero();
+ jointEffort_.setZero();
}
template<size_t DOF>
wam_cmd_pub.publish(jointRef);
}
-
template<size_t DOF>
inline const typename Wam<DOF>::jp_type& Wam<DOF>::getHomePosition() const
{
template<size_t DOF>
typename Wam<DOF>::jt_type Wam<DOF>::getJointTorques() const
{
+ ros::spinOnce();
return jointEffort_;
}
template<size_t DOF>
inline typename Wam<DOF>::jp_type Wam<DOF>::getJointPositions() const
{
+ ros::spinOnce();
return jointPosition_;
}
template<size_t DOF>
inline typename Wam<DOF>::jv_type Wam<DOF>::getJointVelocities() const
{
+ ros::spinOnce();
return jointVelocity_;
}
template<size_t DOF>
typename Wam<DOF>::cp_type Wam<DOF>::getToolPosition() const
{
+ ros::spinOnce();
KDL::JntArray jointPosition(DOF);
for(int i;i < DOF;i++) jointPosition(i)=jointPosition_[i];
template<size_t DOF>
Eigen::Quaterniond Wam<DOF>::getToolOrientation() const
-{
+{
+ ros::spinOnce();
KDL::JntArray jointPosition(DOF);
for(int i;i < DOF;i++) jointPosition(i)=jointPosition_[i];
inline void Wam<DOF>::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<size_t DOF>
inline void Wam<DOF>::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<size_t DOF>
inline void Wam<DOF>::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);
}
/*
template<typename T>
void Wam<DOF>::moveTo(const T& currentPos, /*const typename T::unitless_type& currentVel,*/ const T& destination, bool blocking, double velocity, double acceleration)
{
- bool started = false;
+ std::atomic<bool> started;
+ started = false;
// boost::promise<boost::thread*> threadPtrPromise;
// boost::shared_future<boost::thread*> threadPtrFuture(threadPtrPromise.get_future());
// boost::thread* threadPtr = new boost::thread(&Wam<DOF>::moveToThread<T>, 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<T,jp_type>::value)
+// thread=std::thread(&Wam<DOF>::moveToThread,this,currentPos,destination,velocity,acceleration,&started);
+// else
+ thread=std::thread(&Wam<DOF>::moveToThread<T>,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<size_t DOF>
}
-//template<size_t DOF>
-//template<typename T>
-//void Wam<DOF>::moveToThread(const T& currentPos, /*const typename T::unitless_type& currentVel,*/ const T& destination, double velocity, double acceleration, bool* started, boost::shared_future<boost::thread*> threadPtrFuture)
-//{
+template<size_t DOF>
+template<typename T>
+T Wam<DOF>::currentPosHelper(const T& currentPos)
+{
+// System::Input<T>* 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<size_t DOF>
+template<typename T>
+void Wam<DOF>::moveToThread(const T ¤tPos,const T &destination,double velocity,double acceleration,std::atomic<bool> *started)
+{
+ if(!std::is_same<T,jp_type>::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<KDL::VelocityProfile_Trap> 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<jp_type> 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<size_t DOF>
+template<typename T>
+*/
+//void Wam<DOF>::moveToThread(const T& currentPos, /*const typename T::unitless_type& currentVel,*/ const T& destination, double velocity, double acceleration, bool* started, boost::shared_future<boost::thread*> threadPtrFuture)
+/*
+void Wam<DOF>::moveToThread(const T& currentPos, const T& destination, double velocity, double acceleration, std::atomic<bool>* started)
+{
+
+ // Only remove this thread from mtThreadGroup on orderly exit. (Don't remove on exception.)
+// bool removeThread = false;
+
+// try {
+// std::vector<T, Eigen::aligned_allocator<T> > vec;
+// vec.push_back(currentPos);
+// vec.push_back(destination);
+
+ // TODO(dc): Use currentVel. Requires changes to math::spline<Eigen::Quaternion<T> > specialization.
+ //math::Spline<T> spline(vec, currentVel);
+ //math::TrapezoidalVelocityProfile profile(velocity, acceleration, currentVel.norm(), spline.changeInS());
+// math::Spline<T> spline(vec);
+// math::TrapezoidalVelocityProfile profile(velocity, acceleration, 0.0, spline.changeInS());
+
+// Ramp time(NULL, 1.0);
+// Callback<double, T> 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<size_t DOF>
void Wam<DOF>::jointStatesCB(const sensor_msgs::JointState &jointStates)
{
projects / ufrgs_wam.git / commitdiff