pr2机器人手臂画圆

最新推荐文章于 2024-06-11 22:59:28 发布

ClarkLinus

最新推荐文章于 2024-06-11 22:59:28 发布

阅读量5.3k

点赞数

分类专栏： ros 文章标签：机器人 cmake

本文链接：https://blog.csdn.net/tanhuifang520/article/details/62994164

版权

ros 专栏收录该内容

3 篇文章 0 订阅

订阅专栏

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

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