Esempio n. 1
0
  tf::Pose MotionModel::updateAction(tf::Pose& p)
    {
      double delta_rot1;
      if (dxy < 0.01)
        delta_rot1 = 0.0;
      else
        delta_rot1 = angle_diff(atan2(delta_y, delta_x), delta_yaw);
      double delta_trans = dxy;
      double delta_rot2 = angle_diff(delta_yaw, delta_rot1);

      double delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)), fabs(angle_diff(delta_rot1, M_PI)));
      double delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)), fabs(angle_diff(delta_rot2, M_PI)));

      double delta_rot1_hat = angle_diff(delta_rot1, sampleGaussian(alpha1 * delta_rot1_noise*delta_rot1_noise + 
                                                                    alpha2 * delta_trans*delta_trans));
      double delta_trans_hat = delta_trans - sampleGaussian(alpha3 * delta_trans*delta_trans +
                                                            alpha4 * delta_rot1_noise*delta_rot1_noise +
                                                            alpha4 * delta_rot2_noise*delta_rot2_noise);
      double delta_rot2_hat = angle_diff(delta_rot2, sampleGaussian(alpha1 * delta_rot2_noise*delta_rot2_noise +
                                                                    alpha2 * delta_trans*delta_trans));

      delta_x = delta_trans_hat * cos(delta_rot1_hat);
      delta_y = delta_trans_hat * sin(delta_rot1_hat);
      delta_yaw = delta_rot1_hat + delta_rot2_hat;

   		tf::Pose noisy_pose(tf::createQuaternionFromRPY(0,0,delta_yaw),tf::Vector3(delta_x,delta_y,0));
      tf::Transform base_to_global_ = tf::Transform(p.getRotation());
      noisy_pose.setOrigin(base_to_global_ * noisy_pose.getOrigin());
      p.setOrigin(p.getOrigin() + noisy_pose.getOrigin());
      p.setRotation(p.getRotation() * noisy_pose.getRotation());
    }
Esempio n. 2
0
int CRvizMarker::Send(tf::Pose p, std::string frame) {
    visualization_msgs::Marker marker;
    // Set the frame ID and timestamp.  See the TF tutorials for information on these.
    marker.header.frame_id = frame;
    marker.header.stamp = ros::Time::now();

    // Set the namespace and id for this marker.  This serves to create a unique ID
    // Any marker sent with the same namespace and id will overwrite the old one
    marker.ns = "nist_fanuc";
    marker.id = _id++;

    // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
    marker.type = shape;

    // Set the marker action.  Options are ADD and DELETE
    marker.action = visualization_msgs::Marker::ADD;

    // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
#if 0
    marker.pose.position.x = 0;
    marker.pose.position.y = 0;
    marker.pose.position.z = 0;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;
#endif
    marker.pose.position.x = p.getOrigin().x();
    marker.pose.position.y = p.getOrigin().y();
    marker.pose.position.z = p.getOrigin().z();
    marker.pose.orientation.x = p.getRotation().x();
    marker.pose.orientation.y = p.getRotation().y();
    marker.pose.orientation.z = p.getRotation().z();
    marker.pose.orientation.w = p.getRotation().w();

    // Set the scale of the marker -- 1x1x1 here means 1m on a side!!!
    marker.scale.x = scalex;
    marker.scale.y = scaley;
    marker.scale.z = scalez;

    // Set the color -- be sure to set alpha to something non-zero!
    marker.color.r = r;
    marker.color.g = g;
    marker.color.b = b;
    marker.color.a = a;

    marker.lifetime = ros::Duration(0.0);

    // Publish the marker
    marker_pub.publish(marker);
    ros::spinOnce();
    ros::spinOnce();

    return 0;
}
	// Go down until hit the table. For safety min_height is specified. If no table found until this height, returns false.
	// vertical_speed with which to move downwards
	// thr_force - normal force threshold at which table is assumed to be detected
	bool find_table_for_rolling(double min_height, double vertical_speed, double thr_force) {
		double rate = 200;
		thr_force = fabs(thr_force);
		ros::Rate thread_rate(rate);

		double startz = ee_pose.getOrigin().z();

		msg_pose.pose.position.x = ee_pose.getOrigin().x();
		msg_pose.pose.position.y = ee_pose.getOrigin().y();
		msg_pose.pose.position.z = startz;
		msg_pose.pose.orientation.x = ee_pose.getRotation().x();
		msg_pose.pose.orientation.y = ee_pose.getRotation().y();
		msg_pose.pose.orientation.z = ee_pose.getRotation().z();
		msg_pose.pose.orientation.w = ee_pose.getRotation().w();

		// Publish attractors if running in simulation or with fixed values
		if ((action_mode == ACTION_LASA_FIXED) || (action_mode == ACTION_BOXY_FIXED)) {
		    static tf::TransformBroadcaster br;
			tf::Transform  table;
		    table.setOrigin(tf::Vector3 (ee_pose.getOrigin().x(),ee_pose.getOrigin().y(),ee_pose.getOrigin().z() - min_height));
		    table.setRotation(tf::Quaternion (ee_pose.getRotation().x(),ee_pose.getRotation().y(),ee_pose.getRotation().z(),ee_pose.getRotation().w()));
		    br.sendTransform(tf::StampedTransform(table, ros::Time::now(), world_frame, "/attractor"));
		}

		ROS_INFO_STREAM("Finding table up to max dist. "<<min_height<<" with vertical speed "<<vertical_speed<<" and threshold force "<<thr_force<<"N.");
		while(ros::ok()) {
			msg_pose.pose.position.z = msg_pose.pose.position.z - vertical_speed/rate;
			pub_.publish(msg_pose);

			// Go down until force reaches the threshold
			if(fabs(ee_ft[2]) > thr_force) {
				break;
			}
			if(fabs(ee_pose.getOrigin().z()-startz) > min_height) {
				ROS_INFO("Max distance reached");
				return false;
			}
			thread_rate.sleep();
			feedback_.progress = ee_ft[2];
			as_.publishFeedback(feedback_);
		}
		if(!ros::ok()) {
			return false;
		}
		tf::Vector3 table(ee_pose.getOrigin());
		ROS_INFO_STREAM("Table found at height "<<table[2]);
		msg_pose.pose.position.z = table[2];

		pub_.publish(msg_pose);
		sendAndWaitForNormalForce(0);


		return true;
	}
