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