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; }