geometry_msgs::Pose tfTransformToGeometryPose(const tf::Pose& goal_pose)
{
    geometry_msgs::Pose target_pose1;
    target_pose1.orientation.x = goal_pose.getRotation().getX();
    target_pose1.orientation.y = goal_pose.getRotation().getY();
    target_pose1.orientation.z = goal_pose.getRotation().getZ();
    target_pose1.orientation.w = goal_pose.getRotation().getW();
    target_pose1.position.x = goal_pose.getOrigin().getX(); // + std::sin(angle)*radius;
    target_pose1.position.y = goal_pose.getOrigin().getY(); // + std::cos(angle)*radius;
    target_pose1.position.z = goal_pose.getOrigin().getZ();
    return target_pose1;
}
Esempio n. 5
0
  void MotionModel::setMotion(const tf::Pose& pnew, const tf::Pose& pold)
    {
      tf::Pose delta_pose;
      tf::Transform odom_to_base(pold.inverse().getRotation());
      delta_pose.setOrigin(odom_to_base * (pnew.getOrigin() - pold.getOrigin()));
      delta_pose.setRotation(pnew.getRotation() * pold.getRotation().inverse());

      delta_x = delta_pose.getOrigin().x();
      delta_y = delta_pose.getOrigin().y();
      delta_yaw = atan2(sin(tf::getYaw(delta_pose.getRotation())), cos(tf::getYaw(delta_pose.getRotation())));

			dxy=sqrt(delta_x*delta_x+delta_y*delta_y);
    }
	// The famous sendPose!
	void sendPose(const tf::Pose& pose_) {
		geometry_msgs::PoseStamped msg;
		msg.pose.position.x = pose_.getOrigin().x();
		msg.pose.position.y = pose_.getOrigin().y();
		msg.pose.position.z = pose_.getOrigin().z();

		msg.pose.orientation.x = pose_.getRotation().x();
		msg.pose.orientation.y = pose_.getRotation().y();
		msg.pose.orientation.z = pose_.getRotation().z();
		msg.pose.orientation.w = pose_.getRotation().w();

		pub_.publish(msg);
	}
geometry_msgs::Pose tfPoseToGeometryPose(const tf::Pose tPose)
{
    geometry_msgs::Pose gPose;
    gPose.position.x = tPose.getOrigin().x();
    gPose.position.y = tPose.getOrigin().y();
    gPose.position.z = tPose.getOrigin().z();
    gPose.orientation.x = tPose.getRotation().x();
    gPose.orientation.y = tPose.getRotation().y();
    gPose.orientation.z = tPose.getRotation().z();
    gPose.orientation.w = tPose.getRotation().w();

    return gPose;
}
Esempio n. 8
0
/**
 * Convert tf::Pose to string
 */
template<> std::string toString<tf::Pose>(const tf::Pose& p_pose)
{
  std::stringstream ss;

  ss << "Pose(Quaternion=" << toString(p_pose.getRotation()) << "; Vector3=" << toString(p_pose.getOrigin()) << ")";

  return ss.str();
}
Esempio n. 9
0
	tf::Pose MotionModel::drawFromMotion(tf::Pose& p) 
		{
			double sxy=0.3*srr;
			delta_x+=sampleGaussian(srr*fabs(delta_x)+str*fabs(delta_yaw)+sxy*fabs(delta_y));
			delta_y+=sampleGaussian(srr*fabs(delta_y)+str*fabs(delta_yaw)+sxy*fabs(delta_x));
			delta_yaw+=sampleGaussian(stt*fabs(delta_yaw)+srt*dxy);
			delta_yaw=fmod(delta_yaw, 2*M_PI);
			if (delta_yaw>M_PI)
				delta_yaw-=2*M_PI;

   		tf::Pose noisy_pose(tf::createQuaternionFromRPY(0,0,delta_yaw),tf::Vector3(delta_x,delta_y,0));
      tf::Transform base_to_global_ = tf::Transform(p.getRotation());
      noisy_pose.setOrigin(base_to_global_ * noisy_pose.getOrigin());
      p.setOrigin(p.getOrigin() + noisy_pose.getOrigin());
      p.setRotation(p.getRotation() * noisy_pose.getRotation());

			return p;
		}
  geometry_msgs::Twist PoseFollower::diff2D(const tf::Pose& pose1, const tf::Pose& pose2)
  {
    geometry_msgs::Twist res;
    tf::Pose diff = pose2.inverse() * pose1;
    res.linear.x = diff.getOrigin().x();
    res.linear.y = diff.getOrigin().y();
    res.angular.z = tf::getYaw(diff.getRotation());

    if(holonomic_ || (fabs(res.linear.x) <= tolerance_trans_ && fabs(res.linear.y) <= tolerance_trans_))
      return res;

    //in the case that we're not rotating to our goal position and we have a non-holonomic robot
    //we'll need to command a rotational velocity that will help us reach our desired heading
    
    //we want to compute a goal based on the heading difference between our pose and the target
    double yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(), 
        pose2.getOrigin().x(), pose2.getOrigin().y(), tf::getYaw(pose2.getRotation()));

    //we'll also check if we can move more effectively backwards
    if (allow_backwards_) 
    {
      double neg_yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(), 
					pose2.getOrigin().x(), pose2.getOrigin().y(), M_PI + tf::getYaw(pose2.getRotation()));

      //check if its faster to just back up
      if(fabs(neg_yaw_diff) < fabs(yaw_diff)){
        ROS_DEBUG("Negative is better: %.2f", neg_yaw_diff);
        yaw_diff = neg_yaw_diff;
      }
    }

    //compute the desired quaterion
    tf::Quaternion rot_diff = tf::createQuaternionFromYaw(yaw_diff);
    tf::Quaternion rot = pose2.getRotation() * rot_diff;
    tf::Pose new_pose = pose1;
    new_pose.setRotation(rot);

    diff = pose2.inverse() * new_pose;
    res.linear.x = diff.getOrigin().x();
    res.linear.y = diff.getOrigin().y();
    res.angular.z = tf::getYaw(diff.getRotation());
    return res;
  }
	MathLib::Matrix4 toMatrix4(const tf::Pose& pose) {
		MathLib::Matrix4 mat;
		mat.Identity();
		tf::Matrix3x3 mat33(pose.getRotation());

		mat.SetTranslation(MathLib::Vector3(pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z()));
		mat.SetOrientation(MathLib::Matrix3(mat33[0][0], mat33[0][1], mat33[0][2],
				mat33[1][0], mat33[1][1], mat33[1][2],
				mat33[2][0], mat33[2][1], mat33[2][2]));
		return mat;
	}
	// Go to this pose
	bool go_home(tf::Pose& pose_) {


		tf::Vector3 start_p(pose_.getOrigin());
		tf::Quaternion start_o(pose_.getRotation());

		msg_pose.pose.position.x = start_p.x();
		msg_pose.pose.position.y = start_p.y();
		msg_pose.pose.position.z = start_p.z();
		msg_pose.pose.orientation.w = start_o.w();
		msg_pose.pose.orientation.x = start_o.x();
		msg_pose.pose.orientation.y = start_o.y();
		msg_pose.pose.orientation.z = start_o.z();
		pub_.publish(msg_pose);
		sendNormalForce(0);

		ros::Rate thread_rate(2);
		ROS_INFO("Homing...");
		while(ros::ok()) {
			double oerr =(ee_pose.getRotation() - start_o).length() ;
			double perr = (ee_pose.getOrigin() - start_p).length();
			feedback_.progress = 0.5*(perr+oerr);
			as_.publishFeedback(feedback_);
			ROS_INFO_STREAM("Error: "<<perr<<", "<<oerr);
			if(perr< 0.02 && oerr < 0.02) {
				break;
			}
			if (as_.isPreemptRequested() || !ros::ok() )
			{
				sendNormalForce(0);
				sendPose(ee_pose);
				ROS_INFO("%s: Preempted", action_name_.c_str());
				// set the action state to preempted
				as_.setPreempted();
				return false;
			}
			thread_rate.sleep();
		}
		return ros::ok();

	}
