机器人 关于空间规划由python代码转换到C++代码
flyfish
ROS
原文的空间规划是这样的
moveit_fk_demo.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy, sys
import moveit_commander
from control_msgs.msg import GripperCommand
class MoveItFkDemo:
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_fk_demo', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = moveit_commander.MoveGroupCommander('arm')
# 初始化需要使用move group控制的机械臂中的gripper group
gripper = moveit_commander.MoveGroupCommander('gripper')
# 设置机械臂和夹爪的允许误差值
arm.set_goal_joint_tolerance(0.001)
gripper.set_goal_joint_tolerance(0.001)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(2)
# 设置夹爪的目标位置,并控制夹爪运动
gripper.set_joint_value_target([0.01])
gripper.go()
rospy.sleep(1)
# 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度)
joint_positions = [-0.0867, -1.274, 0.02832, 0.0820, -1.273, -0.003]
arm.set_joint_value_target(joint_positions)
# 控制机械臂完成运动
arm.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
if __name__ == "__main__":
try:
MoveItFkDemo()
except rospy.ROSInterruptException:
pass
经过C++转换是这样的 名字以Test为例 主意大小写敏感
#include <ros/ros.h>
// 包含moveit的API
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <eigen_conversions/eigen_msg.h>
#include <vector>
int main(int argc, char **argv)
{
ros::init (argc, argv, "moveit_fk_demo",ros::init_options::AnonymousName);
ros::AsyncSpinner spinner(1);
spinner.start();
//初始化需要使用move group控制的机械臂中的arm group
moveit::planning_interface::MoveGroupInterface arm("arm");
//初始化需要使用move group控制的机械臂中的gripper group
moveit::planning_interface::MoveGroupInterface gripper("gripper");
//设置机械臂和夹爪的允许误差值
arm.setGoalJointTolerance(0.001);
gripper.setGoalJointTolerance(0.001);
//控制机械臂先回到初始化位置
arm.setNamedTarget("home");
arm.move();
sleep(2);
//设置夹爪的目标位置,并控制夹爪运动
std::vector<double> gripper_positions;
gripper_positions.push_back(0.01);
gripper.setJointValueTarget(gripper_positions);
gripper.move();
sleep(1);
//机械臂随机目标位置
arm.setRandomTarget();
// 开始运动规划,并且让机械臂移动到目标位置
arm.move();
sleep(1);
//设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度)
std::vector<double> arm_positions;
arm_positions.push_back(-0.0867);
arm_positions.push_back(-1.274);
arm_positions.push_back(0.02832);
arm_positions.push_back(0.0820);
arm_positions.push_back(-1.273);
arm_positions.push_back(-0.003);
arm.setJointValueTarget(arm_positions);
// # 控制机械臂完成运动
arm.move();
sleep(1);
ros::shutdown();
}
//
CMakeLists.txt 增加
add_executable(Test
src/Test.cpp
)
add_dependencies(Test ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(Test
${catkin_LIBRARIES}
)