C++ ros::Duration类代码示例

bool planning_environment::KinematicModelStateMonitor::allJointsUpdated(ros::Duration allowed_dur) const {
  current_joint_values_lock_.lock();
  bool ret = true;
  for(std::map<std::string, double>::const_iterator it = current_joint_state_map_.begin();
      it != current_joint_state_map_.end();
      it++) {
    if(last_joint_update_.find(it->first) == last_joint_update_.end()) {
      ROS_DEBUG_STREAM("Joint " << it->first << " not yet updated");
      ret = false;
      continue;
    }
    if(allowed_dur != ros::Duration()) {
      ros::Duration dur = ros::Time::now()-last_joint_update_.find(it->first)->second; 
      if(dur > allowed_dur) {
        ROS_DEBUG_STREAM("Joint " << it->first << " last updated " << dur.toSec() << " where allowed duration is " << allowed_dur.toSec());
        ret = false;
        continue;
      }
    }
  }
  current_joint_values_lock_.unlock();
  return ret;
}

 void read(ros::Time time, ros::Duration period)
 {
   for(int j=0; j < n_joints_; ++j)
   {
     joint_position_prev_[j] = joint_position_[j];
     joint_position_[j] += angles::shortest_angular_distance(joint_position_[j],
                             sim_joints_[j]->GetAngle(0).Radian());
     joint_position_kdl_(j) = joint_position_[j];
     // derivate velocity as in the real hardware instead of reading it from simulation
     joint_velocity_[j] = filters::exponentialSmoothing((joint_position_[j] - joint_position_prev_[j])/period.toSec(), joint_velocity_[j], 0.2);
     joint_effort_[j] = sim_joints_[j]->GetForce((int)(0));
     joint_stiffness_[j] = joint_stiffness_command_[j];
   }
 }

