1.首先参考 pick and place 的 demo 理解
roslaunch panda_moveit_config demo.launch
rosrun moveit_tutorials pick_place_tutorial
pick and place 的源码 传送门
1.1 pick and place 的源码中我们往场景中加入了三个物体,创建向量来容纳3个碰撞对象,第三个是机械臂需要操作的对象。
并且定义了这三个物体的属性和参数,包括物体的形状和位姿.
1.2 定义一个grasp函数用于抓取操作,申明物体相对于底座panda_link0位置,抓取操作分为两步,抓取前机械臂hand相对于物体
的运动,抓取后机械臂hand相对于物体的运动.
1.3 open表存储抓取前hand的位姿,close表存储抓取过程中hand的位姿.
1.4 定义一个place函数用于放置操作,步骤类似于grasp函数
1.5 grasp 和place函数都调用moveit::planning_interface::MoveGroupInterface 函数 中的 pick 和place 方法抓取和放置物体.
2.下面理解操作实体机械臂
1.首先还是根据已有的urdf文件进行相关的配置,生成配置文件,参考 MoveIt教程4 - MoveIt配置助手(MoveIt Setup Assistant) https://www.jianshu.com/p/16e0a4dd4b0c
个人理解还是需要调用demo.launch 文件 和自己的目标点文件,只不过需要修改文件中的内容.
2.1 编写自己的controller.yaml
经过moveit setup assistant设置之后,可以在myrobot_moveit_config包的config文件夹下找到一个fake_controllers.yaml文件,
这个是虚拟控制器配置文件,仅供仿真使用,如果要驱动真实机械臂,就要编写自己的controller.yaml(可参见使用MoveIt!+Arbotix控制六自由度机械臂)以取代fake的那个。
其中的参数根据自己的机械臂属性进行相关的配置.参考 https://blog.csdn.net/Kalenee/article/details/80643189
controller_list:
- name: arm_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- joint1
- joint2
- joint3
- joint4
- joint5
- name: gripper_controller
action_ns: gripper_action
type: GripperCommand
default: true
joints:
- finger_joint1
- finger_joint2
2.2修改launch文件内容,重点关注如下嵌套关系,这里主要修改为真实机械臂的关节信息
打开=xml文件,加载刚才修改的yaml文件
demo.launch
- move_group.launch
- trajectory_execution.launch.xml
- $(arg moveit_controller_manager)_moveit_controller_manager.launch.xml
- trajectory_execution.launch.xml
2.3 根据demo.launch 新建自己的文件moveit_planning_execution.launch,
然后配置机械臂实物姿态发布器joint_state_publish
在控制实体机械臂的时候需要将机械臂的姿态发布到/joint_states话题中,否则不会更新moveit中机械臂的初始姿态。在使用MoveItFakeControllerManager时,move_group会把机械臂在虚拟空间中的姿态发布到/joint_states话题,因此当使用虚拟控制接口进行算法验证时,不需要机械臂实物姿态发布器也可以更新moveit中机械臂的初始姿态。但用MoveItSimpleControllerManager后,move_group不会把机械臂在虚拟空间中的姿态发布到/joint_states话题,需要自行编写实物姿态发布器,否则会发生每一次规划的启动都是最开始的姿态而不是输出后的姿态的错误。
实物姿态发布的话题定义在moveit_planning_execution.launch中。
<!-- publish joint states -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<param name="/use_gui" value="false"/>
<rosparam param="/source_list">[<span style="color:#ff0000;">/move_group/robothand_controller_joint_states</span>]</rosparam>
</node>
因实物机械臂并没角度反馈,无法获得实际姿态,因此通过获取获取规划的最后一组数据作为机械臂运动输出后的实际状态,发送到/move_group/robothand_controller_joint_states,/joint_states会通过其获取机械臂状态。
(个人理解:moveit会读取/move_group/robothand_controller_joint_states话题上的数据然后通过/joint_states话题发布出去)
发布话题的节点内容如下:节点名称为robothand_hard_driver
#include <ros/ros.h>
#include <serial/serial.h>
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryActionGoal.h>
#include <control_msgs/FollowJointTrajectoryActionResult.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <sensor_msgs/JointState.h>
#include <iostream>
#include <string.h>
#include <stdio.h>
#include <vector>
using namespace std;
class FollowJointTrajectoryAction
{
public:
FollowJointTrajectoryAction(std::string name) :
as_(nh_, name, false),
action_name_(name)
{
as_.registerGoalCallback(boost::bind(&FollowJointTrajectoryAction::goalCB, this));
as_.registerPreemptCallback(boost::bind(&FollowJointTrajectoryAction::preemptCB, this));
as_.start();
Pub_jint = nh_.advertise<sensor_msgs::JointState>("/move_group/robothand_controller_joint_states",10);
js.name.resize(6);
js.position.resize(6);
js.name[0] = "Joint_1";
js.name[1] = "Joint_2";
js.name[2] = "Joint_3";
js.name[3] = "Joint_4";
js.name[4] = "Joint_5";
js.name[5] = "Joint_6";
ROS_INFO("action start");
}
~FollowJointTrajectoryAction(void)
{
}
void goalCB()
{
ROS_INFO("goal is receive");
double points_end[6];
if(as_.isNewGoalAvailable())
{
js.position.clear();
points_=&(as_.acceptNewGoal()->trajectory.points);
Pos_length=points_->size();
for(int co =0; co<6;co++)
{
points_end[co] = points_->at(Pos_length-1).positions[co];// points_[Pos_length-6+co];
js.position.push_back(points_->at(Pos_length-1).positions[co]);
}
js.header.stamp = ros::Time::now();
Pub_jint.publish(js);
}
else
{
ROS_INFO("goal is not available");
}
control_msgs::FollowJointTrajectoryResult result;
result.error_code = 0;
as_.setSucceeded(result);
}
void preemptCB()
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
}
protected:
sensor_msgs::JointState js;
ros::NodeHandle nh_;
ros::Publisher Pub_jint;
actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_;
//to the client
control_msgs::FollowJointTrajectoryResult result_;
//receive
control_msgs::FollowJointTrajectoryGoal goal_;
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "robothand_hard_driver");
FollowJointTrajectoryAction followJointTrajectoryAction("six_hand_controller/follow_joint_trajectory");
ros::spin();
return 0;
}
/joint_states话题发布参考:http://wiki.ros.org/urdf/Tutorials/Using%20urdf%20with%20robot_state_publisher
本段参考 https://blog.csdn.net/Kalenee/article/details/80643189
2.4 修改MoveIt默认逆解算法
修改:数值解因其算法本身缺陷,容易导致同样目标位置出现多次规划不同解,可采用几何解代替解析解解决这一问高题并提高运算速度和成功率 .安装ikfast 并修改相应的配置文件
- demo.launch
- plannig_context.launch.xml
- kinematics.yaml
- plannig_context.launch.xml
2.5机械臂状态发布
需要注意,在demo演示时,机械臂的joint_states是通过/move_group/fake_controller_joint_states这个话题发出到/joint_states话题,而驱动真实机械臂时,从上面的Driver模块中可以看出,需要自己编写一个joint_states_pulisher发布/move_group/myrobot_controller_joint_states话题。
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<param name="use_gui" value="$(arg use_gui)"/>
<rosparam param="source_list">[/move_group/myrobot_controller_joint_states]</rosparam>
</node>
节点编写 参考: