#include <moveit/move_group_interface/move_group_interface.h> #include <moveit_msgs/DisplayTrajectory.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> /** * simple demo of a robotic motions based on cartesian paths */ int main(int argc, char **argv) { // init the node ros::init(argc, argv, "basic_cartesian_planner"); ros::NodeHandle node_handle; ros::AsyncSpinner spinner(1); spinner.start(); // setup the planning environment static const std::string PLANNING_GROUP = "panda_arm"; moveit::planning_interface::MoveGroupInterface group(PLANNING_GROUP); ros::Duration(3.0).sleep(); group.setStartStateToCurrentState(); // You can plan a Cartesian path directly by specifying a list of waypoints // for the end-effector to go through. Note that we are starting // from the new start state above. std::vector<geometry_msgs::Pose> waypoints; // initial pose of the robot geometry_msgs::Pose start_pose = group.getCurrentPose().pose; // waypoints of the target trajectory geometry_msgs::Pose target_pose_1 = group.getCurrentPose().pose; geometry_msgs::Pose target_pose_2 = group.getCurrentPose().pose; geometry_msgs::Pose target_pose_3 = group.getCurrentPose().pose; target_pose_1.position.z = start_pose.position.z - 0.4; target_pose_1.position.y = start_pose.position.y; target_pose_1.position.x = start_pose.position.x + 0.5; waypoints.push_back(target_pose_1); target_pose_2.position.z = 0.6; target_pose_2.position.y = -0.6; target_pose_2.position.x = 0; waypoints.push_back(target_pose_2); target_pose_3.position.z = 0.6; target_pose_3.position.y = -0.032; target_pose_3.position.x = -0.6; waypoints.push_back(target_pose_3); group.setPlanningTime(10.0); // create a plan object and compute the cartesian motion moveit::planning_interface::MoveGroupInterface::Plan plan; moveit_msgs::RobotTrajectory trajectory_msg; group.computeCartesianPath(waypoints, 0.01, 0.0, trajectory_msg, true); plan.trajectory_ = trajectory_msg; // uncomment to modify maximum velocity and acceleration of the motion to be planned /* robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), PLANNING_GROUP); trajectory_processing::IterativeParabolicTimeParameterization iptp; double max_velocity_scaling_factor = 0.1; double max_acceleration_scaling_factor = 0.1; iptp.computeTimeStamps(rt, max_velocity_scaling_factor, max_acceleration_scaling_factor); rt.getRobotTrajectoryMsg(trajectory_msg); */ // execute the planned motion group.execute(plan); ros::shutdown(); return 0; }