void AgentCore::setTheta(geometry_msgs::Quaternion &quat, const double &theta) const {
Eigen::Vector3d rpy = getRPY(quat);
Eigen::Quaterniond eigen_quat = Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX())
* Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY())
* Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitZ());
tf::quaternionEigenToMsg(eigen_quat, quat);
std::stringstream s;
s << "Quaternion updated (" << quat.x << ", " << quat.y << ", " << quat.z << ", " << quat.w << ").";
console(__func__, s, DEBUG_VVVV);
}
void NDTScanMatching::scan_matching_callback(const sensor_msgs::PointCloud2::ConstPtr& points,
const geometry_msgs::PoseStamped::ConstPtr& pose)
{
ros::Time scan_time = ros::Time::now();
pcl::PointXYZI p;
pcl::PointCloud<pcl::PointXYZI> scan;
pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_scan_ptr (new pcl::PointCloud<pcl::PointXYZI>());
tf::Quaternion q;
Eigen::Matrix4f t(Eigen::Matrix4f::Identity());
tf::Transform transform;
pcl::fromROSMsg(*points, scan);
pcl::PointCloud<pcl::PointXYZI>::Ptr scan_ptr(new pcl::PointCloud<pcl::PointXYZI>(scan));
if(initial_scan_loaded_ == 0)
{
last_scan_ = *scan_ptr;
last_pose_ = *pose;
initial_scan_loaded_ = 1;
return;
}
// Filtering input scan to roughly 10% of original size to increase speed of registration.
pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_cloud_ptr (new pcl::PointCloud<pcl::PointXYZI>);
pcl::ApproximateVoxelGrid<pcl::PointXYZI> approximate_voxel_filter;
approximate_voxel_filter.setLeafSize (2.0, 2.0, 2.0);
approximate_voxel_filter.setInputCloud (scan_ptr);
approximate_voxel_filter.filter (*filtered_cloud_ptr);
// Setting scale dependent NDT parameters
// Setting minimum transformation difference for termination condition.
ndt_.setTransformationEpsilon (0.01);
// Setting maximum step size for More-Thuente line search.
ndt_.setStepSize (0.1);
//Setting Resolution of NDT grid structure (VoxelGridCovariance).
ndt_.setResolution (1.0);
// Setting max number of registration iterations.
ndt_.setMaximumIterations (30);
// Setting point cloud to be aligned.
ndt_.setInputSource (filtered_cloud_ptr);
pcl::PointCloud<pcl::PointXYZI>::Ptr last_scan_ptr(new pcl::PointCloud<pcl::PointXYZI>(last_scan_));
// Setting point cloud to be aligned to.
ndt_.setInputTarget (last_scan_ptr);
tf::Matrix3x3 init_rotation;
// 一個前のposeと引き算してx, y ,zの偏差を出す
offset_x_ = pose->pose.position.x - last_pose_.pose.position.x;
offset_y_ = pose->pose.position.y - last_pose_.pose.position.y;
double roll, pitch, yaw = 0;
getRPY(pose->pose.orientation, roll, pitch, yaw);
offset_yaw_ = yaw - last_yaw_;
guess_pos_.x = previous_pos_.x + offset_x_;
guess_pos_.y = previous_pos_.y + offset_y_;
guess_pos_.z = previous_pos_.z;
guess_pos_.roll = previous_pos_.roll;
guess_pos_.pitch = previous_pos_.pitch;
guess_pos_.yaw = previous_pos_.yaw + offset_yaw_;
Eigen::AngleAxisf init_rotation_x(guess_pos_.roll, Eigen::Vector3f::UnitX());
Eigen::AngleAxisf init_rotation_y(guess_pos_.pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf init_rotation_z(guess_pos_.yaw, Eigen::Vector3f::UnitZ());
Eigen::Translation3f init_translation(guess_pos_.x, guess_pos_.y, guess_pos_.z);
Eigen::Matrix4f init_guess = (init_translation * init_rotation_z * init_rotation_y * init_rotation_x).matrix();
pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
ros::Time ndt_start = ros::Time::now();
ndt_.align (*output_cloud_ptr, init_guess);
ros::Duration ndt_delta_t = ros::Time::now() - ndt_start;
std::cout << "Normal Distributions Transform has converged:" << ndt_.hasConverged ()
<< " score: " << ndt_.getFitnessScore () << std::endl;
t = ndt_.getFinalTransformation();
// Transforming unfiltered, input cloud using found transform.
pcl::transformPointCloud (*scan_ptr, *output_cloud_ptr, t);
tf::Matrix3x3 tf3d;
tf3d.setValue(
static_cast<double>(t(0, 0)), static_cast<double>(t(0, 1)), static_cast<double>(t(0, 2)),
static_cast<double>(t(1, 0)), static_cast<double>(t(1, 1)), static_cast<double>(t(1, 2)),
static_cast<double>(t(2, 0)), static_cast<double>(t(2, 1)), static_cast<double>(t(2, 2)));
current_pos_.x = t(0, 3);
current_pos_.y = t(1, 3);
current_pos_.z = t(2, 3);
std::cout << "/////////////////////////////////////////////" << std::endl;
std::cout << "count : " << count_ << std::endl;
std::cout << "Process time : " << ndt_delta_t.toSec() << std::endl;
std::cout << "x : " << current_pos_.x << std::endl;
std::cout << "y : " << current_pos_.y << std::endl;
std::cout << "z : " << current_pos_.z << std::endl;
std::cout << "/////////////////////////////////////////////" << std::endl;
tf3d.getRPY(current_pos_.roll, current_pos_.pitch, current_pos_.yaw, 1);
transform.setOrigin(tf::Vector3(current_pos_.x, current_pos_.y, current_pos_.z));
q.setRPY(current_pos_.roll, current_pos_.pitch, current_pos_.yaw);
transform.setRotation(q);
// "map"に対する"base_link"の位置を発行する
br_.sendTransform(tf::StampedTransform(transform, scan_time, "map", "ndt_base_link"));
sensor_msgs::PointCloud2 scan_matched;
pcl::toROSMsg(*output_cloud_ptr, scan_matched);
scan_matched.header.stamp = scan_time;
scan_matched.header.frame_id = "matched_point_cloud";
point_cloud_pub_.publish(scan_matched);
// Update position and posture. current_pos -> previous_pos
previous_pos_ = current_pos_;
// save current scan
last_scan_ += *output_cloud_ptr;
last_pose_ = *pose;
last_yaw_ = yaw;
// geometry_msgs::TransformStamped ndt_trans;
// ndt_trans.header.stamp = scan_time;
// ndt_trans.header.frame_id = "map";
// ndt_trans.child_frame_id = "base_link";
// ndt_trans.transform.translation.x = curren_pos.x;
// ndt_trans.transform.translation.y = curren_pos.y;
// ndt_trans.transform.translation.z = curren_pos.z;
// ndt_trans.transform.rotation = q;
// ndt_broadcaster_.sendTransform(ndt_trans);
count_++;
}
/**
* @function straightPath
* @brief Builds a straight path -- easy
*/
bool goWSOrient::straightPath( const Eigen::VectorXd &_startConfig,
const Eigen::Transform<double, 3, Eigen::Affine> &_goalPose,
std::vector<Eigen::VectorXd> &path )
{
//-- Copy input
startConfig = _startConfig;
goalPose = _goalPose;
goalPos = _goalPose.translation();
//-- Create needed variables
Eigen::VectorXd q; Eigen::VectorXd wsPos;
Eigen::Transform<double, 3, Eigen::Affine> T;
Eigen::MatrixXd Jc(6, 7); Eigen::MatrixXd Jcinv(7, 6);
Eigen::MatrixXd delta_q;
double ws_dist;
//-- Initialize variables
q = startConfig; path.push_back( q );
ws_dist = DBL_MAX;
//-- Loop
int trials = 0;
while( ws_dist > goalThresh )
{
//-- Get ws position
robinaLeftArm_fk( q, TWBase, Tee, T );
wsPos = T.translation();
Eigen::VectorXd wsOrient(3);
getRPY( T, wsOrient(0), wsOrient(1), wsOrient(2) );
//-- Get the Jacobian
robinaLeftArm_jc( q, TWBase, Tee, Jc );
std::cout<< " JC: " << std::endl << Jc << std::endl;
for( int i = 3; i < 6;i++ )
{ for( int j = 0; j < 7; j++ )
{
Jc(i,j) = Jc(i,j)*0.001;
}
}
std::cout<< " JC switch: " << std::endl << Jc << std::endl;
//-- Get the pseudo-inverse(easy way)
pseudoInv( 6, 7, Jc, Jcinv );
std::cout<< " JCW Inv: " << std::endl << Jcinv << std::endl;
Eigen::VectorXd goalOrient(3);
getRPY( goalPose, goalOrient(0), goalOrient(1), goalOrient(2) );
//-- Get delta (orientation + position)
Eigen::VectorXd delta(6);
delta.head(3) = (goalPos - wsPos); delta.tail(3) = (goalOrient - wsOrient);
//-- Get delta_q (jointspace)
delta_q = Jcinv*delta;
//-- Scale
double scal = stepSize/delta_q.norm();
delta_q = delta_q*scal;
//-- Add to q
q = q + delta_q;
//-- Push it
path.push_back( q );
//-- Check distance to goal
ws_dist = (goalPos - wsPos).norm();
printf(" -- Ws dist: %.3f \n", ws_dist );
trials++;
if( trials > maxTrials )
{ break; }
}
if( trials >= maxTrials )
{ path.clear();
printf(" --(!) Did not get to the goal . Thresh: %.3f Trials: %d \n", ws_dist, trials );
return false;}
else
{ printf(" -- Got to the goal . Thresh: %.3f Trials: %d \n", ws_dist, trials );
return true; }
}
double AgentCore::getTheta(const geometry_msgs::Quaternion &quat) const {
Eigen::Vector3d rpy = getRPY(quat);
return rpy(2);
}
/**
* @function balancePath
* @brief Builds a straight path -- easy
*/
bool goWSOrient::balancePath( const Eigen::VectorXd &_startConfig,
const Eigen::Transform<double, 3, Eigen::Affine> &_goalPose,
std::vector<Eigen::VectorXd> &path )
{
srand( time(NULL) );
//-- Copy input
startConfig = _startConfig;
goalPose = _goalPose;
goalPos = _goalPose.translation();
//-- Create needed variables
Eigen::VectorXd q; Eigen::VectorXd wsPos;
Eigen::Transform<double, 3, Eigen::Affine> T;
Eigen::MatrixXd Jc(3, 7); Eigen::MatrixXd Jcinv(7, 3);
Eigen::MatrixXd delta_q;
double ws_dist;
//-- Initialize variables
q = startConfig; path.push_back( q );
ws_dist = DBL_MAX;
//-- Loop
int trials = 0;
//-- Get ws position
robinaLeftArm_fk( q, TWBase, Tee, T );
wsPos = T.translation();
Eigen::VectorXd wsOrient(3);
getRPY( T, wsOrient(0), wsOrient(1), wsOrient(2) );
Eigen::VectorXd phi(7);
Eigen::VectorXd dq(7);
Eigen::VectorXd qorig = q;
//-- Get the Jacobian
robinaLeftArm_j( q, TWBase, Tee, Jc );
//-- Get the pseudo-inverse(easy way)
pseudoInv( 3, 7, Jc, Jcinv );
int count = 0;
for( int i = 0; i < 1000; i++ )
{
for( int j = 0; j < 7; j++ )
{ phi[j] = rand()%6400 / 10000.0 ; }
//-- Get a null space projection displacement that does not affect the thing
dq = ( Eigen::MatrixXd::Identity(7,7) - Jcinv*Jc )*phi;
//-- Add to q
//q = qorig + dq;
Eigen::VectorXd newq;
newq = qorig + dq;
//-- Calculate distance
Eigen::Transform<double, 3, Eigen::Affine> Torig;
Eigen::Transform<double, 3, Eigen::Affine> Tphi;
robinaLeftArm_fk( newq, TWBase, Tee, Tphi );
robinaLeftArm_fk( qorig, TWBase, Tee, Torig );
double dist = ( Tphi.translation() - Torig.translation() ).norm();
if( dist < 0.015 )
{
count++;
q = newq;
printf("--> [%d] Distance to the original xyz is: %.3f \n", count, dist );
//-- Push it
path.push_back( q );
}
}
return true;
}
