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;
}
Exemple #3
0
void ActorSceneCanvas::OnKeyDown(wxKeyEvent& e)
{
	switch(e.GetKeyCode())
	{
	case 'w':
	case 'W':
		{
			Ogre::Camera* camera = GetSceneManipulator()->getCamera();
			assert (camera);

			Ogre::Vector3 oldPos = camera->getPosition();

			Ogre::Vector3 offsetPos = camera->getDirection() * GetSceneManipulator()->getMoveSpeed();

			Ogre::Vector3 newPos = oldPos;
            newPos.x += offsetPos.x;
            newPos.z += offsetPos.z;

			GetSceneManipulator()->setCameraPosition(newPos);
			e.Skip(false);
		}
		break;
	case 's':
	case 'S':
		{
			Ogre::Camera* camera = GetSceneManipulator()->getCamera();
			assert (camera);

			Ogre::Vector3 oldPos = camera->getPosition();

			Ogre::Vector3 offsetPos = camera->getDirection() * -(GetSceneManipulator()->getMoveSpeed());

			Ogre::Vector3 newPos = oldPos;
            newPos.x += offsetPos.x;
            newPos.z += offsetPos.z;

			GetSceneManipulator()->setCameraPosition(newPos);
			e.Skip(false);
		}
		break;
	case 'a':
	case 'A':
		{
			Ogre::Camera* camera = GetSceneManipulator()->getCamera();
			assert (camera);

			Ogre::Radian yawAngle( GetSceneManipulator()->getRotateSpeed() / 360.0f );

			camera->yaw(yawAngle);
			e.Skip(false);
		}
		break;
	case 'd':
	case 'D':
		{
			Ogre::Camera* camera = GetSceneManipulator()->getCamera();
			assert (camera);

			Ogre::Radian yawAngle( -(GetSceneManipulator()->getRotateSpeed() / 360.0f) );

			camera->yaw(yawAngle);
			e.Skip(false);
		}
		break;
	case 'h':
	case 'H':
		{
			mHideAxis = !mHideAxis;
			Fairy::CDataManipulator::m_baseNode->setVisible(!mHideAxis);
			Fairy::CDataManipulator::mAxisNode_x->setVisible(!mHideAxis);
			Fairy::CDataManipulator::mAxisNode_y->setVisible(!mHideAxis);
			Fairy::CDataManipulator::mAxisNode_z->setVisible(!mHideAxis);
		}
		break;
 	case 'b':
	case 'B':
		{
			GetDataManipulator()->switchBoundingBox(true);
		}
		break;
	case 't':
	case 'T':
		{
			GetSceneManipulator()->setTerrainVisible(!GetSceneManipulator()->getTerrainVisible());
		}
		break;
	case 'g':
	case 'G':
		{
			GetSceneManipulator()->setGridVisible(!GetSceneManipulator()->getGridVisible());
		}
		break;
	case 'r':
	case 'R':
		{
			mRotFirst = !mRotFirst;
		}
		break;
	//case 'C':
 //	case 'c':
 //		{
	//		Fairy::LogicModelManager::getSingleton().showModelBoundingBox(!Fairy::LogicModelManager::getSingleton().isShowBoundingBox()); 			
 //		}
 //		break;
// 
// 	case 'y':
// 	case 'Y':
// 		{
// 			mYax = !mYax;
// 			mXax = mZax = false;
// 		}
// 		break;
// 
// 	case 'z':
// 	case 'Z':
// 		{
// 			mZax = !mZax;
// 			mXax = mYax = false;
// 		}
		//break;
	//case 'z':
	//case 'Z':
 //       {
	//		GetSceneManipulator()->setActiveAction("ManipObjectAction");
 //           GetSceneManipulator()->setUseAxis(!GetSceneManipulator()->getUseAxis());
 //       }
 //       e.Skip(false);
 //       break;
	}
}
Exemple #4
0
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();

}