/// Evolve method.
void monte_carlo::evolve(population &pop) const
{
// Let's store some useful variables.
const problem::base &prob = pop.problem();
const problem::base::size_type prob_dimension = prob.get_dimension(), prob_i_dimension = prob.get_i_dimension();
const decision_vector &lb = prob.get_lb(), &ub = prob.get_ub();
const population::size_type pop_size = pop.size();
// Get out if there is nothing to do.
if (pop_size == 0 || m_max_eval == 0) {
return;
}
// Initialise temporary decision vector, fitness vector and decision vector.
decision_vector tmp_x(prob_dimension);
fitness_vector tmp_f(prob.get_f_dimension());
constraint_vector tmp_c(prob.get_c_dimension());
// Main loop.
for (std::size_t i = 0; i < m_max_eval; ++i) {
// Generate a random decision vector.
for (problem::base::size_type j = 0; j < prob_dimension - prob_i_dimension; ++j) {
tmp_x[j] = boost::uniform_real<double>(lb[j],ub[j])(m_drng);
}
for (problem::base::size_type j = prob_dimension - prob_i_dimension; j < prob_dimension; ++j) {
tmp_x[j] = boost::uniform_int<int>(lb[j],ub[j])(m_urng);
}
// Compute fitness and constraints.
prob.objfun(tmp_f,tmp_x);
prob.compute_constraints(tmp_c,tmp_x);
// Locate the worst individual.
const population::size_type worst_idx = pop.get_worst_idx();
if (prob.compare_fc(tmp_f,tmp_c,pop.get_individual(worst_idx).cur_f,pop.get_individual(worst_idx).cur_c)) {
pop.set_x(worst_idx,tmp_x);
}
}
}
bool
updateJointStates (const std::map<std::string, tf::StampedTransform>& frame_transforms,
std::map<std::string, double> joint_positions)
{
tf::Vector3 tmp_x (0, 1, 0);
tf::Vector3 tmp_y (0, 0, 1);
tf::Vector3 tmp_z (1, 0, 0);
btMatrix3x3 home_rot;
home_rot.setValue (tmp_x.x (), tmp_y.x (), tmp_z.x (), tmp_x.y (), tmp_y.y (), tmp_z.y (), tmp_x.z (), tmp_y.z (),
tmp_z.z ());
tf::Vector3 torso_translation = home_rot.transpose () * frame_transforms_[TORSO_FRAME].getOrigin ();
btMatrix3x3 torso_rotation = home_rot.transpose () * frame_transforms_[TORSO_FRAME].getBasis ();
double roll, pitch, yaw;
torso_rotation.getEulerYPR (yaw, pitch, roll, 1);//angles are around ZYX. sequence is YXZ
// btMatrix3x3 r_z (btQuaternion (tf::Vector3 (0, 0, 1), yaw));
// btMatrix3x3 r_y (btQuaternion (tf::Vector3 (0, 1, 0), pitch));
// btMatrix3x3 r_x (btQuaternion (tf::Vector3 (1, 0, 0), roll));
// btMatrix3x3 rot = r_y * r_x * r_z;
joint_positions_["base_footprint_to_human_footprint"] = 0;
joint_positions_["human_footprint_to_torso_x"] = torso_translation.x ();
joint_positions_["torso_x_to_torso_y"] = torso_translation.y ();
joint_positions_["torso_y_to_torso_z"] = torso_translation.z ();
joint_positions_["torso_z_to_pitch"] = pitch;
joint_positions_["torso_pitch_to_roll"] = roll;
joint_positions_["torso_roll_to_yaw"] = yaw;
// tf::Transform transform_sho;
// btMatrix3x3 sho_rotation = frame_transforms_[TORSO_FRAME].getBasis ().transpose ()
// * frame_transforms_[RIGHT_SHOULDER_FRAME].getBasis ();
//
// sho_rotation.getEulerYPR (yaw, pitch, roll, 1);//order of angles ZYX, order of rotation -euler- YXZ.
// std::cout << "+1: y: " << pitch * 180 / M_PI << "\tx: " << roll * 180 / M_PI << "\tz: " << yaw * 180 / M_PI << "\n";
// btMatrix3x3 shoulder_ = sho_rotation * frame_transforms_[TORSO_FRAME].getBasis ();
// btMatrix3x3 shoulder = r_z * r_y * r_x * frame_transforms_[TORSO_FRAME].getBasis ();
//
// tf::StampedTransform transform;
//
// // transform.frame_id_ = "r_shoulder_home";
// transform.frame_id_ = "torso_1";
// transform.child_frame_id_ = "r_shoulder_final";
//
// transform.stamp_ = ros::Time::now ();
// transform.setOrigin (tf::Vector3 (0, 0, 0));
// transform.setBasis (sho_rotation);
// broadcaster_->sendTransform (transform);
// joint_positions_["torso_yaw_to_r_shoulder_home"] = 0;
// joint_positions_["r_shoulder_home_to_pitch"] = pitch;
// joint_positions_["r_shoulder_pitch_to_roll"] = roll;
// joint_positions_["r_shoulder_roll_to_yaw"] = yaw;
//
// btMatrix3x3 elb_rotation = frame_transforms_[RIGHT_SHOULDER_FRAME].getBasis ().transpose ()
// * frame_transforms_[RIGHT_ELBOW_FRAME].getBasis ();
/*
//first method for elbow angle extraction -> 1DOF
btQuaternion q;
elb_rotation.getRotation (q);
joint_positions_["r_shoulder_yaw_to_r_upper_arm"] = 0;
joint_positions_["r_upper_arm_to_r_elbow_home"] = 0;
if (fabs (q.getAxis ().y ()) > 0.90)
joint_positions_["r_elbow_home_to_pitch"] = q.getAngle () * al::math::sgn (q.getAxis ().y ());
else
//singularity, assume 0 degrees
joint_positions_["r_elbow_home_to_pitch"] = 0;
joint_positions_["r_elbow_pitch_to_r_lower_arm"] = 0;*/
//second method for elbow angle extraction-> 3DOF (not so real but it compansates the error)
// joint_positions_["r_elbow_pitch_to_r_hand_home"] = 0;
//
// joint_positions_["r_hand_home_to_yaw"] = 0;
// joint_positions_["r_hand_yaw_to_pitch"] = 0;
// joint_positions_["r_hand_pitch_to_roll"] = 0;
// std::cout << " x: " << std::setw (8) << std::setprecision (3) << q.getAxis ().x () << "\ty: " << std::setw (8)
// << std::setprecision (3) << q.getAxis ().y () << "\tz: " << q.getAxis ().z () << "\t angle: " << std::setw (8)
// << std::setprecision (3) << q.getAngle () * 180 / M_PI << "\n";
// for (uint i = 0; i < 3; i++)
// std::cout << elb_rotation.getRow (i).x () << " " << elb_rotation.getRow (i).y () << " "
// << elb_rotation.getRow (i).z () << "\n";
// std::cout << "\n";
//This doesn't work!
// tf::Transform t;
// t = frame_transforms_[RIGHT_ELBOW_FRAME] * frame_transforms_[RIGHT_SHOULDER_FRAME].inverse ();
// btMatrix3x3 r = t.getBasis ();
//
// for (uint i = 0; i < 3; i++)
// std::cout << r.getRow (i).x () << " " << r.getRow (i).y () << " " << r.getRow (i).z () << "\n";
// std::cout << "\n";
// tf::Vector3 x_prime = elb_rotation.getColumn (0);
// tf::Vector3 x (1, 0, 0);
// float elbow_angle = fabs (al::math::getAngleBetween (x, x_prime));
// elbow_angle *= al::math::sgn (x.cross (x_prime).getY ());
// try
// {
// tf::StampedTransform tmp;
// listener_->lookupTransform ("left_shoulder_1", "left_elbow_1", ros::Time (0), tmp);
//
// std::cout << "*relative: " << std::endl;
// for (uint i = 0; i < 3; i++)
// std::cout << tmp.getBasis ().getRow (i).x () << " " << tmp.getBasis ().getRow (i).y () << " "
// << tmp.getBasis ().getRow (i).z () << "\n";
// std::cout << "\n";
// }
// catch (tf::TransformException ex)
// {
// ROS_WARN ("no such transform to %s", "left_elbow_1");
// //TODO: lost the user, suppose previous joint states same for 1 second, otherwise return false
// return false;
// }
/*
transform_sho = frame_transforms_[LEFT_SHOULDER_FRAME] * frame_transforms_[TORSO_FRAME].inverse ();
sho_rotation = transform_sho.getBasis ();
sho_rotation.getEulerYPR (yaw, pitch, roll, 1);
sho_rotation.getEulerYPR (yaw2, pitch2, roll2, 2);
// std::cout << "sol 2: " << "z: " << (int)(yaw2 * 180 / M_PI) << "\t" << "y: " << (int)(pitch2 * 180 / M_PI) << "\t"
// << "x: " << (int)(roll2 * 180 / M_PI) << "\n";
if (yaw2 < 0)
yaw2 += 2 * M_PI;
if (pitch2 < 0)
pitch2 += 2 * M_PI;
if (roll2 < 0)
roll2 += 2 * M_PI;
joint_positions_["torso_yaw_to_l_shoulder_home"] = 0;
joint_positions_["l_shoulder_home_to_yaw"] = yaw2;
joint_positions_["l_shoulder_yaw_to_pitch"] = pitch2;
joint_positions_["l_shoulder_pitch_to_roll"] = roll2;
joint_positions_["l_shoulder_roll_to_l_upper_arm"] = 0;
elb_rotation = frame_transforms_[LEFT_SHOULDER_FRAME].getBasis ().transpose ()
* frame_transforms_[LEFT_ELBOW_FRAME].getBasis ();
x_prime = elb_rotation.getColumn (0);
elbow_angle = fabs (al::math::getAngleBetween (x, x_prime));
elbow_angle *= al::math::sgn (x.cross (x_prime).getY ());
joint_positions_["l_shoulder_roll_to_l_elbow_home"] = 0;
joint_positions_["l_elbow_home_to_pitch"] = elbow_angle;
joint_positions_["l_elbow_pitch_to_l_lower_arm"] = 0;
joint_positions_["l_elbow_pitch_to_l_hand_home"] = 0;
joint_positions_["l_hand_home_to_yaw"] = 0;
joint_positions_["l_hand_yaw_to_pitch"] = 0;
joint_positions_["l_hand_pitch_to_roll"] = 0;
*/
return true;
}
void ADMMCut::MakeLayers()
{
// Initial Cutting Edge Setting
InitState();
debug_ = false;
int cut_count = 0;
vector<double> cut_energy;
vector<double> res_energy;
do
{
/* Recreate dual graph at the beginning of each cut */
SetStartingPoints(cut_count);
CreateA();
ptr_stiff_->CalculateD(D_, x_, 0, 0, cut_count);
///* for rendering */
//if (cut_count == 0)
//{
// WriteWeight();
// WriteStiffness("offset1.txt", "rotation1.txt");
// getchar();
//}
//if (cut_count == 4)
//{
// WriteStiffness("offset2.txt", "rotation2.txt");
// getchar();
//}
//if (cut_count == 6)
//{
// WriteStiffness("offset3.txt", "rotation3.txt");
// getchar();
//}
/* set x for intial cut setting */
SetBoundary();
/*
* energy specify:
* cut energy : | A * x |
* defomation energy : norm(K D - F)
*/
ptr_stiff_->CreateGlobalK(x_);
ptr_stiff_->CreateF(x_);
SpMat K_init = *(ptr_stiff_->WeightedK());
VX F_init = *(ptr_stiff_->WeightedF());
double icut_energy = 0;
VX V_Cut = A_ * x_;
for (int i = 0; i < Md_; i++)
{
icut_energy += abs(V_Cut[i]);
}
double ideform_energy = (K_init * D_ - F_init).norm();
cout << "****************************************" << endl;
cout << "ADMMCut Round : " << cut_count << endl;
cout << "Initial cut energy before entering ADMM: " << icut_energy << endl;
cout << "Initial Lagrangian energy before entering ADMM: " << ideform_energy << endl;
cout << "---------------------------------" << endl;
int rew_count = 0;
VX x_prev;
VX D_prev;
/* Output energy list for reweighting process in a single
graph cut problem, energy.size() = number of reweighting
process performed.
cut energy = |A_ * x_| = sum_{e_ij} w_ij * |x_i - x_j|
res energy = (K(x)D - F(x)).norm()
*/
cut_energy.clear();
res_energy.clear();
cut_energy.push_back(icut_energy);
res_energy.push_back(ideform_energy);
do
{
/* Reweighting loop for cut */
int ADMM_count = 0;
x_prev = x_;
CreateC(cut_count, rew_count);
do
{
cout << "ADMMCut Round: " << cut_count << ", reweight iteration:" << rew_count
<< ", ADMM " << ADMM_count << " iteration." << endl;
/*-------------------ADMM loop-------------------*/
CalculateX();
D_prev = D_;
CalculateD();
UpdateLambda();
/*-------------------Residual Calculation-------------------*/
SpMat Q_prev;
SpMat Q_new;
CalculateQ(D_prev, Q_prev);
CalculateQ(D_, Q_new);
/* Update K reweighted by new x */
ptr_stiff_->CreateGlobalK(x_);
ptr_stiff_->CreateF(x_);
SpMat K_new = *(ptr_stiff_->WeightedK());
VX F_new = *(ptr_stiff_->WeightedF());
dual_res_ = penalty_ * (D_prev - D_).transpose() * K_new.transpose() * Q_prev
+ lambda_.transpose() * (Q_prev - Q_new);
primal_res_ = K_new * D_ - F_new;
/*-------------------Screenplay-------------------*/
double new_cut_energy = x_.dot(H1_ * x_);
cout << "new quadratic func value record: " << new_cut_energy << endl;
cout << "dual_residual : " << dual_res_.norm() << endl;
cout << "primal_residual(KD-F) : " << primal_res_.norm() << endl;
cout << "---------------------" << endl;
ADMM_count++;
} while (!TerminationCriteria(ADMM_count));
/* One reweighting process ended! */
/* Output energy and residual */
double energy = 0;
VX V_Cut = A_ * x_;
for (int i = 0; i < Md_; i++)
{
energy += ptr_dualgraph_->Weight(i) * abs(V_Cut[i]);
}
/*-------------------Screenplay-------------------*/
double res_tmp = primal_res_.norm();
cout << "Cut " << cut_count << " Reweight " << rew_count << " completed." << endl;
cout << "Cut Energy :" << energy << endl;
cout << "Res Energy :" << res_tmp << endl;
cout << "<-<-<-<-<-<-<-<-<-<-<-<-<-<-<-<-<-" << endl;
cut_energy.push_back(energy);
res_energy.push_back(res_tmp);
/* write x distribution to a file */
string str_x = "Cut_" + to_string(cut_count) + "_Rew_" + to_string(rew_count) + "_x";
Statistics tmp_x(str_x, x_);
tmp_x.GenerateVectorFile();
rew_count++;
} while (!UpdateR(x_prev, rew_count));
/* Output reweighting energy history for last cut process */
string str_eC = "Cut_" + to_string(cut_count) + "_Cut_Energy";
Statistics s_eC(str_eC, cut_energy);
s_eC.GenerateStdVecFile();
string str_eR = "Cut_" + to_string(cut_count) + "_Res_Energy";
Statistics s_eR(str_eR, res_energy);
s_eR.GenerateStdVecFile();
/* Update New Cut information to Rendering (layer_label_) */
UpdateCut();
fprintf(stdout, "ADMMCut No.%d process is Finished!\n", cut_count);
cut_count++;
} while (!CheckLabel(cut_count));
fprintf(stdout, "All done!\n");
}