Esempio n. 13
0
// Convert from tf::Pose to CameraPose (position and orientation)
inline void TFPose2CameraPose(const tf::Pose& pose, CameraPose& cameraPose)
{
  // convert to position opencv vector
  tf::Vector3 position_tf = pose.getOrigin();
  cv::Point3d position = cv::Point3d(position_tf.getX(), position_tf.getY(), position_tf.getZ());

  // Convert to orientation opencv quaternion
  tf::Quaternion orientation_tf = pose.getRotation();
  cv::Vec4d orientation(orientation_tf.getW(), orientation_tf.getX(), orientation_tf.getY(), orientation_tf.getZ());

  cameraPose = CameraPose(position, orientation);
}
    void toMsgPose(const tf::Pose& tf_pose, geometry_msgs::Pose& msg_pose)
    {
        msg_pose.position.x = tf_pose.getOrigin().getX();
        msg_pose.position.y = tf_pose.getOrigin().getY();
        msg_pose.position.z = tf_pose.getOrigin().getZ();

        tf::Quaternion orientation = tf_pose.getRotation();
        msg_pose.orientation.w = orientation.getW();
        msg_pose.orientation.x = orientation.getX();
        msg_pose.orientation.y = orientation.getY();
        msg_pose.orientation.z = orientation.getZ();
    }
// Callback for the desired cartesian pose
void cartCallback(const geometry_msgs::PoseStampedConstPtr& msg) {
	ROS_INFO_STREAM("Received Position Command");
	const geometry_msgs::PoseStamped* data = msg.get();
	des_ee_pose.setOrigin(tf::Vector3(data->pose.position.x,data->pose.position.y,data->pose.position.z));
	des_ee_pose.setRotation(tf::Quaternion(data->pose.orientation.x,data->pose.orientation.y,data->pose.orientation.z,data->pose.orientation.w));

	ROS_INFO_STREAM_THROTTLE(1, "Received Position: "<<des_ee_pose.getOrigin().x()<<","<<des_ee_pose.getOrigin().y()<<","<<des_ee_pose.getOrigin().z());

	if(bOrientCtrl) {
		tf::Quaternion q = des_ee_pose.getRotation();
		ROS_INFO_STREAM_THROTTLE(1, "Received Orientation: "<<q.x()<<","<<q.y()<<","<<q.z()<<","<<q.w());
	}
}
Esempio n. 16
0
static RVector<double> PoseValues(tf::Pose & pose) {
   RVector<double> values;
    values.push_back( pose.getOrigin().x());
    values.push_back( pose.getOrigin().y());
    values.push_back( pose.getOrigin().z());
    double roll, pitch, yaw;
    tf::Quaternion q = pose.getRotation();
    tf::Matrix3x3(q).getRPY(roll, pitch, yaw );
    values.push_back( roll );
    values.push_back( pitch );
    values.push_back( yaw );
    return values;
}
void sendPose(tf::Pose& pose) {

	msg_pose.header.stamp = ros::Time::now();

	tf::Vector3 tmp = pose.getOrigin();
	msg_pose.pose.position.x = tmp.x();
	msg_pose.pose.position.y = tmp.y();
	msg_pose.pose.position.z = tmp.z();

	tf::Quaternion quat = pose.getRotation();
	msg_pose.pose.orientation.w = quat.w();
	msg_pose.pose.orientation.x = quat.x();
	msg_pose.pose.orientation.y = quat.y();
	msg_pose.pose.orientation.z = quat.z();

	pub_pose.publish(msg_pose);
}
	// Roll with "force" and horizontal "speed" until the length "range"
	bool rolling(double force, double speed, double range) {

		ROS_INFO_STREAM("Rolling with force "<<force<<", speed "<<speed<<", range "<<range);
		force = fabs(force);

		sendNormalForce(-force);
		msg_pose.pose.position.x  = ee_pose.getOrigin().x();
		msg_pose.pose.position.y  = ee_pose.getOrigin().y();
		msg_pose.pose.position.z  = ee_pose.getOrigin().z();

		tf::Quaternion q = ee_pose.getRotation();
		msg_pose.pose.orientation.x = q.x();
		msg_pose.pose.orientation.y = q.y();
		msg_pose.pose.orientation.z = q.z();
		msg_pose.pose.orientation.w = q.w();

		double center = ee_pose.getOrigin().y();
		double rate = 200;
		ros::Rate thread_rate(rate);
		int count=0;
		while(ros::ok()) {
			msg_pose.pose.position.y = msg_pose.pose.position.y + speed/rate;
			feedback_.progress = msg_pose.pose.position.y;
			as_.publishFeedback(feedback_);
			if( fabs(msg_pose.pose.position.y - center) >  range) {
				ROS_INFO("Change direction");
				speed *= -1;
				if(++count > 5) {
					break;
				}
			}
			pub_.publish(msg_pose);
			thread_rate.sleep();
		}

		msg_pose.pose.position.z = ee_pose.getOrigin().z() + 0.15;
		pub_.publish(msg_pose);
		sendNormalForce(0);

		return true;
	}
