RTC::ReturnCode_t TorqueController::onExecute(RTC::UniqueId ec_id)
{
m_loop++;
// make timestamp
coil::TimeValue coiltm(coil::gettimeofday());
hrp::dvector dq(m_robot->numJoints());
RTC::Time tm;
tm.sec = coiltm.sec();
tm.nsec = coiltm.usec()*1000;
// update port
if (m_tauCurrentInIn.isNew()) {
m_tauCurrentInIn.read();
}
if (m_tauMaxInIn.isNew()) {
m_tauMaxInIn.read();
}
if (m_qCurrentInIn.isNew()) {
m_qCurrentInIn.read();
}
if (m_qRefInIn.isNew()) {
m_qRefInIn.read();
}
if (m_qRefIn.data.length() == m_robot->numJoints() &&
m_tauCurrentIn.data.length() == m_robot->numJoints() &&
m_qCurrentIn.data.length() == m_robot->numJoints()) {
// update model
for ( int i = 0; i < m_robot->numJoints(); i++ ){
m_robot->joint(i)->q = m_qCurrentIn.data[i];
}
m_robot->calcForwardKinematics();
// calculate dq by torque controller
executeTorqueControl(dq);
// output restricted qRef
for (int i = 0; i < m_robot->numJoints(); i++) {
m_qRefOut.data[i] = std::min(std::max(m_qRefIn.data[i] + dq[i], m_robot->joint(i)->llimit), m_robot->joint(i)->ulimit);
}
} else {
if (isDebug()) {
std::cerr << "TorqueController input is not correct" << std::endl;
std::cerr << " numJoints: " << m_robot->numJoints() << std::endl;
std::cerr << " qCurrent: " << m_qCurrentIn.data.length() << std::endl;
std::cerr << " qRefIn: " << m_qRefIn.data.length() << std::endl;
std::cerr << "tauCurrent: " << m_tauCurrentIn.data.length() << std::endl;
std::cerr << std::endl;
}
// pass qRefIn to qRefOut
for (int i = 0; i < m_robot->numJoints(); i++) {
m_qRefOut.data[i] = m_qRefIn.data[i];
}
}
m_qRefOut.tm = tm;
m_qRefOutOut.write();
return RTC::RTC_OK;
}
RTC::ReturnCode_t StateHolder::onExecute(RTC::UniqueId ec_id)
{
//std::cout << "StateHolder::onExecute(" << ec_id << ")" << std::endl;
RTC::Time tm;
if (m_currentQIn.isNew()){
m_currentQIn.read();
tm = m_currentQ.tm;
}else{
coil::TimeValue coiltm(coil::gettimeofday());
tm.sec = coiltm.sec();
tm.nsec = coiltm.usec()*1000;
}
if (m_qIn.isNew()){
m_qIn.read();
}
if (m_tqIn.isNew()){
m_tqIn.read();
}
if (m_requestGoActual || (m_q.data.length() == 0 && m_currentQ.data.length() > 0)){
m_q = m_currentQ;
if (m_q.data.length() != m_tq.data.length()) m_tq.data.length(m_q.data.length());
// Reset reference wrenches to zero
for (unsigned int i=0; i<m_wrenchesIn.size(); i++){
m_wrenches[i].data[0] = m_wrenches[i].data[1] = m_wrenches[i].data[2] = 0.0;
m_wrenches[i].data[3] = m_wrenches[i].data[4] = m_wrenches[i].data[5] = 0.0;
}
}
if (m_requestGoActual){
m_requestGoActual = false;
m_waitSem.post();
}
if (m_basePosIn.isNew()){
m_basePosIn.read();
}
if (m_baseRpyIn.isNew()){
m_baseRpyIn.read();
}
if (m_zmpIn.isNew()){
m_zmpIn.read();
}
if (m_optionalDataIn.isNew()){
m_optionalDataIn.read();
}
for (size_t i = 0; i < m_wrenchesIn.size(); i++) {
if ( m_wrenchesIn[i]->isNew() ) {
m_wrenchesIn[i]->read();
}
}
double *a = m_baseTform.data.get_buffer();
a[0] = m_basePos.data.x;
a[1] = m_basePos.data.y;
a[2] = m_basePos.data.z;
hrp::Matrix33 R = hrp::rotFromRpy(m_baseRpy.data.r,
m_baseRpy.data.p,
m_baseRpy.data.y);
hrp::setMatrix33ToRowMajorArray(R, a, 3);
m_basePose.data.position = m_basePos.data;
m_basePose.data.orientation = m_baseRpy.data;
// put timestamps
m_q.tm = tm;
m_tq.tm = tm;
m_baseTform.tm = tm;
m_basePos.tm = tm;
m_baseRpy.tm = tm;
m_zmp.tm = tm;
m_basePose.tm = tm;
for (size_t i = 0; i < m_wrenches.size(); i++) {
m_wrenches[i].tm = tm;
}
// write
if (m_q.data.length() > 0){
m_qOut.write();
}
if (m_tq.data.length() > 0){
m_tqOut.write();
}
m_baseTformOut.write();
m_basePosOut.write();
m_baseRpyOut.write();
m_zmpOut.write();
m_basePoseOut.write();
m_optionalDataOut.write();
for (size_t i = 0; i < m_wrenchesOut.size(); i++) {
m_wrenchesOut[i]->write();
}
if (m_timeCount > 0){
m_timeCount--;
if (m_timeCount == 0) m_timeSem.post();
}
return RTC::RTC_OK;
}
RTC::ReturnCode_t TorqueFilter::onExecute(RTC::UniqueId ec_id)
{
// std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
static int loop = 0;
loop ++;
coil::TimeValue coiltm(coil::gettimeofday());
RTC::Time tm;
tm.sec = coiltm.sec();
tm.nsec = coiltm.usec()*1000;
if (m_qCurrentIn.isNew()) {
m_qCurrentIn.read();
}
if (m_tauInIn.isNew()) {
m_tauInIn.read();
}
if (m_tauIn.data.length() == m_robot->numJoints()) {
int num_joints = m_robot->numJoints();
hrp::dvector g_joint_torque(num_joints);
hrp::dvector torque(num_joints);
if (m_qCurrent.data.length() == m_robot->numJoints()) {
// reference robot model
for ( int i = 0; i < m_robot->numJoints(); i++ ){
m_robot->joint(i)->q = m_qCurrent.data[i];
}
m_robot->calcForwardKinematics();
m_robot->calcCM();
m_robot->rootLink()->calcSubMassCM();
// calc gravity compensation of each joints
hrp::Vector3 g(0, 0, 9.8);
for (int i = 0; i < num_joints; i++) {
// subm*g x (submwc/subm - p) . R*a
g_joint_torque[i] = (m_robot->joint(i)->subm*g).cross(m_robot->joint(i)->submwc / m_robot->joint(i)->subm - m_robot->joint(i)->p).dot(m_robot->joint(i)->R * m_robot->joint(i)->a);
}
} else {
if (DEBUGP) {
std::cerr << "[" << m_profile.instance_name << "]" << "input is not correct" << std::endl;
std::cerr << "[" << m_profile.instance_name << "]" << " numJoints: " << m_robot->numJoints() << std::endl;
std::cerr << "[" << m_profile.instance_name << "]" << " qCurrent: " << m_qCurrent.data.length() << std::endl;
std::cerr << std::endl;
}
for (int i = 0; i < num_joints; i++) {
g_joint_torque[i] = 0.0;
}
}
if (DEBUGP) {
std::cerr << "[" << m_profile.instance_name << "]" << "raw torque: ";
for (int i = 0; i < num_joints; i++) {
std::cerr << " " << m_tauIn.data[i] ;
}
std::cerr << std::endl;
std::cerr << "[" << m_profile.instance_name << "]" << "gravity compensation: ";
for (int i = 0; i < num_joints; i++) {
std::cerr << " " << g_joint_torque[i];
}
std::cerr << std::endl;
}
for (int i = 0; i < num_joints; i++) {
// torque calculation from electric current
// torque[j] = m_tauIn.data[path->joint(j)->jointId] - joint_torque(j);
// torque[j] = m_filters[path->joint(j)->jointId].executeFilter(m_tauIn.data[path->joint(j)->jointId]) - joint_torque(j); // use filtered tau
torque[i] = m_filters[i].executeFilter(m_tauIn.data[i]) - m_torque_offset[i];
// torque calclation from error of joint angle
// if ( m_error_to_torque_gain[path->joint(j)->jointId] == 0.0
// || fabs(joint_error(j)) < m_error_dead_zone[path->joint(j)->jointId] ) {
// torque[j] = 0;
// } else {
// torque[j] = error2torque(j, fabs(m_error_dead_zone[path->joint(j)->jointId]));
// }
// set output
// gravity compensation
if(m_is_gravity_compensation){
m_tauOut.data[i] = torque[i] + g_joint_torque[i];
} else {
m_tauOut.data[i] = torque[i];
}
}
if (DEBUGP) {
std::cerr << "[" << m_profile.instance_name << "]" << "filtered torque: ";
for (int i = 0; i < num_joints; i++) {
std::cerr << " " << torque[i];
}
std::cerr << std::endl;
}
m_tauOut.tm = tm;
m_tauOutOut.write();
} else {
if (DEBUGP) {
std::cerr << "[" << m_profile.instance_name << "]" << "input is not correct" << std::endl;
std::cerr << "[" << m_profile.instance_name << "]" << " numJoints: " << m_robot->numJoints() << std::endl;
std::cerr << "[" << m_profile.instance_name << "]" << " tauIn: " << m_tauIn.data.length() << std::endl;
std::cerr << std::endl;
}
}
return RTC::RTC_OK;
}
RTC::ReturnCode_t ThermoEstimator::onExecute(RTC::UniqueId ec_id)
{
// std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
static long long loop = 0;
loop ++;
coil::TimeValue coiltm(coil::gettimeofday());
RTC::Time tm;
tm.sec = coiltm.sec();
tm.nsec = coiltm.usec()*1000;
if (m_tauInIn.isNew()) {
m_tauInIn.read();
}
if ( m_tauIn.data.length() == m_robot->numJoints() ) {
int numJoints = m_robot->numJoints();
if ( DEBUGP ) {
std::cerr << "raw torque: ";
for (int i = 0; i < numJoints; i++) {
std::cerr << " " << m_tauIn.data[i] ;
}
std::cerr << std::endl;
}
if ( DEBUGP ) {
std::cerr << "estimation values: " << std::endl;
}
for (int i = 0; i < numJoints; i++) {
MotorHeatParam param = m_motorHeatParams[i];
double tau, currentHeat, radiation;
// Thermo estimation
// from Design of High Torque and High Speed Leg Module for High Power Humanoid (Junichi Urata et al.)
// Tnew = T + (((Re*K^2/C) * tau^2) - ((1/RC) * (T - Ta))) * dt
tau = m_tauIn.data[i];
currentHeat = param.currentCoeffs * std::pow(tau, 2);
radiation = -param.thermoCoeffs * (param.temperature - m_ambientTemp);
m_motorHeatParams[i].temperature = param.temperature + (currentHeat + radiation) * m_dt;
if ( DEBUGP ) {
std::cerr << currentHeat << " " << radiation << ", ";
}
// output
m_tempOut.data[i] = m_motorHeatParams[i].temperature;
}
if ( DEBUGP ) {
std::cerr << std::endl << "temperature : ";
for (int i = 0; i < numJoints; i++) {
std::cerr << " " << m_motorHeatParams[i].temperature;
}
std::cerr << std::endl;
}
// overwrite temperature in servoState
if ( m_servoStateInIn.isNew() && (m_servoStateIn.data.length() == m_robot->numJoints()) ) {
m_servoStateInIn.read();
for (unsigned int i = 0; i < m_servoStateIn.data.length(); i++){
size_t len = m_servoStateIn.data[i].length();
m_servoStateOut.data[i].length(len + 1); // expand extra_data for temperature
for (unsigned int j = 0; j < len; j++){
m_servoStateOut.data[i][j] = m_servoStateIn.data[i][j];
}
// servoStateOut is int, but extra data will be casted to float in HrpsysSeqStateROSBridge
float tmp_temperature = static_cast<float>(m_motorHeatParams[i].temperature);
std::memcpy(&(m_servoStateOut.data[i][len]), &tmp_temperature, sizeof(float));
}
m_servoStateOutOut.write();
}
m_tempOutOut.write();
}
return RTC::RTC_OK;
}
