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