Eigen::Quaternion<T> quatFromEuler(const T roll, const T pitch, const T yaw)
{
Eigen::AngleAxis<T> rollAngle(roll, Eigen::Matrix<T, 3, 1>::UnitZ());
Eigen::AngleAxis<T> yawAngle(yaw, Eigen::Matrix<T, 3, 1>::UnitY());
Eigen::AngleAxis<T> pitchAngle(pitch, Eigen::Matrix<T, 3, 1>::UnitX());
return rollAngle * yawAngle * pitchAngle;
}
int hormodular::Orientation::getRelativeOrientation(int connector, hormodular::Orientation localOrient, hormodular::Orientation remoteOrient)
{
//-- Create rotation matrices for local orientation:
Eigen::AngleAxisd rollAngle( deg2rad(localOrient.getRoll()), Eigen::Vector3d::UnitZ());
Eigen::AngleAxisd pitchAngle( deg2rad(localOrient.getPitch()), Eigen::Vector3d::UnitY());
Eigen::AngleAxisd yawAngle( deg2rad(localOrient.getYaw()), Eigen::Vector3d::UnitX());
Eigen::Quaterniond q0 = yawAngle * pitchAngle * rollAngle ;
Eigen::Matrix3d rotationMatrix = q0.matrix();
//-- Create rotation matrices for the other orientation:
Eigen::AngleAxisd otherRollAngle( deg2rad(remoteOrient.getRoll()), Eigen::Vector3d::UnitZ());
Eigen::AngleAxisd otherPitchAngle( deg2rad(remoteOrient.getPitch()), Eigen::Vector3d::UnitY());
Eigen::AngleAxisd otherYawAngle( deg2rad(remoteOrient.getYaw()), Eigen::Vector3d::UnitX());
Eigen::Quaterniond q1 = otherYawAngle * otherPitchAngle * otherRollAngle;
Eigen::Matrix3d otherRotationMatrix = q1.matrix();
Eigen::Matrix3d relativeRotation = rotationMatrix.inverse() * otherRotationMatrix;
Eigen::Vector3d new_z = relativeRotation * Eigen::Vector3d::UnitZ();
// std::cout << "New_z: " << std::endl << new_z << std::endl << std::endl;
//-- Get connector base vector for the rotations:
Eigen::Vector3d base_vector;
if ( connector == 0 || connector == 2)
{
//-- Y axis
base_vector = Eigen::Vector3d::UnitY();
}
else if ( connector == 1 || connector == 3)
{
//-- X axis
base_vector = Eigen::Vector3d::UnitX();
}
//-- Try for rotations to fit the vector
for ( int i = 0; i < 4; i++)
{
Eigen::AngleAxisd rotAngle( deg2rad(i*90), base_vector);
Eigen::Matrix3d rotMatrix = rotAngle.matrix();
Eigen::Vector3d result_vector = rotMatrix * Eigen::Vector3d::UnitZ();
// std::cout << "i = " << i << std::endl << result_vector << std::endl << std::endl;
if ( vector_equal(result_vector, new_z) )
return i;
}
return -1;
}
int
main (int argc, char** argv)
{
// Get input object and scene
if (argc < 2)
{
pcl::console::print_error ("Syntax is: %s cloud1.pcd (cloud2.pcd)\n", argv[0]);
return (1);
}
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZRGBA>);
// Load object and scene
pcl::console::print_highlight ("Loading point clouds...\n");
if(argc<3)
{
if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[1], *cloud_in) < 0)
pcl::console::print_error ("Error loading first file!\n");
*cloud_out = *cloud_in;
//transform cloud
Eigen::Affine3f transformation;
transformation.setIdentity();
transformation.translate(Eigen::Vector3f(0.3,0.02,-0.1));
float roll, pitch, yaw;
roll = 0.02; pitch = 1.2; yaw = 0;
Eigen::AngleAxisf rollAngle(roll, Eigen::Vector3f::UnitX());
Eigen::AngleAxisf pitchAngle(pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf yawAngle(yaw, Eigen::Vector3f::UnitZ());
Eigen::Quaternion<float> q = rollAngle*pitchAngle*yawAngle;
transformation.rotate(q);
pcl::transformPointCloud<pcl::PointXYZRGBA>(*cloud_in, *cloud_out, transformation);
std::cout << "Transformed " << cloud_in->points.size () << " data points:"
<< std::endl;
}else{
if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[1], *cloud_in) < 0 ||
pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[2], *cloud_out) < 0)
{
pcl::console::print_error ("Error loading files!\n");
return (1);
}
}
// Fill in the CloudIn data
// cloud_in->width = 100;
// cloud_in->height = 1;
// cloud_in->is_dense = false;
// cloud_in->points.resize (cloud_in->width * cloud_in->height);
// for (size_t i = 0; i < cloud_in->points.size (); ++i)
// {
// cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
// cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
// cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
// }
std::cout << "size:" << cloud_out->points.size() << std::endl;
{
pcl::ScopeTime("icp proces");
pcl::IterativeClosestPoint<pcl::PointXYZRGBA, pcl::PointXYZRGBA> icp;
icp.setInputSource(cloud_in);
icp.setInputTarget(cloud_out);
pcl::PointCloud<pcl::PointXYZRGBA> Final;
icp.setMaximumIterations(1000000);
icp.setRANSACOutlierRejectionThreshold(0.01);
icp.align(Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
//translation, rotation
Eigen::Matrix4f icp_transformation=icp.getFinalTransformation();
Eigen::Matrix3f icp_rotation = icp_transformation.block<3,3>(0,0);
Eigen::Vector3f euler = icp_rotation.eulerAngles(0,1,2);
std::cout << "rotation: " << euler.transpose() << std::endl;
std::cout << "translation:" << icp_transformation.block<3,1>(0,3).transpose() << std::endl;
}
return (0);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "lesson_move_group");
// start a background "spinner", so our node can process ROS messages
// - this lets us know when the move is completed
ros::AsyncSpinner spinner(1);
spinner.start();
moveit::planning_interface::MoveGroup group("manipulator");
//group.setPlannerId("ompl_planning/plan_kinematic_path");
//metto il robot in home
//group.setNamedTarget("home");
//group.setNamedTarget("straight_up");
group.move();
//---------------------------------
std::vector< double > Angles=group.getCurrentRPY();
Eigen::AngleAxisd rollAngle(Angles[0], Eigen::Vector3d::UnitZ());
Eigen::AngleAxisd yawAngle(Angles[1], Eigen::Vector3d::UnitY());
Eigen::AngleAxisd pitchAngle(Angles[2], Eigen::Vector3d::UnitX());
tf::Quaternion qt;
qt=tf::createQuaternionFromRPY ( Angles[0], Angles[1], Angles[2]);
//abbasso il robot in posizion operativa
robot_state::RobotState start_state(*group.getCurrentState());
moveit::core::RobotStatePtr kinematic_state;
moveit_msgs::RobotTrajectory trajectory;
std::vector<geometry_msgs::Pose> waypoints;
geometry_msgs::Pose target_pose3 = group.getCurrentPose().pose;
waypoints.push_back(target_pose3);
//target_pose3.position.x += 0.2;
target_pose3.position.z -= 0.5;
waypoints.push_back(target_pose3);
//waypoints.push_back(poseNew);
//double fraction = group.computeCartesianPath(waypoints,CART_STEP_SIZE_,CART_JUMP_THRESH_,trajectory_,AVOID_COLLISIONS_);
double fraction = group.computeCartesianPath(waypoints,
0.01, // eef_step
0.0, // jump_threshold
trajectory);
kinematic_state = moveit::core::RobotStatePtr(group.getCurrentState());
robot_trajectory::RobotTrajectory rt(kinematic_state->getRobotModel(), "manipulator");
rt.setRobotTrajectoryMsg(*kinematic_state, trajectory);
// check to see if plan successful
move_group_interface::MoveGroup::Plan plan;
plan.trajectory_=trajectory;
group.execute(plan);
//group.move(plan) ;
sleep(1.5);
Angles=group.getCurrentRPY();
waypoints.clear();
target_pose3 = group.getCurrentPose().pose;
waypoints.push_back(target_pose3);
target_pose3.position.y += 0.5;
waypoints.push_back(target_pose3);
fraction = group.computeCartesianPath(waypoints,
0.01, // eef_step
0.0, // jump_threshold
trajectory);
kinematic_state = moveit::core::RobotStatePtr(group.getCurrentState());
//rt(kinematic_state->getRobotModel(), "manipulator");
rt.setRobotTrajectoryMsg(*kinematic_state, trajectory);
plan.trajectory_=trajectory;
//group.execute(plan);
sleep(2.0);
Angles=group.getCurrentRPY();
qt=tf::createQuaternionFromRPY ( Angles[0], Angles[1], Angles[2]);
waypoints.clear();
target_pose3 = group.getCurrentPose().pose;
waypoints.push_back(target_pose3);
tf::Transform point_pos( tf::Transform(tf::createQuaternionFromRPY(Angles[0],Angles[1],Angles[2]-0.5),tf::Vector3(target_pose3.position.x,target_pose3.position.y,target_pose3.position.z)));
tf::poseTFToMsg (point_pos,target_pose3);
waypoints.push_back(target_pose3);
fraction = group.computeCartesianPath(waypoints,
0.01, // eef_step
0.0, // jump_threshold
trajectory);
kinematic_state = moveit::core::RobotStatePtr(group.getCurrentState());
//rt(kinematic_state->getRobotModel(), "manipulator");
rt.setRobotTrajectoryMsg(*kinematic_state, trajectory);
plan.trajectory_=trajectory;
group.execute(plan);
sleep(2.0);
//ruoto il robot rispetto alla z della base
group.setNamedTarget("home");
group.move();
}