Matrix CoordinateSystem::getRotFromCompass(CompassData& compassData, GravityData& gravityData) {
Vector compass_vector(3);
compass_vector(0) = compassData.x();
compass_vector(1) = compassData.y();
compass_vector(2) = compassData.z();
Vector gravity_vector(3);
gravity_vector(0) = gravityData.x();
gravity_vector(1) = gravityData.y();
gravity_vector(2) = gravityData.z();
Vector East(3);
cross_prod(compass_vector, gravity_vector, East); // East = cross(compass_vector, gravity_vector)
double normEast = norm_2(East);
if (normEast < 0)
return IdentityMatrix(3);
East /= normEast; // normalization
gravity_vector /= norm_2(gravity_vector);
Vector North(3);
cross_prod(gravity_vector, East, North); // North = cross(gravity_vector, East)
Matrix rotmat(3, 3);
row(rotmat, 0) = East;
row(rotmat, 1) = North;
row(rotmat, 2) = gravity_vector;
return Matrix(rotmat);
}
bool LoadEstimator::updateRegression()
{
// std::cout << "updateRegression()" << std::endl;
Eigen::MatrixXd regress_matrix(num_input_data_, num_estimated_params_);
Eigen::VectorXd regress_vector(num_input_data_);
regress_matrix.setZero(num_input_data_, num_estimated_params_);
regress_vector.setZero(num_input_data_);
tf::StampedTransform wrench_transform;
tf::Vector3 wrench_torque;
tf::vector3MsgToTF(end_effector_state_msg_.wrench.torque, wrench_torque);
regress_vector(0) = - end_effector_state_msg_.wrench.force.x;
regress_vector(1) = - end_effector_state_msg_.wrench.force.y;
regress_vector(2) = - end_effector_state_msg_.wrench.force.z;
if(sim)
{
regress_vector(3) = end_effector_state_msg_.wrench.torque.x;
regress_vector(4) = end_effector_state_msg_.wrench.torque.y;
regress_vector(5) = end_effector_state_msg_.wrench.torque.z;
}
else
{
regress_vector(3) = - end_effector_state_msg_.wrench.torque.x;
regress_vector(4) = - end_effector_state_msg_.wrench.torque.y;
regress_vector(5) = - end_effector_state_msg_.wrench.torque.z;
}
mass_naive = regress_vector.block(0,0,3,1).norm() / GRAVITY_CONSTANT_;
//get the transform for the FT
//Options:
// ros::Time(0); //Just the latest message
// end_effector_state_msg_.header.stamp; //Time from a header
// ros::Time::now(); //Current time from a clock
bool success = false;
while (!success)
{
try
{
tf_listener_.waitForTransform(tf_base_name_, tf_wrench_name_, ros::Time(0), ros::Duration(0.5));
tf_listener_.lookupTransform(tf_base_name_, tf_wrench_name_, ros::Time(0), wrench_transform);
success = true;
}
catch (tf::ExtrapolationException ex)
{
ROS_ERROR("%s",ex.what());
}
sleep(0.1);
}
wrench_transform.setOrigin(tf::Vector3(0,0,0));
//get an eigen transform
//we need to do everything in FT local frame to get the CoM invariant
// (will not be the case in world frame since the CoM vector will be moving)
Eigen::Affine3d world_to_wrench_transform;
tf::transformTFToEigen(wrench_transform.inverse(), world_to_wrench_transform); //previously it was TransformTFToEigen
//create the total vector from acceleration and gravity
Eigen::Vector3d acceleration_vector;
acceleration_vector(0) = end_effector_state_msg_.linear_acceleration.x;
acceleration_vector(1) = end_effector_state_msg_.linear_acceleration.y;
acceleration_vector(2) = end_effector_state_msg_.linear_acceleration.z;
//get the angular acc and vel in FT frame
Eigen::Vector3d angular_velocity = Eigen::Vector3d(end_effector_state_msg_.angular_velocity.x,
end_effector_state_msg_.angular_velocity.y,
end_effector_state_msg_.angular_velocity.z);
Eigen::Vector3d angular_acceleration = Eigen::Vector3d(end_effector_state_msg_.angular_acceleration.x,
end_effector_state_msg_.angular_acceleration.y,
end_effector_state_msg_.angular_acceleration.z);
//If we assume static case - let's get rid of noises introduced by angular vel and accelerations (just test option)
if(static_assumption_)
{
// acceleration_vector.setZero();
angular_velocity.setZero();
angular_acceleration.setZero();
}
//create the gravity vector
Eigen::Vector3d gravity_vector = Eigen::Vector3d::Zero();
gravity_vector(2) = -GRAVITY_CONSTANT_;
//total contribution
Eigen::Vector3d total_contribution = acceleration_vector - gravity_vector;
//create the moment matrix in local frame
Eigen::Matrix3d moment_matrix;
getCrossMatrix(-total_contribution, moment_matrix);
moment_matrix = world_to_wrench_transform.rotation() * moment_matrix * world_to_wrench_transform.rotation().transpose();
//contribution of angular accelerations to the force
Eigen::Matrix3d angular_vel_cross;
getCrossMatrix(angular_velocity, angular_vel_cross);
Eigen::Matrix3d angular_acc_cross;
getCrossMatrix(angular_acceleration, angular_acc_cross);
Eigen::Matrix3d angular_acc_contrib = angular_acc_cross + angular_vel_cross * angular_vel_cross;
angular_acc_contrib = world_to_wrench_transform.rotation() * angular_acc_contrib * world_to_wrench_transform.rotation().transpose();
//fill the regression matrix
regress_matrix.block(0, 0, 3, 1) = world_to_wrench_transform.rotation() * total_contribution;
regress_matrix.block(3, 1, 3, 3) = moment_matrix;
regress_matrix.block(0, 1, 3, 3) = angular_acc_contrib;
//fill with ones for the offset (i.e. unknown biases of sensors move to the regression matrix <=> add identity matrix)
if(estimate_offset_)
{
regress_matrix.block(0, 4, 3, 3) = Eigen::Matrix3d::Identity();
regress_matrix.block(3, 4 + 3, 3, 3) = Eigen::Matrix3d::Identity();
}
//add optional regession for inertial parameters Iii
if(estimate_inertia_)
{
Eigen::MatrixXd angular_vel_dot = Eigen::MatrixXd(3, 6);
getDotMatrix(world_to_wrench_transform.rotation() * angular_velocity, angular_vel_dot);
Eigen::MatrixXd angular_acc_dot = Eigen::MatrixXd(3, 6);
getDotMatrix(world_to_wrench_transform.rotation() * angular_acceleration, angular_acc_dot);
Eigen::MatrixXd inertial_contrib = angular_acc_dot + world_to_wrench_transform.rotation() * angular_vel_cross * world_to_wrench_transform.rotation().transpose() * angular_vel_dot;
regress_matrix.block(3, 4 + num_offset_params_, 3, 6) = inertial_contrib;
// regress_matrix.block(3, 4, 3, 6) = inertial_contrib;
// std::cout << "Inertial_contrib = " << std::endl << inertial_contrib << std::endl << std::endl;
}
// std::cout << "the regress matrix looks like" << std::endl << std::endl << regress_matrix << std::endl;
// std::cout << "Regressed vector looks like: " << regress_vector << std::endl;
if(use_recursive_least_square_)
{
K_ = P_ * regress_matrix.transpose() * (lambda_ * Eigen::MatrixXd::Identity(num_input_data_, num_input_data_) + regress_matrix * P_ * regress_matrix.transpose()).inverse();
P_ = (1/lambda_) * (Eigen::MatrixXd::Identity(num_estimated_params_, num_estimated_params_) - K_ * regress_matrix) * P_;
regressed_parameters_ = regressed_parameters_ + K_ * (regress_vector - regress_matrix * regressed_parameters_);
// std::cout << regressed_parameters_<< std::endl << std::endl;
// std::cout << "K matrix = " << K_<< std::endl << std::endl;
// std::cout << "P_ matrix = " << P_ << std::endl << std::endl;
}
else
{
regressATA_ += regress_matrix.transpose() * regress_matrix;
regressATb_ += regress_matrix.transpose() * regress_vector;
// std::cout << "A.transpose()*A:" << std::endl << regressATA_ << std::endl << std::endl << std::endl;
// std::cout << "A.transpose()*b:" << std::endl << regressATb_ << std::endl << std::endl << std::endl;
}
return true;
}
