From: Walter Fetter Lages Date: Sun, 30 Dec 2018 04:40:43 +0000 (-0200) Subject: Add /wam/pose_move rosservice. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=28ae075dae3dd708d0ff5a87892b91ecad0a8d58;p=ufrgs_wam.git Add /wam/pose_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 index 4026e9a..44c23c0 100644 --- a/wam_node_sim/include/wam_node_sim/detail/spline-impl.h +++ b/wam_node_sim/include/wam_node_sim/detail/spline-impl.h @@ -66,6 +66,30 @@ Eigen::Quaternion Spline >::eval(double s) con return startPos_.slerp(s/dist_,endPos_); } +template +Spline >::Spline(const std::tuple &startPos,const std::tuple &endPos) +{ + startPos_=startPos; + endPos_=endPos; + dist_=std::max( + (std::get<0>(endPos_)-std::get<0>(startPos_)).norm(), + fabs(std::get<1>(endPos_).angularDistance(std::get<1>(startPos_)))); +} + +template +double Spline >::length(void) const +{ + return dist_; +} + +template +std::tuple Spline >::eval(double s) const +{ + return std::make_tuple( + std::get<0>(startPos_)+s/dist_*(std::get<0>(endPos_)-std::get<0>(startPos_)), + std::get<1>(startPos_).slerp(s/dist_,std::get<1>(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 2e35a32..47a5390 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 @@ -136,7 +136,6 @@ void Wam::trackReferenceSignal(cp_type &referenceSignal) trackReferenceSignal(jp); } - template void Wam::trackReferenceSignal(Eigen::Quaterniond &referenceSignal) { @@ -159,6 +158,27 @@ void Wam::trackReferenceSignal(Eigen::Quaterniond &referenceSignal) } template +void Wam::trackReferenceSignal(pose_type &referenceSignal) +{ + Eigen::Affine3d t; + t.translation()=std::get<0>(referenceSignal); + t.linear()=std::get<1>(referenceSignal).toRotationMatrix(); + 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 { static jp_type home={0.0, -2.0, 0.0, 3.1, 0.0, 0.0, 0.0}; @@ -306,7 +326,7 @@ inline void Wam::moveTo(const Eigen::Quaterniond& destination, bool blockin template inline void Wam::moveTo(const pose_type& destination, bool blocking, double velocity, double acceleration) { -// moveTo(currentPosHelper(getToolPose()), destination, blocking, velocity, acceleration); + moveTo(currentPosHelper(getToolPose()), destination, blocking, velocity, acceleration); } diff --git a/wam_node_sim/include/wam_node_sim/rate_limiter.h b/wam_node_sim/include/wam_node_sim/rate_limiter.h index bf8a176..913d6f2 100644 --- a/wam_node_sim/include/wam_node_sim/rate_limiter.h +++ b/wam_node_sim/include/wam_node_sim/rate_limiter.h @@ -81,6 +81,10 @@ class RateLimiter T data; T delta; + + public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; }; diff --git a/wam_node_sim/include/wam_node_sim/spline.h b/wam_node_sim/include/wam_node_sim/spline.h index 2c71bfa..7f00a80 100644 --- a/wam_node_sim/include/wam_node_sim/spline.h +++ b/wam_node_sim/include/wam_node_sim/spline.h @@ -36,6 +36,8 @@ class Spline Spline(const T &startPos,const T &endPos); double length(void) const; T eval(double s) const; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; }; template @@ -49,6 +51,23 @@ class Spline > Spline(const Eigen::Quaternion &startPos,const Eigen::Quaternion &endPos); Scalar length(void) const; Eigen::Quaternion eval(double s) const; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; +}; + +template +class Spline > +{ + std::tuple startPos_; + std::tuple endPos_; + double dist_; + + public: + Spline(const std::tuple &startPos,const std::tuple &endPos); + double length(void) const; + std::tuple eval(double s) const; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; }; } diff --git a/wam_node_sim/include/wam_node_sim/wam.h b/wam_node_sim/include/wam_node_sim/wam.h index 7c49a27..89dcbf7 100644 --- a/wam_node_sim/include/wam_node_sim/wam.h +++ b/wam_node_sim/include/wam_node_sim/wam.h @@ -84,6 +84,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 + void trackReferenceSignal(pose_type & referenceSignal); //NOLINT: non-const reference for syntax /** getHomePosition() returns home postion of individual joints in Radians */ diff --git a/wam_node_sim/src/wam_node_sim.cpp b/wam_node_sim/src/wam_node_sim.cpp index 5b74d3a..460e917 100644 --- a/wam_node_sim/src/wam_node_sim.cpp +++ b/wam_node_sim/src/wam_node_sim.cpp @@ -344,15 +344,14 @@ template bool WamNode::holdOrtn(wam_srvs::Hold::Request &req, wam_srvs::Hold::Response &res) { ROS_INFO("Orientation Hold request: %s", (req.hold) ? "true" : "false"); -/* + if (req.hold) { - orientationSetPoint.setValue(wam.getToolOrientation()); - wam.trackReferenceSignal(orientationSetPoint.output); + wam.moveTo(wam.getToolOrientation()); } else wam.idle(); -*/ + return true; } @@ -387,11 +386,12 @@ template ortn_cmd.z() = req.pose.orientation.z; ortn_cmd.w() = req.pose.orientation.w; - pose_cmd = std::make_tuple(cp_cmd, ortn_cmd); + pose_cmd=std::make_tuple(cp_cmd,ortn_cmd); - //wam.moveTo(pose_cmd, false); //(TODO:KM Update Libbarrett API for Pose Moves) - ROS_INFO("Pose Commands for WAM not yet supported by API"); - return false; + wam.moveTo(pose_cmd, false); //(TODO:KM Update Libbarrett API for Pose Moves) +// ROS_WARN("Pose Commands for WAM not yet supported by API"); +// return false; + return true; } //Function to command a cartesian move to the WAM