Esempio n. 19
0
void NaoqiJointStates::run()
{
   ros::Rate r(m_rate);
   ros::Time stamp1;
   ros::Time stamp2;
   ros::Time stamp;

   std::vector<float> odomData;
   float odomX, odomY, odomZ, odomWX, odomWY, odomWZ;

   std::vector<float> memData;

   std::vector<float> positionData;

   ROS_INFO("Staring main loop. ros::ok() is %d nh.ok() is %d",ros::ok(),m_nh.ok());
   while(ros::ok() )
   {
      r.sleep();
      ros::spinOnce();
      stamp1 = ros::Time::now();
      try
      {
         memData = m_memoryProxy->getListData(m_dataNamesList);
         // {SPACE_TORSO = 0, SPACE_WORLD = 1, SPACE_NAO = 2}. (second argument)
         odomData = m_motionProxy->getPosition("Torso", 1, true);
         positionData = m_motionProxy->getAngles("Body", true);
      }
      catch (const AL::ALError & e)
      {
         ROS_ERROR( "Error accessing ALMemory, exiting...");
         ROS_ERROR( "%s", e.what() );
         //ros.signal_shutdown("No NaoQI available anymore");
      }
      stamp2 = ros::Time::now();
      //ROS_DEBUG("dt is %f",(stamp2-stamp1).toSec()); % dt is typically around 1/1000 sec
      // TODO: Something smarter than this..
      stamp = stamp1 + ros::Duration((stamp2-stamp1).toSec()/2.0);

      /******************************************************************
       *                              IMU
       *****************************************************************/
      if (memData.size() != m_dataNamesList.getSize())
      {
         ROS_ERROR("memData length %zu does not match expected length %u",memData.size(),m_dataNamesList.getSize() );
         continue;
      }
      // IMU data:
      m_torsoIMU.header.stamp = stamp;

      float angleX = memData[1];
      float angleY = memData[2];
      float angleZ = memData[3];
      float gyroX = memData[4];
      float gyroY = memData[5];
      float gyroZ = memData[6];
      float accX = memData[7];
      float accY = memData[8];
      float accZ = memData[9];

      m_torsoIMU.orientation = tf::createQuaternionMsgFromRollPitchYaw(
                                angleX,
                                angleY,
                                angleZ); // yaw currently always 0

      m_torsoIMU.angular_velocity.x = gyroX;
      m_torsoIMU.angular_velocity.y = gyroY;
      m_torsoIMU.angular_velocity.z = gyroZ; // currently always 0

      m_torsoIMU.linear_acceleration.x = accX;
      m_torsoIMU.linear_acceleration.y = accY;
      m_torsoIMU.linear_acceleration.z = accZ;

      // covariances unknown
      // cf http://www.ros.org/doc/api/sensor_msgs/html/msg/Imu.html
      m_torsoIMU.orientation_covariance[0] = -1;
      m_torsoIMU.angular_velocity_covariance[0] = -1;
      m_torsoIMU.linear_acceleration_covariance[0] = -1;

      m_torsoIMUPub.publish(m_torsoIMU);


      /******************************************************************
       *                            Joint state
       *****************************************************************/
      m_jointState.header.stamp = stamp;
      m_jointState.header.frame_id = m_baseFrameId;
      m_jointState.position.resize(positionData.size());
      for(unsigned i = 0; i<positionData.size(); ++i)
      {
         m_jointState.position[i] = positionData[i];
      }

      // simulated model misses some joints, we need to fill:
      if (m_jointState.position.size() == 22)
      {
         m_jointState.position.insert(m_jointState.position.begin()+6,0.0);
         m_jointState.position.insert(m_jointState.position.begin()+7,0.0);
         m_jointState.position.push_back(0.0);
         m_jointState.position.push_back(0.0);
      }

      m_jointStatePub.publish(m_jointState);


        /******************************************************************
        *                            Odometry
        *****************************************************************/
        if (!m_paused) {

        // apply offset transformation:
        tf::Pose transformedPose;


        if (odomData.size()!=6)
        {
            ROS_ERROR( "Error getting odom data. length is %zu",odomData.size() );
            continue;
        }

        double dt = (stamp.toSec() - m_lastOdomTime);

        odomX = odomData[0];
        odomY = odomData[1];
        odomZ = odomData[2];
        odomWX = odomData[3];
        odomWY = odomData[4];
        odomWZ = odomData[5];

        tf::Quaternion q;
        // roll and pitch from IMU, yaw from odometry:
        if (m_useIMUAngles)
            q.setRPY(angleX, angleY, odomWZ);
        else
            q.setRPY(odomWX, odomWY, odomWZ);

        m_odomPose.setOrigin(tf::Vector3(odomX, odomY, odomZ));
        m_odomPose.setRotation(q);

        if(m_mustUpdateOffset) {
            if(!m_isInitialized) {
                if(m_initializeFromIMU) {
                    // Initialization from IMU: Take x, y, z, yaw from odometry, roll and pitch from IMU
                    m_targetPose.setOrigin(m_odomPose.getOrigin());
                    m_targetPose.setRotation(tf::createQuaternionFromRPY(angleX, angleY, odomWZ));
                } else if(m_initializeFromOdometry) {
                    m_targetPose.setOrigin(m_odomPose.getOrigin());
                    m_targetPose.setRotation(tf::createQuaternionFromRPY(odomWX, odomWY, odomWZ));
                }
                m_isInitialized = true;
            } else {
                // Overwrite target pitch and roll angles with IMU data
                const double target_yaw = tf::getYaw(m_targetPose.getRotation());
                if(m_initializeFromIMU) {
                    m_targetPose.setRotation(tf::createQuaternionFromRPY(angleX, angleY, target_yaw));
                } else if(m_initializeFromOdometry){
                    m_targetPose.setRotation(tf::createQuaternionFromRPY(odomWX, odomWY, target_yaw));
                }
            }
            m_odomOffset = m_targetPose * m_odomPose.inverse();
            transformedPose = m_targetPose;
            m_mustUpdateOffset = false;
        } else {
            transformedPose = m_odomOffset * m_odomPose;
        }

        //
        // publish the transform over tf first
        //
        m_odomTransformMsg.header.stamp = stamp;
        tf::transformTFToMsg(transformedPose, m_odomTransformMsg.transform);
        m_transformBroadcaster.sendTransform(m_odomTransformMsg);


        //
        // Fill the Odometry msg
        //
        m_odom.header.stamp = stamp;
        //set the velocity first (old values still valid)
        m_odom.twist.twist.linear.x = (odomX - m_odom.pose.pose.position.x) / dt;
        m_odom.twist.twist.linear.y = (odomY - m_odom.pose.pose.position.y) / dt;
        m_odom.twist.twist.linear.z = (odomZ - m_odom.pose.pose.position.z) / dt;

        // TODO: calc angular velocity!
        //	m_odom.twist.twist.angular.z = vth; ??

        //set the position from the above calculated transform
        m_odom.pose.pose.orientation = m_odomTransformMsg.transform.rotation;
        m_odom.pose.pose.position.x = m_odomTransformMsg.transform.translation.x;
        m_odom.pose.pose.position.y = m_odomTransformMsg.transform.translation.y;
        m_odom.pose.pose.position.z = m_odomTransformMsg.transform.translation.z;


        m_odomPub.publish(m_odom);


        m_lastOdomTime = stamp.toSec();

        }
    }
    ROS_INFO("naoqi_sensors stopped.");

}
	// Do stuff with learned models
	// Phase - Reach, Roll or Back?
	// Dynamics type - to select the type of master/slave dynamics (linear/model etc.)
	// reachingThreshold - you know
	// model_dt - you know
	bool learned_model_execution(DoughTaskPhase phase, CDSController::DynamicsType masterType, CDSController::DynamicsType slaveType, double reachingThreshold, double model_dt, tf::Transform trans_obj, tf::Transform trans_att, std::string model_base_path, std::string force_gmm_id) {

		ROS_INFO_STREAM(" Model Path "<<model_base_path);	
		ROS_INFO_STREAM(" Learned model execution with phase "<<phase);
		ROS_INFO_STREAM(" Reaching threshold "<<reachingThreshold);
		ROS_INFO_STREAM(" Model DT "<<model_dt);
		if (force_gmm_id!="")
                 ROS_INFO_STREAM(" Force GMM ID: "<< force_gmm_id);


		ros::Rate wait(1);
		tf::Transform  trans_final_target, ee_pos_att;

		// To Visualize EE Frames		    
		static tf::TransformBroadcaster br;
		br.sendTransform(tf::StampedTransform(trans_final_target, ros::Time::now(), world_frame, "/attractor"));
		br.sendTransform(tf::StampedTransform(trans_obj, ros::Time::now(), world_frame, "/object_frame"));


		// convert attractor information to world frame
		trans_final_target.mult(trans_obj, trans_att);

		ROS_INFO_STREAM("Final target origin "<<trans_final_target.getOrigin().getX()<<","<<trans_final_target.getOrigin().getY()<<","<<trans_final_target.getOrigin().getZ());
		ROS_INFO_STREAM("Final target orient "<<trans_final_target.getRotation().getX()<<","<<trans_final_target.getRotation().getY()<<","<<trans_final_target.getRotation().getZ()<<","<<trans_final_target.getRotation().getW());

		// Publish attractors if running in simulation or with fixed values
		if ((action_mode == ACTION_LASA_FIXED) || (action_mode == ACTION_BOXY_FIXED)) {
		    br.sendTransform(tf::StampedTransform(trans_final_target, ros::Time::now(), world_frame, "/attractor"));
		}

		// Initialize CDS
		CDSExecution *cdsRun = new CDSExecution;
		cdsRun->initSimple(model_base_path, phase, force_gmm_id);
		cdsRun->setObjectFrame(toMatrix4(trans_obj));
		cdsRun->setAttractorFrame(toMatrix4(trans_att));
		cdsRun->setCurrentEEPose(toMatrix4(ee_pose));
		cdsRun->setDT(model_dt);


		if (phase==PHASEBACK || phase==PHASEROLL)
			 masterType = CDSController::LINEAR_DYNAMICS;

		// Roll slow but move fast for reaching and back phases.
		// If models have proper speed, this whole block can go!
		if(phase == PHASEROLL) {
			//cdsRun->setMotionParameters(1,1,0.5,reachingThreshold, masterType, slaveType);
			cdsRun->setMotionParameters(1,1,2,reachingThreshold, masterType, slaveType);
			// large threshold to avoid blocking forever
			// TODO: should rely on preempt in action client.
//			reachingThreshold = 0.02;
			reachingThreshold = 0.025;
		} else {
			cdsRun->setMotionParameters(1,1,2,reachingThreshold, masterType, slaveType);
		}


		cdsRun->postInit();

		// If phase is rolling, need force model as well
		GMR* gmr_perr_force = NULL;
		if(phase == PHASEROLL) {
			gmr_perr_force = getNewGMRMappingModel(model_base_path, force_gmm_id);
			if(!gmr_perr_force) {
				ROS_ERROR("Cannot initialize GMR model");
				return false;
			}
		}

		ros::Duration loop_rate(model_dt);
		tf::Pose mNextRobotEEPose = ee_pose;
		std::vector<double> gmr_in, gmr_out;
		gmr_in.resize(1);gmr_out.resize(1);
		double prog_curr, full_err, pos_err, ori_err, new_err, gmr_err;
		tf::Vector3 att_t, curr_t;

		ROS_INFO("Execution started");
		tf::Pose p;
		bool bfirst = true;

		std_msgs::String string_msg, action_name_msg, model_fname_msg;
		std::stringstream ss, ss_model;

		if(phase ==  PHASEREACH) {
			ss << "reach ";
		}
		else if (phase == PHASEROLL){
			ss << "roll";
		}	
		else if (phase == PHASEBACK){
			ss << "back";
		}		

		if (!homing){
			string_msg.data = ss.str();
			pub_action_state_.publish(string_msg);


//			ss_model << path_matlab_plot << "/Phase" <<  phase << "/masterGMM.fig";

			if (force_gmm_id=="")
				ss_model << path_matlab_plot << "/Phase" <<  phase << "/masterGMM.fig";
			else
				ss_model << path_matlab_plot << "/Phase" <<  phase << "/ForceProfile_" << force_gmm_id << ".fig";
			//ss_model << "/Phase" <<  phase << "/masterGMM.fig";
			
			model_fname_msg.data = ss_model.str();		
			pub_model_fname_.publish(model_fname_msg);

			action_name_msg.data = ss.str();
			pub_action_name_.publish(action_name_msg);

			plot_dyn = 1;			
			plot_dyn_msg.data = plot_dyn;
			pub_plot_dyn_.publish(plot_dyn_msg);
			
		}
		
		while(ros::ok()) {
			
			if (!homing)
				plot_dyn = 1;
			else
				plot_dyn = 0;

			plot_dyn_msg.data = plot_dyn;
			pub_plot_dyn_.publish(plot_dyn_msg);

			// Nadia's stuff
			// Current progress variable (position/orientation error).
			// TODO: send this back to action client as current progress
			pos_err = (trans_final_target.getOrigin() - ee_pose.getOrigin()).length();
			//Real Orientation Error qdiff = acos(dot(q1_norm,q2_norm))*180/pi
			ori_err = acos(abs(trans_final_target.getRotation().dot(ee_pose.getRotation())));


			ROS_INFO_STREAM_THROTTLE(0.5,"Position Threshold : " << reachingThreshold << " ... Current Error: "<<pos_err);
			ROS_INFO_STREAM_THROTTLE(0.5,"Orientation Threshold : " << 0.01 << " ... Current Error: "<<ori_err);

			/*double att_pos_err = (trans_final_target.getOrigin() - p.getOrigin()).length();
			double att_ori_err = acos(abs(trans_final_target.getRotation().dot(p.getRotation())));

			ROS_INFO_STREAM_THROTTLE(0.5,"Des-Att Position Error: " << att_pos_err);
			ROS_INFO_STREAM_THROTTLE(0.5,"Des-Att Orientation Error: " << att_ori_err);*/

			switch (phase) {
			// Home, reach and back are the same control-wise
			case PHASEREACH:

			case PHASEBACK:
				// Current progress variable (position/orientation error).
				// TODO: send this back to action client as current progress
				prog_curr = 0.5*((trans_final_target.getOrigin() - ee_pose.getOrigin()).length() + (trans_final_target.getRotation()-ee_pose.getRotation()).length());

				// Compute Next Desired EE Pose
				cdsRun->setCurrentEEPose(toMatrix4(mNextRobotEEPose));
				toPose(cdsRun->getNextEEPose(), mNextRobotEEPose);
				p = mNextRobotEEPose;

				// Aswhini's Hack! Dont rely on model's orientation interpolation. Set it equal to target orientation to avoid
				// going the wrong way around
				p.setRotation(trans_final_target.getRotation());

				//Publish desired force	
				gmr_msg.data = 0.0;
				pub_gmr_out_.publish(gmr_msg);

				break;
			case PHASEROLL:

				// Current progress in rolling phase is simply the position error	
				prog_curr = (trans_final_target.getOrigin() - ee_pose.getOrigin()).length();

				// New position error being fed to GMR Force Model	
				att_t = tf::Vector3(trans_final_target.getOrigin().getX(),trans_final_target.getOrigin().getY(),0.0);
				curr_t = tf::Vector3(ee_pose.getOrigin().getX(),ee_pose.getOrigin().getY(),0.0);
				new_err = (att_t - curr_t).length();

 				gmr_err = new_err;
				//Hack! Truncate errors to corresponding models				
				if ((strncmp(force_gmm_id.c_str(),"first",5)==0) && (new_err > 0.03)){
					gmr_err = 0.03;
 				}
				
				if ((strncmp(force_gmm_id.c_str(),"mid",3)==0) && (new_err > 0.07)){
					gmr_err = 0.07;
 				}
				if ((strncmp(force_gmm_id.c_str(),"last",4)==0) && (new_err > 0.13)){
					gmr_err = 0.13;
				}
				//pos_err = prog_curr;
				ori_err = 0;
				gmr_err = gmr_err;

				gmr_in[0] = gmr_err; // distance between EE and attractor

				// Query the model for desired force
				getGMRResult(gmr_perr_force, gmr_in, gmr_out);
	
/*				double fz_plot;
				getGMRResult(gmr_perr_force, -gmr_in, fz_plot);*/
				//-> INSTEAD OF SENDING GMR OUTPUT / SEND EST_EE_FT(Z)
				// Send fz and distance to attractor for plotting		
				msg_ft.wrench.force.x = gmr_err;
				msg_ft.wrench.force.y = gmr_out[0]; //ee_ft[2]
				msg_ft.wrench.force.z = 0;
				msg_ft.wrench.torque.x = 0;
				msg_ft.wrench.torque.y = 0;
				msg_ft.wrench.torque.z = 0;
				pub_ee_ft_att_.publish(msg_ft);

				// Hack! Scale the force to be in reasonable values
				gmr_out[0] = FORCE_SCALING*fabs(gmr_out[0]);

				ROS_INFO_STREAM_THROTTLE(0.5,"Distance to Attractor: " << new_err << " GMR output (N): " << gmr_out[0]);

				gmr_msg.data = gmr_out[0];
				pub_gmr_out_.publish(gmr_msg);

				// Hack! Safety first!
				if(gmr_out[0] > MAX_ROLLING_FORCE) {
					gmr_out[0] = MAX_ROLLING_FORCE;
				}

				// Give some time for the force to catch up the first time. Then roll with constant force thereafter.
				if(bfirst) {
					sendAndWaitForNormalForce(-gmr_out[0]);
					bfirst = false;
				} else {
					sendNormalForce(-gmr_out[0]);
				}
				ROS_INFO_STREAM_THROTTLE(0.5, "Force applied: "<<gmr_out[0]);



				cdsRun->setCurrentEEPose(toMatrix4(mNextRobotEEPose));
				toPose(cdsRun->getNextEEPose(), mNextRobotEEPose);

				p = mNextRobotEEPose;

				// Hack! Dont rely on model orientation. Use target orientation instead.
				p.setRotation(trans_final_target.getRotation());

				// Hack! Dont rely on the Z component of the model. It might go below the table!
				p.getOrigin().setZ(trans_final_target.getOrigin().getZ());

				homing=false;
				break;
			default:
				ROS_ERROR_STREAM("No such phase defined "<<phase);
				return false;
			}


			// Add rotation of Tool wrt. flange_link for BOXY
			/*if (use_boxy_tool){
				tf::Matrix3x3 R = p.getBasis();
	      			Eigen::Matrix3d R_ee;
				tf::matrixTFToEigen(R,R_ee);

				Eigen::Matrix3d R_tool;
				R_tool << -0.7071, -0.7071, 0.0, 
                           		  0.7071,-0.7071, 0.0,
                            		      0.0,  0.0,  1.0;

	      			//multiply tool rot
				R_ee = R_ee*R_tool;
                                tf::matrixEigenToTF(R_ee,R);				
				p.setBasis(R);
			}*/			


			// Send the computed pose from one of the above phases
			sendPose(p);

			// convert and send ee pose to attractor frame to plots
			ee_pos_att.mult(trans_final_target.inverse(), p);
			geometry_msgs::PoseStamped msg;
			msg.pose.position.x = ee_pos_att.getOrigin().x();
			msg.pose.position.y = ee_pos_att.getOrigin().y();
			msg.pose.position.z = ee_pos_att.getOrigin().z();
			msg.pose.orientation.x = ee_pos_att.getRotation().x();
			msg.pose.orientation.y = ee_pos_att.getRotation().y();
			msg.pose.orientation.z = ee_pos_att.getRotation().z();
			msg.pose.orientation.w = ee_pos_att.getRotation().w();
			pub_ee_pos_att_.publish(msg);
			

			//ROS_INFO_STREAM_THROTTLE(0.5,"Error "<<prog_curr);

			// check that preempt has not been requested by the client
			if (as_.isPreemptRequested() || !ros::ok())
			{
				sendPose(ee_pose);
				sendNormalForce(0);
				ROS_INFO("%s: Preempted", action_name_.c_str());
				// set the action state to preempted
				as_.setPreempted();
				return false;
			}
			feedback_.progress = prog_curr;
			as_.publishFeedback(feedback_);

/*			if(prog_curr < reachingThreshold) {
				break;
			}*/

			//Orientation error	0.05
			if(pos_err < reachingThreshold && (ori_err < 0.05 || isnan(ori_err))) {

				break;
			}

			loop_rate.sleep();
		}
		delete cdsRun;

		if(phase ==  PHASEREACH) {
			// Hack! If phase is "reach", find the table right after reaching
			if (bWaitForForces && !homing)	{		
				bool x = find_table_for_rolling(0.35, 0.05, 5);
//				bool x = find_table_for_rolling(0.35, 0.1, 5);
				//-> Send command to dynamics plotter to stop logging				 
				return x;
			}
			if (!homing){
				//-> Send command to dynamics plotter to stop logging				 
			}
		} else if (phase == PHASEROLL){
			// Hack! wait for zero force before getting ready to recieve further commands.
			// This is to avoid dragging the dough.
			sendAndWaitForNormalForce(0);
			//-> Send command to dynamics plotter to start plotting
			return ros::ok();
		} else {
			//->Send command to dynamics plotter to start plotting

			return ros::ok();
		}
	}
