发布Sensor_msgs::JointState关节位置或速度实现Barrett Hand机械手控制

发布时间:2017年08月10日 18:51:37    浏览数:418次    来自:信雪神话
通过独立的机械手初始化类和操作类能够实现任意机械手控制模式切换,机械手位置控制,机械手速度控制。

效果:通过独立的机械手初始化类和操作类能够实现任意机械手控制模式切换,机械手位置控制,机械手速度控制。

环境: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 获得相关的信息。

1.png

另外可从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.png

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);
    }

标签: ROSClubBarrettHand机械臂

评论共0条评论

登录后再评论!

全部评论

目前没有评论