这几天主要还是在研究用C++编写moveit target pose的例程，中途出现了各种问题，一度怀疑人生，好在现在也算是磕磕碰碰的解决了一点……虽然还是不知道why，不过也是增强了继续学习的信心。
困扰最久的问题是经常出现逆运动学解算结果只有平移，没有旋转，首先，target pose的设置存在一定问题，其次，默认的KDL解算插件确实不给力！后来参考古月大神的解算插件配置，改为TRAC-IK，情况改善了很多，本来想用IKFAST的，但是因为C++11的各种问题，导致make不出来，花式报错，故放弃……
项目上传至github：https://github.com/SCUPRISCILLA/My_ROS_Package/tree/master/crp09_test
对target pose的控制是通过传递goal话题来实现的，如图所示，其中my_crp09_moveit是编写的CPP文件中定义的，我们还可以通过rostopic echo来查看关节位置状态topic: /joint_states，来进一步处理数据
代码如下：
#include <moveit/move_group_interface/move_group_interface.h>
#include<moveit/planning_scene_interface/planning_scene_interface.h>
#include<moveit_msgs/DisplayRobotState.h>
#include<moveit_msgs/DisplayTrajectory.h>
#include<moveit_msgs/AttachedCollisionObject.h>
#include<moveit_msgs/CollisionObject.h>
#include<iostream>
using namespace std;
//每当修改，都要重新catkin_make
int main(int argc, char **argv)
{
ros::init(argc, argv, "my_crp09_moveit");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
sleep(2.0);
moveit::planning_interface::MoveGroupInterface group("arm");
//moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
// ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory> ("/move_group/display_planned_path", 1, true);
// moveit_msgs::DisplayTrajectory display_trajectory;
ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());
geometry_msgs::Pose target_pose1;
target_pose1.orientation.x= 0.343915;
target_pose1.orientation.y = -0.000155354;
target_pose1.orientation.z = -0.000143756;
target_pose1.orientation.w = 0.939001;
target_pose1.position.x = -0.0656481;
target_pose1.position.y = 0.415693;
target_pose1.position.z = 1.04234;
group.setPoseTarget(target_pose1);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
moveit::planning_interface::MoveItErrorCode success = group.plan(my_plan);
ROS_INFO("Visualizing plan 1 (pose goal) %s", success ? "WHY SUCCESS" : "FUCKING FALLED");
if(success)
group.execute(my_plan);
else
cout << "falled"<<endl;
sleep(5.0);
ros::shutdown();
}该CPP文件也可以作为一个模板使用。通过修改orientation和position参数来设置不同的target pose，然后使用group.execute执行。
运行结果：
机械臂自行规划运动到target pose
使用echo > 将joint position数据打印到指定文件中，然后使用Python matplot进行可视化处理
代码为：
import numpy as np
import matplotlib.pyplot as plt
import re
# read files
origin_file = open('joint_states.txt','r')
position_file = open('position_data.txt','w')
lines = origin_file.readlines()
origin_file.close()
# get position datas and put into a list "data"
data = []
for line in lines:
if "position" in line:
position_file.write(line)
data.append(re.findall(r"\-?\d+\.?\d*",line))
position_file.close()
print('All the position data number is:', len(data))
# get each joints' position data
joint1_postion = []
joint2_postion = []
joint3_postion = []
joint4_postion = []
joint5_postion = []
joint6_postion = []
finger_joint1 = []
finger_joint2 = []
for i in range(len(data)): # i is from 0 to data number -1
joint1_postion.append(float(data[i][0]))
joint2_postion.append(float(data[i][1]))
joint3_postion.append(float(data[i][2]))
joint4_postion.append(float(data[i][3]))
joint5_postion.append(float(data[i][4]))
joint6_postion.append(float(data[i][5]))
finger_joint1.append(float(data[i][6]))
finger_joint2.append(float(data[i][7]))
# PLOT A FIGURE BY MATPLOTLIB
t = np.linspace(0, 10, len(data))
plt.xlabel("move time t")
plt.ylabel("joint postion arc")
plt.subplot(2,3,1)
plt.title("joint 1")
plt.plot(t, joint1_postion)
plt.subplot(2,3,2)
plt.title("joint 2")
plt.plot(t, joint2_postion)
plt.subplot(2,3,3)
plt.title("joint 3")
plt.plot(t, joint3_postion)
plt.subplot(2,3,4)
plt.title("joint 4")
plt.plot(t, joint4_postion)
plt.subplot(2,3,5)
plt.title("joint 5")
plt.plot(t, joint5_postion)
plt.subplot(2,3,6)
plt.title("joint 6")
plt.plot(t, joint6_postion)
plt.show()运行结果为：
中途有很奇怪抖动，大概是电机启动时的啥啥啥？