bool ArmMotionInterface::rt_arm_plan_path_current_to_goal_dp_xyz() {
    Eigen::Vector3d dp_vec;
    
    ROS_INFO("called rt_arm_plan_path_current_to_goal_dp_xyz");
    //unpack the goal pose:
    int ndim = cart_goal_.arm_dp_right.size();
    if (ndim!=3) {            
      ROS_WARN("requested displacement, arm_dp_right, is wrong dimension");
      cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::RT_ARM_PATH_NOT_VALID;
      path_is_valid_=false;
      return path_is_valid_;    
    }
    for (int i=0;i<3;i++) dp_vec[i] = cart_goal_.arm_dp_right[i];
    ROS_INFO("requested dp = %f, %f, %f",dp_vec[0],dp_vec[1],dp_vec[2]);
    Vectorq7x1 q_start;
    q_start = get_jspace_start_right_arm_(); // choose last cmd, or current joint angles    
    path_is_valid_= plan_cartesian_delta_p(q_start, dp_vec); 

    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_;        
}
//this is a pretty general function:
// goal contains a desired tool pose;
// path is planned from current joint state to some joint state that achieves desired tool pose
bool ArmMotionInterface::rt_arm_plan_path_current_to_goal_pose() {
    ROS_INFO("computing a cartesian trajectory to right-arm tool goal pose");
    //unpack the goal pose:
    goal_gripper_pose_right_ = cart_goal_.des_pose_gripper_right;
    
    goal_gripper_affine_right_= transformPoseToEigenAffine3d(goal_gripper_pose_right_.pose);
    ROS_INFO("tool frame goal: ");
    display_affine(goal_gripper_affine_right_);    
    goal_flange_affine_right_ = goal_gripper_affine_right_ * A_tool_wrt_flange_.inverse();
    ROS_INFO("flange goal");
    display_affine(goal_flange_affine_right_);
    Vectorq7x1 q_start;
    q_start = get_jspace_start_right_arm_(); // choose last cmd, or current joint angles
    path_is_valid_ = cartTrajPlanner_.cartesian_path_planner(q_start, goal_flange_affine_right_, 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_;    
}
/*
bool ArmMotionInterface::plan_path_current_to_goal_flange_pose() {
    ROS_INFO("computing a joint-space trajectory to right-arm flange goal pose");
    //unpack the goal pose:
    goal_flange_affine_ = xformUtils.transformPoseToEigenAffine3d(cart_goal_.des_pose_flange.pose);

    ROS_INFO("flange goal");
    display_affine(goal_flange_affine_);
    Vectorq7x1 q_start;
    q_start = get_jspace_start_(); // choose last cmd, or current joint angles
    path_is_valid_ = cartTrajPlanner_.cartesian_path_planner(q_start, goal_flange_affine_, optimal_path_);

    if (path_is_valid_) {
        baxter_traj_streamer_.stuff_trajectory_right_arm(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 = cartesian_planner::cart_moveResult::SUCCESS;
        cart_result_.computed_arrival_time = computed_arrival_time_;
        cart_move_as_.setSucceeded(cart_result_);
    } else {
        cart_result_.return_code = cartesian_planner::cart_moveResult::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_;
}
*/
bool ArmMotionInterface::plan_jspace_path_current_to_cart_gripper_pose() {
    ROS_INFO("computing a jspace trajectory to right-arm gripper goal pose");
    //unpack the goal pose:
    goal_gripper_pose_ = cart_goal_.des_pose_gripper;
    xformUtils.printStampedPose(goal_gripper_pose_);
    goal_flange_affine_ = xform_gripper_pose_to_affine_flange_wrt_torso(goal_gripper_pose_);    
    

    ROS_INFO("flange goal");
    display_affine(goal_flange_affine_);
    Vectorq7x1 q_start;
    q_start = get_jspace_start_(); // choose last cmd, or current joint angles
    //    bool jspace_path_planner_to_affine_goal(Vectorq7x1 q_start, Eigen::Affine3d a_flange_end, std::vector<Eigen::VectorXd> &optimal_path);

    path_is_valid_ = cartTrajPlanner_.jspace_path_planner_to_affine_goal(q_start, goal_flange_affine_, optimal_path_);

    if (path_is_valid_) {
        baxter_traj_streamer_.stuff_trajectory_right_arm(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 = cartesian_planner::cart_moveResult::SUCCESS;
        cart_result_.computed_arrival_time = computed_arrival_time_;
        cart_move_as_.setSucceeded(cart_result_);
    } else {
        cart_result_.return_code = cartesian_planner::cart_moveResult::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_;
}
//since baxter_traj_streamer_ object already has a subscription to joint_state,
// can use it to get joint states.  Paranoid--extra tests to make sure data is "fresh"
Eigen::VectorXd ArmMotionInterface::get_right_arm_joint_angles(void) {   
    //not sure this is necessary; DO want "fresh" angles, so make sure values are updated:
    q_vec_right_arm_[0] = 1000;
    while (fabs(q_vec_right_arm_[0]) > 3) { // keep trying until see viable value populated by fnc
        q_vec_right_arm_ = baxter_traj_streamer_.get_qvec_right_arm();
        ros::spinOnce();
        ros::Duration(0.01).sleep();
    }
    Eigen::VectorXd q_vec_xd;
    q_vec_xd = q_vec_right_arm_; // convert from 7DOF to Xd
    //while we're at it, fill in mem variable as well:
    q_vec_right_arm_Xd_ = q_vec_xd;
    return q_vec_xd;
}
//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_;    
}
bool ArmMotionInterface::refine_cartesian_path_soln(void) {
    ROS_INFO("ArmMotionInterface: refining trajectory");
    if (path_is_valid_) {
        bool valid = cartTrajPlanner_.refine_cartesian_path_plan(optimal_path_);
        if (!valid) return false;

        baxter_traj_streamer_.stuff_trajectory_right_arm(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 = cartesian_planner::cart_moveResult::SUCCESS;
        cart_result_.computed_arrival_time = computed_arrival_time_;
        cart_move_as_.setSucceeded(cart_result_);
    } else {
        cart_result_.return_code = cartesian_planner::cart_moveResult::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 false;
    }
    return true;
}