int main(int argc, char **argv)
{
    const double radius = 0.5; // 0.5
    const double pitch_step = 0.2;
    const int circle_points = 6; // 6
    const int circle_rings = 3; // 3
    const double plan_time = 5;
    const int plan_retry = 1;
    //const int pose_id_max = circle_points * 3;
    const ros::Duration wait_settle(1.0);
    const ros::Duration wait_camera(0.5);
    const ros::Duration wait_notreached(3.0);
    const ros::Duration wait_reset(30.0);

    const std::string planning_group = "bumblebee"; //robot, bumblebee
    const std::string end_effector = "tool_flange"; // bumblebee_cam1, tool_flange

    const std::string plannerId = "PRMkConfigDefault";
    const std::string frame_id = "base_link";

    ros::init (argc, argv, "simple_pose_mission");
    ros::AsyncSpinner spinner(1);
    spinner.start();
    ros::NodeHandle node_handle;
    ros::Publisher cameratate_pub = node_handle.advertise<group4_msgs::PointCloudPose>("/robot_rx60b/camerapose", 5);
    ros::Publisher markerPublisher = node_handle.advertise<visualization_msgs::MarkerArray>("/simple_pose_mission/poses", 10);

    tf::TransformListener listener;

    // Marker
    MarkerPose marker("object_center");
    addObjectCenterMarker(marker);

    // Robot and scene
    moveit::planning_interface::MoveGroup group(planning_group);
    group.setPlannerId(plannerId);
    group.setPoseReferenceFrame(frame_id);
    //group.setWorkspace(-1.5,-1.5,1.5,1.5,0,1);
    group.setGoalTolerance(0.01f);
    group.setPlanningTime(plan_time);

    group4_msgs::PointCloudPose msg;
    msg.header.frame_id = frame_id;

    tf::Pose object_center = marker.getPose("object_center");
    NumberedPoses poses = generatePoses(object_center, circle_points, circle_rings, pitch_step, radius);
    NumberedPoses::iterator poses_itt;
    bool poses_updated = true;


    while (ros::ok())
    {
        bool success;

        if (poses_updated || poses_itt == poses.end())
        {
            poses_itt = poses.begin();
            poses_updated = false;
        }

        NumberedPose numbered_pose = *poses_itt;

        visualizePoses(poses, frame_id,markerPublisher, numbered_pose.pose_id);


        ROS_INFO("Moving to pose %i of %i", numbered_pose.pose_id, numbered_pose.pose_id_max);



        tf::StampedTransform transform;
        try{
          listener.lookupTransform("/bumblebee_cam1", "/tool_flange",
                                   ros::Time(0), transform);
        }
        catch (tf::TransformException ex){
          ROS_ERROR("%s",ex.what());
        }

        tf::Pose p = numbered_pose.pose_tf * transform;
        geometry_msgs::Pose desired_pose = tfPoseToGeometryPose(p);
        group.setPoseTarget(desired_pose, end_effector);

        success = false;
        for (int itry = 0; itry < plan_retry; itry++)
        {

            group.setStartStateToCurrentState();
            wait_notreached.sleep();
            success = group.move();
            if (success)
                break;
            else
                ROS_INFO("Plan unsuccessfull.. Retrying! (%i)", itry);
        }

        if (true)
        {

            wait_settle.sleep();

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

void JointPositionController::update(const ros::Time& time, const ros::Duration& period)
{
  command_struct_ = *(command_.readFromRT());
  double command_position = command_struct_.position_;
  double command_velocity = command_struct_.velocity_;
  bool has_velocity_ =  command_struct_.has_velocity_;

  double error, vel_error;
  double commanded_effort;

  double current_position = joint_.getPosition();

  // Make sure joint is within limits if applicable
  enforceJointLimits(command_position);

  // Compute position error
  if (joint_urdf_->type == urdf::Joint::REVOLUTE)
  {
   angles::shortest_angular_distance_with_limits(
      current_position,
      command_position,
      joint_urdf_->limits->lower,
      joint_urdf_->limits->upper,
      error);
  }
  else if (joint_urdf_->type == urdf::Joint::CONTINUOUS)
  {
    error = angles::shortest_angular_distance(current_position, command_position);
  }
  else //prismatic
  {
    error = command_position - current_position;
  }

  // Decide which of the two PID computeCommand() methods to call
  if (has_velocity_)
  {
    // Compute velocity error if a non-zero velocity command was given
    vel_error = command_velocity - joint_.getVelocity();

    // Set the PID error and compute the PID command with nonuniform
    // time step size. This also allows the user to pass in a precomputed derivative error.
    commanded_effort = pid_controller_.computeCommand(error, vel_error, period);
  }
  else
  {
    // Set the PID error and compute the PID command with nonuniform
    // time step size.
    commanded_effort = pid_controller_.computeCommand(error, period);
  }

  joint_.setCommand(commanded_effort);

  // publish state
  if (loop_count_ % 10 == 0)
  {
    if(controller_state_publisher_ && controller_state_publisher_->trylock())
    {
      controller_state_publisher_->msg_.header.stamp = time;
      controller_state_publisher_->msg_.set_point = command_position;
      controller_state_publisher_->msg_.process_value = current_position;
      controller_state_publisher_->msg_.process_value_dot = joint_.getVelocity();
      controller_state_publisher_->msg_.error = error;
      controller_state_publisher_->msg_.time_step = period.toSec();
      controller_state_publisher_->msg_.command = commanded_effort;

      double dummy;
      bool antiwindup;
      getGains(controller_state_publisher_->msg_.p,
        controller_state_publisher_->msg_.i,
        controller_state_publisher_->msg_.d,
        controller_state_publisher_->msg_.i_clamp,
        dummy,
        antiwindup);
      controller_state_publisher_->msg_.antiwindup = static_cast<char>(antiwindup);
      controller_state_publisher_->unlockAndPublish();
    }
  }
  loop_count_++;
}

bool LWRHWreal::read(ros::Time time, ros::Duration period)
{
  // update the robot positions
  for (int j = 0; j < LBR_MNJ; j++)
  {
  	joint_position_prev_[j] = joint_position_[j];
    joint_position_[j] = device_->getMsrMsrJntPosition()[j];
    joint_effort_[j] = device_->getMsrJntTrq()[j];
    joint_velocity_[j] = filters::exponentialSmoothing((joint_position_[j]-joint_position_prev_[j])/period.toSec(), joint_velocity_[j], 0.2);
    joint_stiffness_[j] = joint_stiffness_command_[j];
  }
  
  //this->device_->interface->doDataExchange();

  return true;
}

	// read 'measurement' joint values
	void PanTiltHW::read(ros::Time time, ros::Duration period)
	{
		client_->Receive();
		
		for(int i=0; i<n_joints_;i++)
		{
			joint_position_[i] = client_->drives[i].state.position;
			joint_position_prev_[i] = joint_position_[i];
		    joint_velocity_[i] = filters::exponentialSmoothing((joint_position_[i] - joint_position_prev_[i])/period.toSec(), joint_velocity_[i], 0.2);
		}
	}

void AmclNode::reconfigureCB(AMCLConfig &config, uint32_t level)
{
    boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);

    //we don't want to do anything on the first call
    //which corresponds to startup
    if(first_reconfigure_call_)
    {
        first_reconfigure_call_ = false;
        default_config_ = config;
        return;
    }

    if(config.restore_defaults) {
        config = default_config_;
        //avoid looping
        config.restore_defaults = false;
    }

    d_thresh_ = config.update_min_d;
    a_thresh_ = config.update_min_a;

    resample_interval_ = config.resample_interval;

    laser_min_range_ = config.laser_min_range;
    laser_max_range_ = config.laser_max_range;

    gui_publish_period = ros::Duration(1.0/config.gui_publish_rate);
    save_pose_period = ros::Duration(1.0/config.save_pose_rate);

    transform_tolerance_.fromSec(config.transform_tolerance);

    max_beams_ = config.laser_max_beams;
    alpha1_ = config.odom_alpha1;
    alpha2_ = config.odom_alpha2;
    alpha3_ = config.odom_alpha3;
    alpha4_ = config.odom_alpha4;
    alpha5_ = config.odom_alpha5;

    z_hit_ = config.laser_z_hit;
    z_short_ = config.laser_z_short;
    z_max_ = config.laser_z_max;
    z_rand_ = config.laser_z_rand;
    sigma_hit_ = config.laser_sigma_hit;
    lambda_short_ = config.laser_lambda_short;
    laser_likelihood_max_dist_ = config.laser_likelihood_max_dist;

    if(config.laser_model_type == "beam")
        laser_model_type_ = LASER_MODEL_BEAM;
    else if(config.laser_model_type == "likelihood_field")
        laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD;

    if(config.odom_model_type == "diff")
        odom_model_type_ = ODOM_MODEL_DIFF;
    else if(config.odom_model_type == "omni")
        odom_model_type_ = ODOM_MODEL_OMNI;
    else if(config.odom_model_type == "diff-corrected")
        odom_model_type_ = ODOM_MODEL_DIFF_CORRECTED;
    else if(config.odom_model_type == "omni-corrected")
        odom_model_type_ = ODOM_MODEL_OMNI_CORRECTED;

    if(config.min_particles > config.max_particles)
    {
        ROS_WARN("You've set min_particles to be less than max particles, this isn't allowed so they'll be set to be equal.");
        config.max_particles = config.min_particles;
    }

    min_particles_ = config.min_particles;
    max_particles_ = config.max_particles;
    alpha_slow_ = config.recovery_alpha_slow;
    alpha_fast_ = config.recovery_alpha_fast;
    tf_broadcast_ = config.tf_broadcast;

    pf_ = pf_alloc(min_particles_, max_particles_,
                   alpha_slow_, alpha_fast_,
                   (pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
                   (void *)map_);
    pf_err_ = config.kld_err;
    pf_z_ = config.kld_z;
    pf_->pop_err = pf_err_;
    pf_->pop_z = pf_z_;

    // Initialize the filter
    pf_vector_t pf_init_pose_mean = pf_vector_zero();
    pf_init_pose_mean.v[0] = last_published_pose.pose.pose.position.x;
    pf_init_pose_mean.v[1] = last_published_pose.pose.pose.position.y;
    pf_init_pose_mean.v[2] = tf::getYaw(last_published_pose.pose.pose.orientation);
    pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
    pf_init_pose_cov.m[0][0] = last_published_pose.pose.covariance[6*0+0];
    pf_init_pose_cov.m[1][1] = last_published_pose.pose.covariance[6*1+1];
    pf_init_pose_cov.m[2][2] = last_published_pose.pose.covariance[6*5+5];
    pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
    pf_init_ = false;

    // Instantiate the sensor objects
    // Odometry
    delete odom_;
    odom_ = new AMCLOdom();
    ROS_ASSERT(odom_);
    odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );
//.........这里部分代码省略.........

void OmniDriveController::update(const ros::Time& time, const ros::Duration& period)
{
	//get the wheel velocity from the handle
	double wheel1_vel = wheel1_joint.getVelocity();
	double wheel2_vel = wheel2_joint.getVelocity();
	double wheel3_vel = wheel3_joint.getVelocity();

	// Estimate linear and angular velocity using joint information
			odometry_.update(wheel1_vel, wheel2_vel, wheel3_vel, time);

			// Publish odometry message
			if(last_state_publish_time_ + publish_period_ < time)
			{
				last_state_publish_time_ += publish_period_;
				// Compute and store orientation info
				const geometry_msgs::Quaternion orientation(
						tf::createQuaternionMsgFromYaw(odometry_.getHeading()));

				// Populate odom message and publish
				if(odom_pub_->trylock())
				{
					odom_pub_->msg_.header.stamp = time;
					odom_pub_->msg_.pose.pose.position.x = odometry_.getX();
					odom_pub_->msg_.pose.pose.position.y = odometry_.getY();
					odom_pub_->msg_.pose.pose.orientation = orientation;
					odom_pub_->msg_.twist.twist.linear.x  = odometry_.getLinearX();
					odom_pub_->msg_.twist.twist.linear.y  = odometry_.getLinearY();
					odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular();
					odom_pub_->unlockAndPublish();
				}

				// Publish tf /odom frame
				if (enable_odom_tf_ && tf_odom_pub_->trylock())
				{
					geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0];
					odom_frame.header.stamp = time;
					odom_frame.transform.translation.x = odometry_.getX();
					odom_frame.transform.translation.y = odometry_.getY();
					odom_frame.transform.rotation = orientation;
					tf_odom_pub_->unlockAndPublish();
				}
			}

			// MOVE ROBOT
			// Retreive current velocity command and time step:
			Commands curr_cmd = *(command_.readFromRT());
			const double dt = (time - curr_cmd.stamp).toSec();

			// Brake if cmd_vel has timeout:
			if (dt > cmd_vel_timeout_)
			{
				//ROS_INFO("wocao nima");
				curr_cmd.linear_x = 0.0;
				curr_cmd.linear_y = 0.0;
				curr_cmd.angular = 0.0;
			}

			// Limit velocities and accelerations:
			const double cmd_dt(period.toSec());
			limiter_linear_x_.limit(curr_cmd.linear_x, last_cmd_.linear_x, cmd_dt);
			limiter_linear_y_.limit(curr_cmd.linear_y, last_cmd_.linear_y, cmd_dt);
			limiter_angular_. limit(curr_cmd.angular,  last_cmd_.angular,  cmd_dt);
			last_cmd_ = curr_cmd;

			// Compute wheels velocities:
			const double vth = curr_cmd.angular, vx = - curr_cmd.linear_x, vy = curr_cmd.linear_y;
			const double psi = 2 * 3.14159265 / 3;
			const double L = wheel_center_;


			const double wheel1_cmd  = (L*vth - vy) / wheel_radius_;
			const double wheel2_cmd  = (L*vth + vy*cos(psi/2) - vx*sin(psi/2)) / wheel_radius_;
			const double wheel3_cmd  = (L*vth + vy*cos(psi/2) + vx*sin(psi/2)) / wheel_radius_;

			ROS_INFO("send velocity is %f %f %f", wheel1_cmd,wheel2_cmd,wheel3_cmd);

			wheel1_joint.setCommand(wheel1_cmd);
			wheel2_joint.setCommand(wheel2_cmd);
			wheel3_joint.setCommand(wheel3_cmd);
}

void JointPositionController::update(const ros::Time &time, const ros::Duration &period) {
    ROS_DEBUG_STREAM("updating");
    
    // compute velocities and accelerations by hand just to be sure
    for (int i = 0; i < n_joints_; i++) {
        double current_position = joints_[i].getPosition();
        current_velocities_[i] = (current_position - previous_positions_[i]) / period.toSec();
        current_accelerations_[i] = (current_velocities_[i] - previous_velocities_[i]) / period.toSec();
        previous_positions_[i] = current_position;
        previous_velocities_[i] = current_velocities_[i];
    }
    
    if (new_reference_) {
        // Read the latest commanded trajectory message
        commanded_trajectory_ = *trajectory_command_buffer_.readFromRT();
        new_reference_ = false;
        
        for (int i = 0; i < n_joints_; i ++) {
            if (std::abs(commanded_trajectory_.positions[i] - last_commanded_trajectory_.positions[i]) > command_update_tolerance_) {
                must_recompute_trajectory_ = true;
                reached_reference_ = false;
                last_commanded_trajectory_ = commanded_trajectory_;
            }
        }
    }

    // Initialize RML result
    int rml_result = 0;
    
    ROS_DEBUG_STREAM("checking for having to recompute");


    // Compute RML traj after the start time and if there are still points in the queue
    if (must_recompute_trajectory_) {
        // Compute the trajectory
        ROS_WARN_STREAM("RML Recomputing trajectory...");

        // Update RML input parameters
        for (int i = 0; i < n_joints_; i++) {
            
            rml_in_->CurrentPositionVector->VecData[i] = joints_[i].getPosition();
            rml_in_->CurrentVelocityVector->VecData[i] = current_velocities_[i];
            rml_in_->CurrentAccelerationVector->VecData[i] = current_accelerations_[i];

            rml_in_->TargetPositionVector->VecData[i] = commanded_trajectory_.positions[i];
            rml_in_->TargetVelocityVector->VecData[i] = commanded_trajectory_.velocities[i];

            rml_in_->SelectionVector->VecData[i] = true;
        }
        
        
        ROS_DEBUG_STREAM("Current position: " << std::endl << *(rml_in_->CurrentPositionVector));
        ROS_DEBUG_STREAM("Target position: " << std::endl << *(rml_in_->TargetPositionVector));

        // Store the traj start time
        traj_start_time_ = time;

        // Set desired execution time for this trajectory (definitely > 0)
        rml_in_->SetMinimumSynchronizationTime(minimum_synchronization_time_);

//         ROS_DEBUG_STREAM("RML IN: time: " << rml_in_->GetMinimumSynchronizationTime());

        // Specify behavior after reaching point
        rml_flags_.BehaviorAfterFinalStateOfMotionIsReached = recompute_trajectory_ ? RMLPositionFlags::RECOMPUTE_TRAJECTORY : RMLPositionFlags::KEEP_TARGET_VELOCITY;
        rml_flags_.SynchronizationBehavior = RMLPositionFlags::ONLY_TIME_SYNCHRONIZATION;
        rml_flags_.KeepCurrentVelocityInCaseOfFallbackStrategy = true;

        // Compute trajectory
        rml_result = rml_->RMLPosition(*rml_in_.get(),
                                       rml_out_.get(),
                                       rml_flags_);

        // Disable recompute flag
        must_recompute_trajectory_ = false;
    } 
    
    // Sample the already computed trajectory
    rml_result = rml_->RMLPositionAtAGivenSampleTime(
                        (time - traj_start_time_ + period).toSec(),
                        rml_out_.get());

    // Determine if any of the joint tolerances have been violated
    for (int i = 0; i < n_joints_; i++) {
        double tracking_error = std::abs(rml_out_->NewPositionVector->VecData[i] - joints_[i].getPosition());

        if (tracking_error > position_tolerances_[i]) {
            must_recompute_trajectory_ = true;
            ROS_WARN_STREAM("Tracking for joint " << i << " outside of tolerance! (" << tracking_error 
                << " > " << position_tolerances_[i] << ")");
        }
    }

    // Compute command
    for (int i = 0; i < n_joints_; i++) {
        commanded_positions_[i] = rml_out_->NewPositionVector->VecData[i];
    }

    // Only set a different position command if the
    switch (rml_result) {
    case ReflexxesAPI::RML_WORKING:
//.........这里部分代码省略.........

    void YouBotUniversalController::updateJointVelocity(double setPoint, pr2_mechanism_model::JointState* joint_state_, control_toolbox::Pid* pid_controller_, const ros::Duration& dt, const int& jointIndex) {
        double error(0);
        assert(joint_state_->joint_);

        double command_ = setPoint;
        double velocity_ = joint_state_->velocity_;
        const double T = 1;
        filteredVelocity[jointIndex] = filteredVelocity[jointIndex] + (velocity_ - filteredVelocity[jointIndex]) * dt.toSec() / (dt.toSec() + T);
        error = filteredVelocity[jointIndex] - command_;
        ROS_DEBUG("Current velocity: %f, filtered velocity: %f, Error: %f\n", velocity_, filteredVelocity[jointIndex], error);

        double commanded_effort = pid_controller_ -> updatePid(error, dt);
        joint_state_->commanded_effort_ = commanded_effort;

    }

void QuadrotorHardwareSim::readSim(ros::Time time, ros::Duration period)
{
  // call all available subscriber callbacks now
  callback_queue_.callAvailable();

  // read state from Gazebo
  const double acceleration_time_constant = 0.1;
  gz_pose_             =  link_->GetWorldPose();
  gz_acceleration_     = ((link_->GetWorldLinearVel() - gz_velocity_) + acceleration_time_constant * gz_acceleration_) / (period.toSec() + acceleration_time_constant);
  gz_velocity_         =  link_->GetWorldLinearVel();
  gz_angular_velocity_ =  link_->GetWorldAngularVel();

  if (!subscriber_state_) {
    header_.frame_id = "/world";
    header_.stamp = time;
    pose_.position.x = gz_pose_.pos.x;
    pose_.position.y = gz_pose_.pos.y;
    pose_.position.z = gz_pose_.pos.z;
    pose_.orientation.w = gz_pose_.rot.w;
    pose_.orientation.x = gz_pose_.rot.x;
    pose_.orientation.y = gz_pose_.rot.y;
    pose_.orientation.z = gz_pose_.rot.z;
    twist_.linear.x = gz_velocity_.x;
    twist_.linear.y = gz_velocity_.y;
    twist_.linear.z = gz_velocity_.z;
    twist_.angular.x = gz_angular_velocity_.x;
    twist_.angular.y = gz_angular_velocity_.y;
    twist_.angular.z = gz_angular_velocity_.z;
    acceleration_.x = gz_acceleration_.x;
    acceleration_.y = gz_acceleration_.y;
    acceleration_.z = gz_acceleration_.z;
  }

  if (!subscriber_imu_) {
    imu_.orientation.w = gz_pose_.rot.w;
    imu_.orientation.x = gz_pose_.rot.x;
    imu_.orientation.y = gz_pose_.rot.y;
    imu_.orientation.z = gz_pose_.rot.z;

    gazebo::math::Vector3 gz_angular_velocity_body = gz_pose_.rot.RotateVectorReverse(gz_angular_velocity_);
    imu_.angular_velocity.x = gz_angular_velocity_body.x;
    imu_.angular_velocity.y = gz_angular_velocity_body.y;
    imu_.angular_velocity.z = gz_angular_velocity_body.z;

    gazebo::math::Vector3 gz_linear_acceleration_body = gz_pose_.rot.RotateVectorReverse(gz_acceleration_ - physics_->GetGravity());
    imu_.linear_acceleration.x = gz_linear_acceleration_body.x;
    imu_.linear_acceleration.y = gz_linear_acceleration_body.y;
    imu_.linear_acceleration.z = gz_linear_acceleration_body.z;
  }
}

    void ITRCartesianImpedanceController::update(const ros::Time& time, const ros::Duration& period)
    {
        // get current values
        //std::cout << "Update current values" << std::endl;
        KDL::Rotation cur_R(cart_handles_.at(0).getPosition(),
                            cart_handles_.at(1).getPosition(),
                            cart_handles_.at(2).getPosition(),
                            cart_handles_.at(4).getPosition(),
                            cart_handles_.at(5).getPosition(),
                            cart_handles_.at(6).getPosition(),
                            cart_handles_.at(8).getPosition(),
                            cart_handles_.at(9).getPosition(),
                            cart_handles_.at(10).getPosition());
        KDL::Vector cur_p(cart_handles_.at(3).getPosition(),
                            cart_handles_.at(7).getPosition(),
                            cart_handles_.at(11).getPosition());
        KDL::Frame cur_T( cur_R, cur_p );
        x_cur_ = cur_T;

        std::vector<double> cur_T_FRI;


        // Cartesian speed limit for new positions:

        // create quaternions for current and desired orientation, normalize them to get correct angular distance later!
        x_cur_quat_ = Eigen::Matrix3d(x_cur_.M.data).transpose();
        x_cur_quat_.normalize();
        x_des_quat_ = Eigen::Matrix3d(x_des_.M.data).transpose();
        x_des_quat_.normalize();
        x_set_quat_ = x_des_quat_;

        // get linear desired velocity
        x_dot_.vel = (x_des_.p-x_set_.p) / period.toSec();

        // limit linear velocity to desired position
        double x_dot_vel_norm;
        x_dot_vel_norm = x_dot_.vel.Norm();
        if (x_dot_vel_norm > max_trans_speed_) {
            x_dot_.vel = x_dot_.vel / x_dot_vel_norm * max_trans_speed_;
        }       

        if (cmd_flag_) {

            // Interpolate position
            x_set_.p = x_prev_.p + (x_dot_.vel * period.toSec());

            // Interpolate orientation with SLERP
            double slerp_ratio = max_rot_speed_ / (x_des_quat_.angularDistance(x_prev_quat_)) * period.toSec();
            slerp_ratio = std::min(slerp_ratio, 1.0);
            x_set_quat_ = x_prev_quat_.slerp(slerp_ratio, x_des_quat_);

            // convert quaternion to rot matrix
            x_set_quat_.normalize(); // need to be normalized to get right rotation matrix
            x_set_.M = KDL::Rotation::Quaternion(x_set_quat_.x(), x_set_quat_.y(), x_set_quat_.z(), x_set_quat_.w());
        }

        fromKDLtoFRI(x_set_, cur_T_FRI);
        // forward commands to hwi
        for(int c = 0; c < 30; ++c)
        {
            if(c < 12)
                cart_handles_.at(c).setCommand(cur_T_FRI.at(c));
            if(c > 11 && c < 18)
                cart_handles_.at(c).setCommand(k_des_[c-12]);
            if(c > 17 && c < 24)
                cart_handles_.at(c).setCommand(d_des_[c-18]);
            if(c > 23 && c < 30)
                cart_handles_.at(c).setCommand(f_des_[c-24]);
        }

        x_prev_ = x_set_;
        x_prev_quat_ = x_set_quat_;
    }

void CassiopeiaHW::readEncoders(ros::Duration dt)
{
// read robots joint state
  // right card
  if (! dm6814_right.ReadEncoder6814(1, &encoder_1_val))
    ROS_ERROR("ERROR: ReadEncoder6814(1) FAILED");

  if (! dm6814_right.ReadEncoder6814(2, &encoder_2_val))
    ROS_ERROR("ERROR: ReadEncoder6814(2) FAILED");

  if (! dm6814_right.ReadEncoder6814(3, &encoder_3_val))
    ROS_ERROR("ERROR: ReadEncoder6814(3) FAILED");
  //left card
  if (! dm6814_left.ReadEncoder6814(1, &encoder_4_val))
    ROS_ERROR("ERROR: ReadEncoder6814(1) FAILED");

  if (! dm6814_left.ReadEncoder6814(2, &encoder_5_val))
    ROS_ERROR("ERROR: ReadEncoder6814(2) FAILED");

  if (! dm6814_left.ReadEncoder6814(3, &encoder_6_val))
    ROS_ERROR("ERROR: ReadEncoder6814(3) FAILED");



// Handling of encoder value overflow
  if      ((int)encoder_1_val - (int)encoder_1_old < -62000) encoder_1_ovf++;
  else if ((int)encoder_1_val - (int)encoder_1_old >  62000) encoder_1_ovf--;
  encoder_1_old = encoder_1_val;
  encoder_1 = (int)encoder_1_val + 65535*encoder_1_ovf;

  if      ((int)encoder_2_val - (int)encoder_2_old < -62000) encoder_2_ovf++;
  else if ((int)encoder_2_val - (int)encoder_2_old >  62000) encoder_2_ovf--;
  encoder_2_old = encoder_2_val;
  encoder_2 = (int)encoder_2_val + 65535*encoder_2_ovf;

  if      ((int)encoder_3_val - (int)encoder_3_old < -62000) encoder_3_ovf++;
  else if ((int)encoder_3_val - (int)encoder_3_old >  62000) encoder_3_ovf--;
  encoder_3_old = encoder_3_val;
  encoder_3 = (int)encoder_3_val + 65535*encoder_3_ovf;

  if      ((int)encoder_4_val - (int)encoder_4_old < -62000) encoder_4_ovf++;
  else if ((int)encoder_4_val - (int)encoder_4_old >  62000) encoder_4_ovf--;
  encoder_4_old = encoder_4_val;
  encoder_4 = (int)encoder_4_val + 65535*encoder_4_ovf;

  if      ((int)encoder_5_val - (int)encoder_5_old < -62000) encoder_5_ovf++;
  else if ((int)encoder_5_val - (int)encoder_5_old >  62000) encoder_5_ovf--;
  encoder_5_old = encoder_5_val;
  encoder_5 = (int)encoder_5_val + 65535*encoder_5_ovf;

  if      ((int)encoder_6_val - (int)encoder_6_old < -62000) encoder_6_ovf++;
  else if ((int)encoder_6_val - (int)encoder_6_old >  62000) encoder_6_ovf--;
  encoder_6_old = encoder_6_val;
  encoder_6 = (int)encoder_6_val + 65535*encoder_6_ovf;

  ROS_DEBUG("1: %d, 2: %d, 3: %d, 4: %d, 5: %d, 6: %d", encoder_1, encoder_2, encoder_3, encoder_4, encoder_5, encoder_6);

// Position Calculation
  pos[0]=  (double)encoder_1*2*M_PI/(4096*190) - offset_pos[0];
  pos[1]= -(double)encoder_2*2*M_PI/(4096*190) - offset_pos[1];
  pos[2]=  (double)encoder_3*2*M_PI/(4096*190) - offset_pos[2];//disconnected
  
  pos[3]=  (double)encoder_4*2*M_PI/(4096*190) - offset_pos[3];
  pos[4]=  (double)encoder_5*2*M_PI/(4096*190) - offset_pos[4];
  pos[5]=  (double)encoder_6*2*M_PI/ 4096      - offset_pos[5];

  ROS_DEBUG("encoder6: %d, pos (10^3): %d", encoder_6, (int)(pos[5]*1000));
  
  ROS_DEBUG("POS: 1: %f, 2: %f, 3: %f, 4: %f, 5: %f, 6: %f", pos[0], pos[1], pos[2], pos[3], pos[4], pos[5]);

// Speed Calculation
  for(int i=0; i<6; i++)
  {
    vel[i]=(pos[i] - prev_pos[i])/dt.toSec();
    prev_pos[i] = pos[i];
  }

}

	int executeCB(ros::Duration dt)
		{
			std::cout << "**GoToWaypoint -%- Executing Main Task, elapsed_time: "
			          << dt.toSec() << std::endl;
			std::cout << "**GoToWaypoint -%- execute_time: "
			          << execute_time_.toSec() << std::endl;
			execute_time_ += dt;

			std::cout << "**Counter value:" << counter << std::endl;
			if (counter > 1)
				std::cout << "********************************************************************************" << std::endl;


			if (!init_)
			{
				initialize();
				init_ = true;
			}
			if ( (ros::Time::now() - time_at_pos_).toSec() < 0.2 )
			{
				if( message_.type == "goto")
				{
						// Goal position of ball relative to ROBOT_FRAME
						float goal_x = 0.00;
						float goal_y = 0.00;
						float error_x = message_.x - goal_x;
						float error_y = message_.y - goal_y;
						if (fabs(error_x) < 0.12 && fabs(error_y) < 0.12)
						{
							std::cout << "Closeness count " << closeness_count << std::endl;
							closeness_count++;
							//If the NAO has been close for enough iterations, we consider to goal reached
							if (closeness_count > 0)
							{
								motion_proxy_ptr->stopMove();
								set_feedback(SUCCESS);
								// std::cout << "sleeeping for 2 second before destroying thread" << std::endl;
								// sleep(2.0);
								finalize();
								return 1;
							}
						}
						else
						{
							closeness_count = 0;
						}
						//Limit the "error" in order to limit the walk speed
						error_x = error_x >  0.6 ?  0.6 : error_x;
						error_x = error_x < -0.6 ? -0.6 : error_x;
						error_y = error_y >  0.6 ?  0.6 : error_y;
						error_y = error_y < -0.6 ? -0.6 : error_y;
						// float speed_x = error_x * 1.0/(2+5*closeness_count);
						// float speed_y = error_y * 1.0/(2+5*closeness_count);
						// float frequency = 0.1/(5*closeness_count+(1.0/(fabs(error_x)+fabs(error_y)))); //Frequency of foot steps
						// motion_proxy_ptr->setWalkTargetVelocity(speed_x, speed_y, 0.0, frequency);
						// ALMotionProxy::setWalkTargetVelocity(const float& x, const float& y, const float& theta, const float& frequency)
						AL::ALValue walk_config;
						//walk_config.arrayPush(AL::ALValue::array("MaxStepFrequency", frequency));
						//Lower value of step height gives smoother walking
						// std::cout << "y " << message_.y << std::endl;
						if (fabs(message_.y) < 0.10)
						{
							// walk_config.arrayPush(AL::ALValue::array("StepHeight", 0.01));
							// motion_proxy_ptr->post.moveTo(message_.x, 0.0, 0.0, walk_config);
							motion_proxy_ptr->post.moveTo(message_.x, 0.0, 0.0);
							sleep(2.0);
							//motion_proxy_ptr->post.stopMove();
						}
						else
						{
							// walk_config.arrayPush(AL::ALValue::array("StepHeight", 0.005));
							// motion_proxy_ptr->post.moveTo(0.0, 0.0, message_.y/fabs(message_.y)*0.1, walk_config);
							motion_proxy_ptr->post.moveTo(0.0, 0.0, message_.y/fabs(message_.y)*0.1);
							//sleep(3.0);
							//motion_proxy_ptr->post.stopMove();
						}
				}

			}
			set_feedback(RUNNING);
			return 0;
		}

	void OneTaskInverseDynamicsJL::update(const ros::Time& time, const ros::Duration& period)
	{
		// get joint positions
  		for(int i=0; i < joint_handles_.size(); i++) 
  		{
    		joint_msr_states_.q(i) = joint_handles_[i].getPosition();
    		joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
    	}

    	// clearing msgs before publishing
    	msg_err_.data.clear();
    	msg_pose_.data.clear();
    	
    	if (cmd_flag_)
    	{
    		// resetting N and tau(t=0) for the highest priority task
    		N_trans_ = I_;	
    		SetToZero(tau_);

    		// computing Inertia, Coriolis and Gravity matrices
		    id_solver_->JntToMass(joint_msr_states_.q, M_);
		    id_solver_->JntToCoriolis(joint_msr_states_.q, joint_msr_states_.qdot, C_);
		    id_solver_->JntToGravity(joint_msr_states_.q, G_);
		    G_.data.setZero();

		    // computing the inverse of M_ now, since it will be used often
		    pseudo_inverse(M_.data,M_inv_,false); //M_inv_ = M_.data.inverse(); 


	    	// computing Jacobian J(q)
	    	jnt_to_jac_solver_->JntToJac(joint_msr_states_.q,J_);

	    	// computing the distance from the mid points of the joint ranges as objective function to be minimized
	    	phi_ = task_objective_function(joint_msr_states_.q);

	    	// using the first step to compute jacobian of the tasks
	    	if (first_step_)
	    	{
	    		J_last_ = J_;
	    		phi_last_ = phi_;
	    		first_step_ = 0;
	    		return;
	    	}

	    	// computing the derivative of Jacobian J_dot(q) through numerical differentiation
	    	J_dot_.data = (J_.data - J_last_.data)/period.toSec();

	    	// computing forward kinematics
	    	fk_pos_solver_->JntToCart(joint_msr_states_.q,x_);

	    	if (Equal(x_,x_des_,0.05))
	    	{
	    		ROS_INFO("On target");
	    		cmd_flag_ = 0;
	    		return;	    		
	    	}

	    	// pushing x to the pose msg
	    	for (int i = 0; i < 3; i++)
	    		msg_pose_.data.push_back(x_.p(i));

	    	// setting marker parameters
	    	set_marker(x_,msg_id_);

	    	// computing end-effector position/orientation error w.r.t. desired frame
	    	x_err_ = diff(x_,x_des_);

	    	x_dot_ = J_.data*joint_msr_states_.qdot.data;    	

	    	// setting error reference
	    	for(int i = 0; i < e_ref_.size(); i++)
		    {
	    		// e = x_des_dotdot + Kd*(x_des_dot - x_dot) + Kp*(x_des - x)
	    		e_ref_(i) =  -Kd_(i)*(x_dot_(i)) + Kp_(i)*x_err_(i);
    			msg_err_.data.push_back(e_ref_(i));
	    	}

    		// computing b = J*M^-1*(c+g) - J_dot*q_dot
    		b_ = J_.data*M_inv_*(C_.data + G_.data) - J_dot_.data*joint_msr_states_.qdot.data;

	    	// computing omega = J*M^-1*N^T*J
	    	omega_ = J_.data*M_inv_*N_trans_*J_.data.transpose();

	    	// computing lambda = omega^-1
	    	pseudo_inverse(omega_,lambda_);
	    	//lambda_ = omega_.inverse();

	    	// computing nullspace
	    	N_trans_ = N_trans_ - J_.data.transpose()*lambda_*J_.data*M_inv_;  	    		

	    	// finally, computing the torque tau
	    	tau_.data = J_.data.transpose()*lambda_*(e_ref_ + b_) + N_trans_*(Eigen::Matrix<double,7,1>::Identity(7,1)*(phi_ - phi_last_)/(period.toSec()));

	    	// saving J_ and phi of the last iteration
	    	J_last_ = J_;
	    	phi_last_ = phi_;
	
    	}

    	// set controls for joints
//.........这里部分代码省略.........