How to get waypoints of a trajectory?

asked 2017-12-21 00:09:11 -0500

pdmike gravatar image

updated 2017-12-21 20:40:17 -0500

Hello, I am using the code to plan the robot trajectory and i get a effective one. If i want save all the waypoints of this trajectory, how can i do?

@PeteBlackerThe3rd
Sorry, i didn't post the problem clearly and my English is not very good. In Moveit, I want to simulate a feasible trajectory between two points with UR10 robot model and get all waypoints(the end-effector's position and orientation) on the trajectory, just like the rviz plugin, Motion Planning Slider.

  1. The path constraint is used in the planning

  2. As simulation, the environment with many obstacles is very complicated, so the planning should be circularly run to get a feasible trajectory. Then i want to save this trajectory by exporting all the waypoints to a .txt file

  3. Now the trajectory can be obtained and it seems that the code, using Moveit C++ API, cannot export the interpolated waypoints (the end-effector's position and orientation), but the robot's joint states of each point of the trajectory.

I attached the code as followed.

   int main(int argc, char **argv)
    {
       ros::init(argc, argv, "move_group_interface_tutorial");
        ros::NodeHandle node_handle;
        ros::AsyncSpinner spinner(1);
        spinner.start();


        /* This sleep is ONLY to allow Rviz to come up */
        sleep(20.0);

        moveit::planning_interface::MoveGroup group_r("arm");
        moveit_msgs::DisplayTrajectory display_trajectory;
        ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);

        group_r.setPlannerId("RRTConnectkConfigDefault");
        group_r.setPlanningTime(45);

//OrientationConstraint
moveit_msgs::OrientationConstraint ocm;

        ocm.link_name = group_r.getEndEffectorLink();
        ocm.header.frame_id = group_r.getPlanningFrame();



        ocm.absolute_x_axis_tolerance = 0.01;
        ocm.absolute_y_axis_tolerance = 0.01;
        ocm.absolute_z_axis_tolerance = 3.14;
        ocm.weight = 1.0;
        moveit_msgs::Constraints test_constraints;
        test_constraints.orientation_constraints.push_back(ocm);
        group_r.setPathConstraints(test_constraints);



  //start pose

       robot_state::RobotState start_state(*group_r.getCurrentState());
       geometry_msgs::Pose start_pose=group_r.getCurrentPose().pose;

        const robot_state::JointModelGroup *joint_model_group =    start_state.getJointModelGroup(group_r.getName());
        start_state.setFromIK(joint_model_group, start_pose);
        group_r.setStartState(start_state);



  // target  
        geometry_msgs::Pose next_pose;



            next_pose.position.x =-0.0383861;
            next_pose.position.y = -0.70;
           next_pose.position.z =-0.14;             
            next_pose.orientation.x = ocm.orientation.x;
            next_pose.orientation.y = ocm.orientation.y;
            next_pose.orientation.z = ocm.orientation.z;
            next_pose.orientation.w = ocm.orientation.w;



            group_r.setPoseTarget(next_pose, ocm.link_name );


        moveit::planning_interface::MoveGroup::Plan my_plan;
        bool success = false;

        int k = 0;
        int count = 0;

//plan and print the points into the specific file

    while (1)
    {

          success=group_r.plan(my_plan);
          stringstream ss; 
          if(success==true)
       {
          count++;
          moveit_msgs::RobotTrajectory msg;  
          msg =my_plan.trajectory_
          std::vector<trajectory_msgs::MultiDOFJointTrajectoryPoint> trajectory_points;  

         std::vector<int>::size_type size1 = msg.joint_trajectory.points.size();  

          for (unsigned i=0; i<size1; i++)  
          {  


              ss << "point_index: " << i << endl  

                 << "positions: "  
                 << "[" << msg.joint_trajectory.points[i].positions[0]  
                 << "," << msg.joint_trajectory.points[i].positions[1]  
                 << "," << msg.joint_trajectory.points[i].positions[2]  
                 << "," << msg.joint_trajectory.points[i].positions[3]  
                 << "," << msg.joint_trajectory.points[i].positions[4]  
                 << "," << msg.joint_trajectory.points[i].positions[5]  
                 << "]" << endl; 


         }  

          ofstream outfile ("/home/aicrobo/Downloads/points_save/points.txt",ios::app);
        if(!outfile.is_open())
                    {

                            ROS_INFO("open failed");
                    }
          else
            {  

              outfile<<"trajectory"<<count<<endl;
              outfile<<msg.multi_dof_joint_trajectory.joint_names[0]<<endl;
              outfile<<ss.str()<<endl;
              outfile.close();
            } 
        }


         sleep(5.0);
    }





    group_r.clearPathConstraints();



    }
edit retag flag offensive close merge delete

Comments

Can you expand on this a little, no one can help you with that amount of information. What code are you using, and what format do you want to save the trajectory in or what do you want to do with it when it's saved?

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2017-12-21 09:10:35 -0500 )edit

@PeteBlackerThe3rd
Sorry, i didn't post the problem clearly yesterday. Now i have posted the completed code above. Please check. Thanks a lot.

pdmike gravatar imagepdmike ( 2017-12-21 20:43:41 -0500 )edit

Okay that's some useful information, I can't see any obvious errors in your code, can you tell me how it's not working. It is compiling successfully? Does it crash with errors when you run it? Does it run without errors but not produce the points.txt file?

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2017-12-29 11:09:37 -0500 )edit

Also it looks like your code will loop forever when a successful route has been planned writing the same file every 5 seconds.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2017-12-29 11:10:51 -0500 )edit

Hi @PeteBlackerThe3rd I have implemented your code and it's working. I change some things, for example:

while (count < 1)

Just to only write once the file.

jdeleon gravatar imagejdeleon ( 2018-10-04 17:23:58 -0500 )edit

But I have one question. The data that is in the file, should be in which format? If I only want to get the position of my joint 7

outfile<<path_points.joint_trajectory.joint_names[6]<<std::endl;
jdeleon gravatar imagejdeleon ( 2018-10-04 17:27:58 -0500 )edit