//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_;
}
//given q_start, compute the tool-flange pose, and compute a path to move delta_p with R fixed
// return the optimized joint-space path in optimal_path
// this fnc can be used fairly generally--e.g., special cases such as 20cm descent from current arm pose
// this fnc is used within rt_arm_plan_path_current_to_goal_dp_xyz()
bool ArmMotionInterface::plan_cartesian_delta_p(Vectorq7x1 q_start, Eigen::Vector3d delta_p) {
//ROS_INFO("attempting Cartesian path plan for delta-p = %f, %f, %f",delta_p_(0),delta_p_(1),delta_p_(2));
cout << delta_p.transpose() << endl;
// this fnc will put the optimal_path result in a global vector, accessible by main
path_is_valid_ = cartTrajPlanner_.cartesian_path_planner_delta_p(q_start, delta_p, optimal_path_);
if (path_is_valid_) {
ROS_INFO("plan_cartesian_delta_p: computed valid delta-p path");
q_vec_start_resp_ = optimal_path_.front();
q_vec_end_resp_ = optimal_path_.back();
} else {
ROS_WARN("plan_cartesian_delta_p: path plan attempt not successful");
}
return path_is_valid_;
}
//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;
}
bool ArmMotionInterface::plan_path_qstart_to_Agoal(Vectorq7x1 q_start) { // provide q_start; assumes use of a_flange_end_ and fills in g_optimal_path
path_is_valid_ = cartTrajPlanner_.cartesian_path_planner(q_start, a_flange_end_, optimal_path_);
return path_is_valid_;
}
//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_;
}
