C++ kdl::Rotation类代码示例

int ComauSmartSix::ik(float x, float y,float z,float qx, float qy,float qz, float qw, float* q_in,float* q_out) {

    KDL::Rotation rot = KDL::Rotation::Quaternion(qx,qy,qz,qw);
    double roll,pitch,yaw;
    rot.GetRPY(roll,pitch,yaw);
    return this->ik(x,y,z,roll,pitch,yaw,q_in,q_out,true);
}

void goal_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
  if(msg->header.frame_id.compare(frame_id_) != 0)
  {
    ROS_WARN("frame_id of goal did not match expected '%s'. Ignoring goal", 
        frame_id_.c_str());
    return;
  }

  // copying over goal
  state_[joint_names_.size() + 0] = msg->pose.position.x;
  state_[joint_names_.size() + 1] = msg->pose.position.y;
  state_[joint_names_.size() + 2] = msg->pose.position.z;

  KDL::Rotation rot;
  tf::quaternionMsgToKDL(msg->pose.orientation, rot);
  rot.GetEulerZYX(state_[joint_names_.size() + 3], state_[joint_names_.size() + 4], 
      state_[joint_names_.size() + 5]);

  if (!controller_started_)
  {
    if (controller_.start(state_, nWSR_))
    {
      ROS_INFO("Controller started.");
      controller_started_ = true;
    }
    else
    {
      ROS_ERROR("Couldn't start controller.");
      print_eigen(state_);
    }
  }
}

 void Controller::calcPoseDiff(){
   // Calculate the position difference expressed in the world frame
   KDL::Vector diff_world(m_goal_pose.x - m_current_pose[0], m_goal_pose.y - m_current_pose[1], 0.0);
   // Calculate the YouBot-to-world orientation matrix, based on the angle
   // estimation.
   KDL::Rotation rot = KDL::Rotation::RotZ(m_current_pose[2]);
   // Express the position difference in the YouBot frame
   m_delta_pose = rot.Inverse(diff_world);
   m_delta_pose[2] = m_goal_pose.theta - m_current_pose[2];
 }

Eigen::Quaterniond UpperBodyPlanner::Rmat2Quaternion(const Eigen::Matrix3d& inMat_) {
    Eigen::Quaterniond outQ;
    KDL::Rotation convert;
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            convert.data[3 * i + j] = inMat_(i,j);
        }
    }
    convert.GetQuaternion(outQ.x(), outQ.y(), outQ.z(), outQ.w());
    return outQ;
}

void DetectorFilter::composeTransform(const MatrixWrapper::ColumnVector& vector,
                                      geometry_msgs::PoseWithCovarianceStamped& pose)
{
  assert(vector.rows() == 6);

  // construct kdl rotation from vector (x, y, z, Rx, Ry, Rz), and extract quaternion
  KDL::Rotation rot = KDL::Rotation::RPY(vector(4), vector(5), vector(6));
  rot.GetQuaternion(pose.pose.pose.orientation.x, pose.pose.pose.orientation.y,
                    pose.pose.pose.orientation.z, pose.pose.pose.orientation.w);

  pose.pose.pose.position.x = vector(1);
  pose.pose.pose.position.y = vector(2);
  pose.pose.pose.position.z = vector(3);
};

controllerErrorCode ArmController::setHandCartesianPosLinear(double x, double y, double z, double roll, double pitch, double yaw, double timestep, double stepSize)
{

  double pos;
  //Set up controller
  setMode(CONTROLLER_POSITION);
  this->timeStep = timestep;
  this->stepSize = stepSize;

  //Calculate end position
  KDL::Vector endPos(x,y,z);
  KDL::Rotation endRot;
  endRot = endRot.RPY(roll,pitch,yaw);
  endFrame.p = endPos;
  endFrame.M = endRot;  

  //Get current location
  KDL::JntArray q = KDL::JntArray(pr2_kin.nJnts);

  //Get joint positions
  for(int i=0;i<ARM_MAX_JOINTS;i++){   
    armJointControllers[i].getPosAct(&pos);
    q(i) = pos;
  }

  KDL::Frame f;
  pr2_kin.FK(q,f);

  startPosition = f.p; //Record Starting location
  KDL::Vector move = endPos-startPosition; //Vector to ending position

  double dist = move.Norm(); //Distance to move
  moveDirection = move/dist; //Normalize movement vector
    
  rotInterpolator.SetStartEnd(f.M, endRot);
  double total_angle = rotInterpolator.Angle();

  nSteps = (int)(dist/stepSize);
  if(nSteps==0){
    std::cout<<"ArmController.cpp: Error:: number of steps calculated to be 0"<<std::endl;
    return CONTROLLER_COMPUTATION_ERROR;
  } 
  angleStep = total_angle/nSteps;
  lastTime= getTime(); //Record first time marker
  stepIndex = 0; //Reset step index

  linearInterpolation = true;
  return CONTROLLER_ALL_OK;
}

