From: Walter Fetter Lages Date: Wed, 26 Dec 2018 05:51:10 +0000 (-0200) Subject: Add Cartesian motion topic and services. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=916e9beb93a05fe2232cedf742c65a11dc49e161;p=ufrgs_wam.git Add Cartesian motion topic and services. --- diff --git a/wam_node_sim/CMakeLists.txt b/wam_node_sim/CMakeLists.txt index d89c1ba..e3fb4c2 100644 --- a/wam_node_sim/CMakeLists.txt +++ b/wam_node_sim/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(catkin REQUIRED COMPONENTS trajectory_msgs wam_msgs wam_srvs + eigen_conversions ) find_package(cmake_modules REQUIRED) @@ -114,7 +115,7 @@ find_package(Eigen3 REQUIRED) catkin_package( # INCLUDE_DIRS include # LIBRARIES wam_node_sim - CATKIN_DEPENDS geometry_msgs kdl_parser roscpp std_msgs std_srvs trajectory_msgs wam_msgs wam_srvs + CATKIN_DEPENDS geometry_msgs kdl_parser roscpp std_msgs std_srvs trajectory_msgs wam_msgs wam_srvs eigen_conversions # DEPENDS Eigen3 ) 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 4d0c3bc..37cdb8f 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 @@ -47,13 +47,16 @@ #include //#include -//#include +#include #include #include #include +#include +#include -//#include + +#include namespace barrett { @@ -73,13 +76,15 @@ Wam::Wam(Hand *hand,const std::string& sysName) if (!kdl_parser::treeFromString(robotDescription,tree_)) ROS_ERROR("Failed to construct KDL tree."); - KDL::SegmentMap::const_iterator s=tree_.getRootSegment(); if (!tree_.getChain(GetTreeElementSegment(s->second).getName(),"wam_tool_plate",chain_)) ROS_ERROR("Failed to get chain from KDL tree."); - fwdKinSolverPos_=new KDL::ChainFkSolverPos_recursive(chain_); + fkSolverPos_=new KDL::ChainFkSolverPos_recursive(chain_); + Eigen::Matrix L; + L << 1.0, 1.0, 1.0, 0.0, 0.0, 0.0; + ikSolverPos_=new KDL::ChainIkSolverPos_LMA(chain_,L); jointPosition_.setZero(); jointVelocity_.setZero(); @@ -91,7 +96,8 @@ Wam::~Wam() { wam_cmd_pub.shutdown(); jointStatesSubscriber_.shutdown(); - delete fwdKinSolverPos_; + delete fkSolverPos_; + delete ikSolverPos_; } template @@ -110,13 +116,32 @@ void Wam::trackReferenceSignal(jp_type &referenceSignal) } template +void Wam::trackReferenceSignal(cp_type &referenceSignal) +{ + KDL::JntArray q0(DOF); + q0.data=jointPosition_; + // try to induce convergence to an out-elbow pose + q0(3)=3.1; + q0(4)=0.0; + q0(5)=0.0; + q0(6)=0.0; + KDL::Frame frame(KDL::Vector(referenceSignal[0],referenceSignal[1],referenceSignal[2])); + KDL::JntArray q(DOF); + int error; + if((error=ikSolverPos_->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}; return home; } - template typename Wam::jt_type Wam::getJointTorques() const { @@ -143,14 +168,15 @@ typename Wam::cp_type Wam::getToolPosition() const { ros::spinOnce(); KDL::JntArray jointPosition(DOF); - for(int i;i < DOF;i++) jointPosition(i)=jointPosition_[i]; + for(int i=0;i < DOF;i++) jointPosition(i)=jointPosition_[i]; KDL::Frame frame; - if(fwdKinSolverPos_->JntToCart(jointPosition,frame) < 0) - ROS_ERROR("Failed to compute forward kinematics."); + int error; + if((error=fkSolverPos_->JntToCart(jointPosition,frame)) < 0) + ROS_ERROR_STREAM("Failed to compute forward kinematics: " << fkSolverPos_->strError(error)); cp_type cp; - for(int i=0;i < 3;i++) cp[i]=frame.p.data[i]; + tf::vectorKDLToEigen(frame.p,cp); return cp; } @@ -166,11 +192,12 @@ Eigen::Quaterniond Wam::getToolOrientation() const { ros::spinOnce(); KDL::JntArray jointPosition(DOF); - for(int i;i < DOF;i++) jointPosition(i)=jointPosition_[i]; + for(int i=0;i < DOF;i++) jointPosition(i)=jointPosition_[i]; KDL::Frame frame; - if(fwdKinSolverPos_->JntToCart(jointPosition,frame) < 0) - ROS_ERROR("Failed to compute forward kinematics."); + int error; + 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); @@ -242,7 +269,7 @@ inline void Wam::moveTo(const jp_type& destination, bool blocking, double v template inline void Wam::moveTo(const cp_type& destination, bool blocking, double velocity, double acceleration) { -// moveTo(currentPosHelper(getToolPosition()), /*cv_type(0.0),*/ destination, blocking, velocity, acceleration); + moveTo(currentPosHelper(getToolPosition()), /*cv_type(0.0),*/ destination, blocking, velocity, acceleration); } template diff --git a/wam_node_sim/include/wam_node_sim/wam.h b/wam_node_sim/include/wam_node_sim/wam.h index c1ab677..2a2160c 100644 --- a/wam_node_sim/include/wam_node_sim/wam.h +++ b/wam_node_sim/include/wam_node_sim/wam.h @@ -51,7 +51,9 @@ #include #include -#include +#include +#include + #include namespace barrett @@ -80,6 +82,7 @@ public: /** trackReferenceSignal() used for following updating input. (any barrett input units for control) */ void trackReferenceSignal(jp_type & referenceSignal); //NOLINT: non-const reference for syntax + void trackReferenceSignal(cp_type & referenceSignal); //NOLINT: non-const reference for syntax /** getHomePosition() returns home postion of individual joints in Radians */ @@ -175,7 +178,9 @@ private: // Used to calculate TP and TO if the values aren't already being calculated in the control loop. KDL::Tree tree_; KDL::Chain chain_; - KDL::ChainFkSolverPos_recursive *fwdKinSolverPos_; + KDL::ChainFkSolverPos *fkSolverPos_; + KDL::ChainIkSolverPos *ikSolverPos_; + ros::Publisher wam_cmd_pub; // controller command ros::Subscriber jointStatesSubscriber_; diff --git a/wam_node_sim/package.xml b/wam_node_sim/package.xml index 6bb308d..b7d7f45 100644 --- a/wam_node_sim/package.xml +++ b/wam_node_sim/package.xml @@ -63,6 +63,7 @@ trajectory_msgs wam_msgs wam_srvs + eigen_conversions Eigen3 geometry_msgs kdl_parser @@ -73,6 +74,7 @@ trajectory_msgs wam_msgs wam_srvs + eigen_conversions Eigen3 geometry_msgs kdl_parser @@ -83,6 +85,7 @@ trajectory_msgs wam_msgs wam_srvs + eigen_conversions diff --git a/wam_node_sim/scripts/cart_move.sh b/wam_node_sim/scripts/cart_move.sh new file mode 100755 index 0000000..559bb40 --- /dev/null +++ b/wam_node_sim/scripts/cart_move.sh @@ -0,0 +1,7 @@ +#!/bin/bash + +if [ "$#" -ne 3 ]; then + echo "Error: There should be 3 parameters." + exit -1; +fi; +rosservice call /wam/cart_move "[$1, $2, $3]" diff --git a/wam_node_sim/src/wam_node_sim.cpp b/wam_node_sim/src/wam_node_sim.cpp index c50cfaf..6752e89 100644 --- a/wam_node_sim/src/wam_node_sim.cpp +++ b/wam_node_sim/src/wam_node_sim.cpp @@ -262,7 +262,7 @@ template // ortn_vel_sub = n_.subscribe("ortn_vel_cmd", 1, &WamNode::ortnVelCB, this); // wam/ortn_vel_cmd // jnt_vel_sub = n_.subscribe("jnt_vel_cmd", 1, &WamNode::jntVelCB, this); // wam/jnt_vel_cmd jnt_pos_sub = n_.subscribe("jnt_pos_cmd", 1, &WamNode::jntPosCB, this); // wam/jnt_pos_cmd -// cart_pos_sub = n_.subscribe("cart_pos_cmd", 1, &WamNode::cartPosCB, this); // wam/cart_pos_cmd + cart_pos_sub = n_.subscribe("cart_pos_cmd", 1, &WamNode::cartPosCB, this); // wam/cart_pos_cmd //Advertising the following rosservices @@ -273,7 +273,7 @@ template 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 -// cart_move_srv = n_.advertiseService("cart_move", &WamNode::cartMove, this); // wam/cart_pos_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 } @@ -780,20 +780,15 @@ template { if (!cart_pos_status) { -/* - cp_track.setValue(wam.getToolPosition()); - current_ortn.setValue(wam.getToolOrientation()); // Initializing the orientation + cp_type cp_start = wam.getToolPosition(); + cp_rl.setCurVal(cp_start); // setting initial the joint position command cp_rl.setLimit(rt_cp_rl); - systems::forceConnect(cp_track.output, cp_rl.input); - systems::forceConnect(cp_rl.output, rt_pose_cmd.getInput<0>()); // saving the rate limited cartesian position command to the pose.position - systems::forceConnect(current_ortn.output, rt_pose_cmd.getInput<1>()); // saving the original orientation to the pose.orientation - wam.trackReferenceSignal(rt_pose_cmd.output); //Commanding the WAM to track the real-time pose command. -*/ + wam.trackReferenceSignal(cp_rl.getLimit(cp_start)); // command the WAM to track the RT commanded up to (500 Hz) Cartesian positions } - else if (new_rt_cmd) + else { -// cp_track.setValue(rt_cp_cmd); // Set our cartesian positions to subscribed command -// cp_rl.setLimit(rt_cp_rl); // Updating the rate limit to subscribed rate to control the rate of the moves + if(new_rt_cmd) cp_rl.setLimit(rt_cp_rl); // set our rate limit to subscribed rate to control the rate of the moves + wam.trackReferenceSignal(cp_rl.getLimit(rt_cp_cmd)); } cart_pos_status = true; new_rt_cmd = false; @@ -826,7 +821,9 @@ int main(int argc,char *argv[]) { ros::spinOnce(); wam_node.publishWam(); + ros::spinOnce(); wam_node.updateRT(); + ros::spinOnce(); loop.sleep(); }