moveit规划实现抓取放置的现场
构建urdf模型时一般是简单的定义两个手指为滑动关节。
抓手规划一般是指定开合距离,即左右手指的位置
这就要求我们需要把电机的旋转角位和开合距离做一个转换的功能。
要写一个抓手的controller或是驱动,都应该需要写一个简单映射或者动力学的公式才行。
思路是创建一个service
ros service 好处是实时、一对多。这样我们在驱动和controller里都可以调用了。
消息结构如下
flag是标记 取值 1,2
1时 request 是角位 response是距离
2时 request 是距离 response是角位
驱动读出舵机的角位实际是个pwm值,用最土的映射办法,准备一系列pwm值,测到与之对应的夹子宽度,之间进行线性插值。
下面实现转换功能的service源码
驱动里使用service
urdf里这样设置两个手指坐标为0,夹子距离为0
对称移动,距离/2=abs(坐标)
驱动里读出pwm值call service得到距离,计算坐标并发布手指的joint_states
moveit和controller都要用到这个 joint_states
controller里轨迹(一系列坐标)控制的转换
两个坐标绝对值相加得到距离,call service 得到pwm 最终转换为真实电机的角位(弧度)。
发布消息控制电机运动
有了这个service ,按照示例在抓手的controller 串口驱动加以利用, 就可以进行手臂的抓取和放置操作了。如果再加个kinect利用点云和图像识别环境和抓取物体,完成全自动抓放那就更完美了!
……要什么自行车?
问题
could not find joint 'xxx' in 'hardware_interface::PositionJointInterface'
initializing controller 'xxx' failed
分析
两个controller
照猫画虎,写两个类继承public hardware_interface::RobotHW
每个类都有
hardware_interface::PositionJointInterface pj_interface_;
分别注册
registerInterface(&pj_interface_);
按照套路使用 controllermanager
编译两个节点
每个节点单独运行(只有一个controller)时正常,两个一起则报上边的错误。
问题似乎出在registerInterface
虽然是在两个节点分别注册,但 似乎是覆盖了全局的数据?
还是琢磨不透写ros_controll 作者的心思
解决
最终合二为一,只registerInterface一次,在一个节点里搞定就ok了。
ros service 一对多。
http://blog.csdn.net/pcler/article/details/45587313
Linux环境下段错误的产生原因及调试方法小结
https://www.cnblogs.com/panfeng412/archive/2011/11/06/2237857.html
ROS学习(十三):time 和 Timer
rqtgraph
由于DIY的手臂上的抓手一般是一只舵机控制的夹子(两个手指对称移动的机械结构)
构建urdf模型时一般是简单的定义两个手指为滑动关节。
抓手规划一般是指定开合距离,即左右手指的位置
这就要求我们需要把电机的旋转角位和开合距离做一个转换的功能。
要写一个抓手的controller或是驱动,都应该需要写一个简单映射或者动力学的公式才行。
思路是创建一个service
ros service 好处是实时、一对多。这样我们在驱动和controller里都可以调用了。
消息结构如下
int8 flag
float64 request
---
float64 response
flag是标记 取值 1,2
1时 request 是角位 response是距离
2时 request 是距离 response是角位
驱动读出舵机的角位实际是个pwm值,用最土的映射办法,准备一系列pwm值,测到与之对应的夹子宽度,之间进行线性插值。
下面实现转换功能的service源码
#include "ros/ros.h"
#include "zzz_arm_2_srv_finger/trans_state.h"
unsigned char const len=17;
double pwm[len]={
1350.0,1400.0,1450.0,1500.0,1550.0,1600.0,1650.0,1700.0,1750.0,
1800.0,1850.0,1900.0,1950.0,2000.0,2050.0,2100.0,2200.0
};
double dis[len]={
0.0,0.003,0.0045,0.01,0.012,0.016,0.018,0.022,0.025,
0.032,0.034,0.038,0.042,0.047,0.048,0.052,0.053
};
bool trans(zzz_arm_2_srv_finger::trans_state::Request &req,
zzz_arm_2_srv_finger::trans_state::Response &res)
{
int i=0;
double p=0.0;
if(req.flag==1){
//pwm2dis r2d
for(i=0;i<len-1;i++){
if(req.request>=pwm[i]&&req.request<=pwm[i+1]){
p=(req.request-pwm[i])/(pwm[i+1]-pwm[i]);
break;
}
}
p=p<0?0:p;
p=p>1?1:p;
res.response = dis[i]+p*(dis[i+1]-dis[i]);
}else if(req.flag==2){
//dis2pwm d2r
for(i=0;i<len-1;i++){
if(req.request>=dis[i]&&req.request<=dis[i+1]){
p=(req.request-dis[i])/(dis[i+1]-dis[i]);
break;
}
}
p=p<0?0:p;
p=p>1?1:p;
res.response = pwm[i]+p*(pwm[i+1]-pwm[i]);
}
ROS_INFO("request: flg=%d, req=%lf", ( int)req.flag, (double)req.request);
ROS_INFO("response: res=[%lf]", (double)res.response);
return true;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "zzz_arm_2_trans_state");
ros::NodeHandle nh;
ros::ServiceServer service = nh.advertiseService("zzz_arm_finger_state_trans", trans);
ROS_INFO("ServiceServer zzz_arm_finger_state_trans Ready to trans finger's state.");
ros::spin();
return 0;
}
驱动里使用service
urdf里这样设置两个手指坐标为0,夹子距离为0
对称移动,距离/2=abs(坐标)
驱动里读出pwm值call service得到距离,计算坐标并发布手指的joint_states
moveit和controller都要用到这个 joint_states
srv.request.flag = 1;
srv.request.request = (double) posi;
if (client.call(srv))
{
ROS_INFO("griper dis: %lf", (double)srv.response.response);
double dis=srv.response.response/2;
joint_state_msg.header.stamp = ros::Time::now();
joint_state_msg.name.resize(2);
joint_state_msg.position.resize(2);
joint_state_msg.name[0] ="finger_1_joint";
joint_state_msg.position[0] = -dis ;
joint_state_msg.name[1] ="finger_2_joint";
joint_state_msg.position[1] = dis ;
//publish
joint_pub_arm.publish(joint_state_msg);
ros::spinOnce();
}
else
{
ROS_ERROR("r2d Failed to call service zzz_arm_finger_state_trans");
//return 1;
}
controller里轨迹(一系列坐标)控制的转换
两个坐标绝对值相加得到距离,call service 得到pwm 最终转换为真实电机的角位(弧度)。
发布消息控制电机运动
void GrpHardwareInterface::write(ros::Duration elapsed_time)
{
// Enforce joint limits
//jnt_limits_interface_.enforceLimits(elapsed_time);
zzz_arm_2_control_driver_msgs::joint_msg joint_motor_msg;
// Compute and send commands
double dis=0.0;
char flag=0;
for (size_t i = 0; i < n_dof_; ++i)
{
dis+=fabs(joint_position_command_[i]);
if(joint_position_command_last_[i]!=joint_position_command_[i]){
flag=1;
joint_position_command_last_[i]=joint_position_command_[i];
}
}
//*
//zzz_arm_2_srv_finger::trans_state srv;
if(flag==1){
srv.request.flag = 2;
srv.request.request = dis;
if (client.call(srv))
{
//ROS_INFO("griper r: %lf", (double)srv.response.response);
double pwm=srv.response.response;//test 2000.0;//
double per=double (pwm-1500.0)/1000.0;
double pi=3.1415926535;
double r=per*pi/4.0*3.0;
joint_motor_msg.id=5;
joint_motor_msg.r=r;
joint_motor_msg.t=elapsed_time.toSec()*2.0;
joint_motor_pub_.publish(joint_motor_msg);
ros::spinOnce();
}
else
{
ROS_ERROR("d2r Failed to call service zzz_arm_finger_state_trans");
//return 1;
}//*/
}
}
有了这个service ,按照示例在抓手的controller 串口驱动加以利用, 就可以进行手臂的抓取和放置操作了。如果再加个kinect利用点云和图像识别环境和抓取物体,完成全自动抓放那就更完美了!
……要什么自行车?
问题
could not find joint 'xxx' in 'hardware_interface::PositionJointInterface'
initializing controller 'xxx' failed
分析
两个controller
照猫画虎,写两个类继承public hardware_interface::RobotHW
每个类都有
hardware_interface::PositionJointInterface pj_interface_;
分别注册
registerInterface(&pj_interface_);
按照套路使用 controllermanager
编译两个节点
每个节点单独运行(只有一个controller)时正常,两个一起则报上边的错误。
问题似乎出在registerInterface
虽然是在两个节点分别注册,但 似乎是覆盖了全局的数据?
还是琢磨不透写ros_controll 作者的心思
解决
最终合二为一,只registerInterface一次,在一个节点里搞定就ok了。
不过,总觉的这样不符合ros框架分而治之的思想,求大神赐教!
ros service 一对多。
http://blog.csdn.net/pcler/article/details/45587313
Linux环境下段错误的产生原因及调试方法小结
https://www.cnblogs.com/panfeng412/archive/2011/11/06/2237857.html
ROS学习(十三):time 和 Timer
http://blog.csdn.net/xuehuafeiwu123/article/details/68060377