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 (file)
--- 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<Scalar> Spline<Eigen::Quaternion<Scalar> >::eval(double s) con
        return startPos_.slerp(s/dist_,endPos_);
 }
 
+template<typename T0,typename T1>
+Spline<std::tuple<T0,T1> >::Spline(const std::tuple<T0,T1> &startPos,const std::tuple<T0,T1> &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<typename T0,typename T1>
+double Spline<std::tuple<T0,T1> >::length(void) const
+{
+       return dist_;
+}
+
+template<typename T0,typename T1>
+std::tuple<T0,T1> Spline<std::tuple<T0,T1> >::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 (file)
--- 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<DOF>::trackReferenceSignal(cp_type &referenceSignal)
        trackReferenceSignal(jp);
 }
 
-
 template<size_t DOF>
 void Wam<DOF>::trackReferenceSignal(Eigen::Quaterniond &referenceSignal)
 {      
@@ -159,6 +158,27 @@ void Wam<DOF>::trackReferenceSignal(Eigen::Quaterniond &referenceSignal)
 }
 
 template<size_t DOF>
+void Wam<DOF>::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<size_t DOF>
 inline const typename Wam<DOF>::jp_type& Wam<DOF>::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<DOF>::moveTo(const Eigen::Quaterniond& destination, bool blockin
 template<size_t DOF>
 inline void Wam<DOF>::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 (file)
--- 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 (file)
--- 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<typename Scalar>
@@ -49,6 +51,23 @@ class Spline<Eigen::Quaternion<Scalar> >
        Spline(const Eigen::Quaternion<Scalar> &startPos,const Eigen::Quaternion<Scalar> &endPos);
        Scalar length(void) const;
        Eigen::Quaternion<Scalar> eval(double s) const;
+       
+       EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+};
+
+template<typename T0, typename T1>
+class Spline<std::tuple<T0,T1> >
+{
+       std::tuple<T0,T1> startPos_;
+       std::tuple<T0,T1> endPos_;
+       double dist_;
+               
+       public:
+       Spline(const std::tuple<T0,T1> &startPos,const std::tuple<T0,T1> &endPos);
+       double length(void) const;
+       std::tuple<T0,T1> 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 (file)
--- 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 (file)
--- a/wam_node_sim/src/wam_node_sim.cpp
+++ b/wam_node_sim/src/wam_node_sim.cpp
@@ -344,15 +344,14 @@ template<size_t DOF>
   bool WamNode<DOF>::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<size_t DOF>
     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