Select Git revision
SampleConstraintPlanner.cpp
SampleConstraintPlanner.cpp 8.06 KiB
//
// Created by sebastian on 27.03.20.
//
#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/trajectory_processing/iterative_time_parameterization.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "move_group_interface_tutorial");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
std::cout << ">>>>>>>>>>>>>>>>> SLEEPING <<<<<<<<<<<<<<<< " << std::endl;
ros::Duration(5.0).sleep();
std::cout << ">>>>>>>>>>>>>>>>> WAKING UP <<<<<<<<<<<<<<<< " << std::endl;
//moveit::planning_interface::MoveGroup group("arm_gp");
moveit::planning_interface::MoveGroupInterface group("panda_arm");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
moveit_msgs::DisplayTrajectory display_trajectory;
moveit::planning_interface::MoveGroupInterface::Plan plan;
group.setStartStateToCurrentState();
std::vector<geometry_msgs::Pose> waypoints;
geometry_msgs::Pose target_pose = group.getCurrentPose().pose;
target_pose.position.x += 2.0;
target_pose.position.y += 5.0;
target_pose.position.z -= 2.0;
waypoints.push_back(target_pose);
/*target_pose.position.y -= 0.0;
waypoints.push_back(target_pose);
target_pose.position.z -= 0.08;
target_pose.position.y += 0.0;
target_pose.position.x -= 0.0;
waypoints.push_back(target_pose);*/
moveit_msgs::RobotTrajectory trajectory_msg;
group.setPlanningTime(10.0);
double fraction = group.computeCartesianPath(waypoints,
0.01, // eef_step
0.0, // jump_threshold
trajectory_msg, false);
// The trajectory needs to be modified so it will include velocities as well.
// First to create a RobotTrajectory object
robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");
// Second get a RobotTrajectory from trajectory
rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
// Third create a IterativeParabolicTimeParameterization object
trajectory_processing::IterativeParabolicTimeParameterization iptp;
// Fourth compute computeTimeStamps bool success = iptp.computeTimeStamps(rt);
ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");
// Get RobotTrajectory_msg from RobotTrajectory
rt.getRobotTrajectoryMsg(trajectory_msg);
// Check trajectory_msg for velocities not empty
//std::cout << ">>>>>>>>>>>>>>>>> data before: " << trajectory_msg << std::endl;
// adjust velocities, accelerations and time_from_start
for(int i = 0; i < trajectory_msg.joint_trajectory.points.size(); i++)
{
trajectory_msg.joint_trajectory.points[i].time_from_start.operator*=(5);
for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).velocities.size(); j++)
{
if(trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) != 0.0){
trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) /= 5;
//trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) = 0;
}
}
for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).accelerations.size(); j++)
{
trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j) =
sqrt(std::abs(trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j)));
}
}
//std::cout << ">>>>>>>>>>>>>>>>> data after: " << trajectory_msg << std::endl;
plan.trajectory_ = trajectory_msg;
ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",fraction * 100.0);
sleep(5.0);
group.execute(plan);
ros::shutdown();
return 0;
}
/*int main(int argc, char** argv)
{
ros::init(argc, argv, "CONSTRAINT PLANNER");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
static const std::string PLANNING_GROUP = "panda_arm";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
const robot_state::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
// Visualization Setup
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
visual_tools.deleteAllMarkers();
visual_tools.loadRemoteControl();
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "Sample constraint planner node", rvt::WHITE, rvt::XLARGE);
// Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
visual_tools.trigger();
// Getting Basic Information
ROS_INFO_NAMED("constraint_planner", "Planning frame: %s", move_group.getPlanningFrame().c_str());
ROS_INFO_NAMED("constraint_planner", "End effector link: %s", move_group.getEndEffectorLink().c_str());
ROS_INFO_NAMED("constraint_planner", "Available Planning Groups:");
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
std::ostream_iterator<std::string>(std::cout, ", "));
// Define a collision object ROS message.
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = move_group.getPlanningFrame();
// The id of the object is used to identify it.
collision_object.id = "box1";
// Define a box to add to the world.
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.4;
primitive.dimensions[1] = 0.1;
primitive.dimensions[2] = 0.4;
// Define a pose for the box (specified relative to frame_id)
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.4;
box_pose.position.y = -0.2;
box_pose.position.z = 1.0;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);
// Now, let's add the collision object into the world
ROS_INFO_NAMED("constraint_planner", "Add an object into the world");
planning_scene_interface.addCollisionObjects(collision_objects);
// Show text in RViz of status
visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");
// Now when we plan a trajectory it will avoid the obstacle
move_group.setStartState(*move_group.getCurrentState());
geometry_msgs::Pose another_pose;
another_pose.orientation.w = 1.0;
another_pose.position.x = 0.4;
another_pose.position.y = -0.4;
another_pose.position.z = 0.9;
move_group.setPoseTarget(another_pose);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("constraint_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");
visual_tools.prompt("Press 'next' to move the simulated robot.");
visual_tools.trigger();
// Move the simulated robot in gazebo
move_group.move();
ros::shutdown();
return 0;
}*/