void DetectorFilter::decomposeTransform(const geometry_msgs::PoseWithCovarianceStamped& pose,
                                        MatrixWrapper::ColumnVector& vector)
{
  assert(vector.rows() == 6);

  // construct kdl rotation from quaternion, and extract RPY
  KDL::Rotation rot = KDL::Rotation::Quaternion(pose.pose.pose.orientation.x,
                                                pose.pose.pose.orientation.y,
                                                pose.pose.pose.orientation.z,
                                                pose.pose.pose.orientation.w);
  rot.GetRPY(vector(4), vector(5), vector(6));

  vector(1) = pose.pose.pose.position.x;
  vector(2) = pose.pose.pose.position.y;
  vector(3) = pose.pose.pose.position.z;
};

  void YouBotBaseService::update()
  {
    if(is_in_visualization_mode)
    {
      in_odometry_state.read(m_odometry_state);

      simxFloat pos[3];
      pos[0] = m_odometry_state.pose.pose.position.x;
      pos[1] = m_odometry_state.pose.pose.position.y;
      pos[2] = m_odometry_state.pose.pose.position.z;

      simxSetObjectPosition(clientID,all_robot_handle,-1,pos,simx_opmode_oneshot);

      double euler[3];
      simxFloat euler_s[3];

      //different coordinate systems, fixing it here
      KDL::Rotation orientation  = KDL::Rotation::Quaternion(
                          m_odometry_state.pose.pose.orientation.x,
                          m_odometry_state.pose.pose.orientation.y,
                          m_odometry_state.pose.pose.orientation.z,
                          m_odometry_state.pose.pose.orientation.w);

      // Instead of transforming for the inverse of this rotation,
      // we should find the right transform. (this is legacy code that
      // transforms vrep coords to rviz coords)
      KDL::Rotation rotation = KDL::Rotation::RPY(0,-M_PI/2,M_PI).Inverse();
      
      orientation = orientation * rotation;
      orientation.GetRPY(euler[0],euler[1],euler[2]); 

      euler_s[0] = euler[0];
      euler_s[1] = euler[1];
      euler_s[2] = euler[2];

      simxSetObjectOrientation(clientID,all_robot_handle,-1,euler_s,simx_opmode_oneshot);

      simxSetJointPosition(clientID,vrep_joint_handle[0],m_joint_state.position[0], simx_opmode_oneshot);
      simxSetJointPosition(clientID,vrep_joint_handle[1],m_joint_state.position[1], simx_opmode_oneshot);
      simxSetJointPosition(clientID,vrep_joint_handle[2],m_joint_state.position[2], simx_opmode_oneshot);
      simxSetJointPosition(clientID,vrep_joint_handle[3],m_joint_state.position[3], simx_opmode_oneshot);
      return;
    }

      Hilas::IRobotBaseService::update();
  }

TEST(TFKDLConversions, tf_kdl_rotation)
{
  tf::Quaternion t;
  t[0] = gen_rand(-1.0,1.0);
  t[1] = gen_rand(-1.0,1.0);
  t[2] = gen_rand(-1.0,1.0);
  t[3] = gen_rand(-1.0,1.0);
  t.normalize();

  KDL::Rotation k;
  RotationTFToKDL(t,k);

  double x,y,z,w;
  k.GetQuaternion(x,y,z,w);

  ASSERT_NEAR(fabs(t[0]),fabs(x),1e-6);
  ASSERT_NEAR(fabs(t[1]),fabs(y),1e-6);
  ASSERT_NEAR(fabs(t[2]),fabs(z),1e-6);
  ASSERT_NEAR(fabs(t[3]),fabs(w),1e-6);
  ASSERT_NEAR(t[0]*t[1]*t[2]*t[3],x*y*z*w,1e-6);

  
}

