From: Walter Fetter Lages Date: Sun, 23 Dec 2018 06:01:39 +0000 (-0200) Subject: Add pose_type. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=b7edc773879ac3909b66e82b6418051e53a43af5;p=ufrgs_wam.git 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 --- 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::getToolOrientation() const frame.M.GetQuaternion(x,y,z,w); return Eigen::Quaterniond(w,x,y,z); } -/* + template inline typename Wam::pose_type Wam::getToolPose() const { - return boost::make_tuple(getToolPosition(), getToolOrientation()); + return std::make_tuple(getToolPosition(), getToolOrientation()); } -*/ -/* + + template -inline math::Matrix<6,DOF> Wam::getToolJacobian() const +inline Eigen::Matrix Wam::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(0); } -*/ + template void Wam::gravityCompensate(bool compensate) @@ -250,13 +251,13 @@ inline void Wam::moveTo(const Eigen::Quaterniond& destination, bool blockin moveTo(currentPosHelper(getToolOrientation()), destination, blocking, velocity, acceleration); } -/* + 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); } -*/ + template template 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 --- 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 jv_type; typedef Eigen::Matrix cp_type; typedef Eigen::Matrix cv_type; + typedef std::tuple 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 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 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 --- a/wam_node_sim/src/wam_node_sim.cpp +++ b/wam_node_sim/src/wam_node_sim.cpp @@ -83,6 +83,8 @@ template typedef Eigen::Matrix jp_type; typedef Eigen::Matrix jv_type; typedef Eigen::Matrix cp_type; + typedef std::tuple 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 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_rl; wam_node_sim::RateLimiter cp_rl; @@ -270,7 +272,7 @@ template 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 bool WamNode::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 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;