Add pose_type.

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 2b2743c..ec36b84 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
@@ -176,22 +176,23 @@ Eigen::Quaterniond Wam<DOF>::getToolOrientation() const
        frame.M.GetQuaternion(x,y,z,w);
        return Eigen::Quaterniond(w,x,y,z);
 }
-/*
+
 template<size_t DOF>
 inline typename Wam<DOF>::pose_type Wam<DOF>::getToolPose() const
 {
-       return boost::make_tuple(getToolPosition(), getToolOrientation());
+       return std::make_tuple(getToolPosition(), getToolOrientation());
 }
-*/
 
-/*
+
+
 template<size_t DOF>
-inline math::Matrix<6,DOF> Wam<DOF>::getToolJacobian() const
+inline Eigen::Matrix<double,6,DOF> Wam<DOF>::getToolJacobian() const
 {
-       kin.eval(getJointPositions(), getJointVelocities());
-       return math::Matrix<6,DOF>(kin.impl->tool_jacobian);
+//     kin.eval(getJointPositions(), getJointVelocities());
+//     return Eigen::Matrix<6,DOF>(kin.impl->tool_jacobian);
+       return Eigen::Matrix<double,6,DOF>(0);
 }
-*/
+
 
 template<size_t DOF>
 void Wam<DOF>::gravityCompensate(bool compensate)
@@ -250,13 +251,13 @@ inline void Wam<DOF>::moveTo(const Eigen::Quaterniond& destination, bool blockin
        moveTo(currentPosHelper(getToolOrientation()), destination, blocking, velocity, acceleration);
 }
 
-/*
+
 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);
 }
-*/
+
 
 template<size_t DOF>
 template<typename T>

diff --git a/wam_node_sim/include/wam_node_sim/wam.h b/wam_node_sim/include/wam_node_sim/wam.h
index ee6b9b2..fbb58b9 100644 (file)
--- a/wam_node_sim/include/wam_node_sim/wam.h
+++ b/wam_node_sim/include/wam_node_sim/wam.h
@@ -68,6 +68,7 @@ class Wam {
     typedef Eigen::Matrix<double,DOF,1> jv_type;
     typedef Eigen::Matrix<double,3,1> cp_type;
     typedef Eigen::Matrix<double,3,1> cv_type;
+    typedef std::tuple<cp_type,Eigen::Quaterniond> pose_type;
 
 public:
    /** Constructor for Wam
@@ -114,11 +115,11 @@ public:
 
        /** getToolPose() returns Tool Pose as a combination of 
      */
-//     pose_type getToolPose() const;
+       pose_type getToolPose() const;
 
     /** getToolJacobian() returns matrix of first order partial derivatives.
      */
-//     math::Matrix<6,DOF> getToolJacobian() const;
+       Eigen::Matrix<double,6,DOF> getToolJacobian() const;
 
     /** gravityCompensate() method activates Gravity Compensation for WAM
      */
@@ -154,7 +155,7 @@ public:
        void moveTo(const jp_type& destination, bool blocking = true, double velocity = 0.5, double acceleration = 0.5);
        void moveTo(const cp_type& destination, bool blocking = true, double velocity = 0.1, double acceleration = 0.2);
        void moveTo(const Eigen::Quaterniond& destination, bool blocking = true, double velocity = 0.5, double acceleration = 0.5);
-//     void moveTo(const pose_type& destination, bool blocking = true, double velocity = 0.1, double acceleration = 0.2);
+       void moveTo(const pose_type& destination, bool blocking = true, double velocity = 0.1, double acceleration = 0.2);
        template<typename T> void moveTo(const T& currentPos, const T& destination, bool blocking, double velocity, double acceleration);
 
        /** moveIsDone() method returns false while the trajectory controller for the most recent moveTo() command is still active. 

diff --git a/wam_node_sim/src/wam_node_sim.cpp b/wam_node_sim/src/wam_node_sim.cpp
index 312b5d1..c50cfaf 100644 (file)
--- a/wam_node_sim/src/wam_node_sim.cpp
+++ b/wam_node_sim/src/wam_node_sim.cpp
@@ -83,6 +83,8 @@ template<size_t DOF>
     typedef Eigen::Matrix<double,DOF,1> jp_type;
     typedef Eigen::Matrix<double,DOF,1> jv_type;
     typedef Eigen::Matrix<double,3,1> cp_type;
+    typedef std::tuple<cp_type,Eigen::Quaterniond> pose_type;
+
 //    protected:
     bool cart_vel_status, ortn_vel_status, jnt_vel_status;
     bool jnt_pos_status, cart_pos_status, ortn_pos_status, new_rt_cmd;
@@ -97,7 +99,7 @@ template<size_t DOF>
 
     Eigen::Quaterniond ortn_cmd, rt_op_cmd, rt_op_rl;
 
-//    pose_type pose_cmd;
+    pose_type pose_cmd;
     Eigen::Vector3d rt_ortn_cmd;
     wam_node_sim::RateLimiter<jp_type> jp_rl;
     wam_node_sim::RateLimiter<cp_type> cp_rl;
@@ -270,7 +272,7 @@ template<size_t DOF>
     hold_cpos_srv = n_.advertiseService("hold_cart_pos", &WamNode::holdCPos, this); // wam/hold_cart_pos
     hold_ortn_srv = n_.advertiseService("hold_ortn", &WamNode::holdOrtn, this); // wam/hold_ortn
     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
+    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
 
@@ -376,7 +378,7 @@ template<size_t DOF>
   bool WamNode<DOF>::poseMove(wam_srvs::PoseMove::Request &req, wam_srvs::PoseMove::Response &res)
   {
     ROS_INFO("Moving Robot to Commanded Pose");
-/*
+
     cp_cmd[0] = req.pose.position.x;
     cp_cmd[1] = req.pose.position.y;
     cp_cmd[2] = req.pose.position.z;
@@ -385,8 +387,8 @@ template<size_t DOF>
     ortn_cmd.z() = req.pose.orientation.z;
     ortn_cmd.w() = req.pose.orientation.w;
 
-    pose_cmd = boost::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;