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 CartesianComputedTorqueController::set_gains(const std_msgs::Float64MultiArray::ConstPtr &msg) { if(msg->data.size() == 2*joint_handles_.size()) { for(unsigned int i = 0; i < joint_handles_.size(); i++) { Kp_(i) = msg->data[i]; Kv_(i) = msg->data[i + joint_handles_.size()]; } } else ROS_INFO("Number of Joint handles = %lu", joint_handles_.size()); ROS_INFO("Num of Joint handles = %lu, dimension of message = %lu", joint_handles_.size(), msg->data.size()); ROS_INFO("New gains Kp: %.1lf, %.1lf, %.1lf %.1lf, %.1lf, %.1lf, %.1lf", Kp_(0), Kp_(1), Kp_(2), Kp_(3), Kp_(4), Kp_(5), Kp_(6)); ROS_INFO("New gains Kv: %.1lf, %.1lf, %.1lf %.1lf, %.1lf, %.1lf, %.1lf", Kv_(0), Kv_(1), Kv_(2), Kv_(3), Kv_(4), Kv_(5), Kv_(6)); }
void CartesianComputedTorqueController::starting(const ros::Time& time) { Kp_(0) = 3000; Kp_(1) = 1800; Kp_(2) = 1800; Kp_(3) = 1400; Kp_(4) = 100; Kp_(5) = 100; Kp_(6) = 100; Kv_(0) = 181; Kv_(1) = 82; Kv_(2) = 83; Kv_(3) = 54; Kv_(4) = 10; Kv_(5) = 10; Kv_(6) = 10; // get joint positions for(size_t i=0; i<joint_handles_.size(); i++) { //Kp_(i) = 500.0; //Kv_(i) = 150.0; 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(); joint_des_states_.q(i) = joint_msr_states_.q(i); } lambda = 0.1; // lower values: flatter cmd_flag_ = 0; ROS_INFO(" Number of joints in handle = %lu", joint_handles_.size() ); }
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(); }
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)); } }