bool
FootstepNavigation::getFootstep(const tf::Pose& from,
                                const State& from_planned,
		                            const State& to,
		                            humanoid_nav_msgs::StepTarget* footstep)
{
  // get footstep to reach 'to' from 'from'
  tf::Transform step = from.inverse() *
                       tf::Pose(tf::createQuaternionFromYaw(to.getTheta()),
                                tf::Point(to.getX(), to.getY(), 0.0));

  // set the footstep
  footstep->pose.x = step.getOrigin().x();
  footstep->pose.y = step.getOrigin().y();
  footstep->pose.theta = tf::getYaw(step.getRotation());
  if (to.getLeg() == LEFT)
    footstep->leg = humanoid_nav_msgs::StepTarget::left;
  else // to.leg == RIGHT
    footstep->leg = humanoid_nav_msgs::StepTarget::right;


  /* check if the footstep can be performed by the NAO robot ******************/

  // check if the step lies within the executable range
  if (performable(*footstep))
  {
    return true;
  }
  else
  {
    // check if there is only a minor divergence between the current support
	// foot and the foot placement used during the plannig task: in such a case
	// perform the step that has been used during the planning
    float step_diff_x = fabs(from.getOrigin().x() - from_planned.getX());
    float step_diff_y = fabs(from.getOrigin().y() - from_planned.getY());
    float step_diff_theta = fabs(
        angles::shortest_angular_distance(
            tf::getYaw(from.getRotation()), from_planned.getTheta()));
    if (step_diff_x < ivAccuracyX && step_diff_y < ivAccuracyY &&
        step_diff_theta < ivAccuracyTheta)
    {
	  step = tf::Pose(tf::createQuaternionFromYaw(from_planned.getTheta()),
	                  tf::Point(from_planned.getX(), from_planned.getY(), 0.0)
	                  ).inverse() *
		     tf::Pose(tf::createQuaternionFromYaw(to.getTheta()),
				      tf::Point(to.getX(), to.getY(), 0.0));

	  footstep->pose.x = step.getOrigin().x();
	  footstep->pose.y = step.getOrigin().y();
	  footstep->pose.theta = tf::getYaw(step.getRotation());

	  return true;
    }

    return false;
  }

//  // ALTERNATIVE: clip the footstep into the executable range; if nothing was
//  // clipped: perform; if too much was clipped: do not perform
//  humanoid_nav_msgs::ClipFootstep footstep_clip;
//  footstep_clip.request.step = footstep;
//  ivClipFootstepSrv.call(footstep_clip);
//
//  if (performanceValid(footstep_clip))
//  {
//  	footstep.pose.x = footstep_clip.response.step.pose.x;
//  	footstep.pose.y = footstep_clip.response.step.pose.y;
//  	footstep.pose.theta = footstep_clip.response.step.pose.theta;
//  	return true;
//  }
//  else
//  {
//    return false;
//  }
}
Esempio n. 22
0
void OdometryRemap::torsoOdomCallback(const nao_msgs::TorsoOdometryConstPtr& msgOdom, const nao_msgs::TorsoIMUConstPtr& msgIMU){
	if (m_paused){
		ROS_DEBUG("Skipping odometry callback, paused");
		return;
	}

	double odomTime = (msgOdom->header.stamp).toSec();
	ROS_DEBUG("Received [%f %f %f %f (%f/%f) (%f/%f) %f]", odomTime, msgOdom->x, msgOdom->y, msgOdom->z, msgOdom->wx, msgIMU->angleX, msgOdom->wy, msgIMU->angleY, msgOdom->wz);

	double dt = (odomTime - m_lastOdomTime);


	tf::Quaternion q;
	// roll and pitch from IMU, yaw from odometry:
	if (m_useIMUAngles)
		q.setRPY(msgIMU->angleX, msgIMU->angleY, msgOdom->wz);
	else
		q.setRPY(msgOdom->wx, msgOdom->wy, msgOdom->wz);


	m_odomPose.setOrigin(tf::Vector3(msgOdom->x, msgOdom->y, msgOdom->z));
	m_odomPose.setRotation(q);

	// apply offset transformation:
	tf::Pose transformedPose;

	if(m_mustUpdateOffset) {
        if(!m_isInitialized) {
            if(m_initializeFromIMU) {
                // Initialization from IMU: Take x, y, z, yaw from odometry, roll and pitch from IMU
                m_targetPose.setOrigin(m_odomPose.getOrigin());
                m_targetPose.setRotation(tf::createQuaternionFromRPY(msgIMU->angleX, msgIMU->angleY, msgOdom->wz));
            } else if(m_initializeFromOdometry) {
                m_targetPose.setOrigin(m_odomPose.getOrigin());
                m_targetPose.setRotation(tf::createQuaternionFromRPY(msgOdom->wx, msgOdom->wy, msgOdom->wz));
            }
            m_isInitialized = true;
        } else {
            // Overwrite target pitch and roll angles with IMU data
            const double target_yaw = tf::getYaw(m_targetPose.getRotation());
            if(m_initializeFromIMU) {
                m_targetPose.setRotation(tf::createQuaternionFromRPY(msgIMU->angleX, msgIMU->angleY, target_yaw));
            } else if(m_initializeFromOdometry){
                m_targetPose.setRotation(tf::createQuaternionFromRPY(msgOdom->wx, msgOdom->wy, target_yaw));
            }
        }
 		m_odomOffset = m_targetPose * m_odomPose.inverse();
		transformedPose = m_targetPose;
		m_mustUpdateOffset = false;
	} else {
	    transformedPose = m_odomOffset * m_odomPose;
	}

	// publish the transform over tf first:
	m_odomTransformMsg.header.stamp = msgOdom->header.stamp;
	tf::transformTFToMsg(transformedPose, m_odomTransformMsg.transform);
	m_transformBroadcaster.sendTransform(m_odomTransformMsg);

	//next, publish the actual odometry message:
	m_odom.header.stamp = msgOdom->header.stamp;
	m_odom.header.frame_id = m_odomFrameId;


	//set the velocity first (old values still valid)
	m_odom.twist.twist.linear.x = (msgOdom->x - m_odom.pose.pose.position.x) / dt;
	m_odom.twist.twist.linear.y = (msgOdom->y - m_odom.pose.pose.position.y) / dt;
	m_odom.twist.twist.linear.z = (msgOdom->z - m_odom.pose.pose.position.z) / dt;
	// TODO: calc angular velocity!
//	m_odom.twist.twist.angular.z = vth; ??

	//set the position from the above calculated transform
	m_odom.pose.pose.orientation = m_odomTransformMsg.transform.rotation;
	m_odom.pose.pose.position.x = m_odomTransformMsg.transform.translation.x;
	m_odom.pose.pose.position.y = m_odomTransformMsg.transform.translation.y;
	m_odom.pose.pose.position.z = m_odomTransformMsg.transform.translation.z;

	//publish the message
	m_odomPub.publish(m_odom);

	m_lastOdomTime = odomTime;

	// TODO: not used
//	m_lastWx = msgOdom->wx;
//	m_lastWy = msgOdom->wy;
//	m_lastWz = msgOdom->wz;
}
// Convert current cartesian commands to joint velocity
void computeJointVelocity(Eigen::VectorXd& jvel) {
	if(jvel.size() != numdof) {
		jvel.resize(numdof);
	}

	if(est_ee_ft.norm() > max_ee_ft_norm) {
		for(int i=0; i<jvel.size(); ++i) {
			jvel[i]=0.0;
		}
		ROS_WARN_THROTTLE(0.1, "Too much force! Sending zero velocity");
		return;
	}

//	tf::Pose curr_ee_pose;
	mRobot->setJoints(read_jpos);
	mRobot->getEEPose(curr_ee_pose);

	// Two kinds of cartesian velocities - due to position error and force error
	Eigen::VectorXd vel_due_to_pos, vel_due_to_force;
	if(bOrientCtrl) {
		vel_due_to_pos.resize(6);
		vel_due_to_force.resize(6);
	} else {
		vel_due_to_pos.resize(3);
		vel_due_to_force.resize(3);
	}
	for(int i=0; i<vel_due_to_force.size(); ++i) {
		vel_due_to_force[i] = 0;
		vel_due_to_pos[i] = 0;
	}

	ROS_INFO_STREAM_THROTTLE(0.5, "Publishing EE TF");
	static tf::TransformBroadcaster br;
	br.sendTransform(tf::StampedTransform(des_ee_pose, ros::Time::now(), world_frame, "/des_ee_tf"));
	br.sendTransform(tf::StampedTransform(curr_ee_pose, ros::Time::now(), world_frame, "/curr_ee_tf"));


  // Cartesian velocity due to position/orientation error
	tf::Vector3 linvel = des_ee_pose.getOrigin() - curr_ee_pose.getOrigin();
	vel_due_to_pos(0) = linvel.getX();
	vel_due_to_pos(1) = linvel.getY();
	vel_due_to_pos(2) = linvel.getZ();
	double pos_err =  linvel.length();
	ROS_INFO_STREAM_THROTTLE(0.5, "Position Err:\t"<< pos_err);

// Nadia's way...
	// If Orientation is activated from launch file
	double qdiff = acos(abs(des_ee_pose.getRotation().dot(curr_ee_pose.getRotation())));
	if(bOrientCtrl) {
		// Computing angular velocity using quaternion difference
		tf::Quaternion tmp = curr_ee_pose.getRotation();
		tf::Quaternion angvel = (des_ee_pose.getRotation() - tmp)*tmp.inverse();
		vel_due_to_pos(3) = angvel.getX();
		vel_due_to_pos(4) = angvel.getY();
		vel_due_to_pos(5) = angvel.getZ();
		ROS_INFO_STREAM_THROTTLE(0.5, "Orient. Err:\t"<<qdiff);
	}

	if (pos_err < 0.01 && qdiff < 0.05){ // LASA 0.5deg
//      if (pos_err < 0.015 && qdiff < 0.05){ // Boxy 1.7 deg
		vel_due_to_pos(3) = 0;
		vel_due_to_pos(4) = 0;
		vel_due_to_pos(5) = 0;
	}


/*
// Aswhini's way...
	// If Orientation is activated from launch file
	if(bOrientCtrl) {
		// Computing angular velocity using quaternion difference
		tf::Quaternion tmp = curr_ee_pose.getRotation();
		tf::Quaternion angvel = (des_ee_pose.getRotation() - tmp)*tmp.inverse();
		vel_due_to_pos(3) = angvel.getX();
		vel_due_to_pos(4) = angvel.getY();
		vel_due_to_pos(5) = angvel.getZ();
        tf::Quaternion t = angvel;
//		ROS_INFO_STREAM_THROTTLE(0.5, "Orient. Err:\t"<<angvel.length());
	}
*/

	// Compute errors in position
	double err = vel_due_to_pos.norm();

	// Increase tracking performance by tuning traj_traking_gain
	vel_due_to_pos *= traj_tracking_gain;

	// If force is activated from launch file
	if(bUseForce) {

		// Compute force error
		Eigen::VectorXd ft_err = des_ee_ft - est_ee_ft;
		double ft_err_nrm;
		if(bOrientCtrl) {
			ft_err_nrm = ft_err.norm();
		} else {
			double force_error = ft_err[0]*ft_err[0] +ft_err[1]*ft_err[1]+ft_err[2]*ft_err[2];
			ft_err_nrm = sqrt(force_error);
		}

		// Remove force if too small. This is necessary due to residual errors in EE force estimation.
		// Dont apply external forces until desired position is almost reached within reach_tol
		if(ft_err_nrm > ft_tol && err < reach_tol) {
			forceErrorToVelocity(ft_err, vel_due_to_force);
		}
		ROS_INFO_STREAM_THROTTLE(0.5, "Force Err:\t"<<ft_err_nrm);
	}

	// Add the cartesian velocities due to position and force errors and compute the joint_velocity
//	mRobot->getIKJointVelocity(vel_due_to_force+vel_due_to_pos, jvel);
    mRobot->getIKJointVelocity(vel_due_to_pos, jvel);

}