pr2机器人手臂画圆

欢迎转载请注明出处：海漩涡

http://blog.csdn.net/tanhuifang520

pr2机器人手臂画圆

零、sudo apt-get install ros-indigo-pr2-desktop

一、cd ~/catkin_ws/src/

二、catkin_create_pkg pr2_draw_circle catkin cmake_modules interactive_markers moveit_core moveit_ros_perception moveit_ros_planning_interface pluginlib roscpp std_msgs

三、vi pr2_draw_circle/src/draw_circle.cpp

写入以下代码（cao了第一次在CSDN里贴代码，居然不会插入代码）

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include "ompl/util/Time.h"
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

typedef move_group_interface::MoveGroup C_moveGroup;
typedef moveit::planning_interface::MoveGroup::Plan S_Plan;
typedef moveit::planning_interface::PlanningSceneInterface S_PlanningSceneInterface;

void IK_action_one(C_moveGroup &group, double position_action[])
{
	geometry_msgs::PoseStamped target_pose;
    target_pose.header.frame_id    = group.getPoseReferenceFrame();
    target_pose.header.stamp       = ros::Time::now();  
    target_pose.pose.position.x    = position_action[0];
    target_pose.pose.position.y    = position_action[1];
    target_pose.pose.position.z    = position_action[2];	
	target_pose.pose.orientation.x = position_action[3];
    target_pose.pose.orientation.y = position_action[4];
    target_pose.pose.orientation.z = position_action[5];
	target_pose.pose.orientation.w = position_action[6];

	group.setPoseTarget(target_pose, group.getEndEffectorLink());	
	group.setStartStateToCurrentState()	;
	group.move();
	usleep(500000);
}

int discretize_circle(double circleCenter[], std::vector > &V_pose_value)
{	
	std::vector pose_value(7);
	pose_value[0] = circleCenter[0];
	pose_value[3] = circleCenter[3];
	pose_value[4] = circleCenter[4];
	pose_value[5] = circleCenter[5];
	pose_value[6] = circleCenter[6];

	double y_center = circleCenter[1];
	double z_center = circleCenter[2];		
	double angle= 0;
	double radius = 0.1;		
	double angle_resolution = 10;                /* 两个采样点间的角度值，减小可提高轨迹规划成功率 */
    double d_angle = angle_resolution*3.14/180; /* 两个采样点间的弧度值 */

    //  采样圆上的点
    for (int i= 0; i< (360/angle_resolution); i++)
    {
    	angle+= d_angle; 
		pose_value[1] = y_center + radius*sin(angle);
        pose_value[2] = z_center + radius*cos(angle);        
        V_pose_value.push_back(pose_value);		
	    printf("%f %f %f %f %f %f %f \n",pose_value[0],pose_value[1],pose_value[2],pose_value[3],pose_value[4],pose_value[5],pose_value[6]);
    }	
	return 0;
}


int draw_circle(C_moveGroup &group, double circleCenter[])
{
	int i, j, len;
	S_Plan plan; 
	std::vector > V_pose_value;
	std::vector waypoints;
	geometry_msgs::Pose ee_point_goal_tmp;

	/* 采样圆上的点 */
	discretize_circle(circleCenter, V_pose_value);
	for(i=0; i

四、vi pr2_draw_circle/CMakeLists.txt

在末尾添加

aux_source_directory(./src src_list)
add_executable(draw ${src_list})
target_link_libraries(draw ${catkin_LIBRARIES})
add_dependencies(draw pr2_draw_circle_generate_messages_cpp)

五、cd ~/catkin_ws/;catkin_make

六、在虚拟机的桌面打开个终端运行命令：

roslaunch pr2_moveit_config demo.launch

你将看到pr2机器人

七、运行下面命令你将看到pr2机器人的左手在画圆了

rosrun pr2_draw_circle draw