void MultiTaskPriorityInverseKinematics::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 error msg before publishing
    	msg_err_.data.clear();

    	if (cmd_flag_)
    	{
    		// resetting P and qdot(t=0) for the highest priority task
    		P_ = I_;	
    		SetToZero(joint_des_states_.qdot);

    		for (int index = 0; index < ntasks_; index++)
    		{
		    	// computing Jacobian
		    	jnt_to_jac_solver_->JntToJac(joint_msr_states_.q,J_,links_index_[index]);

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

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

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

		    	for(int i = 0; i < e_dot_.size(); i++)
		    	{
		    		e_dot_(i) = x_err_(i);
	    			msg_err_.data.push_back(e_dot_(i));
		    	}

		    	// computing (J[i]*P[i-1])^pinv
		    	J_star_.data = J_.data*P_;
		    	pseudo_inverse(J_star_.data,J_pinv_);

		    	// computing q_dot (qdot(i) = qdot[i-1] + (J[i]*P[i-1])^pinv*(x_err[i] - J[i]*qdot[i-1]))
		    	joint_des_states_.qdot.data = joint_des_states_.qdot.data + J_pinv_*(e_dot_ - J_.data*joint_des_states_.qdot.data);

		    	// stop condition
		    	if (!on_target_flag_[index])
		    	{
			    	if (Equal(x_,x_des_[index],0.01))
			    	{
			    		ROS_INFO("Task %d on target",index);
			    		on_target_flag_[index] = true;
			    		if (index == (ntasks_ - 1))
			    			cmd_flag_ = 0;
			    	}
			    }

		    	// updating P_ (it mustn't make use of the damped pseudo inverse)
		    	pseudo_inverse(J_star_.data,J_pinv_,false);
		    	P_ = P_ - J_pinv_*J_star_.data;
		    }

		    // integrating q_dot -> getting q (Euler method)
		    for (int i = 0; i < joint_handles_.size(); i++)
		    	joint_des_states_.q(i) += period.toSec()*joint_des_states_.qdot(i);			
    	}

    	// set controls for joints
    	for (int i = 0; i < joint_handles_.size(); i++)
    	{
    		tau_cmd_(i) = PIDs_[i].computeCommand(joint_des_states_.q(i) - joint_msr_states_.q(i),joint_des_states_.qdot(i) - joint_msr_states_.qdot(i),period);
    		joint_handles_[i].setCommand(tau_cmd_(i));
    	}

    	// publishing markers for visualization in rviz
    	pub_marker_.publish(msg_marker_);
    	msg_id_++;

	    // publishing error for all tasks as an array of ntasks*6
	    pub_error_.publish(msg_err_);
	    ros::spinOnce();

	}
	void TaskInverseKinematics::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();
    	}

    	if (cmd_flag_)
    	{
	    	// computing Jacobian
	    	jnt_to_jac_solver_->JntToJac(joint_msr_states_.q,J_);

	    	// computing J_pinv_
	    	pseudo_inverse(J_.data,J_pinv_);

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

	    	// end-effector position/orientation error
	    	x_err_.vel = x_des_.p - x_.p;

	    	// getting quaternion from rotation matrix
	    	x_.M.GetQuaternion(quat_curr_.v(0),quat_curr_.v(1),quat_curr_.v(2),quat_curr_.a);
	    	x_des_.M.GetQuaternion(quat_des_.v(0),quat_des_.v(1),quat_des_.v(2),quat_des_.a);

	    	skew_symmetric(quat_des_.v,skew_);

	    	for (int i = 0; i < skew_.rows(); i++)
	    	{
	    		v_temp_(i) = 0.0;
	    		for (int k = 0; k < skew_.cols(); k++)
	    			v_temp_(i) += skew_(i,k)*(quat_curr_.v(k));
	    	}

	    	x_err_.rot = quat_curr_.a*quat_des_.v - quat_des_.a*quat_curr_.v - v_temp_; 

	    	// computing q_dot
	    	for (int i = 0; i < J_pinv_.rows(); i++)
	    	{
	    		joint_des_states_.qdot(i) = 0.0;
	    		for (int k = 0; k < J_pinv_.cols(); k++)
	    			joint_des_states_.qdot(i) += J_pinv_(i,k)*x_err_(k); //removed scaling factor of .7
          
	    	}

	    	// integrating q_dot -> getting q (Euler method)
	    	for (int i = 0; i < joint_handles_.size(); i++)
	    		joint_des_states_.q(i) += period.toSec()*joint_des_states_.qdot(i);
			
			// joint limits saturation
	        for (int i =0;  i < joint_handles_.size(); i++)
	        {
	        	if (joint_des_states_.q(i) < joint_limits_.min(i))
	            	joint_des_states_.q(i) = joint_limits_.min(i);
	          	if (joint_des_states_.q(i) > joint_limits_.max(i))
	            	joint_des_states_.q(i) = joint_limits_.max(i);
	        }

	    	if (Equal(x_,x_des_,0.005))
	    	{
	    		ROS_INFO("On target");
	    		cmd_flag_ = 0;
	    	}
	    }
	    
    	// set controls for joints
    	for (int i = 0; i < joint_handles_.size(); i++)
    	{
    		tau_cmd_(i) = PIDs_[i].computeCommand(joint_des_states_.q(i) - joint_msr_states_.q(i),period);
		   	joint_handles_[i].setCommand(tau_cmd_(i));
        }
	}
    void CartesianComputedTorqueController::update(const ros::Time& time, const ros::Duration& period)
    {
    	// get joint positions
		for(size_t 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();
			joint_msr_states_.qdotdot(i) = joint_handles_[i].getAcceleration();
		}

		//ROS_INFO("-> msr values : %f, %f, %f, %f, %f, %f, %f!",joint_msr_states_.q(0), joint_msr_states_.q(1), joint_msr_states_.q(2), joint_msr_states_.q(3), joint_msr_states_.q(4), joint_msr_states_.q(5), joint_msr_states_.q(6));

    	if(cmd_flag_)
    	{
				#if TRACE_CartesianComputedTorqueController_ACTIVATED
					print_frame_("x_Solve_",x_Solve_);
				#endif

				// Get next 'smooth' position in 'OP_' object. 
				resultValue_  =   RML_->RMLPosition(*IP_,OP_.get(),Flags_);
				
				if (resultValue_ != ReflexxesAPI::RML_WORKING && resultValue_ != ReflexxesAPI::RML_FINAL_STATE_REACHED)
				{
					ROS_INFO("ERROR Reflexxes -> %d", resultValue_);
					exit(-1);
				}
				
				if (resultValue_ < 0)
				{
					ROS_INFO("GroupCommandControllerFRI::update : ERROR during trajectory generation err n°%d",resultValue_);
					exit(-1);
				}
				
				// set next desired 'smooth' states : position, velocity, acceleration
				for (int i = 0; i < joint_handles_.size(); i++)
				{
					joint_des_states_.q(i) = OP_->NewPositionVector->VecData[i];
					joint_des_states_.qdot(i) = OP_->NewVelocityVector->VecData[i];
					joint_des_states_.qdotdot(i) = OP_->NewAccelerationVector->VecData[i];

				}

				#if TRACE_CartesianComputedTorqueController_ACTIVATED
					fk_pos_solver_->JntToCart(joint_des_states_.q, x_Reflexxes_);
					print_frame_("x_Reflexxes_",x_Reflexxes_); 
				#endif

				double distance;
				
				// computing forward kinematics. Transform 'q' angle position to 3D pose in 'x_'.
				// It is necessary to get the 'euler' distance from 'x_des_' to 'x_'.
				fk_pos_solver_->JntToCart(joint_msr_states_.q, x_);
				//#if TRACE_CartesianComputedTorqueController_ACTIVATED
					//print_frame_(" x_", x_);
					//distance = sqrt(pow(x_des_.p.x()-x_.p.x(),2) + pow(x_des_.p.y()-x_.p.y(),2) + pow(x_des_.p.z()-x_.p.z(),2));
					//ROS_INFO("distance = %f\n",distance);  // Show the distance only for information.
				//#endif

				*IP_->CurrentPositionVector      =   *OP_->NewPositionVector      ;
				*IP_->CurrentVelocityVector      =   *OP_->NewVelocityVector      ;
				*IP_->CurrentAccelerationVector  =   *OP_->NewAccelerationVector  ;

				// Is the distance from 'x_des_' to 'x_' is close to 0.005 meter ?
				if (Equal(x_, x_des_, 0.005))
				{
						ROS_INFO("On target");
						// Computing 'euler' distance between measure (x_) to desired (x_des_).
						distance = sqrt(pow(x_des_.p.x()-x_.p.x(),2) + pow(x_des_.p.y()-x_.p.y(),2) + pow(x_des_.p.z()-x_.p.z(),2));
						ROS_INFO("distance = %f\n",distance);  // Show the distance only for information.
						cmd_flag_ = 0;
				}

    	}

    	// 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_);

        // PID controller
        KDL::JntArray pid_cmd_(joint_handles_.size());
        
        // Compensation of Coriolis and Gravity
        KDL::JntArray cg_cmd_(joint_handles_.size());
        
        for(size_t i=0; i<joint_handles_.size(); i++)
        {
            // PID control law.
            pid_cmd_(i) = joint_des_states_.qdotdot(i) + Kv_(i)*(joint_des_states_.qdot(i) - joint_msr_states_.qdot(i)) + Kp_(i)*(joint_des_states_.q(i) - joint_msr_states_.q(i));

			//pid_cmd_(i) =  Kv_(i)*(joint_des_states_.qdot(i) - joint_msr_states_.qdot(i)) + Kp_(i)*(joint_des_states_.q(i) - joint_msr_states_.q(i));
            //cg_cmd_(i) = C_(i)*joint_msr_states_.qdot(i) + G_(i);
            
            // Global control law
            cg_cmd_(i) = C_(i) + G_(i);
        }
        
        // Torque command : 'tau_cmd_'.
        tau_cmd_.data = M_.data * pid_cmd_.data;
        KDL::Add(tau_cmd_,cg_cmd_,tau_cmd_);
        
		//ROS_INFO("-> tau cmd values : %f, %f, %f, %f, %f, %f, %f!",tau_cmd_(0), tau_cmd_(1), tau_cmd_(2), tau_cmd_(3), tau_cmd_(4),tau_cmd_(5), tau_cmd_(6));

        for(size_t i=0; i<joint_handles_.size(); i++)
        {
            joint_handles_[i].setCommandTorque(tau_cmd_(i));
			joint_handles_[i].setCommandPosition(joint_msr_states_.q(i));
			//joint_handles_[i].setCommandPosition(pid_cmd_(i));
			//joint_handles_[i].setCommandPosition(joint_des_states_.q(i));
        }
    }