void Arm_Cartesian_Control::process(
double dt,
KDL::JntArray& joint_positions,
KDL::Twist& targetVelocity, KDL::JntArrayVel& out_jnt_velocities) {
out_jnt_velocities.q.data = joint_positions.data;
double max_lin_frame_velocitiy = 0.1; // m/s
double max_joint_vel = 0.1; // radian/s
double eps_velocity = 0.0001;
// calc Jacobian
//KDL::Jacobian jacobian(arm.getNrOfJoints());
//jnt2jac->JntToJac(joint_positions, jacobian);
// calc absolute relative velocity; used for joint velocitiy reduction
double linear_frame_vel = targetVelocity.vel.Norm();
double angular_frame_vel = targetVelocity.rot.Norm();
if (linear_frame_vel < eps_velocity && angular_frame_vel < eps_velocity) {
out_jnt_velocities.qdot.data.setZero();
return;
}
if (linear_frame_vel > max_lin_frame_velocitiy) {
//reduce target velocity to maximum limit
targetVelocity.vel.data[0] *= (max_lin_frame_velocitiy / linear_frame_vel) ;
targetVelocity.vel.data[1] *= (max_lin_frame_velocitiy / linear_frame_vel) ;
targetVelocity.vel.data[2] *= (max_lin_frame_velocitiy / linear_frame_vel) ;
}
// calc joint velocitites
KDL::JntArray jntVel(arm_chain->getNrOfJoints());
ik_solver->CartToJnt(joint_positions, targetVelocity, jntVel);
// limit joint velocities
if (jntVel.data.norm() > 0.01) {
double max_velocity = jntVel.data.cwiseAbs().maxCoeff();
if (max_velocity > max_joint_vel) {
jntVel.data /= max_velocity;
jntVel.data *= max_joint_vel;
}
} else {
jntVel.data.Zero(arm_chain->getNrOfJoints());
}
//check for joint limits
checkLimits(dt, joint_positions, jntVel);
out_jnt_velocities.qdot.data = jntVel.data;
}
void KukaRMControllerRTNET::updateHook()
{
std::string fri_mode("e_fri_unkown_mode");
bool fri_cmd_mode = false;
RTT::FlowStatus fs_event = iport_events.read(fri_mode);
if (fri_mode == "e_fri_cmd_mode")
fri_cmd_mode = true;
else if (fri_mode == "e_fri_mon_mode")
fri_cmd_mode = false;
Eigen::VectorXd tau_dyn(LWRDOF);
Eigen::VectorXd F(LWRDOF);
std::vector<double> jntPos(LWRDOF);
std::vector<double> jntVel(LWRDOF);
std::vector<double> jntTrq(LWRDOF);
std::vector<double> tau_FRI(LWRDOF);
/* Get des mesures des capteurs */
RTT::FlowStatus joint_pos_fs = iport_msr_joint_pos.read(jntPos);
RTT::FlowStatus joint_vel_fs = iport_msr_joint_vel.read(jntVel);
RTT::FlowStatus joint_trq_fs = iport_msr_joint_trq.read(jntTrq);
if(fri_cmd_mode){
if(m_compteur<5000)
{
/* Mise a jour des mesures */
if(joint_trq_fs==RTT::NewData){
for(unsigned int i=0;i<LWRDOF;i++){
m_tau_mes[i] = jntTrq[i];
}
}
if(joint_pos_fs==RTT::NewData){
for(unsigned int i=0;i<LWRDOF;i++){
m_q_mes[i] = jntPos[i];
}
m_model->setJointPositions(m_q_mes); //Mise a jour des positions dans le modele
}
if(joint_vel_fs==RTT::NewData){
fdm(jntVel); //calcule l acceleration qpp
for(unsigned int i=0;i<LWRDOF;i++){
m_qp_mes[i] = jntVel[i];
}
m_model->setJointVelocities(m_qp_mes); //Mise a jour des vitesses dans le modele
}
/* Mise a jour des attributs */
tau_dyn = m_tau_mes-m_tau_FRI;
m_H = m_model->getInertiaMatrix();
m_c = m_model->getNonLinearTerms();
m_g = m_model->getGravityTerms();
/* Estimation de F */
F = m_qpp_mes-m_H.inverse()*(m_tau_FRI-m_c-m_g-m_pid);
//F = m_qpp_mes-m_H.inverse()*(m_tau_FRI+tau_dyn-m_c-m_g-m_pid);
estimateF(F);
//estimateAMAF(F);
/* Calcul des positions, vitesses et accelerations desirees */
m_time = m_time+m_dt; //Mise a jour du temps
nextDesVal();
/* Calcul du PID */
pidCalc();
/* Calcul de la commande */
m_tau_FRI.setZero(); //m_H*(m_qpp_des-F)+m_c+m_g+m_pid;
//m_tau_FRI = m_H*(m_qpp_des-F)+m_c+m_g-tau_dyn+m_pid;
if(m_writeInFile)
{
m_flux << std::endl;
m_flux << "temps : " << m_time << std::endl;
m_flux << "q_des : " << m_q_des.transpose() << std::endl;
m_flux << "q_mes : " << m_q_mes.transpose() << std::endl;
m_flux << "qp_des : " << m_qp_des.transpose() << std::endl;
m_flux << "qp_mes : " << m_qp_mes.transpose() << std::endl;
m_flux << "qpp_des : " << m_qpp_des.transpose() << std::endl;
m_flux << "qpp_mes : " << m_qpp_mes.transpose() << std::endl;
//m_flux << "F_est_sat : " << F.transpose() << std::endl;
//m_flux << "PID : " << m_pid.transpose() << std::endl;
//m_flux << "Hqpp : " << (m_H*m_qpp_des).transpose() << std::endl;
//m_flux << "coriolis : " << m_c.transpose() << std::endl;
//m_flux << "gravity : " << m_g.transpose() << std::endl;
m_flux << "tau_FRI : " << m_tau_FRI.transpose() << std::endl;
m_flux << "f_dynamics : " << tau_dyn.transpose() << std::endl;
m_flux << "gravity : " << m_g.transpose() << std::endl;
//m_flux << "tau_mes : " << m_tau_mes.transpose() << std::endl;
m_fluxTrq << std::endl << m_tau_mes.transpose() << ";...";
m_fluxErr << std::endl << (m_q_des-m_q_mes).transpose() << ";...";
}
for(unsigned int i=0;i<LWRDOF;i++){
tau_FRI[i] = m_tau_FRI[i];
}
oport_add_joint_trq.write(tau_FRI);
oport_joint_position.write(jntPos);
}
else{
m_compteur++;
}
}
}
