void BacksteppingController::starting(const ros::Time& time) { // 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(); joint_des_states_.q(i) = joint_msr_states_.q(i); joint_des_states_.qdot(i) = joint_msr_states_.qdot(i); Kp_(i) = 500; Kd_(i) = 2; } Kp = 200; Ki = 1; Kd = 5; for (int i = 0; i < PIDs_.size(); i++) PIDs_[i].initPid(Kp,Ki,Kd,0.1,-0.1); //ROS_INFO("PIDs gains are: Kp = %f, Ki = %f, Kd = %f",Kp,Ki,Kd); fk_pos_solver_->JntToCart(joint_msr_states_.q,x0_); ROS_INFO("x: %f, y: %f, z:%f",x0_.p(0),x0_.p(1),x0_.p(2)); first_step_ = 1; cmd_flag_ = 1; step_ = 0; }
void OneTaskInverseDynamicsJL::starting(const ros::Time& time) { // 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(); joint_des_states_.q(i) = joint_msr_states_.q(i); joint_des_states_.qdot(i) = joint_msr_states_.qdot(i); Kp_(i) = 100; Kd_(i) = 20; } Kp = 200; Ki = 1; Kd = 5; for (int i = 0; i < PIDs_.size(); i++) PIDs_[i].initPid(Kp,Ki,Kd,0.1,-0.1); //ROS_INFO("PIDs gains are: Kp = %f, Ki = %f, Kd = %f",Kp,Ki,Kd); I_ = Eigen::Matrix<double,7,7>::Identity(7,7); e_ref_ = Eigen::Matrix<double,6,1>::Zero(); first_step_ = 0; cmd_flag_ = 0; step_ = 0; }
void DynamicSlidingModeControllerTaskSpace::starting(const ros::Time& time) { // 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(); joint_des_states_.q(i) = joint_msr_states_.q(i); sigma_(i) = 0; // coefficients > 0 Kd_(i) = 10; gamma_(i) = 1; alpha_(i) = 12; lambda_(i) = 1; k_(i) = 3; } //x_des_ = KDL::Frame(KDL::Rotation::RPY(0,0,0),KDL::Vector(-0.4,0.3,1.5)); cmd_flag_ = 0; step_ = 0; }
void BacksteppingController::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(); //joint_des_states_.q(i) = joint_msr_states_.q(i); //joint_des_states_.qdot(i) = joint_msr_states_.qdot(i); } /* TASK 0 (Main)*/ double freq_ = 3.0; double amp_ = 0.1; //Desired posture 8 figure (circle figure) x_des_.p(0) = /*0.3*/0.25 + /*amp_*(1 - cos(freq_*period.toSec()*step_));*/amp_/2*sin(freq_*period.toSec()*step_); x_des_.p(1) = /*0.3*/0 + /*amp_*sin(freq_*period.toSec()*step_);*/amp_/2*sin(2*freq_*period.toSec()*step_); x_des_.p(2) = /*1.1*/1.75; x_des_.M = KDL::Rotation(KDL::Rotation::RPY(0,0,0));//RPY(0,3.1415,0)); //velocity x_des_dot_(0) = /*amp_*freq_*sin(freq_*period.toSec()*step_);*/amp_/2*freq_*cos(freq_*period.toSec()*step_); x_des_dot_(1) = /*amp_*freq_*cos(freq_*period.toSec()*step_);*/amp_/2*2*freq_*cos(2*freq_*period.toSec()*step_); x_des_dot_(2) = 0; x_des_dot_(3) = 0; x_des_dot_(4) = 0; x_des_dot_(5) = 0; //acc x_des_dotdot_(0) = /*amp_*freq_*freq_*cos(freq_*period.toSec()*step_);*/-amp_/2*freq_*freq_*sin(freq_*period.toSec()*step_); x_des_dotdot_(1) = /*-amp_*freq_*freq_*sin(freq_*period.toSec()*step_);*/-amp_/2*2*2*freq_*freq_*sin(freq_*period.toSec()*step_); x_des_dotdot_(2) = 0; x_des_dotdot_(3) = 0; x_des_dotdot_(4) = 0; x_des_dotdot_(5) = 0; step_++; // clearing msgs before publishing msg_err_.data.clear(); msg_pose_.data.clear(); msg_traj_.data.clear(); if (cmd_flag_) { // 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_); // computing Jacobian J(q) jnt_to_jac_solver_->JntToJac(joint_msr_states_.q,J_); // computing Jacobian pseudo-inversion pseudo_inverse(J_.data,J_pinv_); // using the first step to compute jacobian of the tasks if (first_step_) { J_last_ = J_; 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_); // pushing x to the pose msg for (int i = 0; i < 3; i++) { msg_pose_.data.push_back(x_.p(i)); msg_traj_.data.push_back(x_des_.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_); // computing task space velocity (of end-effector) x_dot_ = J_.data*joint_msr_states_.qdot.data; // computing reference variables in task space for (int i = 0; i < 6; i++) { x_ref_dot_(i) = x_des_dot_(i) + Kp_(i)*x_err_(i); x_ref_dotdot_(i) = x_des_dotdot_(i) + Kd_(i)*(x_des_dot_(i) - x_dot_(i)) + Kp_(i)*x_err_(i); } // computing reference variable in joint space joint_ref_.qdot.data = J_pinv_*x_ref_dot_; joint_ref_.qdotdot.data = J_pinv_*(x_ref_dotdot_ - J_dot_.data*joint_msr_states_.qdot.data); joint_des_states_.qdot.data = J_pinv_*x_des_dot_; joint_des_states_.q.data += period.toSec()*joint_des_states_.qdot.data; // finally, computing the torque tau tau_.data = M_.data*joint_ref_.qdotdot.data + C_.data.cwiseProduct(joint_ref_.qdot.data) + G_.data + /*K_d*/(joint_ref_.qdot.data - joint_msr_states_.qdot.data) + (joint_des_states_.q.data - joint_msr_states_.q.data); // saving J_ of the last iteration J_last_ = J_; } // set controls for joints for (int i = 0; i < joint_handles_.size(); i++) { if(cmd_flag_) joint_handles_[i].setCommand(tau_(i)); else joint_handles_[i].setCommand(PIDs_[i].computeCommand(joint_des_states_.q(i) - joint_msr_states_.q(i),period)); } // 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_); // publishing actual and desired trajectory for each task (links) as an array of ntasks*3 pub_pose_.publish(msg_pose_); pub_traj_.publish(msg_traj_); ros::spinOnce(); }
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 for (int i = 0; i < joint_handles_.size(); i++) { if(cmd_flag_) joint_handles_[i].setCommand(tau_(i)); else joint_handles_[i].setCommand(PIDs_[i].computeCommand(joint_des_states_.q(i) - joint_msr_states_.q(i),period)); } // publishing markers for visualization in rviz pub_marker_.publish(msg_marker_); msg_id_++; // publishing error pub_error_.publish(msg_err_); // publishing pose pub_pose_.publish(msg_pose_); ros::spinOnce(); }