Hi all!
I am working on a project where I have to build lego structures collaberative with a human. Now I am new to moveit and have the cartesian path planning working in gazebo (sinusoidals, pick and place movement).
I was trying to use the move_group.setWorspace() function and for some reason it just doesn't give this information to the planning_scene_interface. When I execute a trajectory outside of it's workspace it will just go there for no reason. Also i wanted to implement anti-collision using octomapping however I don't think it will be a good idea to start on that if i can't control the size of my workspace properly.
PS: I also have changed the workspace settings in the moveit.rviz file from the ur5, I can see the workspace getting these dimensions in rviz however the ur5 will still go there.
Kind regards,
Edwin van Emmerik
CODE:
int main(int argc, char **argv)
{
ros::init(argc, argv, "trajectory_tester");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
// Construct planning scene for the move_group
const std::string PLANNING_GROUP = "manipulator";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
move_group.setWorkspace(-0.2, -0.2, 0.05, 1, 1, 1); // set workspace size for move_group
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
// Make sure that the robot model gets loaded
if (robot_model != 0)
{
ROS_INFO("Robot model loaded");
}
else
{
ROS_INFO("Could not load robot model");
}
// Get the planning frames
ROS_INFO("Reference frame: %s", move_group.getPlanningFrame().c_str());
ROS_INFO("Reference frame: %s", move_group.getEndEffectorLink().c_str());
move_group.setPlannerId("PRMstarkConfigDefault"); //default planner is set to RTTCONNECT in ompl_planning_.yaml
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
/*
int chosenone;
std::cout << "You can choose between pick and place(1) and sinusoidal movements(2)" << std::endl;
std::cin >> chosenone;
std::cout << std::endl;
*/
// Construct target_pose object of type geometry_msgs
// Construct waypoints vector of type geometry_msgs::Pose
std::vector waypoints;
geometry_msgs::Pose start_pose = move_group.getCurrentPose().pose;
geometry_msgs::Pose target_pose = start_pose;
// Keep initiazlize state and also keep the UR5 facing downwards for default
target_pose.position.x = 0.6;
target_pose.position.y = 0.05;
target_pose.position.z = 0.1;
target_pose.orientation.w = 0.707;
target_pose.orientation.x = 0;
target_pose.orientation.y = 0.707;
target_pose.orientation.z = 0;
move_group.setPoseTarget(target_pose);
move_group.plan(my_plan);
move_group.move();
int number_of_waypoints = 7;
double x[number_of_waypoints];
x[0] = 0.5;
x[1] = 0.0;
x[2] = 0.0;
x[3] = 0.0;
x[4] = 0.5;
x[5] = 0.5;
x[6] = 0.5;
double y[number_of_waypoints];
y[0] = 0.0;
y[1] = 0.5;
y[2] = 0.5;
y[3] = 0.5;
y[4] = 0.0;
y[5] = 0.0;
y[6] = 0.0;
double z[number_of_waypoints];
z[0] = 0.4;
z[1] = 0.4;
z[2] = 0.1;
z[3] = 0.4;
z[4] = 0.4;
z[5] = 0.1;
z[6] = 0.4;
for (int i = 0; i < number_of_waypoints; i++)
{
target_pose.position.x = x[i];
target_pose.position.y = y[i];
target_pose.position.z = z[i];
waypoints.push_back(target_pose);
}
/* NOTE THAT THIS IS COMMENTED
// Creating the sine data in x and y coordinates
double amplitude = 0.1;
double w = 30.0;
double step = 0.005;
double upperLimit_x = 0.2;
double lowerLimit_x = -0.2;
double offset = 0.20;
int size = round( (upperLimit_x - lowerLimit_x)/step) + 1;
double x[size];
double y[size];
x[0] = lowerLimit_x;
for(int i = 1; i < size; i++) {
x[i] = x[i-1] + step;
}
for(int i = 0; i < size; i++) {
y[i] = amplitude*sin((x[i]+offset)*w)+0.4;
std::cout << y[i] << " ";
}
// Inserting the sine data to waypoints
for(int i = 0; i < size; i++) {
target_pose.position.x = x[i];
target_pose.position.y = y[i];
waypoints.push_back(target_pose);
}
// The same sine data but backwards
for(int i = size-2; i != -1; i--) {
target_pose.position.x = x[i];
target_pose.position.y = -y[i]+0.8; // and inverted
waypoints.push_back(target_pose);
}
// Creating sine data in x and z coordinates TODO
*/
// After whatever case is chosen return to its starting position
waypoints.push_back(start_pose);
moveit_msgs::RobotTrajectory trajectory;
double fraction = move_group.computeCartesianPath(waypoints, 0.001, 0.0, trajectory);
ROS_INFO("Visualizing plan (cartesian path) (%.2f%% acheived)", fraction * 100.0);
// Adding time parametrization to the trajectory
robot_trajectory::RobotTrajectory rt(move_group.getCurrentState()->getRobotModel(), "manipulator");
rt.setRobotTrajectoryMsg(*move_group.getCurrentState(), trajectory);
trajectory_processing::IterativeParabolicTimeParameterization iptp;
bool success = iptp.computeTimeStamps(rt);
ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");
rt.getRobotTrajectoryMsg(trajectory);
std::cout << move_group.getPathConstraints() << std::endl; // NO CONSTRAINTS
sleep(2);
my_plan.trajectory_ = trajectory;
if (fraction == 1.0) {
sleep(2);
move_group.execute(my_plan);
}
else
{
ROS_WARN("Could not compute the cartesian path :( ");
std::cout << fraction << std::endl;
}
// Debug waypoints
/*
for(int k = 0; k < waypoints.size(); k++)
{
std::cout << waypoints[k] << std::endl;
}
*/
waypoints.clear();
ros::shutdown();
return 0;
}
↧