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