From: Walter Fetter Lages Date: Sat, 29 Dec 2018 05:22:03 +0000 (-0200) Subject: Add /wam/ortn_move rosservice. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=5bfe236ae3f7159477464e33be9942829ff5a398;p=ufrgs_wam.git Add /wam/ortn_move rosservice. --- diff --git a/wam_node_sim/include/wam_node_sim/detail/spline-impl.h b/wam_node_sim/include/wam_node_sim/detail/spline-impl.h new file mode 100644 index 0000000..4026e9a --- /dev/null +++ b/wam_node_sim/include/wam_node_sim/detail/spline-impl.h @@ -0,0 +1,71 @@ +/* WAM Node Simulator Spline Template Implementation + Copyright (c) 2018 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 + 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 + . + + +*/ + +#ifndef WAM_NODE_SIM_SPLINE_IMPL_H_ +#define WAM_NODE_SIM_SPLINE_IMPL_H_ + +#include + +namespace wam_node_sim +{ + +template +Spline::Spline(const T &startPos,const T &endPos) +{ + startPos_=startPos; + disp_=endPos-startPos; +} + +template +double Spline::length(void) const +{ + return disp_.norm(); +} + +template +T Spline::eval(double s) const +{ + return startPos_+s/length()*disp_; +} + + +template +Spline >::Spline(const Eigen::Quaternion &startPos,const Eigen::Quaternion &endPos) +{ + startPos_=startPos; + endPos_=endPos; + dist_=fabs(endPos_.angularDistance(startPos_)); +} + +template +Scalar Spline >::length(void) const +{ + return dist_; +} + +template +Eigen::Quaternion Spline >::eval(double s) const +{ + return startPos_.slerp(s/dist_,endPos_); +} + +} + +#endif /* WAM_NODE_SIM_SPLINE_IMPL_H_ */ 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 37cdb8f..097b705 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 @@ -55,8 +55,8 @@ #include #include - #include +#include namespace barrett { @@ -85,6 +85,7 @@ Wam::Wam(Hand *hand,const std::string& sysName) 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_); jointPosition_.setZero(); jointVelocity_.setZero(); @@ -135,6 +136,29 @@ void Wam::trackReferenceSignal(cp_type &referenceSignal) trackReferenceSignal(jp); } + +template +void Wam::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 inline const typename Wam::jp_type& Wam::getHomePosition() const { @@ -199,9 +223,9 @@ Eigen::Quaterniond Wam::getToolOrientation() 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 @@ -275,7 +299,8 @@ inline void Wam::moveTo(const cp_type& destination, bool blocking, double v template inline void Wam::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); } @@ -345,42 +370,35 @@ template template void Wam::moveToThread(const T ¤tPos,const T &destination,double velocity,double acceleration,std::atomic *started) { - static const int PUBLISH_FREQ=250.0; - int longest; - (currentPos-destination).cwiseAbs().maxCoeff(&longest); + wam_node_sim::Spline spline(currentPos,destination); - std::vector 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 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 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; } diff --git a/wam_node_sim/include/wam_node_sim/spline.h b/wam_node_sim/include/wam_node_sim/spline.h new file mode 100644 index 0000000..2c71bfa --- /dev/null +++ b/wam_node_sim/include/wam_node_sim/spline.h @@ -0,0 +1,61 @@ +/* WAM Node Simulator Spline Template + Copyright (c) 2018 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 + 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 + . + + +*/ + +#ifndef WAM_NODE_SIM_SPLINE_H_ +#define WAM_NODE_SIM_SPLINE_H_ + +#include + +namespace wam_node_sim +{ + +template +class Spline +{ + T startPos_; + T disp_; + + public: + Spline(const T &startPos,const T &endPos); + double length(void) const; + T eval(double s) const; +}; + +template +class Spline > +{ + Eigen::Quaternion startPos_; + Eigen::Quaternion endPos_; + Scalar dist_; + + public: + Spline(const Eigen::Quaternion &startPos,const Eigen::Quaternion &endPos); + Scalar length(void) const; + Eigen::Quaternion eval(double s) const; +}; + +} + + +// include template definitions +#include + + +#endif /* WAM_NODE_SIM_SPLINE_H_ */ diff --git a/wam_node_sim/include/wam_node_sim/wam.h b/wam_node_sim/include/wam_node_sim/wam.h index 2a2160c..7c49a27 100644 --- a/wam_node_sim/include/wam_node_sim/wam.h +++ b/wam_node_sim/include/wam_node_sim/wam.h @@ -83,6 +83,7 @@ public: */ void trackReferenceSignal(jp_type & referenceSignal); //NOLINT: non-const reference for syntax void trackReferenceSignal(cp_type & referenceSignal); //NOLINT: non-const reference for syntax + void trackReferenceSignal(Eigen::Quaterniond & referenceSignal); //NOLINT: non-const reference for syntax /** getHomePosition() returns home postion of individual joints in Radians */ @@ -180,6 +181,7 @@ private: KDL::Chain chain_; KDL::ChainFkSolverPos *fkSolverPos_; KDL::ChainIkSolverPos *ikSolverPos_; + KDL::ChainIkSolverPos *ikSolverPose_; ros::Publisher wam_cmd_pub; // controller command @@ -189,6 +191,8 @@ private: jt_type jointEffort_; Hand *hand_; + cp_type moveToOrtnToolPos; + void jointStatesCB(const sensor_msgs::JointState &jointStates); public: diff --git a/wam_node_sim/scripts/ortn_move.sh b/wam_node_sim/scripts/ortn_move.sh new file mode 100755 index 0000000..5b027c4 --- /dev/null +++ b/wam_node_sim/scripts/ortn_move.sh @@ -0,0 +1,7 @@ +#!/bin/bash + +if [ "$#" -ne 4 ]; then + echo "Error: There should be 4 parameters." + exit -1; +fi; +rosservice call /wam/ortn_move "[$1, $2, $3, $4]" diff --git a/wam_node_sim/src/wam_node_sim.cpp b/wam_node_sim/src/wam_node_sim.cpp index b645361..5b74d3a 100644 --- a/wam_node_sim/src/wam_node_sim.cpp +++ b/wam_node_sim/src/wam_node_sim.cpp @@ -274,7 +274,7 @@ template 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 + ortn_move_srv = n_.advertiseService("ortn_move", &WamNode::ortnMove, this); // wam/ortn_move }