controllerErrorCode
ArmController::setHandCartesianPos(double x, double y, double z, double roll, double pitch, double yaw)
{	
  linearInterpolation = false;
  
  //Define position and rotation
  KDL::Vector position(x,y,z);
  KDL::Rotation rotation;
  rotation = rotation.RPY(roll,pitch,yaw);
  
  cmdPos[0] = x;
  cmdPos[1] = y;
  cmdPos[2] = z;
  cmdPos[3] = roll;
  cmdPos[4] = pitch;
  cmdPos[5] = yaw;

  //Create frame based on position and rotation
  KDL::Frame f;
  f.p = position;
  f.M = rotation;
    
  return commandCartesianPos(f);
 }

void yarp_IMU_interface::sense(KDL::Rotation &orientation,
                               KDL::Vector &linearAcceleration,
                               KDL::Vector &angularVelocity)
{
    yarp::sig::Vector yOrientation;
    yarp::sig::Vector yLinearAcceleration;
    yarp::sig::Vector yAngularVelocity;

    this->sense(yOrientation,
                yLinearAcceleration,
                yAngularVelocity);
    orientation.Identity();
    orientation = KDL::Rotation::RPY(yOrientation(0),
                                     yOrientation(1),
                                     yOrientation(2));
    YarptoKDL(yLinearAcceleration, linearAcceleration);
    YarptoKDL(yAngularVelocity, angularVelocity);
}

int main(int argc, char **argv)
{

        float x=0.0f;
        float y=0.0f;
        float z=0.0f;
        float roll=0.0f;
        float pitch=0.0f;
        float yaw=0.0f;

        ros::init(argc, argv, "dan_tester");

        ros::NodeHandle n;

        ros::Subscriber sub = n.subscribe( "lar_comau/comau_joint_states",1,state_callback );
        ros::Publisher joint_state_pub = n.advertise<sensor_msgs::JointState>("lar_comau/comau_joint_state_publisher", 1);
        ros::Publisher cartesian_controller = n.advertise<lar_comau::ComauCommand>("lar_comau/comau_cartesian_controller", 1);

        ros::Rate loop_rate(10);

        //robot
        std::string robot_desc_string;
        n.param("robot_description", robot_desc_string, std::string());
        lar_comau::ComauSmartSix robot(robot_desc_string,"base_link", "link6");


        //ROS_INFO("\n\n%s\n\n",argv[1]);
        initPoses();
        int count = 0;
        while (ros::ok())
        {


                sensor_msgs::JointState msg;
                geometry_msgs::Pose pose;


                //std::stringstream ss;
                //ss << "hello world " << count;
                //msg.data = ss.str();
                std::string command;
                int command_index;


                cout << "Please enter pose value: ";
                cin >> command_index;
                if(command_index<my_poses.size() && command_index>=0) {
                        std::cout << "OK command "<<command_index<<std::endl;
                        MyPose mypose = my_poses[command_index];
                        std::cout << mypose.x << "," <<mypose.y<<std::endl;
                        pose.position.x = mypose.x;
                        pose.position.y = mypose.y;
                        pose.position.z = mypose.z;

                        KDL::Rotation rot = KDL::Rotation::RPY(
                                mypose.roll*M_PI/180.0f,
                                mypose.pitch*M_PI/180.0f,
                                mypose.yaw*M_PI/180.0f
                                );

                        double qx,qy,qz,qw;
                        rot.GetQuaternion(qx,qy,qz,qw);
                        pose.orientation.x = qx;
                        pose.orientation.y = qy;
                        pose.orientation.z = qz;
                        pose.orientation.w = qw;

                        lar_comau::ComauCommand comau_command;
                        comau_command.pose = pose;
                        comau_command.command = "joint";

                        cartesian_controller.publish(comau_command);
                        ros::spinOnce();
                        std::cout << "Published "<<comau_command<<std::endl;
                }





        }


        return 0;
}

