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

void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
{
	PointCloud cloud;
	pcl::fromROSMsg(*msg, cloud);

	g_cloud_frame = cloud.header.frame_id;
	g_cloud_ready = true;

	if(!g_head_depth_ready) return;

	Mat img3D;
	img3D = Mat::zeros(cloud.height, cloud.width, CV_32FC3);
	//img3D.create(cloud.height, cloud.width, CV_32FC3);
	
	int yMin, xMin, yMax, xMax;
	yMin = 0; xMin = 0;
	yMax = img3D.rows; xMax = img3D.cols;

	//get 3D from depth
	for(int y = yMin ; y < img3D.rows; y++) {
		Vec3f* img3Di = img3D.ptr<Vec3f>(y);
	
		for(int x = xMin; x < img3D.cols; x++) {
			pcl::PointXYZ p = cloud.at(x,y);
			if((p.z>g_head_depth-0.2) && (p.z<g_head_depth+0.2)) {
				img3Di[x][0] = p.x*1000;
				img3Di[x][1] = p.y*1000;
				img3Di[x][2] = hypot(img3Di[x][0], p.z*1000); //they seem to have trained with incorrectly projected 3D points
				//img3Di[x][2] = p.z*1000;
			} else {
				img3Di[x] = 0;
			}
		}
	}
	
	g_means.clear();
	g_votes.clear();
	g_clusters.clear();
	
	//do the actual estimate
	estimator.estimate( 	img3D,
							g_means,
							g_clusters,
							g_votes,
							g_stride,
							g_maxv,
							g_prob_th,
							g_larger_radius_ratio,
							g_smaller_radius_ratio,
							false,
							g_th
						);
	
	//assuming there's only one head in the image!
	if(g_means.size() > 0) {	
		geometry_msgs::PoseStamped pose_msg;
		pose_msg.header.frame_id = msg->header.frame_id;
	
		cv::Vec<float,POSE_SIZE> pose(g_means[0]);
	
		KDL::Rotation r = KDL::Rotation::RPY(
											 from_degrees( pose[5]+180+g_roll_bias ), 
											 from_degrees(-pose[3]+180+g_pitch_bias),
											 from_degrees(-pose[4]+    g_yaw_bias  )
											);
		double qx, qy, qz, qw;
		r.GetQuaternion(qx, qy, qz, qw);
	
		geometry_msgs::PointStamped head_point;
		geometry_msgs::PointStamped head_point_transformed;
	
		head_point.header = pose_msg.header;

		head_point.point.x = pose[0]/1000;
		head_point.point.y = pose[1]/1000;
		head_point.point.z = pose[2]/1000;
	
		try {
			listener->waitForTransform(head_point.header.frame_id, g_head_target_frame, ros::Time::now(), ros::Duration(2.0));
			listener->transformPoint(g_head_target_frame, head_point, head_point_transformed);
		} catch(tf::TransformException ex) {
			ROS_WARN(
				"Transform exception, when transforming point from %s to %s\ndropping pose (don't worry, this is probably ok)",
				head_point.header.frame_id.c_str(), g_head_target_frame.c_str());
				ROS_WARN("Exception was %s", ex.what());
			return;
		}
		
		tf::Transform trans;
		
		pose_msg.pose.position = head_point_transformed.point;
		pose_msg.header.frame_id = head_point_transformed.header.frame_id;
	
		pose_msg.pose.orientation.x = qx;
		pose_msg.pose.orientation.y = qy;
		pose_msg.pose.orientation.z = qz;
		pose_msg.pose.orientation.w = qw;

		trans.setOrigin(tf::Vector3(
			pose_msg.pose.position.x, 
//.........这里部分代码省略.........

void UrdfModelMarker::getJointState(boost::shared_ptr<const Link> link)
{
  string link_frame_name_ =  tf_prefix_ + link->name;
  boost::shared_ptr<Joint> parent_joint = link->parent_joint;
  if(parent_joint != NULL){
    KDL::Frame initialFrame;
    KDL::Frame presentFrame;
    KDL::Rotation rot;
    KDL::Vector rotVec;
    KDL::Vector jointVec;
    double jointAngle;
    double jointAngleAllRange;
    switch(parent_joint->type){
    case Joint::REVOLUTE:
    case Joint::CONTINUOUS:
      {
	linkProperty *link_property = &linkMarkerMap[link_frame_name_];
	tf::poseMsgToKDL (link_property->initial_pose, initialFrame);
	tf::poseMsgToKDL (link_property->pose, presentFrame);
	rot = initialFrame.M.Inverse() * presentFrame.M;
	jointAngle = rot.GetRotAngle(rotVec);
	jointVec = KDL::Vector(link_property->joint_axis.x,
			       link_property->joint_axis.y,
			       link_property->joint_axis.z);
	if( KDL::dot(rotVec,jointVec) < 0){
	  jointAngle = - jointAngle;
	}
	if(link_property->joint_angle > M_PI/2 && jointAngle < -M_PI/2){
	  link_property->rotation_count += 1;
	}else if(link_property->joint_angle < -M_PI/2 && jointAngle > M_PI/2){
	  link_property->rotation_count -= 1;
	}
	link_property->joint_angle = jointAngle;
	jointAngleAllRange = jointAngle + link_property->rotation_count * M_PI * 2;

	if(parent_joint->type == Joint::REVOLUTE && parent_joint->limits != NULL){
	  bool changeMarkerAngle = false;
	  if(jointAngleAllRange < parent_joint->limits->lower){
	    jointAngleAllRange = parent_joint->limits->lower + 0.001;
	    changeMarkerAngle = true;
	  }
	  if(jointAngleAllRange > parent_joint->limits->upper){
	    jointAngleAllRange = parent_joint->limits->upper - 0.001;
	    changeMarkerAngle = true;
	  }

	  if(changeMarkerAngle){
	    setJointAngle(link, jointAngleAllRange);
	  }
	}

	joint_state_.position.push_back(jointAngleAllRange);
	joint_state_.name.push_back(parent_joint->name);
	break;
      }
    case Joint::PRISMATIC:
      {
	KDL::Vector pos;
	linkProperty *link_property = &linkMarkerMap[link_frame_name_];
	tf::poseMsgToKDL (link_property->initial_pose, initialFrame);
	tf::poseMsgToKDL (link_property->pose, presentFrame);

	pos = presentFrame.p - initialFrame.p;

	jointVec = KDL::Vector(link_property->joint_axis.x,
			       link_property->joint_axis.y,
			       link_property->joint_axis.z);
	jointVec = jointVec / jointVec.Norm(); // normalize vector
	jointAngle = KDL::dot(jointVec, pos);

	link_property->joint_angle = jointAngle;
	jointAngleAllRange = jointAngle;

	if(parent_joint->type == Joint::PRISMATIC && parent_joint->limits != NULL){
	  bool changeMarkerAngle = false;
	  if(jointAngleAllRange < parent_joint->limits->lower){
	    jointAngleAllRange = parent_joint->limits->lower + 0.003;
	    changeMarkerAngle = true;
	  }
	  if(jointAngleAllRange > parent_joint->limits->upper){
	    jointAngleAllRange = parent_joint->limits->upper - 0.003;
	    changeMarkerAngle = true;
	  }
	  if(changeMarkerAngle){
	    setJointAngle(link, jointAngleAllRange);
	  }
	}

	joint_state_.position.push_back(jointAngleAllRange);
	joint_state_.name.push_back(parent_joint->name);
	break;
      }
    case Joint::FIXED:
      break;
    default:
      break;
    }
    server_->applyChanges();
  }

//.........这里部分代码省略.........

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

	m_time_passed = os::TimeService::Instance()->secondsSince(m_time_begin);

	geometry_msgs::Pose pose;
	cmdCartPose.read(pose);
	desired_pose = 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);

	xi = pose_meas.position.x;
	yi = pose_meas.position.y;
	zi = pose_meas.position.z;
	xf = pose.position.x;
	yf = pose.position.y;
	zf = pose.position.z;

	TrajVectorMagnitude = sqrt((xf - xi) * (xf - xi) + (yf - yi) * (yf - yi) + (zf - zi) * (zf - zi));
	TrajVectorDirection.x = (xf - xi) / TrajVectorMagnitude;
	TrajVectorDirection.y = (yf - yi) / TrajVectorMagnitude;
	TrajVectorDirection.z = (zf - zi) / TrajVectorMagnitude;

	double tx, ty, tz;
	tx = abs(xf - xi) / m_maximum_velocity[0];
	ty = abs(yf - yi) / m_maximum_velocity[0];
	tz = abs(zf - zi) / m_maximum_velocity[0];

	t_sync = TrajVectorMagnitude / m_maximum_velocity[0];

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

	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();
	theta_vel = deltaTheta / t_sync;

	cout << "[CG::GenerateProfiles]:" << endl;

	m_time_begin = os::TimeService::Instance()->getTicks();
	m_time_passed = 0;

	return true;
}

// construct rotation
urdf::Rotation toUrdf(const KDL::Rotation & r)
{
    double x,y,z,w;
    r.GetQuaternion(x,y,z,w);
    return urdf::Rotation(x,y,z,w);
}

bool publishMarkerArray(geometry_msgs::Pose axis, std::vector<geometry_msgs::Pose> trajectoyPoints, std::string refFrameName){
	visualization_msgs::MarkerArray tmpMarkerArray;

	KDL::Rotation axisR = KDL::Rotation::Quaternion(axis.orientation.x, axis.orientation.y,axis.orientation.z,axis.orientation.w);
	KDL::Rotation deltaR = KDL::Rotation::Quaternion(0.0,std::sin(-PI/4),0.0,std::cos(-PI/4));
	KDL::Rotation markerR = axisR*deltaR;

	visualization_msgs::Marker axisMarker;
	axisMarker.header.frame_id = refFrameName;
	axisMarker.header.stamp = ros::Time();
	axisMarker.ns = "articulationNS";
	axisMarker.id = 0;
	axisMarker.type = visualization_msgs::Marker::ARROW;
	axisMarker.action = visualization_msgs::Marker::ADD;
	axisMarker.scale.x = 2;
	axisMarker.scale.y = 2;
	axisMarker.scale.z = 2;
	axisMarker.color.a = 1.0;
	axisMarker.color.r = 0.0;
	axisMarker.color.g = 1.0;
	axisMarker.color.b = 0.0;

	//std::cout << "Before: "<<axisMarker.pose.orientation.x << " " << axisMarker.pose.orientation.y << " " << axisMarker.pose.orientation.z << " " << axisMarker.pose.orientation.w << std::endl;
	markerR.GetQuaternion(axisMarker.pose.orientation.x,axisMarker.pose.orientation.y,axisMarker.pose.orientation.z,axisMarker.pose.orientation.w);
	//std::cout << "After: "<<axisMarker.pose.orientation.x << " " << axisMarker.pose.orientation.y << " " << axisMarker.pose.orientation.z << " " << axisMarker.pose.orientation.w << std::endl;


	visualization_msgs::Marker axisText;
	axisText.header.frame_id = refFrameName;
	axisText.header.stamp = ros::Time();
	axisText.ns = "articulationNS";
	axisText.id = 1;
	axisText.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
	axisText.action = visualization_msgs::Marker::ADD;
	axisText.scale.x = 0.5;
	axisText.scale.y = 0.5;
	axisText.scale.z = 0.5;
	axisText.color.a = 1.0;
	axisText.color.r = 1.0;
	axisText.color.g = 1.0;
	axisText.color.b = 1.0;

	visualization_msgs::Marker trajPoint;
	trajPoint.header.frame_id = refFrameName;
	trajPoint.header.stamp = ros::Time();
	trajPoint.ns = "articulationNS";
	trajPoint.type = visualization_msgs::Marker::SPHERE;
	trajPoint.action = visualization_msgs::Marker::ADD;
	trajPoint.scale.x = .3;
	trajPoint.scale.y = .3;
	trajPoint.scale.z = .3;
	trajPoint.color.a = 1.0;
	trajPoint.color.r = 0.0;
	trajPoint.color.g = 1.0;
	trajPoint.color.b = 0.0;

	axisText.pose = axis;
	//std::stringstream s;
	//s << "(" << axisText.pose.position.x << ", " << axisText.pose.position.y << ")" ;
	axisText.text = "Axis of Rotation";

	tmpMarkerArray.markers.push_back(axisMarker);
	tmpMarkerArray.markers.push_back(axisText);

	for(int i=0; i<(int)trajectoyPoints.size(); i++){
		trajPoint.pose = trajectoyPoints[i];
		trajPoint.id = 2 + i;
		tmpMarkerArray.markers.push_back(trajPoint);
	}

	ROS_INFO("Publishing MarkerArray: Size=%d",(int)tmpMarkerArray.markers.size());
	vis_pub.publish( tmpMarkerArray );

	return true;
}