arm draw a circle problem

asked 2016-05-14 09:15:17 -0600

WuJeff gravatar image

my node programs:

#ifdef __i386__
#pragma message("i386 Architecture detected, disabling EIGEN VECTORIZATION")
#define EIGEN_DONT_VECTORIZE
#define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
#else
#pragma message("64bit Architecture detected, enabling EIGEN VECTORIZATION")
#endif

#include<ros/ros.h>
#include<moveit/move_group_interface/move_group.h>
#include <descartes_moveit/moveit_state_adapter.h>

#include<eigen3/Eigen/Geometry>
#include<eigen_conversions/eigen_msg.h>
int main(int argc, char *argv[])
{
    ros::init(argc,argv,"lesson_move_group_node");
    //start a background "spinner",so the node can process ROS messages
    ros::AsyncSpinner spinner(1);
    spinner.start();
    //create a Descartes robot model
    descartes_core::RobotModelPtr robot_model_ptr(new descartes_moveit::MoveitStateAdapter());
    if(robot_model_ptr->initialize("robot_description","manipulator","base","tool0"))
      {
        ROS_INFO_STREAM("Robot Model initialized");
      }
      else
      {
        ROS_ERROR_STREAM("Failed to initialized Robot Model");
        return 1;
      }

    moveit::planning_interface::MoveGroup group("manipulator");
std::vector<double> joints(robot_model_ptr->getDOF(),0);
   //std::vector< std::vector<double> > solutions;
   std::vector<double>solution;
   Eigen::Affine3d tool_pose;
   const double radius(0.10);
   double X(0.0),Y(0.0),angle(0.0);
   while(ros::ok())
   {
       angle=angle*3.141592/180.0;
       X=radius*sin(angle);
       Y=radius*cos(angle);
       tool_pose=Eigen::Translation3d(X, Y, 0.390)
                   * Eigen::Quaterniond(-0.014, 0.733, 0.680, -0.010);

        if(robot_model_ptr->getIK(tool_pose,joints,solution))
        {
            ROS_INFO_STREAM("Found IK solution: ["<<solution[0]<<", "
                       <<solution[1]<<", "
                       <<solution[2]<<", "
                       <<solution[3]<<", "
                       <<solution[4]<<", "
                       <<solution[5]<<"] ");
        }
        else
        {
            ROS_ERROR_STREAM("IK failed");
            return 1;
        }
       for(unsigned int i=0;i<6;i++)
       {
           joints[i]=solution[i];
       }
       angle=(angle+10.0)/360.0;
       group.setJointValueTarget(joints);
       group.move();

       //sleep(0.3);
   }
}

it just move once,and never move again?I followed the tutorials ROS-Industrial exercises 3.6 and exercises 4.1! I don't know why?

edit retag flag offensive close merge delete