geometry_msgs::Quaternion conversions::KDLRotToQuaternionMsg(const KDL::Rotation & in){
	geometry_msgs::Quaternion out;
	in.GetQuaternion(out.x, out.y, out.z, out.w);
	return out;
}

    bool CartesianGenerator::generateNewVelocityProfiles(RTT::base::PortInterface* portInterface){
    	

    	m_time_passed = os::TimeService::Instance()->secondsSince(m_time_begin);
    	
    	geometry_msgs::Pose pose;
    	cmdCartPose.read(pose);
#if 1
    	std::cout << "A new pose arrived" << std::endl;
    	std::cout << "position: " << pose.position.x <<  " " << pose.position.y << " " << pose.position.z  << std::endl;
#endif


		m_traject_end.p.x(pose.position.x);
		m_traject_end.p.y(pose.position.y);
		m_traject_end.p.z(pose.position.z);
		m_traject_end.M=Rotation::Quaternion(pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w);

		// get current position
		geometry_msgs::Pose pose_meas;

		m_position_meas_port.read(pose_meas);
		m_traject_begin.p.x(pose_meas.position.x);
		m_traject_begin.p.y(pose_meas.position.y);
		m_traject_begin.p.z(pose_meas.position.z);
		m_traject_begin.M=Rotation::Quaternion(pose_meas.orientation.x,pose_meas.orientation.y,pose_meas.orientation.z,pose_meas.orientation.w);

		KDL::Rotation errorRotation = (m_traject_end.M)*(m_traject_begin.M.Inverse());

//		KDL::Rotation tmp=errorRotation;
//		double q1, q2, q3, q4;
//		cout << "tmp" << endl;
//		cout << tmp(0,0) << ", " << tmp(0,1) << ", " << tmp(0,2) << endl;
//		cout << tmp(1,0) << ", " << tmp(1,1) << ", " << tmp(1,2) << endl;
//		cout << tmp(2,0) << ", " << tmp(2,1) << ", " << tmp(2,2) << endl;
//		cout << endl;
//		tmp.GetQuaternion(q1,q2,q3,q4);
//		cout << "tmp quaternion: " << q1 << q2 << q3 << q4 << endl;

		double x,y,z,w;
		errorRotation.GetQuaternion(x,y,z,w);

		Eigen::AngleAxis<double> aa;
		aa = Eigen::Quaterniond(w,x,y,z);
		currentRotationalAxis = aa.axis();
		deltaTheta = aa.angle();

//		std::cout << "----EIGEN---------" << std::endl << "currentRotationalAxis: "  << std::endl << currentRotationalAxis << std::endl;
//		std::cout << "deltaTheta" << deltaTheta << std::endl;

//		currentRotationalAxis[0]=x;
//		currentRotationalAxis[1]=y;
//		currentRotationalAxis[2]=z;
//		if(currentRotationalAxis.norm() > 0.001){
//			currentRotationalAxis.normalize();
//			deltaTheta = 2*acos(w);
//		}else{
//			currentRotationalAxis.setZero();
//			deltaTheta = 0.0;
//		}

//		std::cout << "----KDL-----------" << std::endl << "currentRotationalAxis: "  << std::endl << currentRotationalAxis << std::endl;
//		std::cout << "deltaTheta" << deltaTheta << std::endl;

		std::vector<double> cartPositionCmd = std::vector<double>(3,0.0);
		cartPositionCmd[0] = pose.position.x;
		cartPositionCmd[1] = pose.position.y;
		cartPositionCmd[2] = pose.position.z;

		std::vector<double> cartPositionMsr = std::vector<double>(3,0.0);
		//the following 3 assignments will get over written except for the first time
		cartPositionMsr[0] = pose_meas.position.x;
		cartPositionMsr[1] = pose_meas.position.y;
		cartPositionMsr[2] = pose_meas.position.z;

		std::vector<double> cartVelocity = std::vector<double>(3,0.0);

		if ((int)motionProfile.size() == 0){//Only for the first run
			for(int i = 0; i < 3; i++)
			{
				cartVelocity[i] = 0.0;
			}
		}else{
			for(int i = 0; i < 3; i++)
			{
				cartVelocity[i] = motionProfile[i].Vel(m_time_passed);
				cartPositionMsr[i] = motionProfile[i].Pos(m_time_passed);
			}
		}

		motionProfile.clear();

		// Set motion profiles
		for(int i = 0; i < 3; i++){
			motionProfile.push_back(VelocityProfile_NonZeroInit(m_maximum_velocity[i], m_maximum_acceleration[i]));
			motionProfile[i].SetProfile(cartPositionMsr[i], cartPositionCmd[i], cartVelocity[i]);
		}
		//motionProfile for theta
		motionProfile.push_back(VelocityProfile_NonZeroInit(m_maximum_velocity[3], m_maximum_acceleration[3]));
		motionProfile[3].SetProfile(0.0,deltaTheta,0.0);
//.........这里部分代码省略.........

 void quaternionKDLToTF(const KDL::Rotation &k, tf::Quaternion &t)
 {
   double x, y, z, w;
   k.GetQuaternion(x, y, z, w);
   t = tf::Quaternion(x, y, z, w);
 }