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;
}