--- /dev/null
+/* WAM Node Simulator Spline Template Implementation
+ Copyright (c) 2018 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see
+ <http://www.gnu.org/licenses/>.
+
+
+*/
+
+#ifndef WAM_NODE_SIM_SPLINE_IMPL_H_
+#define WAM_NODE_SIM_SPLINE_IMPL_H_
+
+#include <Eigen/Core>
+
+namespace wam_node_sim
+{
+
+template<typename T>
+Spline<T>::Spline(const T &startPos,const T &endPos)
+{
+ startPos_=startPos;
+ disp_=endPos-startPos;
+}
+
+template<typename T>
+double Spline<T>::length(void) const
+{
+ return disp_.norm();
+}
+
+template<typename T>
+T Spline<T>::eval(double s) const
+{
+ return startPos_+s/length()*disp_;
+}
+
+
+template<typename Scalar>
+Spline<Eigen::Quaternion<Scalar> >::Spline(const Eigen::Quaternion<Scalar> &startPos,const Eigen::Quaternion<Scalar> &endPos)
+{
+ startPos_=startPos;
+ endPos_=endPos;
+ dist_=fabs(endPos_.angularDistance(startPos_));
+}
+
+template<typename Scalar>
+Scalar Spline<Eigen::Quaternion<Scalar> >::length(void) const
+{
+ return dist_;
+}
+
+template<typename Scalar>
+Eigen::Quaternion<Scalar> Spline<Eigen::Quaternion<Scalar> >::eval(double s) const
+{
+ return startPos_.slerp(s/dist_,endPos_);
+}
+
+}
+
+#endif /* WAM_NODE_SIM_SPLINE_IMPL_H_ */
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainiksolverpos_lma.hpp>
-
#include <wam_node_sim/rate_limiter.h>
+#include <wam_node_sim/spline.h>
namespace barrett
{
Eigen::Matrix<double,6,1> 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_);
jointPosition_.setZero();
jointVelocity_.setZero();
trackReferenceSignal(jp);
}
+
+template<size_t DOF>
+void Wam<DOF>::trackReferenceSignal(Eigen::Quaterniond &referenceSignal)
+{
+ Eigen::Affine3d t;
+ t.translation()=moveToOrtnToolPos;
+ t.linear()=referenceSignal.toRotationMatrix();
+ std::cout << "t=" << t.matrix() << std::endl;
+ KDL::Frame frame;
+ 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));
+ jp_type jp;
+ jp=q.data;
+ trackReferenceSignal(jp);
+}
+
template<size_t DOF>
inline const typename Wam<DOF>::jp_type& Wam<DOF>::getHomePosition() const
{
if((error=fkSolverPos_->JntToCart(jointPosition,frame)) < 0)
ROS_ERROR_STREAM("Failed to compute forward kinematics: " << fkSolverPos_->strError(error));
- double x,y,z,w;
- frame.M.GetQuaternion(x,y,z,w);
- return Eigen::Quaterniond(w,x,y,z);
+ Eigen::Quaterniond quaternion;
+ tf::quaternionKDLToEigen(frame.M,quaternion);
+ return quaternion;
}
template<size_t DOF>
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);
+ moveToOrtnToolPos=getToolPosition();
+ moveTo(currentPosHelper(getToolOrientation()), destination, blocking, velocity, acceleration);
}
template<typename T>
void Wam<DOF>::moveToThread(const T ¤tPos,const T &destination,double velocity,double acceleration,std::atomic<bool> *started)
{
- static const int PUBLISH_FREQ=250.0;
- int longest;
- (currentPos-destination).cwiseAbs().maxCoeff(&longest);
+ wam_node_sim::Spline<T> spline(currentPos,destination);
- std::vector<KDL::VelocityProfile_Trap> trajectory(currentPos.size());
-
- trajectory[longest].SetMax(velocity,acceleration);
- trajectory[longest].SetProfile(currentPos[longest],destination[longest]);
- double tf=trajectory[longest].Duration();
-
- for(int i=0;i < currentPos.size();i++)
- {
- if(i == longest) continue;
- trajectory[i].SetMax(velocity,acceleration);
- trajectory[i].SetProfileDuration(currentPos[i],destination[i],tf);
- }
+ KDL::VelocityProfile_Trap profile;
+ profile.SetMax(velocity,acceleration);
+ profile.SetProfile(0,spline.length());
+ double tf=profile.Duration();
doneMoving=false;
*started=true;
- T limit=T::Constant(2.0);
- wam_node_sim::RateLimiter<T> rateLimiter(limit,1.0/PUBLISH_FREQ);
- rateLimiter.setCurVal(currentPos);
+// T limit=T::Constant(2.0);
+ static const int PUBLISH_FREQ=250.0;
+// wam_node_sim::RateLimiter<T> 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;
- for(size_t i=0; i < pos.size();i++) pos[i]=trajectory[i].Pos(t);
- trackReferenceSignal(rateLimiter.getLimit(pos));
+ T pos=spline.eval(profile.Pos(t));
+// trackReferenceSignal(rateLimiter.getLimit(pos));
+ trackReferenceSignal(pos);
loop.sleep();
}
- trackReferenceSignal(rateLimiter.getLimit(destination));
+// trackReferenceSignal(rateLimiter.getLimit(destination));
+ T pos=destination;
+ trackReferenceSignal(pos);
doneMoving=true;
}
--- /dev/null
+/* WAM Node Simulator Spline Template
+ Copyright (c) 2018 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see
+ <http://www.gnu.org/licenses/>.
+
+
+*/
+
+#ifndef WAM_NODE_SIM_SPLINE_H_
+#define WAM_NODE_SIM_SPLINE_H_
+
+#include <Eigen/Core>
+
+namespace wam_node_sim
+{
+
+template<typename T>
+class Spline
+{
+ T startPos_;
+ T disp_;
+
+ public:
+ Spline(const T &startPos,const T &endPos);
+ double length(void) const;
+ T eval(double s) const;
+};
+
+template<typename Scalar>
+class Spline<Eigen::Quaternion<Scalar> >
+{
+ Eigen::Quaternion<Scalar> startPos_;
+ Eigen::Quaternion<Scalar> endPos_;
+ Scalar dist_;
+
+ public:
+ Spline(const Eigen::Quaternion<Scalar> &startPos,const Eigen::Quaternion<Scalar> &endPos);
+ Scalar length(void) const;
+ Eigen::Quaternion<Scalar> eval(double s) const;
+};
+
+}
+
+
+// include template definitions
+#include <wam_node_sim/detail/spline-impl.h>
+
+
+#endif /* WAM_NODE_SIM_SPLINE_H_ */
projects / ufrgs_wam.git / commitdiff