#include <kdl_parser/kdl_parser.hpp>
#include <kdl/velocityprofile_trap.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
-#include <kdl/chainiksolverpos_lma.hpp>
+#include <kdl/chainiksolvervel_wdls.hpp>
+#include <kdl/chainiksolverpos_nr_jl.hpp>
#include <wam_node_sim/rate_limiter.h>
#include <wam_node_sim/spline.h>
+#include <kdl/frames_io.hpp>
+
namespace barrett
{
ROS_ERROR("Failed to get chain from KDL tree.");
fkSolverPos_=new KDL::ChainFkSolverPos_recursive(chain_);
- Eigen::Matrix<double,6,1> L;
- L << 1.0, 1.0, 1.0, 0.0, 0.0, 0.0;
- ikSolverPos_=new KDL::ChainIkSolverPos_LMA(chain_,L);
- ikSolverPose_=new KDL::ChainIkSolverPos_LMA(chain_);
+ KDL::JntArray qMin(DOF);
+ qMin(0)=-2.6;
+ qMin(1)=-2.0;
+ qMin(2)=-2.8;
+ qMin(3)=-0.9;
+ qMin(4)=-4.76;
+ qMin(5)=-M_PI_2;
+ qMin(6)=-3.0;
+ KDL::JntArray qMax(DOF);
+ qMax(0)=2.6;
+ qMax(1)=2.0;
+ qMax(2)=2.8;
+ qMax(3)=3.1;
+ qMax(4)=1.24;
+ qMax(5)=M_PI_2;
+ qMax(6)=3.0;
+
+ ikSolverVel_=new KDL::ChainIkSolverVel_wdls(chain_,1000);
+ ikSolverPos_=new KDL::ChainIkSolverPos_NR_JL(chain_,qMin,qMax,*fkSolverPos_,*ikSolverVel_,1000);
jointPosition_.setZero();
jointVelocity_.setZero();
jointEffort_.setZero();
+ doneMoving=true;
}
template<size_t DOF>
jointStatesSubscriber_.shutdown();
delete fkSolverPos_;
delete ikSolverPos_;
+ delete ikSolverVel_;
}
template<size_t DOF>
{
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;
+ q0(3)=2.0;
+ Eigen::MatrixXd Mq(DOF,DOF);
+ Mq.setIdentity();
+ Mq(3,3)=0.7;
+ static_cast<KDL::ChainIkSolverVel_wdls *>(ikSolverVel_)->setWeightJS(Mq);
+
+ // ignore orientation
+ Eigen::MatrixXd Mx(6,6);
+ Mx.setIdentity();
+ Mx(3,3)=0.0001;
+ Mx(4,4)=0.0001;
+ Mx(5,5)=0.0001;
+ static_cast<KDL::ChainIkSolverVel_wdls *>(ikSolverVel_)->setWeightTS(Mx);
+
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));
+
+ int error=ikSolverPos_->CartToJnt(q0,frame,q);
+
+ Mq.setIdentity();
+ static_cast<KDL::ChainIkSolverVel_wdls *>(ikSolverVel_)->setWeightJS(Mq);
+ Mx.setIdentity();
+ static_cast<KDL::ChainIkSolverVel_wdls *>(ikSolverVel_)->setWeightTS(Mx);
+
+ if(error < 0)
+ {
+ ROS_ERROR_STREAM("Failed to compute inverse kinematics while tracking Cartesian position: " << ikSolverPos_->strError(error));
+ return;
+ }
jp_type jp;
jp=q.data;
trackReferenceSignal(jp);
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));
+ if((error=ikSolverPos_->CartToJnt(q0,frame,q)) < 0)
+ {
+ ROS_ERROR_STREAM("Failed to compute inverse kinematics while tracking orientation: " << ikSolverPos_->strError(error));
+ return;
+ }
jp_type jp;
jp=q.data;
trackReferenceSignal(jp);
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));
+ if((error=ikSolverPos_->CartToJnt(q0,frame,q)) < 0)
+ {
+ ROS_ERROR_STREAM("Failed to compute inverse kinematics while tracking Cartesian pose: " << ikSolverPos_->strError(error));
+ return;
+ }
jp_type jp;
jp=q.data;
trackReferenceSignal(jp);
template<typename T>
void Wam<DOF>::moveToThread(const T ¤tPos,const T &destination,double velocity,double acceleration,std::atomic<bool> *started)
{
+ // avoids more than one move at the same time
+ // Is it possible more than one controller to be active in the real WAM
+ // supervisory controller?
+ std::lock_guard<std::mutex> lock(moveToMutex);
+
wam_node_sim::Spline<T> spline(currentPos,destination);
KDL::VelocityProfile_Trap profile;
doneMoving=false;
*started=true;
-
-// T limit=T::Constant(2.0);
static const int PUBLISH_FREQ=250.0;
-// wam_node_sim::RateLimiter<T> rateLimiter(limit,1.0/PUBLISH_FREQ);
-// rateLimiter.setCurVal(currentPos);
-
ros::Rate loop(PUBLISH_FREQ);
double t=0.0;
ros::Time t0=ros::Time::now();
while(ros::ok() && (t=(ros::Time::now()-t0).toSec()) < tf)
{
T pos=spline.eval(profile.Pos(t));
-// trackReferenceSignal(rateLimiter.getLimit(pos));
trackReferenceSignal(pos);
loop.sleep();
}
-// trackReferenceSignal(rateLimiter.getLimit(destination));
T pos=destination;
trackReferenceSignal(pos);
doneMoving=true;
/******************************************************************************
Node to test the wam_node
- Copyright (C) 2018 Walter Fetter Lages <w.fetter@ieee.org>
+ Copyright (C) 2018, 2019 Walter Fetter Lages <w.fetter@ieee.org>
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
#include <thread>
#include <ros/ros.h>
+#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/JointState.h>
-
#include <wam_msgs/RTJointPos.h>
+#include <wam_msgs/RTCartPos.h>
+
+#include <std_srvs/Empty.h>
#include <wam_srvs/BHandFingerPos.h>
#include <wam_srvs/BHandSpreadPos.h>
+#include <wam_srvs/BHandGraspPos.h>
+#include <wam_srvs/Hold.h>
+#include <wam_srvs/JointMove.h>
+#include <wam_srvs/CartPosMove.h>
+#include <wam_srvs/OrtnMove.h>
+#include <wam_srvs/PoseMove.h>
#define sqr(x) ((x)*(x))
class WamNodeOp
{
public:
- WamNodeOp(const ros::NodeHandle &node,double updateRate,double tolerance,double jointRateLimit);
+ WamNodeOp(const ros::NodeHandle &node,double updateRate,double jointTolerance,double jointRateLimit,double cartTolerance,double cartRateLimit);
~WamNodeOp(void);
- void wamMove(const std::vector<float> &jpCmd);
- void wamWait(const std::vector<float> &jpCmd) const;
- void fingerMove(double f1,double f2,double f3);
- void fingerMove(double finger);
+ void jointPos(const std::vector<float> &jpCmd,bool block=true);
+ void jointWait(const std::vector<float> &jpCmd) const;
+ void fingerPos(double f1,double f2,double f3,bool block=true);
+ void graspPos(double pos,bool block=true);
void fingerWait(double f1,double f2,double f3) const;
- void fingerWait(double finger) const;
- void spreadMove(double spread);
+ void spreadPos(double spread,bool block=true);
void spreadWait(double spread) const;
+ void closeGrasp(bool block=true);
+ void closeSpread(bool block=true);
+ void openGrasp(bool block=true);
+ void openSpread(bool block=true);
+ void goHome(void);
+ void holdCartPos(bool hold);
+ void holdJointPos(bool hold);
+ void holdOrtnPos(bool hold);
+ void jointMove(const std::vector<float> &jpCmd,bool block=true);
+ void trackJointPos(bool track);
+ void cartPosMove(const std::vector<float> &cpCmd,bool block=true);
+ void cartPosWait(const std::vector<float> &cpCmd) const;
+ void ortnMove(const std::vector<float> &ortnCmd,bool block=true);
+ void ortnWait(const std::vector<float> &ortnCmd) const;
+ void poseMove(const std::vector<float> &cpCmd,const std::vector<float> &ortnCmd,bool block=true);
+ void cartPos(const std::vector<float> &cpCmd,bool block=true);
+ void trackCartPos(bool track);
private:
-
void jointStatesCB(const sensor_msgs::JointState &jointStates);
+ void poseCB(const geometry_msgs::PoseStamped &pose);
void update(void) const;
ros::NodeHandle node_;
ros::Publisher jointPosPub_;
+ ros::Publisher cartPosPub_;
ros::Subscriber jointStatesSub_;
+ ros::Subscriber poseSub_;
ros::ServiceClient fingerPosCli_;
ros::ServiceClient spreadPosCli_;
+ ros::ServiceClient graspPosCli_;
+ ros::ServiceClient closeGraspCli_;
+ ros::ServiceClient closeSpreadCli_;
+ ros::ServiceClient openGraspCli_;
+ ros::ServiceClient openSpreadCli_;
+ ros::ServiceClient goHomeCli_;
+ ros::ServiceClient holdCartPosCli_;
+ ros::ServiceClient holdJointPosCli_;
+ ros::ServiceClient holdOrtnCli_;
+ ros::ServiceClient jointMoveCli_;
+ ros::ServiceClient cartPosMoveCli_;
+ ros::ServiceClient ortnMoveCli_;
+ ros::ServiceClient poseMoveCli_;
std::vector< std::atomic<float> > jpCmd_;
+ std::vector< std::atomic<float> > cpCmd_;
std::vector< std::atomic<float> > wamJp_;
std::vector< std::atomic<float> > bhandJp_;
+ std::vector< std::atomic<float> > wamCp_;
+ std::vector< std::atomic<float> > wamOrtn_;
std::thread update_;
bool runUpdate_;
+ bool trackJointPos_;
+ bool trackCartPos_;
double updateRate_;
- double tolerance_;
+ double jointTolerance_;
double jointRateLimit_;
+ double cartTolerance_;
+ double cartRateLimit_;
+ const double msgTimeout;
};
-WamNodeOp::WamNodeOp(const ros::NodeHandle &node,double updateRate,double tolerance,double jointRateLimit):
- jpCmd_(7), wamJp_(7), bhandJp_(4), runUpdate_(true)
+WamNodeOp::WamNodeOp(const ros::NodeHandle &node,double updateRate,double jointTolerance,double jointRateLimit,double cartTolerance,double cartRateLimit):
+ jpCmd_(7), cpCmd_(3), wamJp_(7), bhandJp_(4), wamCp_(3), wamOrtn_(4), runUpdate_(true), trackJointPos_(false), trackCartPos_(false), msgTimeout(0.4)
{
node_=node;
jointPosPub_=node_.advertise<wam_msgs::RTJointPos>("wam/jnt_pos_cmd",10);
+ cartPosPub_=node_.advertise<wam_msgs::RTCartPos>("wam/cart_pos_cmd",10);
+ closeGraspCli_=node_.serviceClient<std_srvs::Empty>("bhand/close_grasp");
+ closeSpreadCli_=node_.serviceClient<std_srvs::Empty>("bhand/close_spread");
+ openGraspCli_=node_.serviceClient<std_srvs::Empty>("bhand/open_grasp");
+ openSpreadCli_=node_.serviceClient<std_srvs::Empty>("bhand/open_spread");
fingerPosCli_=node_.serviceClient<wam_srvs::BHandFingerPos>("bhand/finger_pos");
spreadPosCli_=node_.serviceClient<wam_srvs::BHandSpreadPos>("bhand/spread_pos");
+ graspPosCli_=node_.serviceClient<wam_srvs::BHandGraspPos>("bhand/grasp_pos");
+ cartPosMoveCli_=node_.serviceClient<wam_srvs::CartPosMove>("wam/cart_move");
+ goHomeCli_=node_.serviceClient<std_srvs::Empty>("wam/go_home");
+ holdCartPosCli_=node_.serviceClient<wam_srvs::Hold>("wam/hold_cart_pos");
+ holdJointPosCli_=node_.serviceClient<wam_srvs::Hold>("wam/hold_joint_pos");
+ holdOrtnCli_=node_.serviceClient<wam_srvs::Hold>("wam/hold_ortn");
+ jointMoveCli_=node_.serviceClient<wam_srvs::JointMove>("wam/joint_move");
+ ortnMoveCli_=node_.serviceClient<wam_srvs::OrtnMove>("wam/ortn_move");
+ poseMoveCli_=node_.serviceClient<wam_srvs::PoseMove>("wam/pose_move");
+// gravityCompCli_=node_.serviceClient<wam_srvs::GravityComp>("wam/gravity_comp");
jointStatesSub_=node_.subscribe("/joint_states",10,&WamNodeOp::jointStatesCB,this);
+ poseSub_=node_.subscribe("wam/pose",10,&WamNodeOp::poseCB,this);
- tolerance_=tolerance;
+ jointTolerance_=jointTolerance;
updateRate_=updateRate;
jointRateLimit_=jointRateLimit;
+ cartTolerance_=cartTolerance;
+ cartRateLimit_=cartRateLimit;
update_=std::thread(&WamNodeOp::update,this);
}
runUpdate_=false;
update_.join();
jointPosPub_.shutdown();
+ cartPosPub_.shutdown();
jointStatesSub_.shutdown();
+ poseSub_.shutdown();
}
-void WamNodeOp::wamMove(const std::vector<float> &jpCmd)
+void WamNodeOp::jointPos(const std::vector<float> &jpCmd,bool block)
{
- for(int i=0;i < jpCmd_.size() && i < jpCmd.size();i++) jpCmd_[i]=jpCmd[i];
+ for(int i=0;i < jpCmd_.size();i++) jpCmd_[i]=jpCmd[i];
+ trackJointPos(true);
+ if(block) jointWait(jpCmd);
}
-void WamNodeOp::wamWait(const std::vector<float> &jpCmd) const
+void WamNodeOp::trackJointPos(bool track)
+{
+ trackJointPos_=track;
+ ros::Duration(msgTimeout).sleep();
+}
+
+void WamNodeOp::jointWait(const std::vector<float> &jpCmd) const
{
float error;
do
error+=sqr(wamJp_[i]-jpCmd[i]);
error=sqrt(error);
}
- while(error > 7*tolerance_);
+ while(error > 7*jointTolerance_);
}
-void WamNodeOp::fingerMove(double f1,double f2,double f3)
+void WamNodeOp::fingerPos(double f1,double f2,double f3,bool block)
{
wam_srvs::BHandFingerPos fingerPosSrv;
- fingerPosSrv.request.radians[0]=f1;;
+ fingerPosSrv.request.radians[0]=f1;
fingerPosSrv.request.radians[1]=f2;
fingerPosSrv.request.radians[2]=f3;
- if(fingerPosCli_.call(fingerPosSrv)) ROS_INFO_STREAM("Finger move to " << f1 << " " << f2 << " " << f3);
- else ROS_ERROR_STREAM("Error in finger move.");
+ if(fingerPosCli_.call(fingerPosSrv)) ROS_INFO_STREAM("Finger moved to " << f1 << " " << f2 << " " << f3 << ".");
+ else ROS_ERROR_STREAM("Error moving finger.");
+ if(block) fingerWait(f1,f2,f3);
}
void WamNodeOp::fingerWait(double f1,double f2,double f3) const
ros::Duration(0.01).sleep();
error=sqrt(sqr(f1-bhandJp_[0])+sqr(f2-bhandJp_[1])+sqr(f3-bhandJp_[2]));
}
- while(error > 3*tolerance_);
-}
-
-void WamNodeOp::fingerWait(double finger) const
-{
- fingerWait(finger,finger,finger);
+ while(error > 3*jointTolerance_);
}
-
-void WamNodeOp::fingerMove(double finger)
+void WamNodeOp::graspPos(double pos,bool block)
{
- fingerMove(finger,finger,finger);
+ wam_srvs::BHandGraspPos graspPosSrv;
+
+ graspPosSrv.request.radians=pos;
+ if(graspPosCli_.call(graspPosSrv)) ROS_INFO_STREAM("Grasp moved to " << pos << ".");
+ else ROS_ERROR_STREAM("Error moving grasp.");
+ if(block) fingerWait(pos,pos,pos);
}
-void WamNodeOp::spreadMove(double spread)
+void WamNodeOp::spreadPos(double spread,bool block)
{
wam_srvs::BHandSpreadPos spreadPosSrv;
spreadPosSrv.request.radians=spread;
- if(spreadPosCli_.call(spreadPosSrv)) ROS_INFO_STREAM("Spread move to " << spread);
- else ROS_ERROR_STREAM("Error in spread move.");
+ if(spreadPosCli_.call(spreadPosSrv)) ROS_INFO_STREAM("Spread moved to " << spread << ".");
+ else ROS_ERROR_STREAM("Error moving spread.");
+ if(block) spreadWait(spread);
}
void WamNodeOp::spreadWait(double spread) const
ros::Duration(0.01).sleep();
error=fabs(spread-bhandJp_[3]);
}
- while(error > tolerance_);
+ while(error > jointTolerance_);
+}
+
+void WamNodeOp::closeGrasp(bool block)
+{
+ std_srvs::Empty emptySrv;
+
+ if(closeGraspCli_.call(emptySrv)) ROS_INFO_STREAM("Grasp closed.");
+ else ROS_ERROR_STREAM("Error closing grasp.");
+ if(block) fingerWait(2.44,2.44,2.44);
+}
+
+void WamNodeOp::closeSpread(bool block)
+{
+ std_srvs::Empty emptySrv;
+
+ if(closeSpreadCli_.call(emptySrv)) ROS_INFO_STREAM("Spread closed.");
+ else ROS_ERROR_STREAM("Error closing spread.");
+ if(block) spreadWait(0);
+}
+
+void WamNodeOp::openGrasp(bool block)
+{
+ std_srvs::Empty emptySrv;
+
+ if(openGraspCli_.call(emptySrv)) ROS_INFO_STREAM("Grasp open.");
+ else ROS_ERROR_STREAM("Error opening grasp.");
+ if(block) fingerWait(0,0,0);
+}
+
+void WamNodeOp::openSpread(bool block)
+{
+ std_srvs::Empty emptySrv;
+
+ if(openSpreadCli_.call(emptySrv)) ROS_INFO_STREAM("Spread open.");
+ else ROS_ERROR_STREAM("Error opening spread.");
+ if(block) spreadWait(M_PI);
+}
+
+void WamNodeOp::goHome(void)
+{
+ std_srvs::Empty emptySrv;
+
+ if(goHomeCli_.call(emptySrv)) ROS_INFO_STREAM("Returned home position.");
+ else ROS_ERROR_STREAM("Error returning home position.");
+}
+
+void WamNodeOp::holdCartPos(bool hold)
+{
+ wam_srvs::Hold holdSrv;
+
+ holdSrv.request.hold=hold;
+ if(holdCartPosCli_.call(holdSrv)) ROS_INFO_STREAM("Cartesian position held.");
+ else ROS_ERROR_STREAM("Error holding Cartesian position.");
+}
+
+void WamNodeOp::holdJointPos(bool hold)
+{
+ wam_srvs::Hold holdSrv;
+
+ holdSrv.request.hold=hold;
+ if(holdJointPosCli_.call(holdSrv)) ROS_INFO_STREAM("Joint position held.");
+ else ROS_ERROR_STREAM("Error holding joint position.");
+}
+
+void WamNodeOp::holdOrtnPos(bool hold)
+{
+ wam_srvs::Hold holdSrv;
+
+ holdSrv.request.hold=hold;
+ if(holdOrtnCli_.call(holdSrv)) ROS_INFO_STREAM("Orientation held.");
+ else ROS_ERROR_STREAM("Error holding orientation.");
+}
+
+void WamNodeOp::jointMove(const std::vector<float> &jpCmd,bool block)
+{
+ wam_srvs::JointMove jointMoveSrv;
+
+ jointMoveSrv.request.joints=jpCmd;
+ if(jointMoveCli_.call(jointMoveSrv)) ROS_INFO_STREAM("Joints moved to "
+ << jpCmd[0] << " " << jpCmd [1] << " " << jpCmd[2] << " "
+ << jpCmd[3] << " " << jpCmd[4] << " " << jpCmd[5] << " "
+ << jpCmd[6] << " " << ".");
+ else ROS_ERROR_STREAM("Error moving joints.");
+ if(block) jointWait(jpCmd);
+}
+
+void WamNodeOp::cartPosMove(const std::vector<float> &cpCmd,bool block)
+{
+ wam_srvs::CartPosMove cartPosMoveSrv;
+
+ for(int i=0;i < 3;i++) cartPosMoveSrv.request.position[i]=cpCmd[i];
+ if(cartPosMoveCli_.call(cartPosMoveSrv)) ROS_INFO_STREAM("Cartesian posiiton moved to "
+ << cpCmd[0] << " " << cpCmd [1] << " " << cpCmd[2] << ".");
+ else ROS_ERROR_STREAM("Error moving to Cartesian position.");
+ if(block) cartPosWait(cpCmd);
+}
+
+void WamNodeOp::cartPosWait(const std::vector<float> &cpCmd) const
+{
+ float error;
+ do
+ {
+ ros::Duration(0.01).sleep();
+ error=0.0;
+ for(int i=0;i < wamCp_.size() && i < cpCmd.size();i++)
+ error+=sqr(wamCp_[i]-cpCmd[i]);
+ error=sqrt(error);
+ }
+ while(error > cartTolerance_);
+}
+
+void WamNodeOp::ortnMove(const std::vector<float> &ortnCmd,bool block)
+{
+ wam_srvs::OrtnMove ortnMoveSrv;
+
+ for(int i=0;i < 4;i++) ortnMoveSrv.request.orientation[i]=ortnCmd[i];
+ if(ortnMoveCli_.call(ortnMoveSrv)) ROS_INFO_STREAM("Tool orientation moved to "
+ << ortnCmd[0] << " " << ortnCmd [1] << " " << ortnCmd[2] << " " << ortnCmd[3] << ".");
+ else ROS_ERROR_STREAM("Error moving tool orientation.");
+ if(block) ortnWait(ortnCmd);
+}
+
+void WamNodeOp::ortnWait(const std::vector<float> &ortnCmd) const
+{
+ float error;
+ do
+ {
+ ros::Duration(0.01).sleep();
+ error=0.0;
+ for(int i=0;i < wamOrtn_.size() && i < ortnCmd.size();i++)
+ error+=sqr(wamOrtn_[i]-ortnCmd[i]);
+ error=sqrt(error);
+ }
+ while(error > 8*jointTolerance_);
+}
+
+void WamNodeOp::poseMove(const std::vector<float> &cpCmd,const std::vector<float> &ortnCmd,bool block)
+{
+ wam_srvs::PoseMove poseMoveSrv;
+
+ poseMoveSrv.request.pose.position.x=cpCmd[0];
+ poseMoveSrv.request.pose.position.y=cpCmd[1];
+ poseMoveSrv.request.pose.position.z=cpCmd[2];
+
+ poseMoveSrv.request.pose.orientation.x=ortnCmd[0];
+ poseMoveSrv.request.pose.orientation.y=ortnCmd[1];
+ poseMoveSrv.request.pose.orientation.z=ortnCmd[2];
+ poseMoveSrv.request.pose.orientation.w=ortnCmd[3];
+
+ if(poseMoveCli_.call(poseMoveSrv)) ROS_INFO_STREAM("Pose moved to "
+ << cpCmd[0] << " " << cpCmd [1] << " " << cpCmd[2] << " "
+ << ortnCmd[0] << " " << ortnCmd [1] << " " << ortnCmd[2] << " " << ortnCmd[3] << ".");
+ else ROS_ERROR_STREAM("Error moving to pose.");
+ if(block)
+ {
+ cartPosWait(cpCmd);
+ ortnWait(ortnCmd);
+ }
+}
+
+void WamNodeOp::cartPos(const std::vector<float> &cpCmd,bool block)
+{
+ for(int i=0;i < cpCmd_.size();i++) cpCmd_[i]=cpCmd[i];
+
+ trackCartPos(true);
+
+ if(block) cartPosWait(cpCmd);
+}
+
+void WamNodeOp::trackCartPos(bool track)
+{
+ trackCartPos_=track;
+ ros::Duration(msgTimeout).sleep();
}
void WamNodeOp::update(void) const
ros::Rate loop(updateRate_);
while(ros::ok() && runUpdate_)
{
- wam_msgs::RTJointPos jointPosMsg;
+ if(trackJointPos_)
+ {
+ wam_msgs::RTJointPos jointPosMsg;
+
+ for(int i=0;i < jpCmd_.size();i++) jointPosMsg.joints.push_back(jpCmd_[i]);
+ jointPosMsg.rate_limits.resize(jpCmd_.size(),jointRateLimit_);
+ jointPosPub_.publish(jointPosMsg);
+ }
+
+ if(trackCartPos_)
+ {
+ wam_msgs::RTCartPos cartPosMsg;
- for(int i=0;i < jpCmd_.size();i++) jointPosMsg.joints.push_back(jpCmd_[i]);
- jointPosMsg.rate_limits.resize(jpCmd_.size(),jointRateLimit_);
- jointPosPub_.publish(jointPosMsg);
+ for(int i=0;i < cpCmd_.size();i++)
+ {
+ cartPosMsg.position[i]=cpCmd_[i];
+ cartPosMsg.rate_limits[i]=cartRateLimit_;
+ }
+ cartPosPub_.publish(cartPosMsg);
+ }
ros::spinOnce();
loop.sleep();
}
}
+void WamNodeOp::poseCB(const geometry_msgs::PoseStamped &pose)
+{
+ wamCp_[0]=pose.pose.position.x;
+ wamCp_[1]=pose.pose.position.y;
+ wamCp_[2]=pose.pose.position.z;
+
+ wamOrtn_[0]=pose.pose.orientation.x;
+ wamOrtn_[1]=pose.pose.orientation.y;
+ wamOrtn_[2]=pose.pose.orientation.z;
+ wamOrtn_[3]=pose.pose.orientation.w;
+}
+
int main(int argc,char* argv[])
{
ros::init(argc,argv,"wam_node_test");
ros::NodeHandle node;
-
- WamNodeOp wamCmd(node,50.0,0.01,0.5);
+
+ WamNodeOp wam(node,50.0,0.02,0.5,0.005,0.1);
std::vector<float> jpCmd(7,0);
jpCmd[1]=-2.0;
jpCmd[3]=3.1;
- wamCmd.wamMove(jpCmd);
- wamCmd.wamWait(jpCmd);
+ wam.jointPos(jpCmd);
jpCmd[0]=M_PI_2;
jpCmd[1]=-M_PI_2;
jpCmd[3]=M_PI_2;
- wamCmd.wamMove(jpCmd);
- wamCmd.wamWait(jpCmd);
+ wam.jointPos(jpCmd);
+ wam.trackJointPos(false);
- wamCmd.spreadMove(M_PI);
- wamCmd.spreadWait(M_PI);
+ wam.openSpread();
+
+ wam.graspPos(M_PI_2);
- wamCmd.fingerMove(M_PI_2);
- wamCmd.fingerWait(M_PI_2);
+ wam.openGrasp();
- wamCmd.fingerMove(2.44);
- wamCmd.fingerWait(2.44);
+ wam.closeGrasp();
- wamCmd.fingerMove(0.0);
- wamCmd.fingerWait(0.0);
+ wam.openGrasp();
- wamCmd.spreadMove(0);
- wamCmd.spreadWait(0);
+ wam.closeSpread();
- wamCmd.fingerMove(2.44,2.44,0.0);
- wamCmd.fingerWait(2.44,2.44,0.0);
+ wam.fingerPos(2.44,2.44,0.0);
- wamCmd.fingerMove(2.44);
- wamCmd.fingerWait(2.44);
+ wam.closeGrasp();
- wamCmd.fingerMove(0.0);
- wamCmd.fingerWait(0.0);
+ wam.openGrasp();
- wamCmd.spreadMove(0.0);
- wamCmd.spreadWait(0.0);
+ wam.closeSpread();
+ wam.goHome();
+
+ std::vector<float> cpCmd(3);
+ cpCmd[0]=0.0;
+ cpCmd[1]=-0.6;
+ cpCmd[2]=1.3;
+ wam.cartPosMove(cpCmd);
+
+ std::vector<float> ortnCmd(4);
+ ortnCmd[0]=0.0;
+ ortnCmd[1]=0.0;
+ ortnCmd[2]=0.0;
+ ortnCmd[3]=1.0;
+ wam.ortnMove(ortnCmd);
+
+ cpCmd[0]=0.0;
+ cpCmd[1]=-0.6;
+ cpCmd[2]=1.5;
+ ortnCmd[0]=0.0;
+ ortnCmd[1]=0.0;
+ ortnCmd[2]=0.0;
+ ortnCmd[3]=1.0;
+ wam.poseMove(cpCmd,ortnCmd);
+
+ cpCmd[0]=-0.6;
+ cpCmd[1]=0.0;
+ cpCmd[2]=1.5;
+ wam.cartPos(cpCmd);
+
+ cpCmd[0]=0.0;
+ cpCmd[1]=0.0;
+ cpCmd[2]=1.5;
+ wam.cartPos(cpCmd);
+
+ cpCmd[0]=0.0;
+ cpCmd[1]=0.0;
+ cpCmd[2]=1.76;
+ wam.cartPos(cpCmd);
+ wam.trackCartPos(false);
+
std::fill(jpCmd.begin(),jpCmd.end(),0.0);
jpCmd[1]=-2.0;
jpCmd[3]=3.1;
- wamCmd.wamMove(jpCmd);
- wamCmd.wamWait(jpCmd);
+ wam.jointMove(jpCmd);
return 0;
}
projects / ufrgs_wam.git / commitdiff