void RK_CALL manipulator_dynamics_model::computeStateRate(double aTime,const vect_n<double>& aState, vect_n<double>& aStateRate) {
setJointStates(aState);
doMotion();
clearForce();
doForce();
aStateRate.resize(getJointStatesCount());
unsigned int j = 0;
for(std::vector< shared_ptr< gen_coord<double> > >::const_iterator it = mCoords.begin();
it < mCoords.end(); ++it, ++j)
aStateRate[j] = (*it)->q_dot;
for(std::vector< shared_ptr< frame_2D<double> > >::const_iterator it = mFrames2D.begin();
it < mFrames2D.end(); ++it) {
aStateRate[j] = (*it)->Velocity[0]; ++j;
aStateRate[j] = (*it)->Velocity[1]; ++j;
aStateRate[j] = -(*it)->Rotation[1] * (*it)->AngVelocity; ++j;
aStateRate[j] = (*it)->Rotation[0] * (*it)->AngVelocity; ++j;
};
for(std::vector< shared_ptr< frame_3D<double> > >::const_iterator it = mFrames3D.begin();
it < mFrames3D.end(); ++it) {
aStateRate[j] = (*it)->Velocity[0]; ++j;
aStateRate[j] = (*it)->Velocity[1]; ++j;
aStateRate[j] = (*it)->Velocity[2]; ++j;
aStateRate[j] = (*it)->QuatDot[0]; ++j;
aStateRate[j] = (*it)->QuatDot[1]; ++j;
aStateRate[j] = (*it)->QuatDot[2]; ++j;
aStateRate[j] = (*it)->QuatDot[3]; ++j;
};
for(std::vector< shared_ptr< gen_coord<double> > >::const_iterator it = mCoords.begin();
it < mCoords.end(); ++it, ++j)
aStateRate[j] = (*it)->f;
for(std::vector< shared_ptr< frame_2D<double> > >::const_iterator it = mFrames2D.begin();
it < mFrames2D.end(); ++it) {
aStateRate[j] = (*it)->Force[0]; ++j;
aStateRate[j] = (*it)->Force[1]; ++j;
aStateRate[j] = (*it)->Torque; ++j;
};
for(std::vector< shared_ptr< frame_3D<double> > >::const_iterator it = mFrames3D.begin();
it < mFrames3D.end(); ++it) {
aStateRate[j] = (*it)->Force[0]; ++j;
aStateRate[j] = (*it)->Force[1]; ++j;
aStateRate[j] = (*it)->Force[2]; ++j;
aStateRate[j] = (*it)->Torque[0]; ++j;
aStateRate[j] = (*it)->Torque[1]; ++j;
aStateRate[j] = (*it)->Torque[2]; ++j;
};
mat<double,mat_structure::symmetric> Msys(getJointAccelerationsCount());
getMassMatrix(Msys);
try {
mat_vect_adaptor< vect_n<double> > acc_as_mat(aStateRate, getJointAccelerationsCount(), 1, getJointPositionsCount());
linsolve_Cholesky(Msys,acc_as_mat);
} catch(singularity_error& e) { RK_UNUSED(e);
std::stringstream ss; ss << "Mass matrix is singular in the manipulator model '" << getName() << "' at time " << aTime << " seconds.";
throw singularity_error(ss.str());
};
};
void JtATIController::updateHook()
{
if(use_ft_sensor && port_ftdata.read(wrench_msg) != RTT::NewData)
return;
if(!updateState()) return;
jnt_trq_cmd.setZero();
if(0)
{
getMassMatrix(mass);
id_dyn_solver->JntToMass(jnt_pos_kdl,mass_kdl);
RTT::log(RTT::Warning) << "mass : \n"<<mass<<RTT::endlog();
RTT::log(RTT::Warning) << "mass_kdl : \n"<<mass_kdl.data<<RTT::endlog();
}
if(use_ft_sensor){
if(use_kdl){
// remove B(q,qdot)
jnt_vel_kdl.data.setZero();
// Get the Wrench in the last segment's frame
tf::wrenchMsgToKDL(wrench_msg.wrench,wrench_kdl);
f_ext_kdl.back() = wrench_kdl;
// Get The full dynamics with external forces
id_rne_solver->CartToJnt(jnt_pos_kdl,jnt_vel_kdl,jnt_acc_kdl,f_ext_kdl,jnt_trq_kdl);
// Get Joint Gravity torque
id_dyn_solver->JntToGravity(jnt_pos_kdl,gravity_kdl);
// remove G(q)
jnt_trq_cmd = jnt_trq_kdl.data - gravity_kdl.data;
RTT::log(RTT::Debug) << "Adding FT data : \n"<<jnt_trq_cmd.transpose()<<RTT::endlog();
}else{
// Get Jacobian with KRC
getJacobian(J_tip_base);
getCartesianPosition(cart_pos);
tf::poseMsgToKDL(cart_pos,tool_in_base_frame);
tf::poseMsgToEigen(cart_pos,tool_in_base_frame_eigen);
J_tip_base.changeBase(tool_in_base_frame.M);
RTT::log(RTT::Debug) << "J : \n"<<J_tip_base.data<<RTT::endlog();
// Get Jacobian with KDL
jnt_to_jac_solver->JntToJac(jnt_pos_kdl,J_kdl,kdl_chain.getNrOfSegments());
RTT::log(RTT::Debug) << "J_kdl : \n"<<J_kdl.data<<RTT::endlog();
tf::wrenchMsgToEigen(wrench_msg.wrench,wrench);
//tf::wrenchMsgToKDL(wrench_msg.wrench,wrench_kdl);
wrench_kdl = tool_in_base_frame.M * wrench_kdl;
///tf::wrenchKDLToEigen(wrench_kdl,wrench);
jnt_trq_cmd = J_tip_base.data.transpose() * wrench;
}
}
if(use_kdl_gravity)
{
getGravityTorque(jnt_grav);
id_dyn_solver->JntToGravity(jnt_pos_kdl,gravity_kdl);
jnt_trq_cmd += kg.asDiagonal() * gravity_kdl.data - jnt_grav;
RTT::log(RTT::Debug) << "Adding gravity : \n"<<(kg.asDiagonal() * gravity_kdl.data - jnt_grav).transpose()<<RTT::endlog();
}
if(compensate_coriolis)
{
id_dyn_solver->JntToCoriolis(jnt_pos_kdl,jnt_vel_kdl,coriolis_kdl);
jnt_trq_cmd -= coriolis_kdl.data.asDiagonal()*jnt_vel;
RTT::log(RTT::Debug) << "Removing Coriolis : \n"<<(coriolis_kdl.data.asDiagonal()*jnt_vel).transpose()<<RTT::endlog();
}
if(add_damping)
{
jnt_trq_cmd -= kd.asDiagonal() * jnt_vel;
RTT::log(RTT::Debug) << "Adding damping : \n"<<-(kd.asDiagonal() * jnt_vel).transpose()<<RTT::endlog();
}
RTT::log(RTT::Debug) << "jnt_trq_cmd : \n"<<jnt_trq_cmd.transpose()<<RTT::endlog();
sendJointTorque(jnt_trq_cmd);
}
