C++ (Cpp) CartTrajPlanner::jspace_trivial_path_planner示例

示例#1

文件： baxter_cart_move_as.cpp 项目： ShaunHoward/cwru_baxter

//take in q_start and q_end and build trivial path in optimal_path_ for pure joint-space move
bool ArmMotionInterface::plan_jspace_path_qstart_to_qend(Vectorq7x1 q_start, Vectorq7x1 q_goal) {
    ROS_INFO("setting up a joint-space path");
    path_is_valid_ = cartTrajPlanner_.jspace_trivial_path_planner(q_start, q_goal, optimal_path_);
   if (path_is_valid_) {
        baxter_traj_streamer_.stuff_trajectory(optimal_path_, des_trajectory_); //convert from vector of poses to trajectory message   
        computed_arrival_time_ = des_trajectory_.points.back().time_from_start.toSec();
        cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::SUCCESS;
        cart_result_.computed_arrival_time = computed_arrival_time_;
        cart_move_as_.setSucceeded(cart_result_); 
    }
    else {
        cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::RT_ARM_PATH_NOT_VALID;
        cart_result_.computed_arrival_time = -1.0; //impossible arrival time        
        cart_move_as_.setSucceeded(cart_result_); //the communication was a success, but not the computation 
    }             
    return path_is_valid_;    
}

示例#2

文件： arm_motion_interface.cpp 项目： xpharry/ROS-learning

//take in q_start and q_end and build trivial path in g_optimal_path for pure joint-space move
bool ArmMotionInterface::plan_jspace_path_qstart_to_qend(Vectorq7x1 q_start,Vectorq7x1 q_goal) {
    ROS_INFO("setting up a joint-space path");
    path_is_valid_ = cartTrajPlanner_.jspace_trivial_path_planner(q_start, q_goal, g_optimal_path);
    return path_is_valid_;
}