qhRobot和PR2机器人左手画圆

int my_draw_circle_qhRobot(C_moveGroup &group, double circleCenter[])

{

int i, j, len;

S_Plan plan; 

std::vector<geometry_msgs::Pose> waypoints;

geometry_msgs::Pose ee_point_goal,ee_point_goal_tmp; //end_effector_trajectory

ee_point_goal.position.x    = circleCenter[0];

ee_point_goal.orientation.x = circleCenter[3];

ee_point_goal.orientation.y = circleCenter[4];

ee_point_goal.orientation.z = circleCenter[5];

ee_point_goal.orientation.w = circleCenter[6];

double y_center = circleCenter[1];

double z_center = circleCenter[2];

double angle= 0;

double radius = 0.1;

double angle_resolution = 5;    /* 两个状态间的弧度值，减小可提高轨迹规划成功率 */

    double d_angle = angle_resolution*3.14/180;

    //----------------------------------------------------------------------

    //  采用圆的点

    for (i= 0; i< (360/angle_resolution); i++)

    {

        //discretize the trajectory

        angle+= d_angle;

        ee_point_goal.position.z = z_center + radius*cos(angle);

        ee_point_goal.position.y = y_center + radius*sin(angle);

        waypoints.push_back(ee_point_goal);

if(0 == i)

{

   /* 先移动到起始位置 */ 

double position_action[7];

position_action[0] = ee_point_goal.position.x;

position_action[1] = ee_point_goal.position.y;

position_action[2] = ee_point_goal.position.z;

position_action[3] = ee_point_goal.orientation.x;

position_action[4] = ee_point_goal.orientation.y;

position_action[5] = ee_point_goal.orientation.z;

position_action[6] = ee_point_goal.orientation.w;

printf("T  --- x[%f] y[%f] z[%f] w[%f]\n", position_action[0],position_action[1],position_action[2],position_action[6]);

IK_action_one(group, "left_middle3_Link", position_action); 

}

   printf("%f %f %f %f %f %f %f \n", ee_point_goal.position.x, ee_point_goal.position.y, ee_point_goal.position.z,

   ee_point_goal.orientation.x, ee_point_goal.orientation.y, ee_point_goal.orientation.z, ee_point_goal.orientation.w);

    }

//---------添加画圆的圈数------------------------------------------------------------------------------------------------------------

len = waypoints.size();

for(j=0; j<20; j++) /* 定义画圆的圈数 */

{

for(i=0; i<len; i++)

{

ee_point_goal_tmp = waypoints[i];

waypoints.push_back(ee_point_goal_tmp);

}

}

  

    printf("There are [%lu] number of waypoints", waypoints.size());

    // We want cartesian path to be interpolated at a eef_resolution

    double eef_resolution = std::min(0.01,radius*angle_resolution);

    double jump_threshold = 0.0; //disable the jump threshold =0

    moveit_msgs::RobotTrajectory trajectory;

i = 0;

while(i<100)

{/* 多次规划直到轨迹规划成功 */

i++;

double fraction =group.computeCartesianPath(waypoints, eef_resolution,jump_threshold, trajectory);   

printf("T  ---  fraction[%f]\n", fraction);

if(1.0 == fraction)

break;

}

plan.trajectory_ = trajectory;

group.execute(plan);

//showTrajectoryPoints(plan);

return 0;

}

=================================================================

int my_draw_circle_pr2(C_moveGroup &group)

{

//{0.585362431331,0.18799252512,1.24996379951-0.1,-3.09082887994e-05,0.0335924893992,-5.01658022839e-05,0.999435611325},

//2. Create Cartesian Paths

int i, j, len;

S_Plan plan; 

    std::vector<geometry_msgs::Pose> waypoints;

    geometry_msgs::Pose ee_point_goal,ee_point_goal_tmp; //end_effector_trajectory

    ee_point_goal.orientation.x = -3.09082887994e-05;

ee_point_goal.orientation.y = 0.0335924893992;

ee_point_goal.orientation.z = -5.01658022839e-05;

ee_point_goal.orientation.w = 0.999435611325;

    ee_point_goal.position.x = 0.585362431331;

    double angle_resolution = 5;    /* 两个状态间的弧度值，减小可提高轨迹规划成功率 */

    double d_angle = angle_resolution*3.14/180;

    //double d_angle = 360/angle_resolution;

    double angle= 0;

double z_center = 1.24996379951;

double y_center = 0.18799252512;

double radius = 0.1;

  

    //----------------------------------------------------------------------

    //3. Plan for the trajectory

    for (i= 0; i< (360/angle_resolution); i++)

    {

      //discretize the trajectory

      angle+= d_angle;

      ee_point_goal.position.z = z_center + radius*cos(angle);

      ee_point_goal.position.y = y_center + radius*sin(angle);

      waypoints.push_back(ee_point_goal);

 if(0 == i)

 {

            /* 先移动到起始位置 */ 

double position_action[7];

position_action[0] = ee_point_goal.position.x;

position_action[1] = ee_point_goal.position.y;

position_action[2] = ee_point_goal.position.z;

position_action[3] = ee_point_goal.orientation.x;

position_action[4] = ee_point_goal.orientation.y;

position_action[5] = ee_point_goal.orientation.z;

position_action[6] = ee_point_goal.orientation.w;

printf("T  --- x[%f] y[%f] z[%f] w[%f]\n", position_action[0],position_action[1],position_action[2],position_action[6]);

IK_action_one(group, "l_wrist_roll_link", position_action);

 }

 printf("%f %f %f %f %f %f %f \n",

  ee_point_goal.position.x,

  ee_point_goal.position.y,

  ee_point_goal.position.z,

  ee_point_goal.orientation.x,

  ee_point_goal.orientation.y,

  ee_point_goal.orientation.z,

  ee_point_goal.orientation.w);

    }

len = waypoints.size();

for(j=0; j<20; j++) /* 定义画圆的圈数 */

{

for(i=0; i<len; i++)

{

ee_point_goal_tmp = waypoints[i];

waypoints.push_back(ee_point_goal_tmp);

}

}

  

    printf("There are %d number of waypoints", waypoints.size());

    // We want cartesian path to be interpolated at a eef_resolution

    double eef_resolution = std::min(0.01,radius*angle_resolution);

    double jump_threshold = 0.0; //disable the jump threshold =0

    moveit_msgs::RobotTrajectory trajectory;

i = 0;

while(i<100)

{/* 多次规划直到轨迹规划成功 */

i++;

   double fraction =group.computeCartesianPath(waypoints, eef_resolution,jump_threshold, trajectory);  

   printf("T  ---  fraction[%f]\n", fraction);

if(1.0 == fraction)

break;

}

plan.trajectory_ = trajectory;

group.execute(plan);

//showTrajectoryPoints(plan);

return 0;

}