发布Sensor_msgs::JointState关节位置或速度实现Barrett Hand机械手控制
效果：通过独立的机械手初始化类和操作类能够实现任意机械手控制模式切换，机械手位置控制，机械手速度控制。
环境：ubuntu 14.04+indigo. p.s.快速开发可参考http://wiki.ros.org/bhand_controller资料。
[正文]
1，获得机械手控制相关话题的细节信息。
官方驱动包安装成功后可以通过官方demo实现机械手控制，然后可以通过ros提供的监听指令如rostopic list运行相关的节点，然后通过rostopic type /bhand_node/command获得相关的话题类型，然后通过rosmsgshow sesor_msgs/JointState 获得相关的信息。
另外可从baretthand ros网站获得。获得信息如下：
header:
seq: 4912
stamp:
secs: 1408685894
nsecs: 728996992
frame_id: ''
name: ['bh_j23_joint', 'bh_j12_joint','bh_j22_joint', 'bh_j32_joint', 'bh_j33_joint', 'bh_j13_joint', 'bh_j11_joint','bh_j21_joint']
position: [0.0, 0.0, 0.0, -0.0, 0.0, 0.0,0.0, 0.0]
velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0]
effort: [0.03743090386539949,0.06140198104320094, 0.03743090386539949, 0.06676343437499943,0.06676343437499943, 0.06140198104320094, 0.0, 0.0]
主要关注name（控制的关节名称），position（需要设置的关节位置），velocity（需要设置的关节速度），effort（需要设置的最大功率，速度一定时决定机械手停止的最大握力）。
网页给出的参考控制例程如下：
rostopic pub /bhand_node/commandsensor_msgs/JointState "header:
seq: 0
stamp: {secs: 0, nsecs: 0}
frame_id: ''
name: ['bh_j11_joint', 'bh_j32_joint','bh_j12_joint', 'bh_j22_joint']
position: [0 , 0, 0, 0]
velocity: [0]
effort: [0]"
barett机械手共有4个自由度，3个手指屈伸自由度，一个旋转自由度。根据机械手关节分布图，可知j11为手指的旋转自由度，j32-F3手指, j12-F1手指, j22-F2手指。
2 构建话题控制实现类。
2.1机械手状态切换类
机械手控制器包含了不同机械手状态，某些状态下不能进行关节位置控制，机械手所以状态如下：机械手进入READY_STATE后可以进行机械手关节位置控制。
//INIT_STATE= 100
//STANDBY_STATE = 200
//READY_STATE = 300
//EMERGENCY_STATE = 400
//FAILURE_STATE = 500
//SHUTDOWN_STATE = 600
另外机械手控制分为position模式（直接发布机械手关节绝对位置，机械手自动达到），velocity模式（设置某关节的运动角速度），为了方便控制，需要构建状态相关的类。
完成的状态控制类如下所示（具体成员实现见后文所附代码）：
class bhand_state_init
{
public:
boost::function<void (constbhand_controller::State::ConstPtr&)> StateCallbackFun;
bhand_state_init(string mode);
void bhand_control_mode(string mode);
void bhand_action_test(int act,int delay);
void bhand_spinner(char set);
private:
ros::NodeHandle bhand_state;
ros::SubscribeOptions ops;
ros::Subscriber listen_state;
ros::AsyncSpinner *state_spinner;
ros::CallbackQueue state_callback_queue;
void state_callback(const bhand_controller::State::ConstPtr& msg);
ros::ServiceClient client;
bhand_controller::SetControlMode control_mode;
bhand_controller::Actions test_act;
string set_mode;
public:
__uint8_t flag;
char first_init_state;
};
机械手状态切换与控制类的使用示例如下：
ros::init(argc,argv,"bhand_force_control");
ros::NodeHandle test_handle;//useless
bhand_state_init state_test("POSITION");//切换到位置控制模式。
int loop_hz=100;
ros::Rate loop_rate(loop_hz);
while(ros::ok())
{
if (state_test.flag)
{
state_test.bhand_spinner(0);
break;//等待状态切换完成 }
else
state_test.bhand_spinner(1);
loop_rate.sleep();
}
//velocity control
state_test.bhand_control_mode("VELOCITY");//切换到速度控制模式。
2.2 机械手速度和位置控制类的实现。
必要的初始化构造函数如下：主要完成话题格式的设置。目标话题/bhand_node/ command，格式是sensor_msgs::JointState。
bhand_pose()
{
flag=0;
command_pub=bhand_command.advertise<sensor_msgs::JointState>("/bhand_node/command",1000);
cmdCallbackFun=boost::bind(&bhand_pose::command_callback,this,_1);
ops=ros::SubscribeOptions::create<sensor_msgs::JointState>(
"/joint_states",
1,
cmdCallbackFun/*command_callback*/,
ros::VoidPtr(),
&command_callback_queue
);
listen_position=bhand_command.subscribe(ops);//monitor hand pose
command_spinner=new ros::AsyncSpinner(1,&command_callback_queue);
}
然后是位置控制的实现，按话题数据格式和关节对应关系填充数据。
void bhand_move_position(float base,floatf1,float f2,float f3,int delay)
{
msg_state.name={"bh_j11_joint","bh_j32_joint","bh_j12_joint","bh_j22_joint"};
//4 Freedom: j11-Basespread,j32-F3,j12-F1,j22-F2
msg_state.position={base,f3,f1,f2};
//ROS_INFO("Position desired-Base: %f,F1: %f,F2: %f,F3:%f",msg_state.position[0],msg_state.position[2],msg_state.position[3],msg_state.position[1]);
msg_state.velocity={0.05,0.1,0.1,0.1};
msg_state.effort={0.1,0.3,0.3,0.3};
//monitor state: READY && POSITIONS
ros::Duration(delay).sleep();
command_pub.publish(msg_state);
}
然后是速度控制的实现，按话题数据格式和关节对应关系填充数据。
void bhand_move_velocity(float base,floatf1,float f2,float f3,int delay)
{
msg_state.name={"bh_j11_joint","bh_j32_joint","bh_j12_joint","bh_j22_joint"};
//4 Freedom: j11-Base spread,j32-F3,j12-F1,j22-F2
msg_state.position={0,0,0,0};
//ROS_INFO("Position desired-Base: %f,F1: %f,F2: %f,F3:%f",msg_state.position[0],msg_state.position[2],msg_state.position[3],msg_state.position[1]);
msg_state.velocity={base,f3,f1,f2};
msg_state.effort={0.1,0.3,0.3,0.3};
//monitor state: READY && VELOCITY
ros::Duration(delay).sleep();
command_pub.publish(msg_state);
}
完整的类如下：
class bhand_pose
{
public:
boost::function<void (constsensor_msgs::JointState::ConstPtr&)> cmdCallbackFun;//C++ Template
bhand_pose()
{
flag=0;
command_pub=bhand_command.advertise<sensor_msgs::JointState>("/bhand_node/command",1000);
cmdCallbackFun=boost::bind(&bhand_pose::command_callback,this,_1);
ops=ros::SubscribeOptions::create<sensor_msgs::JointState>(
"/joint_states",
1,
cmdCallbackFun/*command_callback*/,
ros::VoidPtr(),
&command_callback_queue
);
listen_position=bhand_command.subscribe(ops);//monitor hand pose
command_spinner=new ros::AsyncSpinner(1,&command_callback_queue);
}
void bhand_move_position(float base,float f1,float f2,float f3,intdelay)
{
msg_state.name={"bh_j11_joint","bh_j32_joint","bh_j12_joint","bh_j22_joint"};
//4 Freedom: j11-Basespread,j32-F3,j12-F1,j22-F2
msg_state.position={base,f3,f1,f2};
//ROS_INFO("Position desired-Base: %f,F1: %f,F2: %f,F3:%f",msg_state.position[0],msg_state.position[2],msg_state.position[3],msg_state.position[1]);
msg_state.velocity={0.05,0.1,0.1,0.1};
msg_state.effort={0.1,0.3,0.3,0.3};
//monitor state: READY && POSITIONS
ros::Duration(delay).sleep();
command_pub.publish(msg_state);
}
void bhand_move_velocity(float base,float f1,float f2,float f3,intdelay)
{
msg_state.name={"bh_j11_joint","bh_j32_joint","bh_j12_joint","bh_j22_joint"};
//4 Freedom: j11-Base spread,j32-F3,j12-F1,j22-F2
msg_state.position={0,0,0,0};
//ROS_INFO("Position desired-Base: %f,F1: %f,F2: %f,F3:%f",msg_state.position[0],msg_state.position[2],msg_state.position[3],msg_state.position[1]);
msg_state.velocity={base,f3,f1,f2};
msg_state.effort={0.1,0.3,0.3,0.3};
//monitor state: READY && VELOCITY
ros::Duration(delay).sleep();
command_pub.publish(msg_state);
}
void bhand_spinner(char set)
{
if (set==1)
command_spinner->start();
else
command_spinner->stop();
}
private:
ros::NodeHandle bhand_command;
ros::Publisher command_pub;
sensor_msgs::JointState msg_state;
ros::SubscribeOptions ops;
ros::Subscriber listen_position;
ros::AsyncSpinner *command_spinner;
ros::CallbackQueue command_callback_queue;
void command_callback(const sensor_msgs::JointState::ConstPtr& msg);
public:
float base_pose,f1_pose,f2_pose,f3_pose;
float rec_base,rec_f1,rec_f2,rec_f3;
float delta_base,delta_f1,delta_f2,delta_f3;
__uint8_t flag;
};
void bhand_pose::command_callback(constsensor_msgs::JointState::ConstPtr& msg)
{
//ROS_INFO("Position readed-Base: %f,F1: %f,F2: %f,F3:%f",msg->position[6],msg->position[1],msg->position[2],msg->position[3]);
rec_base=msg->position[6];
if(rec_base<0) rec_base=0;//avoid some abnormal phenomenons
if(rec_base>3) rec_base=2.5;
rec_f1=msg->position[1];
if(rec_f1<0) rec_f1=0;
if(rec_f1>3) rec_f1=2.5;
rec_f2=msg->position[2];
if(rec_f2<0) rec_f2=0;
if(rec_f2>3) rec_f2=2.5;
rec_f3=msg->position[3];
if(rec_f3<0) rec_f3=0;
if(rec_f3>3) rec_f3=2.5;
delta_base=base_pose-rec_base;
delta_f1 =f1_pose-rec_f1;
delta_f2 =f2_pose-rec_f2;
delta_f3 =f3_pose-rec_f3;
//ROS_INFO("Position delta-Base: %f,F1: %f,F2: %f,F3:%f",delta_base,delta_f1,delta_f2,delta_f3);
//4 Freedom: j11-Base spread,j32-F3,j12-F1,j22-F2
//other process
if(fabs(delta_base)<=0.02 && fabs(delta_f1)<=0.02&& fabs(delta_f2)<=0.02 && fabs(delta_f3)<=0.02)
flag=1;
}
实际使用时，先实体化类，然后引用成员函数。
bhand_pose bhandpose1;
bhandpose1.bhand_move_velocity(v_base,v_f1,v_f2,v_f3,0);
state_test.bhand_control_mode("POSITION");
sleep(3);
bhandpose1.bhand_move_position(0,1.5,1.5,1.5,1);
sleep(5);
state_test.bhand_control_mode("VELOCITY");
本例测试代码bhand_force_control.cpp。如下所示。读者可参看bhand_state_init类，bhand_pose类，以及bhand_PoseSets类，以及文中实体化的bhand_pose bhandpose1。
#include <ros/ros.h>
#include "bhand_controller/Service.h"
#include "bhand_controller/Actions.h"
#include "bhand_controller/SetControlMode.h"
#include "sensor_msgs/JointState.h"
#include "bhand_controller/State.h"
#include "ros/callback_queue.h"
#include "string"
#include "iostream"
#include "cmath"
#include "ros/subscribe_options.h"
#include "beginner_tutorials/ForceData.h"
#include "geometry_msgs/Twist.h"//use for move bhand by keyboard
#include "id_data_msgs/ID_Data.h"//using for notie event
/******************************************************************************************debug note:* well suitable to the keyboard_control.cpp of 2016,11,18 17:00.**date:.* ***************************************************************************************/
using namespace std;//pre-define
#define f1_sensor_id 0X0758//1880
#define f2_sensor_id 0x0708//1800
#define f3_sensor_id 0x06e0//1760//globals
float key_control_value=0;
bool close_hand_flag=false;
bool open_hand_flag=false;struct sensor_flag
{char row0_flag,row1_flag,row2_flag,row3_flag,row4_flag,row5_flag,row6_flag,row7_flag;
};
sensor_flag sensor1_flag={0},sensor2_flag={0},sensor3_flag={0};int f1_heavy_force_point_num,f2_heavy_force_point_num,f3_heavy_force_point_num;
int force_threshold=90;
//when catch the joy, the threshold is 80.
//when catch some hard object, it is better set the threshold as 90,or 100.
int num_count=0;//function declaration
void Sensor_ForceHeavyPoint_Count(beginner_tutorials::ForceData force_msg,int sensor_id,int *f_heavy_force_point_num,sensor_flag *sensorflag);
void notice_data_clear(id_data_msgs::ID_Data *test);//
class bhand_state_init
{
public:boost::function<void (const bhand_controller::State::ConstPtr&)> StateCallbackFun;bhand_state_init(string mode);void bhand_control_mode(string mode);void bhand_action_test(int act,int delay);void bhand_spinner(char set);private:ros::NodeHandle bhand_state;ros::SubscribeOptions ops;ros::Subscriber listen_state;ros::AsyncSpinner *state_spinner;ros::CallbackQueue state_callback_queue;void state_callback(const bhand_controller::State::ConstPtr& msg);ros::ServiceClient client;bhand_controller::SetControlMode control_mode;bhand_controller::Actions test_act;string set_mode;
public:__uint8_t flag;char first_init_state;};bhand_state_init::bhand_state_init(string mode)
{StateCallbackFun=boost::bind(&bhand_state_init::state_callback,this,_1);ops=ros::SubscribeOptions::create<bhand_controller::State>("/bhand_node/state",1,StateCallbackFun,ros::VoidPtr(),&state_callback_queue);listen_state=bhand_state.subscribe(ops);state_spinner=new ros::AsyncSpinner(1,&state_callback_queue);flag=0;set_mode=mode;first_init_state=1;
}void bhand_state_init::state_callback(const bhand_controller::State::ConstPtr& msg)
{//ROS_INFO("Callback Hand state: %s",msg->state_description.c_str());//ROS_INFO("Callback Control mode: %s",msg->control_mode.c_str());//set hand state: INIT_STATE->READY_STATE//INIT_STATE = 100//STANDBY_STATE = 200//READY_STATE = 300//EMERGENCY_STATE = 400//FAILURE_STATE = 500//SHUTDOWN_STATE = 600if(msg->state==100){//Init barrett hand//Perform the init action then delay 4 sec.ros::Duration(1).sleep();if(first_init_state==1){bhand_action_test(bhand_controller::Service::INIT_HAND,4);first_init_state=0;}}if(msg->state==300)first_init_state=0;if(msg->state==500){ros::Duration(4).sleep();}//set control modeif(msg->control_mode != set_mode){//set control modebhand_control_mode(set_mode);}flag=1;
}void bhand_state_init::bhand_control_mode(string mode)
{set_mode=mode;client=bhand_state.serviceClient<bhand_controller::SetControlMode>("bhand_node/set_control_mode");control_mode.request.mode=mode;if(client.call(control_mode))ROS_INFO("Set %s mode successfully!",mode.c_str());elseROS_INFO("Failed to set control mode!");}void bhand_state_init::bhand_action_test(int act,int delay)
{client=bhand_state.serviceClient<bhand_controller::Actions>("bhand_node/actions");test_act.request.action=act;switch (act) {case 1:ROS_INFO("Call INIT_HAND service!");break;case 2:ROS_INFO("Call CLOSE_GRASP service!");break;case 3:ROS_INFO("Call OPEN_GRASP service!");break;case 4:ROS_INFO("Call SET_GRASP_1 service!");break;case 5:ROS_INFO("Call SET_GRASP_2 service!");break;case 6:ROS_INFO("Call CLOSE_HALF_GRASP service!");break;default:ROS_INFO("Warnnig:Please Call correct service!");break;}if(client.call(test_act))ROS_INFO("Call service successfully!");elseROS_INFO("Failed to call service!");ros::Duration(delay).sleep();}void bhand_state_init::bhand_spinner(char set)
{if (set==1)state_spinner->start();elsestate_spinner->stop();
}//monitor position
//sensor_msgs::JointState msg_state;
//volatile unsigned char command_flag=0;class bhand_pose
{
public:boost::function<void (const sensor_msgs::JointState::ConstPtr&)> cmdCallbackFun;//C++ Templatebhand_pose(){flag=0;command_pub=bhand_command.advertise<sensor_msgs::JointState>("/bhand_node/command",1000);cmdCallbackFun=boost::bind(&bhand_pose::command_callback,this,_1);ops=ros::SubscribeOptions::create<sensor_msgs::JointState>("/joint_states",1,cmdCallbackFun/*command_callback*/,ros::VoidPtr(),&command_callback_queue);listen_position=bhand_command.subscribe(ops);//monitor hand posecommand_spinner=new ros::AsyncSpinner(1,&command_callback_queue);}void bhand_move_position(float base,float f1,float f2,float f3,int delay){msg_state.name={"bh_j11_joint","bh_j32_joint","bh_j12_joint","bh_j22_joint"};//4 Freedom: j11-Base spread,j32-F3,j12-F1,j22-F2msg_state.position={base,f3,f1,f2};//ROS_INFO("Position desired-Base: %f,F1: %f,F2: %f,F3: %f",msg_state.position[0],msg_state.position[2],msg_state.position[3],msg_state.position[1]);msg_state.velocity={0.05,0.1,0.1,0.1};msg_state.effort={0.1,0.3,0.3,0.3};//monitor state: READY && POSITIONSros::Duration(delay).sleep();command_pub.publish(msg_state);}void bhand_move_velocity(float base,float f1,float f2,float f3,int delay){msg_state.name={"bh_j11_joint","bh_j32_joint","bh_j12_joint","bh_j22_joint"};//4 Freedom: j11-Base spread,j32-F3,j12-F1,j22-F2msg_state.position={0,0,0,0};//ROS_INFO("Position desired-Base: %f,F1: %f,F2: %f,F3: %f",msg_state.position[0],msg_state.position[2],msg_state.position[3],msg_state.position[1]);msg_state.velocity={base,f3,f1,f2};msg_state.effort={0.1,0.3,0.3,0.3};//monitor state: READY && VELOCITYros::Duration(delay).sleep();command_pub.publish(msg_state);}void bhand_spinner(char set){if (set==1)command_spinner->start();elsecommand_spinner->stop();}private:ros::NodeHandle bhand_command;ros::Publisher command_pub;sensor_msgs::JointState msg_state;ros::SubscribeOptions ops;ros::Subscriber listen_position;ros::AsyncSpinner *command_spinner;ros::CallbackQueue command_callback_queue;void command_callback(const sensor_msgs::JointState::ConstPtr& msg);
public:float base_pose,f1_pose,f2_pose,f3_pose;float rec_base,rec_f1,rec_f2,rec_f3;float delta_base,delta_f1,delta_f2,delta_f3;__uint8_t flag;};void bhand_pose::command_callback(const sensor_msgs::JointState::ConstPtr& msg)
{//ROS_INFO("Position readed-Base: %f,F1: %f,F2: %f,F3: %f",msg->position[6],msg->position[1],msg->position[2],msg->position[3]);rec_base=msg->position[6];if(rec_base<0) rec_base=0;//avoid some abnormal phenomenonsif(rec_base>3) rec_base=2.5;rec_f1=msg->position[1];if(rec_f1<0) rec_f1=0;if(rec_f1>3) rec_f1=2.5;rec_f2=msg->position[2];if(rec_f2<0) rec_f2=0;if(rec_f2>3) rec_f2=2.5;rec_f3=msg->position[3];if(rec_f3<0) rec_f3=0;if(rec_f3>3) rec_f3=2.5;delta_base=base_pose-rec_base;delta_f1 =f1_pose-rec_f1;delta_f2 =f2_pose-rec_f2;delta_f3 =f3_pose-rec_f3;//ROS_INFO("Position delta-Base: %f,F1: %f,F2: %f,F3: %f",delta_base,delta_f1,delta_f2,delta_f3);//4 Freedom: j11-Base spread,j32-F3,j12-F1,j22-F2//other processif(fabs(delta_base)<=0.02 && fabs(delta_f1)<=0.02 && fabs(delta_f2)<=0.02 && fabs(delta_f3)<=0.02)flag=1;
}//Usage:set barett hand in any position
//Input:pose data of each freedom are base,finger 1,finger 2,finger 3,pose number stamp.
//example:
//bhand_PoseSets bhandpose1(0,1.6,1.55,1.65,1);
class bhand_PoseSets{public:bhand_PoseSets();void Set(float base,float f1,float f2,float f3,int delay,int no);void Set_velocity(float base,float f1,float f2,float f3,float limit_base,float limit_f1,float limit_f2,float limit_f3);
private:bhand_pose bhand_pose1;__uint8_t flag;
};bhand_PoseSets::bhand_PoseSets()
{bhand_pose1.flag=0;
}void bhand_PoseSets::Set(float base,float f1,float f2,float f3,int delay,int no)
{bhand_pose1.base_pose=base;bhand_pose1.f1_pose=f1;bhand_pose1.f2_pose=f2;bhand_pose1.f3_pose=f3;bhand_pose1.bhand_move_position(base,f1,f2,f3,delay);ros::Rate loop_rate(10);while(ros::ok()){bhand_pose1.bhand_spinner(1);if(bhand_pose1.flag==1){bhand_pose1.bhand_spinner(0);//ROS_INFO("No.%d Bhand pose achieved!",no);break;}loop_rate.sleep();}bhand_pose1.flag=0;
}
void bhand_PoseSets::Set_velocity(float base, float f1, float f2, float f3, float limit_base, float limit_f1, float limit_f2, float limit_f3)
{bhand_pose1.base_pose=limit_base;bhand_pose1.f1_pose=limit_f1;bhand_pose1.f2_pose=limit_f2;bhand_pose1.f3_pose=limit_f3;ros::Rate loop_rate(10);if(bhand_pose1.rec_base<0) bhand_pose1.rec_base=0;//avoid some abnormal phenomenonsif(bhand_pose1.rec_base>3) bhand_pose1.rec_base=limit_base;if(bhand_pose1.rec_f1<0) bhand_pose1.rec_f1=0;if(bhand_pose1.rec_f1>3) bhand_pose1.rec_f1=limit_f1;if(bhand_pose1.rec_f2<0) bhand_pose1.rec_f2=0;if(bhand_pose1.rec_f2>3) bhand_pose1.rec_f2=limit_f2;if(bhand_pose1.rec_f3<0) bhand_pose1.rec_f3=0;if(bhand_pose1.rec_f3>3) bhand_pose1.rec_f3=limit_f3;while(ros::ok()){ROS_INFO("base %f,f1 %f,f2 %f,f3 %f",bhand_pose1.rec_base,bhand_pose1.rec_f1,bhand_pose1.rec_f2,bhand_pose1.rec_f3);if((bhand_pose1.rec_base>=0 && bhand_pose1.rec_base<=limit_base) && (bhand_pose1.rec_f1>=0 && bhand_pose1.rec_f1<=limit_f1)&& (bhand_pose1.rec_f2>=0 && bhand_pose1.rec_f2<=limit_f2)&& (bhand_pose1.rec_f3>=0 && bhand_pose1.rec_f3<=limit_f3)){bhand_pose1.bhand_move_velocity(base,f1,f2,f3,0);}else{break;}bhand_pose1.bhand_spinner(1);loop_rate.sleep();}}class tactile_listener
{
public:boost::function<void (const beginner_tutorials::ForceData::ConstPtr&)> tactile_msgCallbackFun;tactile_listener();void tactile_sub_spinner(char set);private:ros::NodeHandle tactile_handle;ros::Subscriber tactile_subscriber;ros::SubscribeOptions tactile_ops;ros::AsyncSpinner *tactile_spinner;ros::CallbackQueue tactile_callbackqueue;void tactile_msgCallback(const beginner_tutorials::ForceData::ConstPtr &tactile_msg);
};tactile_listener::tactile_listener()
{tactile_msgCallbackFun=boost::bind(&tactile_listener::tactile_msgCallback,this,_1);tactile_ops=ros::SubscribeOptions::create<beginner_tutorials::ForceData>("/ArrayForcePUB",1,tactile_msgCallbackFun,ros::VoidPtr(),&tactile_callbackqueue);tactile_subscriber=tactile_handle.subscribe(tactile_ops);tactile_spinner=new ros::AsyncSpinner(1,&tactile_callbackqueue);
}void tactile_listener::tactile_sub_spinner(char set)
{if (set==1)tactile_spinner->start();if (set==0)tactile_spinner->stop();}void tactile_listener::tactile_msgCallback(const beginner_tutorials::ForceData::ConstPtr &tactile_msg)
{
// printf("Tactile Sensor,ID: %u,Data: ",tactile_msg->id);
// for(char i=0;i<8;i++)
// {
// printf("%u ",tactile_msg->data[i]);
// if(i==7) printf("\n");
// }beginner_tutorials::ForceData force_msg;force_msg.id=tactile_msg->id;force_msg.data=tactile_msg->data;Sensor_ForceHeavyPoint_Count(force_msg,f1_sensor_id,&f1_heavy_force_point_num,&sensor1_flag);Sensor_ForceHeavyPoint_Count(force_msg,f2_sensor_id,&f2_heavy_force_point_num,&sensor2_flag);Sensor_ForceHeavyPoint_Count(force_msg,f3_sensor_id,&f3_heavy_force_point_num,&sensor3_flag);
}class keyboard_monitor
{
public:boost::function<void (const geometry_msgs::Twist::ConstPtr&)> keyboard_monitor_CallbackFun;keyboard_monitor();void keyboard_monitor_spinner(char set);
private:ros::NodeHandle keyboard_handle;ros::Subscriber keyboard_subscriber;ros::SubscribeOptions keyboard_ops;ros::AsyncSpinner *keyboard_spinner;ros::CallbackQueue keyboard_callbackQueue;void keyboard_monitor_callback(const geometry_msgs::Twist::ConstPtr &key_control_msg);
};keyboard_monitor::keyboard_monitor()
{keyboard_monitor_CallbackFun=boost::bind(&keyboard_monitor::keyboard_monitor_callback,this,_1);keyboard_ops=ros::SubscribeOptions::create<geometry_msgs::Twist>("/keyboard_control",1,keyboard_monitor_CallbackFun,ros::VoidPtr(),&keyboard_callbackQueue);keyboard_subscriber=keyboard_handle.subscribe(keyboard_ops);keyboard_spinner=new ros::AsyncSpinner(1,&keyboard_callbackQueue);}void keyboard_monitor::keyboard_monitor_spinner(char set)
{if(set==1)keyboard_spinner->start();if(set==0)keyboard_spinner->stop();}void keyboard_monitor::keyboard_monitor_callback(const geometry_msgs::Twist::ConstPtr &key_control_msg)
{
// ROS_INFO("Keyboard control speed:%f",key_control_msg->linear.x);key_control_value=key_control_msg->linear.x;
}class notice_pub_sub
{
public:boost::function<void (const id_data_msgs::ID_Data::ConstPtr&)> notice_pub_sub_msgCallbackFun;notice_pub_sub();void notice_pub_sub_listener();void notice_pub_sub_pulisher(id_data_msgs::ID_Data id_data);void notice_display(id_data_msgs::ID_Data notice_msg,bool set);void notice_sub_spinner(char set);private:ros::NodeHandle notice_handle;ros::Subscriber notice_subscriber;ros::Publisher notice_publisher;ros::SubscribeOptions notice_ops;ros::AsyncSpinner *notice_spinner;ros::CallbackQueue notice_callbackqueue;void notice_msgCallback(const id_data_msgs::ID_Data::ConstPtr ¬ice_msg);
};notice_pub_sub::notice_pub_sub()
{notice_pub_sub_msgCallbackFun=boost::bind(¬ice_pub_sub::notice_msgCallback,this,_1);notice_ops=ros::SubscribeOptions::create<id_data_msgs::ID_Data>("/notice",1,notice_pub_sub_msgCallbackFun,ros::VoidPtr(),¬ice_callbackqueue);notice_subscriber=notice_handle.subscribe(notice_ops);notice_spinner=new ros::AsyncSpinner(1,¬ice_callbackqueue);notice_publisher=notice_handle.advertise<id_data_msgs::ID_Data>("/notice",1);
}void notice_pub_sub::notice_pub_sub_listener()
{}void notice_pub_sub::notice_pub_sub_pulisher(id_data_msgs::ID_Data id_data)
{notice_publisher.publish(id_data);
}void notice_pub_sub::notice_display(id_data_msgs::ID_Data notice_msg,bool set)
{if(set){printf("REC Notice message,ID: %d,Data: ",notice_msg.id);for(char i=0;i<8;i++){printf("%d ",notice_msg.data[i]);if(i==7) printf("\n");}}}
void notice_pub_sub::notice_msgCallback(const id_data_msgs::ID_Data::ConstPtr ¬ice_msg)
{id_data_msgs::ID_Data notice_message;notice_message.id=0;for(char i=0;i<8;i++)notice_message.data[i]=0;notice_message.id=notice_msg->id;for(char i=0;i<8;i++)notice_message.data[i]=notice_msg->data[i];notice_pub_sub::notice_display(notice_message,false);if(notice_message.id==1 && notice_message.data[0]==1)//close flag{close_hand_flag=true;notice_data_clear(¬ice_message);notice_message.id=1;notice_message.data[0]=14;notice_publisher.publish(notice_message);}if(notice_message.id==1 && notice_message.data[0]==0)//open flag{open_hand_flag=true;notice_data_clear(¬ice_message);notice_message.id=1;notice_message.data[0]=14;notice_publisher.publish(notice_message);}}void notice_pub_sub::notice_sub_spinner(char set)
{if(set==1)notice_spinner->start();if(set==0)notice_spinner->stop();
}int main(int argc,char **argv)
{ros::init(argc,argv,"bhand_force_control");ros::NodeHandle test_handle;//uselessbhand_state_init state_test("POSITION");int loop_hz=100;ros::Rate loop_rate(loop_hz);while(ros::ok()){if (state_test.flag){state_test.bhand_spinner(0);break;}elsestate_test.bhand_spinner(1);loop_rate.sleep();}//velocity controlstate_test.bhand_control_mode("VELOCITY");//soft contact control//strategy://step delta,then monitor the tactile sensor, and if its' value do not get the threshold,//then continue moving delta.bhand_pose bhandpose1;float v_base=0.001,v_f1=0,v_f2=0,v_f3=0,rec_v_base,rec_v_f1,rec_v_f2,rec_v_f3;float delta_v_base,delta_v_f1,delta_v_f2,delta_v_f3;tactile_listener tactile_test;keyboard_monitor keyboard_control;ros::Rate speed_loop_rate(loop_hz);float limit_speed=0.4;bhand_PoseSets bhand_pose_pre_catch;bhand_pose_pre_catch.Set_velocity(0,0.4,0.4,0.4,0,0.5,0.5,0.5);printf("Pre-catch pose have achieved!\n");notice_pub_sub notice_test;id_data_msgs::ID_Data notice_data_pub;bool f1_close_flag=false,f2_close_flag=false,f3_close_flag=false;while(ros::ok()){//monitor tactile sensor topic//while base<=2, f1<=2,...//bhandpose1.Set(base+0.1,f1+0.1,f2+0.1,f3+0.1,count+1);v_base=-v_base;v_f1+=key_control_value;v_f2+=key_control_value;v_f3+=key_control_value;if(v_f1<=-1.0) v_f1=-limit_speed;if(v_f1>= 1.0) v_f1= limit_speed;if(v_f2<=-1.0) v_f2=-limit_speed;if(v_f2>= 1.0) v_f2= limit_speed;if(v_f3<=-1.0) v_f3=-limit_speed;if(v_f3>= 1.0) v_f3= limit_speed;//ROS_INFO("Joint Speed:base %f,f1 %f,f2 %f,f3 %f.\n",v_base,v_f1,v_f2,v_f3);if((num_count++)%100==0){ROS_INFO("base %f,f1 %f,f2 %f,f3 %f",bhandpose1.rec_base,bhandpose1.rec_f1,bhandpose1.rec_f2,bhandpose1.rec_f3);//ROS_INFO("Joint Speed:f1 %f,f2 %f,f3 %f.",v_f1,v_f2,v_f3);//ROS_INFO("Sensor Heavy Point number:F1 %d,F2 %d,F3 %d\n",f1_heavy_force_point_num,f2_heavy_force_point_num,f3_heavy_force_point_num);}key_control_value=0;delta_v_f1=v_f1-rec_v_f1;delta_v_f2=v_f2-rec_v_f2;delta_v_f3=v_f3-rec_v_f3;if((bhandpose1.rec_f1<=2.2 && bhandpose1.rec_f1>=0)&&(bhandpose1.rec_f2<=2.2 && bhandpose1.rec_f2>=0)&&(bhandpose1.rec_f3<=2.2 && bhandpose1.rec_f3>=0)){if(f1_heavy_force_point_num>=3) v_f1=0;if(f2_heavy_force_point_num>=3) v_f2=0;if(f3_heavy_force_point_num>=3) v_f3=0;bhandpose1.bhand_move_velocity(v_base,v_f1,v_f2,v_f3,0);}elsev_f1=v_f2=v_f3=0;rec_v_f1=v_f1;rec_v_f2=v_f2;rec_v_f3=v_f3;//------notice topic control hand close or open start......if(close_hand_flag){open_hand_flag=false;state_test.bhand_control_mode("POSITION");sleep(3);bhandpose1.bhand_move_position(0,1.5,1.5,1.5,1);sleep(5);state_test.bhand_control_mode("VELOCITY");notice_data_clear(¬ice_data_pub);notice_data_pub.id=1;notice_data_pub.data[0]=2;notice_test.notice_pub_sub_pulisher(notice_data_pub);close_hand_flag=false;printf("Close hand successfully!\n");}if(open_hand_flag){close_hand_flag=false;//action dealv_f1=v_f2=v_f3=-0.4;bhandpose1.bhand_move_velocity(v_base,v_f1,v_f2,v_f3,0);if(bhandpose1.rec_f1<=0.2) v_f1=0;if(bhandpose1.rec_f2<=0.2) v_f2=0;if(bhandpose1.rec_f3<=0.2) v_f3=0;if(bhandpose1.rec_f1<=0.2 && bhandpose1.rec_f2<=0.2 && bhandpose1.rec_f3<=0.2){v_f1=v_f2=v_f3=0;bhandpose1.bhand_move_velocity(v_base,v_f1,v_f2,v_f3,0);notice_data_clear(¬ice_data_pub);notice_data_pub.id=1;notice_data_pub.data[0]=2;notice_test.notice_pub_sub_pulisher(notice_data_pub);open_hand_flag=false;printf("Open hand successfully!\n");state_test.bhand_control_mode("POSITION");state_test.bhand_action_test(3,1);sleep(3);bhandpose1.bhand_move_position(0,0.5,0.5,0.5,1);sleep(5);state_test.bhand_control_mode("VELOCITY");}}v_f1=rec_v_f1;v_f2=rec_v_f2;v_f3=rec_v_f3;//------notice topic control hand close or open end......state_test.bhand_spinner(1);bhandpose1.bhand_spinner(1);tactile_test.tactile_sub_spinner(1);keyboard_control.keyboard_monitor_spinner(1);//notice call back function triggernotice_test.notice_sub_spinner(1);speed_loop_rate.sleep();}return 0;
}void Sensor_ForceHeavyPoint_Count(beginner_tutorials::ForceData force_msg,int sensor_id,int *f_heavy_force_point_num,sensor_flag *sensorflag)
{if(force_msg.id==sensor_id+0 ){for(char i=0;i<8;i++){if(force_msg.data[i]>=force_threshold)*f_heavy_force_point_num=*f_heavy_force_point_num+1;}sensorflag->row0_flag=1;}if(force_msg.id==sensor_id+1 ){for(char i=0;i<8;i++){if(force_msg.data[i]>=force_threshold)*f_heavy_force_point_num=*f_heavy_force_point_num+1;}sensorflag->row1_flag=1;}if(force_msg.id==sensor_id+2 ){for(char i=0;i<8;i++){if(force_msg.data[i]>=force_threshold)*f_heavy_force_point_num=*f_heavy_force_point_num+1;}sensorflag->row2_flag=1;}if(force_msg.id==sensor_id+3 ){for(char i=0;i<8;i++){if(force_msg.data[i]>=force_threshold)*f_heavy_force_point_num=*f_heavy_force_point_num+1;}sensorflag->row3_flag=1;}if(force_msg.id==sensor_id+4 ){for(char i=0;i<8;i++){if(force_msg.data[i]>=force_threshold)*f_heavy_force_point_num=*f_heavy_force_point_num+1;}sensorflag->row4_flag=1;}if(force_msg.id==sensor_id+5 ){for(char i=0;i<8;i++){if(force_msg.data[i]>=force_threshold)*f_heavy_force_point_num=*f_heavy_force_point_num+1;}sensorflag->row5_flag=1;}if(force_msg.id==sensor_id+6 ){for(char i=0;i<8;i++){if(force_msg.data[i]>=force_threshold)*f_heavy_force_point_num=*f_heavy_force_point_num+1;}sensorflag->row6_flag=1;}if(force_msg.id==sensor_id+7 ){for(char i=0;i<8;i++){if(force_msg.data[i]>=force_threshold)*f_heavy_force_point_num=*f_heavy_force_point_num+1;}sensorflag->row7_flag=1;}if(sensorflag->row0_flag && sensorflag->row1_flag && sensorflag->row2_flag && sensorflag->row3_flag && sensorflag->row4_flag && sensorflag->row5_flag && sensorflag->row6_flag && sensorflag->row7_flag){*(sensorflag)={0};*f_heavy_force_point_num=0;}
}void notice_data_clear(id_data_msgs::ID_Data *test)
{test->id=0;for(int i=0;i<8;i++) test->data[i]=0;
}发布Sensor_msgs::JointState关节位置或速度实现Barrett Hand机械手控制相关推荐
- 基于ｓｔｍ３２的位置和速度双闭环ＰＩＤ直流电机控制
基于stm32的位置和速度双闭环PID直流电机控制 为什么要用双闭环PID算法? 如何将位置环和速度环一起使用? 程序部分代码 为什么要用双闭环PID算法? 在PID算法中,我们如果只是使用单个闭环P ...
- vrep中设置joint的位置、速度需要根据关节的模式来设置。
vrep中设置joint的位置.速度需要根据关节的模式来设置. simSetJointPosition() (when your joint is not in force/torque mode 比 ...
- 【四足机器人--关节初始化时足端位置（速度、加速度）轨迹规划】（4.2）JPosInitializer（B样条曲线计算脚的摆动轨迹）代码解析
系列文章目录 提示:这里可以添加系列文章的所有文章的目录,目录需要自己手动添加 TODO:写完再整理 文章目录 系列文章目录 前言 一.关节初始化构造函数(设置目标时间及间隔) 二.给定并B样条起始点 ...
- 可以装在手机里的3D姿态估计，模型尺寸仅同类1/7，平均关节位置误差却只有5厘米 | CPVR 2021...
博雯 发自 凹非寺 量子位 报道 | 公众号 QbitAI 长久以来,三维姿态估计都在追求准确性上一路狂奔. 但精度提高的同时,也带来了计算成本的上升. 而刚刚被CPVR 2021接受的论文中所提出的 ...
- 电机高频注入原理_永磁同步电机转子位置与速度估算的新方法，精度好，性价比高...
北京航空航天大学惯性技术重点实验室.北京市高速磁悬浮电机技术及应用工程技术研究中心的研究人员赵远洋.韩邦成.陈宝栋,在2019年第15期<电工技术学报>上撰文指出(论文标题为"基 ...
- java 弹性碰撞_球体弹性碰撞位置和速度计算算法
#本人原创,费了不少功夫计算推导公式,通过验证非常完美 #两球的位置和速度,R为半径,这里设质量一样,容易加上不同的质量和半径 def collide(loc1,loc2,sp1,sp2): x,y= ...
- 电机位置、速度检测方法大合集
本篇我们学习一些常用的电动机的位置.速度检测方法,有的通过电磁感应效应检测.有的通过光电转换后检测.有的通过霍尔元器件检测,等等.下面我们就一一来讲解. 1)光电编码器 光电编码器可以把角度或者速度转 ...
- 高、低成本MEMS惯导系统姿态、位置、速度更新算法的对比
高.低成本MEMS惯导系统姿态.位置.速度更新算法的对比 一.高成本MEMS惯导系统姿态.位置.速度更新算法 1.速度更新 2.位置更新 3.姿态更新 4.程序仿真及实验结果 4.1 主函数 4.2 ...
- 自车坐标系下的物体相对和绝对位置和速度计算
自车坐标系与绝对坐标系的速度位置换算原理 自车坐标系 坐标系--右-前-天坐标(RFU) 坐标系--前-左-上(FLU) 位置换算 速度换算 示意图 情形一 情形二 小结 参考 自车坐标系 自车坐标系 ...
最新文章
- 在 ubuntu 20.04 LTS 上安装 ROS2 执行 rosdep update 命令时出现的问题的解决办法
- 「SVN」Linux下svn使用命令
- java反射 初始化bean_通用javabean初始化（反射机制）
- as3位图绘制器（矢量器）：as3potrace
- java 修改源码_再谈给应用程序diy启动画面和java源代码补丁修改
- UML类图各符号含义
- html5 画布保存,html5 (canvas)画布save()和restore()的理解和使用方法
- php项目宝塔搭建出租屋租赁系统源码带小程序源码
- 从Visual SourceSafe (VSS)服务器下载文件(C#)
- autorunner
- Win10锁屏壁纸不能自动更新最全处理方法
- 我的政治理想《爱因斯坦文集》
- 最新 2022中国大学排名发布~
- 如何测量两组汇编指令的执行效率
- 欧框语言框架标准C2,雅思成绩与欧洲语言共同参考框架的对应关系
- CIDR地址规划方法
- STATA 森林图 基于OR值和CI直接画的
- matlab非线性数值方程的求解
- 计算机还原默认的配置,如何恢复IE浏览器的默认设置？如何还原IE浏览器的默认设置...
- 24位真彩和32位真彩