bool CartTrajPlanner::cartesian_path_planner(Eigen::Affine3d a_flange_start,Eigen::Affine3d a_flange_end, std::vector<Eigen::VectorXd> &optimal_path) {
std::vector<std::vector<Eigen::VectorXd> > path_options;
path_options.clear();
std::vector<Eigen::VectorXd> single_layer_nodes;
Eigen::VectorXd node;
Eigen::Affine3d a_flange_des;
Eigen::Matrix3d R_des = a_flange_end.linear();
a_flange_des.linear() = R_des;
//a_flange_start = ur10FwdSolver_.fwd_kin_solve(q_start);
//cout << "fwd kin from q_start: " << a_flange_start.translation().transpose() << endl;
//cout << "fwd kin from q_start R: " << endl;
//cout << a_flange_start.linear() << endl;
//store a vector of Cartesian affine samples for desired path:
cartesian_affine_samples_.clear();
a_flange_des = a_flange_end;
a_flange_start.linear() = R_des; // no interpolation of orientation; set goal orientation immediately
a_flange_des.linear() = R_des; //expected behavior: will try to achieve orientation first, before translating
int nsolns;
bool reachable_proposition;
int nsteps = 0;
Eigen::Vector3d p_des,dp_vec,del_p,p_start,p_end;
p_start = a_flange_start.translation();
p_end = a_flange_end.translation();
del_p = p_end-p_start;
double dp_scalar = CARTESIAN_PATH_SAMPLE_SPACING;
nsteps = round(del_p.norm()/dp_scalar);
if (nsteps<1) nsteps=1;
dp_vec = del_p/nsteps;
nsteps++; //account for pose at step 0
std::vector<Eigen::VectorXd> q_solns;
p_des = p_start;
cartesian_affine_samples_.push_back(a_flange_start);
for (int istep=0;istep<nsteps;istep++)
{
a_flange_des.translation() = p_des;
cartesian_affine_samples_.push_back(a_flange_des);
cout<<"trying: "<<p_des.transpose()<<endl;
//int ik_solve(Eigen::Affine3d const& desired_hand_pose,vector<Eigen::VectorXd> &q_ik_solns);
nsolns = ur10IkSolver_.ik_solve(a_flange_des, q_solns);
std::cout<<"cartesian step "<<istep<<" has = "<<nsolns<<" IK solns"<<endl;
single_layer_nodes.clear();
if (nsolns>0) {
single_layer_nodes.resize(nsolns);
for (int isoln = 0; isoln < nsolns; isoln++) {
// this is annoying: can't treat std::vector<Vectorq7x1> same as std::vector<Eigen::VectorXd>
//node = q_solns[isoln];
single_layer_nodes[isoln] = q_solns[isoln]; //node;
//single_layer_nodes = q_solns;
}
path_options.push_back(single_layer_nodes);
}
p_des += dp_vec;
}
//plan a path through the options:
int nlayers = path_options.size();
if (nlayers < 1) {
ROS_WARN("no viable options: quitting");
return false; // give up if no options
}
//std::vector<Eigen::VectorXd> optimal_path;
optimal_path.resize(nlayers);
double trip_cost;
// set joint penalty weights for computing optimal path in joint space
Eigen::VectorXd weights;
weights.resize(NJNTS);
for (int i = 0; i < NJNTS; i++) {
weights(i) = 1.0;
}
// compute min-cost path, using Stagecoach algorithm
cout << "instantiating a JointSpacePlanner:" << endl;
{ //limit the scope of jsp here:
JointSpacePlanner jsp(path_options, weights);
cout << "recovering the solution..." << endl;
jsp.get_soln(optimal_path);
trip_cost = jsp.get_trip_cost();
}
//now, jsp is deleted, but optimal_path lives on:
cout << "resulting solution path: " << endl;
for (int ilayer = 0; ilayer < nlayers; ilayer++) {
cout << "ilayer: " << ilayer << " node: " << optimal_path[ilayer].transpose() << endl;
}
cout << "soln min cost: " << trip_cost << endl;
return true;
}
int main(){
jsp();
return 0;
}
// alt version: specify start as a q_vec, and desired delta-p Cartesian motion while holding R fixed
bool CartTrajPlanner::cartesian_path_planner_delta_p(Eigen::VectorXd q_start, Eigen::Vector3d delta_p, std::vector<Eigen::VectorXd> &optimal_path) {
std::vector<std::vector<Eigen::VectorXd> > path_options;
path_options.clear();
std::vector<Eigen::VectorXd> single_layer_nodes;
Eigen::VectorXd node;
Eigen::Affine3d a_tool_des,a_tool_start,a_tool_end;
Eigen::Vector3d p_des,dp_vec,del_p,p_start,p_end;
a_tool_start = ur10FwdSolver_.fwd_kin_solve(q_start);
Eigen::Matrix3d R_des = a_tool_start.linear();
p_des = a_tool_start.translation();
cout<<"fwd kin from q_start p: "<<p_des.transpose()<<endl;
cout<<"fwd kin from q_start R: "<<endl;
cout<<a_tool_start.linear()<<endl;
//a_tool_start.linear() = R_des; // override the orientation component--require point down
//construct a goal pose, based on start pose:
a_tool_end.linear() = R_des;
a_tool_des.linear() = R_des; // variable used to hold steps of a_des...always with same R
a_tool_end.translation() = p_des+delta_p;
int nsolns;
bool reachable_proposition;
int nsteps = 0;
p_start = a_tool_start.translation();
p_end = a_tool_end.translation();
del_p = p_end-p_start; // SHOULD be same as delta_p input
cout<<"p_start: "<<p_start.transpose()<<endl;
cout<<"p_end: "<<p_end.transpose()<<endl;
cout<<"del_p: "<<del_p.transpose()<<endl;
double dp_scalar = 0.05;
nsteps = round(del_p.norm()/dp_scalar);
if (nsteps<1) nsteps=1;
dp_vec = del_p/nsteps;
cout<<"dp_vec for nsteps = "<<nsteps<<" is: "<<dp_vec.transpose()<<endl;
nsteps++; //account for pose at step 0
//coerce path to start from provided q_start;
single_layer_nodes.clear();
//node = q_start;
single_layer_nodes.push_back(q_start);
path_options.push_back(single_layer_nodes);
std::vector<Eigen::VectorXd> q_solns;
p_des = p_start;
for (int istep=1;istep<nsteps;istep++)
{
p_des += dp_vec;
a_tool_des.translation() = p_des;
cout<<"trying: "<<p_des.transpose()<<endl;
nsolns = ur10IkSolver_.ik_solve(a_tool_des, q_solns);
std::cout<<"nsolns = "<<nsolns<<endl;
single_layer_nodes.clear();
if (nsolns>0) {
single_layer_nodes.resize(nsolns);
for (int isoln = 0; isoln < nsolns; isoln++) {
// this is annoying: can't treat std::vector<Vectorq7x1> same as std::vector<Eigen::VectorXd>
//node = q_solns[isoln];
single_layer_nodes[isoln] = q_solns[isoln];//node;
}
path_options.push_back(single_layer_nodes);
}
else {
return false;
}
}
//plan a path through the options:
int nlayers = path_options.size();
if (nlayers < 1) {
ROS_WARN("no viable options: quitting");
return false; // give up if no options
}
//std::vector<Eigen::VectorXd> optimal_path;
optimal_path.resize(nlayers);
double trip_cost;
// set joint penalty weights for computing optimal path in joint space
Eigen::VectorXd weights;
weights.resize(NJNTS);
for (int i = 0; i < NJNTS; i++) {
weights(i) = 1.0;
}
// compute min-cost path, using Stagecoach algorithm
cout << "instantiating a JointSpacePlanner:" << endl;
{ //limit the scope of jsp here:
JointSpacePlanner jsp(path_options, weights);
cout << "recovering the solution..." << endl;
jsp.get_soln(optimal_path);
trip_cost = jsp.get_trip_cost();
}
//now, jsp is deleted, but optimal_path lives on:
cout << "resulting solution path: " << endl;
for (int ilayer = 0; ilayer < nlayers; ilayer++) {
cout << "ilayer: " << ilayer << " node: " << optimal_path[ilayer].transpose() << endl;
}
cout << "soln min cost: " << trip_cost << endl;
return true;
}
