Description

Hi, your work is very good. It is worth to learn.

I create myself 6-axis robot arm. I want to try the motion planning so I set my waypoints. I use the "computeCartesianPath" to compute the degree of joints every time. I choose item of the avoid collision, but the computed result is collision. What I can deal with it.

Your environment

ROS Distro: [Noetic]
OS Version: e.g. Ubuntu 20.04

My mesh of urdf is set high leve.

Expected behaviour

The computed result is not collision.

Actual behaviour

The computed result is collision.

My code

include <moveit/move_group_interface/move_group_interface.h>

include <moveit/planning_scene_interface/planning_scene_interface.h>

include <moveit_msgs/DisplayRobotState.h>

include <moveit_msgs/DisplayTrajectory.h>

include <moveit_msgs/AttachedCollisionObject.h>

include <moveit_msgs/CollisionObject.h>

include <moveit_visual_tools/moveit_visual_tools.h>

include <moveit/robot_model_loader/robot_model_loader.h>

include <moveit/planning_scene/planning_scene.h>

include <moveit/kinematic_constraints/utils.h>

int main(int argc, char** argv) { ros::init(argc, argv, "move_group_interface_tutorial"); ros::NodeHandle node_handle;

// AsyncSpinner: solving the callback is the asynchronous
ros::AsyncSpinner spinner(1);
spinner.start();

static const std::string PLANNING_GROUP = "auo_arm";

moveit::planning_interface::MoveGroupInterface arm(PLANNING_GROUP);
std::string reference_frame = "base_link";

arm.setPoseReferenceFrame(reference_frame);

arm.allowReplanning(true);

arm.setGoalPositionTolerance(0.001);
arm.setGoalOrientationTolerance(0.01);

arm.setMaxAccelerationScalingFactor(0.8);
arm.setMaxVelocityScalingFactor(0.8);

arm.setNamedTarget("home");
arm.move();
sleep(1);

geometry_msgs::Pose target_pose;
target_pose.orientation.x = 0.14858;
target_pose.orientation.x = 0.14858;
target_pose.orientation.y = 0.73236;
target_pose.orientation.z = -0.03444;
target_pose.orientation.w = 0.66362;

target_pose.position.x = 0.050308;
target_pose.position.y = 0.039851;
target_pose.position.z = 0.65264;

arm.setPoseTarget(target_pose);
arm.move();
sleep(1);

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

waypoints.push_back(target_pose);

double centerA = target_pose.position.x;
double centerB = target_pose.position.z;
double radius = 0.13;

for(double th=0.0; th<6.28; th=th+0.01)
{
    target_pose.position.y = centerA + radius * cos(th);
    target_pose.position.z = centerB + radius * sin(th);
    waypoints.push_back(target_pose);
}

moveit_msgs::RobotTrajectory trajectory;
const double jump_threshold = 0.00;
const double eef_step = 0.01;
double fraction = 0.0;
int maxtries = 100;  
int attempts = 0;  

while(fraction < 1.0 && attempts < maxtries){
    fraction = arm.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, true);
    attempts++;

    if(attempts % 10 == 0)
        ROS_INFO("Still trying after %d attempts...", attempts);
}

if(fraction == 1.0){   
    ROS_INFO("Path computed successfully. Moving the arm.");

    moveit::planning_interface::MoveGroupInterface::Plan plan;
    plan.trajectory_ = trajectory;
    arm.execute(plan);
    sleep(1);
}else{
    ROS_INFO("Path planning failed with only %0.6f success after %d attempts.", fraction, maxtries);
}

arm.setNamedTarget("home");
arm.move();
sleep(1);

ros::shutdown();
return 0;

}

This is the result.

Here is the video

moveit / moveit

Links using "computeCartesianPath" are collision. #3053