编码控制移动

环境准备

配置package.xml

<build_depend>moveit_visual_tools</build_depend>
<build_depend>moveit_core</build_depend>
<build_depend>moveit_msgs</build_depend>
<build_depend>moveit_planners_ompl</build_depend>
<build_depend>moveit_ros_move_group</build_depend>
<build_depend>moveit_ros_planning_interface</build_depend>
<build_depend>moveit_ros_visualization</build_depend>

<exec_depend>moveit_visual_tools</exec_depend>
<exec_depend>moveit_core</exec_depend>
<exec_depend>moveit_msgs</exec_depend>
<exec_depend>moveit_planners_ompl</exec_depend>
<exec_depend>moveit_ros_move_group</exec_depend>
<exec_depend>moveit_ros_planning_interface</exec_depend>
<exec_depend>moveit_ros_visualization</exec_depend>

配置CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS
    roscpp
    rosmsg
    rospy

    moveit_core
    moveit_msgs
    moveit_planners_ompl
    moveit_visual_tools
    moveit_ros_move_group
    moveit_ros_planning_interface
    moveit_ros_visualization
)

Note

moveit_core: moveit核心库。

moveit_msgs:moveit消息库。

moveit_planners_ompl:moveit运动路径规划库。

moveit_visual_tools:moveit可视化工具。

moveit_ros_move_group: moveit ros移动接口库。

moveit_ros_planning_interface: moveit ros路径规划接口库。

moveit_ros_visualization: moveit ros可视化接口。

移动到Up

C++实现

// 移动到up点
string groupName = "manipulator";
moveit::planning_interface::MoveGroupInterface group(groupName);
group.setNamedTarget("up");
group.move();

Python实现

groupName = "manipulator"
move_group = moveit_commander.MoveGroupCommander(groupName)
move_group.set_named_target("home")
move_group.go()

移动到Home

C++实现

// 移动到home点
string groupName = "manipulator";
moveit::planning_interface::MoveGroupInterface group(groupName);
group.setNamedTarget("home");
group.move();

Python实现

groupName = "manipulator"
move_group = moveit_commander.MoveGroupCommander(groupName)
move_group.set_named_target("up")
move_group.go()

移动到随机位置

C++实现

//移动到随机位置
string groupName = "manipulator";
moveit::planning_interface::MoveGroupInterface group(groupName);
group.setRandomTarget();
group.move();

Python实现

groupName = "manipulator"
move_group = moveit_commander.MoveGroupCommander(groupName)
move_group.set_random_target()
move_group.go()

移动到指定位置

C++实现

string groupName = "manipulator";
moveit::planning_interface::MoveGroupInterface group(groupName);

geometry_msgs::Pose pose;
//位置
pose.position.x = 0.28;
pose.position.y = 0.2;
pose.position.z = 0.5;
//姿态
pose.orientation.x = 0;
pose.orientation.y = 0;
pose.orientation.z = 0;
pose.orientation.w = 1;
group.setPoseTarget(pose);
group.move();

Python实现

groupName = "manipulator"
move_group = moveit_commander.MoveGroupCommander(groupName)

pose = Pose()
# 位置
pose.position.x = 0.3
pose.position.y = 0.2
pose.position.z = 0.3
# 姿态
pose.orientation.x = 0
pose.orientation.y = 0
pose.orientation.z = 0
pose.orientation.w = 1

move_group.set_pose_target(pose)
move_group.go()

C++欧拉角转四元素

// 导入库
#include "tf/LinearMath/Quaternion.h"

// 转换
tf::Quaternion quat;
quat.setRPY(roll, pitch, yaw);

//　使用
double x = quat.x();
double y = quat.y();
double z = quat.z();
double w = quat.w();

Python欧拉角转四元素

# 导入模块
from tf import transformations

# 转换
quat = transformations.quaternion_from_euler(roll, pitch, yaw)

# 使用
x = quat[0]
y = quat[1]
z = quat[2]
w = quat[3]

移动结果判断

C++ 实现

...
const moveit::planning_interface::MoveItErrorCode &code = group.move();
if (code == code.SUCCESS) {
    // success
} else if(code == code.PLANNING_FAILED) {
    // failed
}

Python实现

...
success = move_group.go()
if success:
    print "success"
else:
    print "failed"