From 91ec2f29e429af7c0baeb69444f2f570ef1750ea Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Fri, 31 May 2019 17:15:19 +0900 Subject: [PATCH 01/26] Still trying to fix ModifiedServo [WIP] --- rtc/ModifiedServo/ModifiedServo.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/rtc/ModifiedServo/ModifiedServo.cpp b/rtc/ModifiedServo/ModifiedServo.cpp index 544dd1653e6..a4064b9d1ef 100644 --- a/rtc/ModifiedServo/ModifiedServo.cpp +++ b/rtc/ModifiedServo/ModifiedServo.cpp @@ -8,6 +8,7 @@ */ #include "ModifiedServo.h" +// #include // Added by Rafa // Module specification // @@ -16,12 +17,11 @@ static const char* modifiedservo_spec[] = "implementation_id", "ModifiedServo", "type_name", "ModifiedServo", "description", "ModifiedServo component", - "version", "0.1", + "version", HRPSYS_PACKAGE_VERSION, "vendor", "AIST", "category", "example", - "activity_type", "SPORADIC", - "kind", "DataFlowComponent", - "max_instance", "1", + "activity_type", "DataFlowComponent", + "max_instance", "10", "language", "C++", "lang_type", "compile", // Configuration variables @@ -109,7 +109,7 @@ RTC::ReturnCode_t ModifiedServo::onInitialize() if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()))) std::cerr << "[" << m_profile.instance_name << "] failed to load model " - << "[" << prop["model"] << "]" << std::endl; + << "[" << prop["model"] << "]" << std::endl; return RTC::RTC_OK; } @@ -215,6 +215,10 @@ RTC::ReturnCode_t ModifiedServo::onExecute(RTC::UniqueId ec_id) double tau_limit = m_robot->joint(i)->torqueConst * m_robot->joint(i)->climit * fabs(m_robot->joint(i)->gearRatio); m_tau.data[i] = std::max(std::min(tau, tau_limit), -tau_limit); + + // if (i == 11 || i == 21) + // std::cout << "Rafa, in ModifiedServo::onExecute, for i = " << i << ", q[i] = " << q << ", qRef[i] = " << qRef + // << ", tau[i] = " << tau << ", tau_limit[i] = " << tau_limit << ", m_tau[i] = " << m_tau.data[i] << std::endl; } step--; From 108fb6b0432bab8db10d1148da48170739ff938f Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Mon, 3 Jun 2019 00:08:18 +0900 Subject: [PATCH 02/26] Changed ModifiedServo to look more like PDcontroller [WIP] --- rtc/ModifiedServo/ModifiedServo-original.cpp | 290 +++++++++++++++++++ rtc/ModifiedServo/ModifiedServo-original.h | 161 ++++++++++ rtc/ModifiedServo/ModifiedServo.cpp | 109 +++++-- rtc/ModifiedServo/ModifiedServo.h | 5 +- 4 files changed, 540 insertions(+), 25 deletions(-) create mode 100644 rtc/ModifiedServo/ModifiedServo-original.cpp create mode 100644 rtc/ModifiedServo/ModifiedServo-original.h diff --git a/rtc/ModifiedServo/ModifiedServo-original.cpp b/rtc/ModifiedServo/ModifiedServo-original.cpp new file mode 100644 index 00000000000..00c991f5e46 --- /dev/null +++ b/rtc/ModifiedServo/ModifiedServo-original.cpp @@ -0,0 +1,290 @@ +// -*- C++ -*- +/*! + * @file ModifiedServo.cpp + * @brief ModifiedServo component + * $Date$ + * + * $Id$ + */ + +#include "ModifiedServo.h" +// #include // Added by Rafa + +// Module specification +// +static const char* modifiedservo_spec[] = + { + "implementation_id", "ModifiedServo", + "type_name", "ModifiedServo", + "description", "ModifiedServo component", + "version", HRPSYS_PACKAGE_VERSION, + "vendor", "AIST", + "category", "example", + "activity_type", "DataFlowComponent", + "max_instance", "10", + "language", "C++", + "lang_type", "compile", + // Configuration variables + "" + }; +// + +ModifiedServo::ModifiedServo(RTC::Manager* manager) + // + : RTC::DataFlowComponentBase(manager), + m_tauRefIn("tauRef", m_tauRef), + m_qRefIn("qRef", m_qRef), + m_qIn("q", m_q), + m_torqueModeIn("torqueMode", m_torqueMode), + m_tauOut("tau", m_tau), + // + gain_fname(""), + dt(0.005), + dof(0) +{ +} + +ModifiedServo::~ModifiedServo() +{ +} + + +RTC::ReturnCode_t ModifiedServo::onInitialize() +{ + // Registration: InPort/OutPort/Service + // + // Set InPort buffers + addInPort("tauRef", m_tauRefIn); + addInPort("qRef", m_qRefIn); + addInPort("q", m_qIn); + addInPort("torqueMode", m_torqueModeIn); + + // Set OutPort buffer + addOutPort("tau", m_tauOut); + + // Set service provider to Ports + + // Set service consumers to Ports + + // Set CORBA Service Ports + + // + + // + // Bind variables and configuration variable + + // + + std::cout << m_profile.instance_name << ": onInitialize() " << std::endl; + + RTC::Properties & prop = getProperties(); + + coil::stringTo(dt, prop["dt"].c_str()); + coil::stringTo(ref_dt, prop["ref_dt"].c_str()); + nstep = ref_dt/dt; + step = nstep; + + m_robot = hrp::BodyPtr(new hrp::Body()); + + RTC::Manager & rtcManager = RTC::Manager::instance(); + std::string nameServer = rtcManager.getConfig()["corba.nameservers"]; + int comPos = nameServer.find(","); + + if (comPos < 0) + comPos = nameServer.length(); + + // In case there is more than one, retrieves only the first one + nameServer = nameServer.substr(0, comPos); + + RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str()); + + if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), + CosNaming::NamingContext::_duplicate(naming.getRootContext()))) + std::cerr << "[" << m_profile.instance_name << "] failed to load model " + << "[" << prop["model"] << "]" << std::endl; + + return RTC::RTC_OK; +} + +/* +RTC::ReturnCode_t ModifiedServo::onFinalize() +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t ModifiedServo::onStartup(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t ModifiedServo::onShutdown(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +RTC::ReturnCode_t ModifiedServo::onActivated(RTC::UniqueId ec_id) +{ + std::cout << m_profile.instance_name << ": on Activated" << std::endl; + + if (m_qIn.isNew()) { + m_qIn.read(); + if (dof == 0) { + dof = m_q.data.length(); + readGainFile(); + } + } + + q_old.resize(dof); + qRef_old.resize(dof); + + m_tauRef.data.length(dof); + m_qRef.data.length(dof); + m_torqueMode.data.length(dof); + + m_tau.data.length(dof); + + for (size_t i = 0; i < dof; i++) { + m_tauRef.data[i] = 0.0; + m_qRef.data[i] = qRef_old[i] = q_old[i] = m_q.data[i]; + m_torqueMode.data[i] = false; + } + + return RTC::RTC_OK; +} + +RTC::ReturnCode_t ModifiedServo::onDeactivated(RTC::UniqueId ec_id) +{ + std::cout << m_profile.instance_name << ": on Deactivated" << std::endl; + + return RTC::RTC_OK; +} + +RTC::ReturnCode_t ModifiedServo::onExecute(RTC::UniqueId ec_id) +{ + if (m_tauRefIn.isNew()) + m_tauRefIn.read(); + + if (m_qIn.isNew()) + m_qIn.read(); + + if (m_qRefIn.isNew()) { + m_qRefIn.read(); + step = nstep; + } + + if (m_torqueModeIn.isNew()) + m_torqueModeIn.read(); + + for (size_t i = 0; i < dof; i++) { + + double q = m_q.data[i]; + double qRef = step > 0 ? qRef_old[i] + (m_qRef.data[i] - qRef_old[i]) / step : qRef_old[i]; + + double dq = (q - q_old[i]) / dt; + double dqRef = (qRef - qRef_old[i]) / dt; + + q_old[i] = q; + qRef_old[i] = qRef; + + double tau = m_torqueMode.data[i] ? m_tauRef.data[i] : Pgain[i] * (qRef - q) + Dgain[i] * (dqRef - dq); + + double tau_limit = m_robot->joint(i)->torqueConst * m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio; + + m_tau.data[i] = std::max(std::min(tau, tau_limit), -tau_limit); + + // if (i == 11 || i == 21) + // std::cout << "Rafa, in ModifiedServo::onExecute, for i = " << i << ", q[i] = " << q << ", qRef[i] = " << qRef + // << ", tau[i] = " << tau << ", tau_limit[i] = " << tau_limit << ", m_tau[i] = " << m_tau.data[i] << std::endl; + } + + step--; + + m_tau.tm = m_q.tm; + m_tauOut.write(); + + return RTC::RTC_OK; +} + +/* +RTC::ReturnCode_t ModifiedServo::onAborting(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t ModifiedServo::onError(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t ModifiedServo::onReset(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t ModifiedServo::onStateUpdate(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t ModifiedServo::onRateChanged(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +void ModifiedServo::readGainFile() +{ + if (gain_fname == "") { + RTC::Properties & prop = getProperties(); + coil::stringTo(gain_fname, prop["pdgains_sim_file_name"].c_str()); + } + + gain.open(gain_fname.c_str()); + + if (gain.is_open()) { + + double val; + + Pgain.resize(dof); + Dgain.resize(dof); + + for (unsigned int i = 0; i < dof; i++) { + + if (gain >> val) + Pgain[i] = val; + else + std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; + + if (gain >> val) + Dgain[i] = val; + else + std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; + } + + gain.close(); + + std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] successfully read" << std::endl; + } + else + std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] could not be opened" << std::endl; +} + +extern "C" +{ + + void ModifiedServoInit(RTC::Manager* manager) + { + coil::Properties profile(modifiedservo_spec); + manager->registerFactory(profile, + RTC::Create, + RTC::Delete); + } + +}; diff --git a/rtc/ModifiedServo/ModifiedServo-original.h b/rtc/ModifiedServo/ModifiedServo-original.h new file mode 100644 index 00000000000..a58ed0863a0 --- /dev/null +++ b/rtc/ModifiedServo/ModifiedServo-original.h @@ -0,0 +1,161 @@ +// -*- C++ -*- +/*! + * @file ModifiedServo.h + * @brief ModifiedServo component + * @date $Date$ + * + * $Id$ + */ + +#ifndef MODIFIEDSERVO_H +#define MODIFIEDSERVO_H + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +// Service implementation headers +// + +// + +// Service Consumer stub headers +// + +// + +using namespace RTC; + +class ModifiedServo : public RTC::DataFlowComponentBase +{ + public: + ModifiedServo(RTC::Manager* manager); + ~ModifiedServo(); + + // The initialize action (on CREATED->ALIVE transition) + // formaer rtc_init_entry() + virtual RTC::ReturnCode_t onInitialize(); + + // The finalize action (on ALIVE->END transition) + // formaer rtc_exiting_entry() + // virtual RTC::ReturnCode_t onFinalize(); + + // The startup action when ExecutionContext startup + // former rtc_starting_entry() + // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id); + + // The shutdown action when ExecutionContext stop + // former rtc_stopping_entry() + // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id); + + // The activated action (Active state entry action) + // former rtc_active_entry() + virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id); + + // The deactivated action (Active state exit action) + // former rtc_active_exit() + virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); + + // The execution action that is invoked periodically + // former rtc_active_do() + virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id); + + // The aborting action when main logic error occurred. + // former rtc_aborting_entry() + // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id); + + // The error action in ERROR state + // former rtc_error_do() + // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id); + + // The reset action that is invoked resetting + // This is same but different the former rtc_init_entry() + // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id); + + // The state update action that is invoked after onExecute() action + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id); + + // The action that is invoked when execution context's rate is changed + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); + + + protected: + // Configuration variable declaration + // + + // + + // DataInPort declaration + // + TimedDoubleSeq m_tauRef; + InPort m_tauRefIn; + TimedDoubleSeq m_qRef; + InPort m_qRefIn; + TimedDoubleSeq m_q; + InPort m_qIn; + TimedBooleanSeq m_torqueMode; + InPort m_torqueModeIn; + + // + + // DataOutPort declaration + // + TimedDoubleSeq m_tau; + OutPort m_tauOut; + + // + + // CORBA Port declaration + // + + // + + // Service declaration + // + + // + + // Consumer declaration + // + + // + + private: + + void readGainFile(); + + hrp::BodyPtr m_robot; + + double dt; // sampling time of the controller + double ref_dt; // sampling time of reference angles + double step; // current interpolation step + double nstep; // number of steps to interpolate references + + size_t dof; + + std::string gain_fname; + std::ifstream gain; + + hrp::dvector Pgain, Dgain; + hrp::dvector q_old, qRef_old; +}; + + +extern "C" +{ + DLL_EXPORT void ModifiedServoInit(RTC::Manager* manager); +}; + +#endif // MODIFIEDSERVO_H + diff --git a/rtc/ModifiedServo/ModifiedServo.cpp b/rtc/ModifiedServo/ModifiedServo.cpp index a4064b9d1ef..c2a8843ab61 100644 --- a/rtc/ModifiedServo/ModifiedServo.cpp +++ b/rtc/ModifiedServo/ModifiedServo.cpp @@ -25,6 +25,8 @@ static const char* modifiedservo_spec[] = "language", "C++", "lang_type", "compile", // Configuration variables + "conf.default.pdgains_sim_file_name", "", + "conf.default.debugLevel", "0", "" }; // @@ -44,7 +46,8 @@ ModifiedServo::ModifiedServo(RTC::Manager* manager) // gain_fname(""), dt(0.005), - dof(0) + dof(0), loop(0), + dummy(0) { } @@ -81,35 +84,36 @@ RTC::ReturnCode_t ModifiedServo::onInitialize() // // Bind variables and configuration variable + bindParameter("pdgains_sim_file_name", gain_fname, ""); + bindParameter("debugLevel", m_debugLevel, "0"); + // std::cout << m_profile.instance_name << ": onInitialize() " << std::endl; - RTC::Properties & prop = getProperties(); - + RTC::Properties& prop = getProperties(); coil::stringTo(dt, prop["dt"].c_str()); + ref_dt = dt; coil::stringTo(ref_dt, prop["ref_dt"].c_str()); nstep = ref_dt/dt; step = nstep; m_robot = hrp::BodyPtr(new hrp::Body()); - RTC::Manager & rtcManager = RTC::Manager::instance(); + RTC::Manager& rtcManager = RTC::Manager::instance(); std::string nameServer = rtcManager.getConfig()["corba.nameservers"]; int comPos = nameServer.find(","); if (comPos < 0) - comPos = nameServer.length(); - - // In case there is more than one, retrieves only the first one + comPos = nameServer.length(); + nameServer = nameServer.substr(0, comPos); - RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str()); - - if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), + + if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()))) - std::cerr << "[" << m_profile.instance_name << "] failed to load model " - << "[" << prop["model"] << "]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] + << "]" << std::endl; return RTC::RTC_OK; } @@ -177,11 +181,18 @@ RTC::ReturnCode_t ModifiedServo::onDeactivated(RTC::UniqueId ec_id) RTC::ReturnCode_t ModifiedServo::onExecute(RTC::UniqueId ec_id) { + loop++; + if (m_tauRefIn.isNew()) m_tauRefIn.read(); - if (m_qIn.isNew()) + if (m_qIn.isNew()) { m_qIn.read(); + if (dof == 0) { + dof = m_q.data.length(); + readGainFile(); + } + } if (m_qRefIn.isNew()) { m_qRefIn.read(); @@ -215,10 +226,6 @@ RTC::ReturnCode_t ModifiedServo::onExecute(RTC::UniqueId ec_id) double tau_limit = m_robot->joint(i)->torqueConst * m_robot->joint(i)->climit * fabs(m_robot->joint(i)->gearRatio); m_tau.data[i] = std::max(std::min(tau, tau_limit), -tau_limit); - - // if (i == 11 || i == 21) - // std::cout << "Rafa, in ModifiedServo::onExecute, for i = " << i << ", q[i] = " << q << ", qRef[i] = " << qRef - // << ", tau[i] = " << tau << ", tau_limit[i] = " << tau_limit << ", m_tau[i] = " << m_tau.data[i] << std::endl; } step--; @@ -269,34 +276,88 @@ void ModifiedServo::readGainFile() coil::stringTo(gain_fname, prop["pdgains_sim_file_name"].c_str()); } + // initialize length of vectors + + q_old.resize(dof); + qRef_old.resize(dof); + + m_tauRef.data.length(dof); + m_qRef.data.length(dof); + m_torqueMode.data.length(dof); + + tau_limit_ratio.resize(dof); + + Pgain.resize(dof); + Dgain.resize(dof); + gain.open(gain_fname.c_str()); if (gain.is_open()) { double val; - Pgain.resize(dof); - Dgain.resize(dof); - for (unsigned int i = 0; i < dof; i++) { if (gain >> val) Pgain[i] = val; else - std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; if (gain >> val) Dgain[i] = val; else - std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; } gain.close(); - std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] successfully read" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] successfully read" << std::endl; } else - std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] could not be opened" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] could not be opened" << std::endl; + + // tau_limit_ratio initialize + RTC::Properties& prop = getProperties(); + + if (prop["pdcontrol_tau_limit_ratio"] != "") { + + coil::vstring tau_limit_ratio_str = coil::split(prop["pdcontrol_tau_limit_ratio"], ","); + + if (tau_limit_ratio_str.size() == dof) { + + for (size_t i = 0; i < dof; i++) + coil::stringTo(tau_limit_ratio[i], tau_limit_ratio_str[i].c_str()); + + std::cerr << "[" << m_profile.instance_name << "] tau_limit_ratio is set to " << prop["pdcontrol_tau_limit_ratio"] << std::endl; + } + else { + + for (size_t i = 0; i < dof; i++) + tau_limit_ratio[i] = 1.0; + + std::cerr << "[" << m_profile.instance_name << "] pdcontrol_tau_limit_ratio found, but invalid length (" << tau_limit_ratio_str.size() << " != " << dof << ")." << std::endl; + std::cerr << "[" << m_profile.instance_name << "] All tau_limit_ratio are set to 1.0." << std::endl; + } + } + else { + for (size_t i = 0; i < dof; i++) + tau_limit_ratio[i] = 1.0; + + std::cerr << "[" << m_profile.instance_name << "] No pdcontrol_tau_limit_ratio found." << std::endl; + std::cerr << "[" << m_profile.instance_name << "] All tau_limit_ratio are set to 1.0." << std::endl; + } + + // initialize angleRef, old_ref and old with angle + for(unsigned int i=0; i < dof; ++i) { + m_tauRef.data[i] = 0.0; + m_qRef.data[i] = qRef_old[i] = q_old[i] = m_q.data[i]; + m_torqueMode.data[i] = false; + } + + // Print loaded gain + std::cerr << "[" << m_profile.instance_name << "] loadGain" << std::endl; + for (unsigned int i=0; inumJoints(); i++) + std::cerr << "[" << m_profile.instance_name << "] " << m_robot->joint(i)->name << ", pgain = " << Pgain[i] << ", dgain = " << Dgain[i] << std::endl; } extern "C" diff --git a/rtc/ModifiedServo/ModifiedServo.h b/rtc/ModifiedServo/ModifiedServo.h index 287af997312..df9bb4afa12 100644 --- a/rtc/ModifiedServo/ModifiedServo.h +++ b/rtc/ModifiedServo/ModifiedServo.h @@ -150,13 +150,16 @@ class ModifiedServo : public RTC::DataFlowComponentBase double step; // current interpolation step double nstep; // number of steps to interpolate references - size_t dof; + size_t dof, loop; + unsigned int m_debugLevel; + int dummy; std::string gain_fname; std::ifstream gain; hrp::dvector Pgain, Dgain; hrp::dvector q_old, qRef_old; + hrp::dvector tau_limit_ratio; }; From 3eb9a0f6884032cd14dde56818fd9e0bbbb9cadd Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Mon, 3 Jun 2019 16:58:53 +0900 Subject: [PATCH 03/26] Several versions of ModifiedServo [WIP] --- rtc/ModifiedServo/ModifiedServo-PDcopy.cpp | 305 ++++++++++++++++++ rtc/ModifiedServo/ModifiedServo-PDcopy.h | 151 +++++++++ rtc/ModifiedServo/ModifiedServo-PDlike.cpp | 354 +++++++++++++++++++++ rtc/ModifiedServo/ModifiedServo-PDlike.h | 164 ++++++++++ rtc/ModifiedServo/ModifiedServo.cpp | 109 ++----- rtc/ModifiedServo/ModifiedServo.h | 5 +- 6 files changed, 999 insertions(+), 89 deletions(-) create mode 100644 rtc/ModifiedServo/ModifiedServo-PDcopy.cpp create mode 100644 rtc/ModifiedServo/ModifiedServo-PDcopy.h create mode 100644 rtc/ModifiedServo/ModifiedServo-PDlike.cpp create mode 100644 rtc/ModifiedServo/ModifiedServo-PDlike.h diff --git a/rtc/ModifiedServo/ModifiedServo-PDcopy.cpp b/rtc/ModifiedServo/ModifiedServo-PDcopy.cpp new file mode 100644 index 00000000000..c7fea36d4a0 --- /dev/null +++ b/rtc/ModifiedServo/ModifiedServo-PDcopy.cpp @@ -0,0 +1,305 @@ +// -*- mode: c++; -*- +/*! + * @file ModifiedServo.cpp + * @brief Sample PD component + * $Date$ + * + * $Id$ + */ + +#include "ModifiedServo.h" +#include +#include + +// Module specification +// +static const char* ModifiedServo_spec[] = + { + "implementation_id", "ModifiedServo", + "type_name", "ModifiedServo", + "description", "ModifiedServo component", + "version", HRPSYS_PACKAGE_VERSION, + "vendor", "AIST", + "category", "example", + "activity_type", "DataFlowComponent", + "max_instance", "10", + "language", "C++", + "lang_type", "compile", + // Configuration variables + "conf.default.pdgains_sim_file_name", "", + "conf.default.debugLevel", "0", + "" + }; +// + +ModifiedServo::ModifiedServo(RTC::Manager* manager) + : RTC::DataFlowComponentBase(manager), + // + m_angleIn("angle", m_angle), + m_angleRefIn("angleRef", m_angleRef), + m_torqueOut("torque", m_torque), + dt(0.005), + // + gain_fname(""), + dof(0), loop(0), + dummy(0) +{ +} + +ModifiedServo::~ModifiedServo() +{ +} + + +RTC::ReturnCode_t ModifiedServo::onInitialize() +{ + std::cout << m_profile.instance_name << ": onInitialize() " << std::endl; + + RTC::Properties& prop = getProperties(); + coil::stringTo(dt, prop["dt"].c_str()); + ref_dt = dt; + coil::stringTo(ref_dt, prop["ref_dt"].c_str()); + nstep = ref_dt/dt; + step = nstep; + + m_robot = hrp::BodyPtr(new hrp::Body()); + + RTC::Manager& rtcManager = RTC::Manager::instance(); + std::string nameServer = rtcManager.getConfig()["corba.nameservers"]; + int comPos = nameServer.find(","); + if (comPos < 0){ + comPos = nameServer.length(); + } + nameServer = nameServer.substr(0, comPos); + RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str()); + if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), + CosNaming::NamingContext::_duplicate(naming.getRootContext()) + )){ + std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" + << std::endl; + } + + // + // Bind variables and configuration variable + bindParameter("pdgains_sim_file_name", gain_fname, ""); + bindParameter("debugLevel", m_debugLevel, "0"); + + // Set InPort buffers + addInPort("angle", m_angleIn); + addInPort("angleRef", m_angleRefIn); + + // Set OutPort buffer + addOutPort("torque", m_torqueOut); + + // + + return RTC::RTC_OK; +} + + + +/* +RTC::ReturnCode_t ModifiedServo::onFinalize() +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t ModifiedServo::onStartup(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t ModifiedServo::onShutdown(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +RTC::ReturnCode_t ModifiedServo::onActivated(RTC::UniqueId ec_id) +{ + std::cout << m_profile.instance_name << ": on Activated " << std::endl; + if(m_angleIn.isNew()){ + m_angleIn.read(); + if (dof == 0) { + dof = m_angle.data.length(); + readGainFile(); + } + } + return RTC::RTC_OK; +} + +RTC::ReturnCode_t ModifiedServo::onDeactivated(RTC::UniqueId ec_id) +{ + std::cout << m_profile.instance_name << ": on Deactivated " << std::endl; + return RTC::RTC_OK; +} + + +RTC::ReturnCode_t ModifiedServo::onExecute(RTC::UniqueId ec_id) +{ + loop++; + if(m_angleIn.isNew()){ + m_angleIn.read(); + if (dof == 0) { + dof = m_angle.data.length(); + readGainFile(); + } + } + if(m_angleRefIn.isNew()){ + m_angleRefIn.read(); + step = nstep; + } + + for(size_t i=0; i 0 ? qold_ref[i] + (m_angleRef.data[i] - qold_ref[i])/step : qold_ref[i]; + double dq = (q - qold[i]) / dt; + double dq_ref = (q_ref - qold_ref[i]) / dt; + qold[i] = q; + qold_ref[i] = q_ref; + m_torque.data[i] = -(q - q_ref) * Pgain[i] - (dq - dq_ref) * Dgain[i]; + double tlimit; + if (m_robot && m_robot->numJoints() == dof) { + tlimit = m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst * tlimit_ratio[i]; + } else { + tlimit = (std::numeric_limits::max)() * tlimit_ratio[i]; + if (i == 0 && loop % 500 == 0) { + std::cerr << "[" << m_profile.instance_name << "] m_robot is not set properly!! Maybe ModelLoader is missing?" << std::endl; + } + } + if (loop % 100 == 0 && m_debugLevel == 1) { + std::cerr << "[" << m_profile.instance_name << "] joint = " + << i << ", tq = " << m_torque.data[i] << ", q,qref = (" << q << ", " << q_ref << "), dq,dqref = (" << dq << ", " << dq_ref << "), pd = (" << Pgain[i] << ", " << Dgain[i] << "), tlimit = " << tlimit << std::endl; + } + m_torque.data[i] = std::max(std::min(m_torque.data[i], tlimit), -tlimit); + } + step--; + + m_torqueOut.write(); + + return RTC::RTC_OK; +} + +void ModifiedServo::readGainFile() +{ + if (gain_fname == "") { + RTC::Properties& prop = getProperties(); + coil::stringTo(gain_fname, prop["pdgains_sim_file_name"].c_str()); + } + // initialize length of vectors + qold.resize(dof); + qold_ref.resize(dof); + m_torque.data.length(dof); + m_angleRef.data.length(dof); + Pgain.resize(dof); + Dgain.resize(dof); + gain.open(gain_fname.c_str()); + tlimit_ratio.resize(dof); + if (gain.is_open()){ + double tmp; + for (unsigned int i=0; i> tmp) { + Pgain[i] = tmp; + } else { + std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; + } + if (gain >> tmp) { + Dgain[i] = tmp; + } else { + std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; + } + } + gain.close(); + std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] opened" << std::endl; + }else{ + std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] not opened" << std::endl; + } + // tlimit_ratio initialize + { + RTC::Properties& prop = getProperties(); + if (prop["pdcontrol_tlimit_ratio"] != "") { + coil::vstring tlimit_ratio_str = coil::split(prop["pdcontrol_tlimit_ratio"], ","); + if (tlimit_ratio_str.size() == dof) { + for (size_t i = 0; i < dof; i++) { + coil::stringTo(tlimit_ratio[i], tlimit_ratio_str[i].c_str()); + } + std::cerr << "[" << m_profile.instance_name << "] tlimit_ratio is set to " << prop["pdcontrol_tlimit_ratio"] << std::endl; + } else { + for (size_t i = 0; i < dof; i++) { + tlimit_ratio[i] = 1.0; + } + std::cerr << "[" << m_profile.instance_name << "] pdcontrol_tlimit_ratio found, but invalid length (" << tlimit_ratio_str.size() << " != " << dof << ")." << std::endl; + std::cerr << "[" << m_profile.instance_name << "] All tlimit_ratio are set to 1.0." << std::endl; + } + } else { + for (size_t i = 0; i < dof; i++) { + tlimit_ratio[i] = 1.0; + } + std::cerr << "[" << m_profile.instance_name << "] No pdcontrol_tlimit_ratio found." << std::endl; + std::cerr << "[" << m_profile.instance_name << "] All tlimit_ratio are set to 1.0." << std::endl; + } + } + // initialize angleRef, old_ref and old with angle + for(unsigned int i=0; i < dof; ++i){ + m_angleRef.data[i] = qold_ref[i] = qold[i] = m_angle.data[i]; + } + // Print loaded gain + std::cerr << "[" << m_profile.instance_name << "] loadGain" << std::endl; + for (unsigned int i=0; inumJoints(); i++) { + std::cerr << "[" << m_profile.instance_name << "] " << m_robot->joint(i)->name << ", pgain = " << Pgain[i] << ", dgain = " << Dgain[i] << std::endl; + } +} + +/* + RTC::ReturnCode_t ModifiedServo::onAborting(RTC::UniqueId ec_id) + { + return RTC::RTC_OK; + } +*/ + +/* + RTC::ReturnCode_t ModifiedServo::onError(RTC::UniqueId ec_id) + { + return RTC::RTC_OK; + } +*/ + +/* + RTC::ReturnCode_t ModifiedServo::onReset(RTC::UniqueId ec_id) + { + return RTC::RTC_OK; + } +*/ + +/* + RTC::ReturnCode_t ModifiedServo::onStateUpdate(RTC::UniqueId ec_id) + { + return RTC::RTC_OK; + } +*/ + +/* + RTC::ReturnCode_t ModifiedServo::onRateChanged(RTC::UniqueId ec_id) + { + return RTC::RTC_OK; + } +*/ + +extern "C" +{ + + void ModifiedServoInit(RTC::Manager* manager) + { + RTC::Properties profile(ModifiedServo_spec); + manager->registerFactory(profile, + RTC::Create, + RTC::Delete); + } + +}; + + diff --git a/rtc/ModifiedServo/ModifiedServo-PDcopy.h b/rtc/ModifiedServo/ModifiedServo-PDcopy.h new file mode 100644 index 00000000000..75048ecc9cd --- /dev/null +++ b/rtc/ModifiedServo/ModifiedServo-PDcopy.h @@ -0,0 +1,151 @@ +// -*- mode: c++; -*- +/*! + * @file ModifiedServo.h + * @brief Sample PD component + * @date $Date$ + * + * $Id$ + */ + +#ifndef ModifiedServo_H +#define ModifiedServo_H + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +// Service implementation headers +// + +// + +// Service Consumer stub headers +// + +// + +using namespace RTC; + +class ModifiedServo + : public RTC::DataFlowComponentBase +{ + public: + ModifiedServo(RTC::Manager* manager); + ~ModifiedServo(); + + // The initialize action (on CREATED->ALIVE transition) + // formaer rtc_init_entry() + virtual RTC::ReturnCode_t onInitialize(); + + // The finalize action (on ALIVE->END transition) + // formaer rtc_exiting_entry() + // virtual RTC::ReturnCode_t onFinalize(); + + // The startup action when ExecutionContext startup + // former rtc_starting_entry() + // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id); + + // The shutdown action when ExecutionContext stop + // former rtc_stopping_entry() + // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id); + + // The activated action (Active state entry action) + // former rtc_active_entry() + virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id); + + // The deactivated action (Active state exit action) + // former rtc_active_exit() + virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); + + // The execution action that is invoked periodically + // former rtc_active_do() + virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id); + + // The aborting action when main logic error occurred. + // former rtc_aborting_entry() + // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id); + + // The error action in ERROR state + // former rtc_error_do() + // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id); + + // The reset action that is invoked resetting + // This is same but different the former rtc_init_entry() + // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id); + + // The state update action that is invoked after onExecute() action + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id); + + // The action that is invoked when execution context's rate is changed + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); + + protected: + // Configuration variable declaration + // + + // + + // DataInPort declaration + // + TimedDoubleSeq m_angle; + InPort m_angleIn; + TimedDoubleSeq m_angleRef; + InPort m_angleRefIn; + + // + + // DataOutPort declaration + // + TimedDoubleSeq m_torque; + OutPort m_torqueOut; + + // + + // CORBA Port declaration + // + + // + + // Service declaration + // + + // + + // Consumer declaration + // + + // + + private: + void readGainFile(); + hrp::BodyPtr m_robot; + double dt; // sampling time of pd control + double ref_dt; // sampling time of renference angles + int step; // current interpolation step + int nstep; // the number of steps to interpolate references + std::ifstream gain; + std::string gain_fname; + hrp::dvector qold, qold_ref, Pgain, Dgain, tlimit_ratio; + size_t dof, loop; + unsigned int m_debugLevel; + int dummy; +}; + +extern "C" +{ + void ModifiedServoInit(RTC::Manager* manager); +}; + +#endif // ModifiedServo_H diff --git a/rtc/ModifiedServo/ModifiedServo-PDlike.cpp b/rtc/ModifiedServo/ModifiedServo-PDlike.cpp new file mode 100644 index 00000000000..14e087e2837 --- /dev/null +++ b/rtc/ModifiedServo/ModifiedServo-PDlike.cpp @@ -0,0 +1,354 @@ +// -*- C++ -*- +/*! + * @file ModifiedServo.cpp + * @brief ModifiedServo component + * $Date$ + * + * $Id$ + */ + +#include "ModifiedServo.h" +// #include // Added by Rafa + +// Module specification +// +static const char* modifiedservo_spec[] = + { + "implementation_id", "ModifiedServo", + "type_name", "ModifiedServo", + "description", "ModifiedServo component", + "version", HRPSYS_PACKAGE_VERSION, + "vendor", "AIST", + "category", "example", + "activity_type", "DataFlowComponent", + "max_instance", "10", + "language", "C++", + "lang_type", "compile", + // Configuration variables + "conf.default.pdgains_sim_file_name", "", + "conf.default.debugLevel", "0", + "" + }; +// + +ModifiedServo::ModifiedServo(RTC::Manager* manager) + // + : RTC::DataFlowComponentBase(manager), + m_tauRefIn("tauRef", m_tauRef), + m_qRefIn("qRef", m_qRef), + m_qIn("q", m_q), + m_torqueModeIn("torqueMode", m_torqueMode), + m_tauOut("tau", m_tau), + // + gain_fname(""), + dt(0.005), + dof(0), loop(0), + dummy(0) +{ +} + +ModifiedServo::~ModifiedServo() +{ +} + + +RTC::ReturnCode_t ModifiedServo::onInitialize() +{ + // Registration: InPort/OutPort/Service + // + // Set InPort buffers + addInPort("tauRef", m_tauRefIn); + addInPort("qRef", m_qRefIn); + addInPort("q", m_qIn); + addInPort("torqueMode", m_torqueModeIn); + + // Set OutPort buffer + addOutPort("tau", m_tauOut); + + // Set service provider to Ports + + // Set service consumers to Ports + + // Set CORBA Service Ports + + // + + // + // Bind variables and configuration variable + + bindParameter("pdgains_sim_file_name", gain_fname, ""); + bindParameter("debugLevel", m_debugLevel, "0"); + + // + + std::cout << m_profile.instance_name << ": onInitialize() " << std::endl; + + RTC::Properties& prop = getProperties(); + coil::stringTo(dt, prop["dt"].c_str()); + ref_dt = dt; + coil::stringTo(ref_dt, prop["ref_dt"].c_str()); + nstep = ref_dt/dt; + step = nstep; + + m_robot = hrp::BodyPtr(new hrp::Body()); + + RTC::Manager& rtcManager = RTC::Manager::instance(); + std::string nameServer = rtcManager.getConfig()["corba.nameservers"]; + int comPos = nameServer.find(","); + + if (comPos < 0) + comPos = nameServer.length(); + + nameServer = nameServer.substr(0, comPos); + RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str()); + + if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), + CosNaming::NamingContext::_duplicate(naming.getRootContext()))) + std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] + << "]" << std::endl; + + return RTC::RTC_OK; +} + +/* +RTC::ReturnCode_t ModifiedServo::onFinalize() +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t ModifiedServo::onStartup(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t ModifiedServo::onShutdown(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +RTC::ReturnCode_t ModifiedServo::onActivated(RTC::UniqueId ec_id) +{ + std::cout << m_profile.instance_name << ": on Activated" << std::endl; + + if (m_qIn.isNew()) { + m_qIn.read(); + if (dof == 0) { + dof = m_q.data.length(); + readGainFile(); + } + } + + return RTC::RTC_OK; +} + +RTC::ReturnCode_t ModifiedServo::onDeactivated(RTC::UniqueId ec_id) +{ + std::cout << m_profile.instance_name << ": on Deactivated" << std::endl; + + return RTC::RTC_OK; +} + +RTC::ReturnCode_t ModifiedServo::onExecute(RTC::UniqueId ec_id) +{ + loop++; + + if (m_tauRefIn.isNew()) + m_tauRefIn.read(); + + if (m_qIn.isNew()) { + m_qIn.read(); + if (dof == 0) { + dof = m_q.data.length(); + readGainFile(); + } + } + + if (m_qRefIn.isNew()) { + m_qRefIn.read(); + step = nstep; + } + + if (m_torqueModeIn.isNew()) + m_torqueModeIn.read(); + + for (size_t i = 0; i < dof; i++) { + + double q = m_q.data[i]; + double qRef = step > 0 ? qRef_old[i] + (m_qRef.data[i] - qRef_old[i]) / step : qRef_old[i]; + + double dq = (q - q_old[i]) / dt; + double dqRef = (qRef - qRef_old[i]) / dt; + + q_old[i] = q; + qRef_old[i] = qRef; + + double tau = m_torqueMode.data[i] ? m_tauRef.data[i] : Pgain[i] * (qRef - q) + Dgain[i] * (dqRef - dq); + + double tau_limit; + + if (m_robot && m_robot->numJoints() == dof) + + tau_limit = m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst * tau_limit_ratio[i]; + + else { + + tau_limit = (std::numeric_limits::max)() * tau_limit_ratio[i]; + + if (i == 0 && loop % 500 == 0) + std::cerr << "[" << m_profile.instance_name << "] m_robot is not set properly!! Maybe ModelLoader is missing?" << std::endl; + } + + if (loop % 100 == 0 && m_debugLevel == 1) + std::cerr << "[" << m_profile.instance_name << "] joint = " + << i << ", tau = " << tau << ", q , qRef = (" << q << ", " << qRef + << "), dq, dqRef = (" << dq << ", " << dqRef << "), pd = (" + << Pgain[i] << ", " << Dgain[i] << "), tau_limit = " << tau_limit << std::endl; + + m_tau.data[i] = std::max(std::min(tau, tau_limit), -tau_limit); + } + + step--; + + m_tau.tm = m_q.tm; + m_tauOut.write(); + + return RTC::RTC_OK; +} + +/* +RTC::ReturnCode_t ModifiedServo::onAborting(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t ModifiedServo::onError(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t ModifiedServo::onReset(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t ModifiedServo::onStateUpdate(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t ModifiedServo::onRateChanged(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +void ModifiedServo::readGainFile() +{ + if (gain_fname == "") { + RTC::Properties & prop = getProperties(); + coil::stringTo(gain_fname, prop["pdgains_sim_file_name"].c_str()); + } + + // initialize length of vectors + + q_old.resize(dof); + qRef_old.resize(dof); + + m_tauRef.data.length(dof); + m_qRef.data.length(dof); + m_torqueMode.data.length(dof); + + tau_limit_ratio.resize(dof); + + Pgain.resize(dof); + Dgain.resize(dof); + + gain.open(gain_fname.c_str()); + + if (gain.is_open()) { + + double val; + + for (unsigned int i = 0; i < dof; i++) { + + if (gain >> val) + Pgain[i] = val; + else + std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; + + if (gain >> val) + Dgain[i] = val; + else + std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; + } + + gain.close(); + + std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] successfully read" << std::endl; + } + else + std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] could not be opened" << std::endl; + + // tau_limit_ratio initialize + RTC::Properties& prop = getProperties(); + + if (prop["pdcontrol_tau_limit_ratio"] != "") { + + coil::vstring tau_limit_ratio_str = coil::split(prop["pdcontrol_tau_limit_ratio"], ","); + + if (tau_limit_ratio_str.size() == dof) { + + for (size_t i = 0; i < dof; i++) + coil::stringTo(tau_limit_ratio[i], tau_limit_ratio_str[i].c_str()); + + std::cerr << "[" << m_profile.instance_name << "] tau_limit_ratio is set to " << prop["pdcontrol_tau_limit_ratio"] << std::endl; + } + else { + + for (size_t i = 0; i < dof; i++) + tau_limit_ratio[i] = 1.0; + + std::cerr << "[" << m_profile.instance_name << "] pdcontrol_tau_limit_ratio found, but invalid length (" << tau_limit_ratio_str.size() << " != " << dof << ")." << std::endl; + std::cerr << "[" << m_profile.instance_name << "] All tau_limit_ratio are set to 1.0." << std::endl; + } + } + else { + for (size_t i = 0; i < dof; i++) + tau_limit_ratio[i] = 1.0; + + std::cerr << "[" << m_profile.instance_name << "] No pdcontrol_tau_limit_ratio found." << std::endl; + std::cerr << "[" << m_profile.instance_name << "] All tau_limit_ratio are set to 1.0." << std::endl; + } + + // initialize angleRef, old_ref and old with angle + for(unsigned int i=0; i < dof; ++i) { + m_tauRef.data[i] = 0.0; + m_qRef.data[i] = qRef_old[i] = q_old[i] = m_q.data[i]; + m_torqueMode.data[i] = false; + } + + // Print loaded gain + std::cerr << "[" << m_profile.instance_name << "] loadGain" << std::endl; + for (unsigned int i=0; inumJoints(); i++) + std::cerr << "[" << m_profile.instance_name << "] " << m_robot->joint(i)->name << ", pgain = " << Pgain[i] << ", dgain = " << Dgain[i] << std::endl; +} + +extern "C" +{ + + void ModifiedServoInit(RTC::Manager* manager) + { + coil::Properties profile(modifiedservo_spec); + manager->registerFactory(profile, + RTC::Create, + RTC::Delete); + } + +}; diff --git a/rtc/ModifiedServo/ModifiedServo-PDlike.h b/rtc/ModifiedServo/ModifiedServo-PDlike.h new file mode 100644 index 00000000000..ddfe72b709b --- /dev/null +++ b/rtc/ModifiedServo/ModifiedServo-PDlike.h @@ -0,0 +1,164 @@ +// -*- C++ -*- +/*! + * @file ModifiedServo.h + * @brief ModifiedServo component + * @date $Date$ + * + * $Id$ + */ + +#ifndef MODIFIEDSERVO_H +#define MODIFIEDSERVO_H + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +// Service implementation headers +// + +// + +// Service Consumer stub headers +// + +// + +using namespace RTC; + +class ModifiedServo : public RTC::DataFlowComponentBase +{ + public: + ModifiedServo(RTC::Manager* manager); + ~ModifiedServo(); + + // The initialize action (on CREATED->ALIVE transition) + // formaer rtc_init_entry() + virtual RTC::ReturnCode_t onInitialize(); + + // The finalize action (on ALIVE->END transition) + // formaer rtc_exiting_entry() + // virtual RTC::ReturnCode_t onFinalize(); + + // The startup action when ExecutionContext startup + // former rtc_starting_entry() + // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id); + + // The shutdown action when ExecutionContext stop + // former rtc_stopping_entry() + // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id); + + // The activated action (Active state entry action) + // former rtc_active_entry() + virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id); + + // The deactivated action (Active state exit action) + // former rtc_active_exit() + virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); + + // The execution action that is invoked periodically + // former rtc_active_do() + virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id); + + // The aborting action when main logic error occurred. + // former rtc_aborting_entry() + // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id); + + // The error action in ERROR state + // former rtc_error_do() + // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id); + + // The reset action that is invoked resetting + // This is same but different the former rtc_init_entry() + // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id); + + // The state update action that is invoked after onExecute() action + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id); + + // The action that is invoked when execution context's rate is changed + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); + + + protected: + // Configuration variable declaration + // + + // + + // DataInPort declaration + // + TimedDoubleSeq m_tauRef; + InPort m_tauRefIn; + TimedDoubleSeq m_qRef; + InPort m_qRefIn; + TimedDoubleSeq m_q; + InPort m_qIn; + TimedBooleanSeq m_torqueMode; + InPort m_torqueModeIn; + + // + + // DataOutPort declaration + // + TimedDoubleSeq m_tau; + OutPort m_tauOut; + + // + + // CORBA Port declaration + // + + // + + // Service declaration + // + + // + + // Consumer declaration + // + + // + + private: + + void readGainFile(); + + hrp::BodyPtr m_robot; + + double dt; // sampling time of the controller + double ref_dt; // sampling time of reference angles + double step; // current interpolation step + double nstep; // number of steps to interpolate references + + size_t dof, loop; + unsigned int m_debugLevel; + int dummy; + + std::string gain_fname; + std::ifstream gain; + + hrp::dvector Pgain, Dgain; + hrp::dvector q_old, qRef_old; + hrp::dvector tau_limit_ratio; +}; + + +extern "C" +{ + DLL_EXPORT void ModifiedServoInit(RTC::Manager* manager); +}; + +#endif // MODIFIEDSERVO_H + diff --git a/rtc/ModifiedServo/ModifiedServo.cpp b/rtc/ModifiedServo/ModifiedServo.cpp index c2a8843ab61..a4064b9d1ef 100644 --- a/rtc/ModifiedServo/ModifiedServo.cpp +++ b/rtc/ModifiedServo/ModifiedServo.cpp @@ -25,8 +25,6 @@ static const char* modifiedservo_spec[] = "language", "C++", "lang_type", "compile", // Configuration variables - "conf.default.pdgains_sim_file_name", "", - "conf.default.debugLevel", "0", "" }; // @@ -46,8 +44,7 @@ ModifiedServo::ModifiedServo(RTC::Manager* manager) // gain_fname(""), dt(0.005), - dof(0), loop(0), - dummy(0) + dof(0) { } @@ -84,36 +81,35 @@ RTC::ReturnCode_t ModifiedServo::onInitialize() // // Bind variables and configuration variable - bindParameter("pdgains_sim_file_name", gain_fname, ""); - bindParameter("debugLevel", m_debugLevel, "0"); - // std::cout << m_profile.instance_name << ": onInitialize() " << std::endl; - RTC::Properties& prop = getProperties(); + RTC::Properties & prop = getProperties(); + coil::stringTo(dt, prop["dt"].c_str()); - ref_dt = dt; coil::stringTo(ref_dt, prop["ref_dt"].c_str()); nstep = ref_dt/dt; step = nstep; m_robot = hrp::BodyPtr(new hrp::Body()); - RTC::Manager& rtcManager = RTC::Manager::instance(); + RTC::Manager & rtcManager = RTC::Manager::instance(); std::string nameServer = rtcManager.getConfig()["corba.nameservers"]; int comPos = nameServer.find(","); if (comPos < 0) - comPos = nameServer.length(); - + comPos = nameServer.length(); + + // In case there is more than one, retrieves only the first one nameServer = nameServer.substr(0, comPos); + RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str()); - - if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), + + if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()))) - std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] - << "]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] failed to load model " + << "[" << prop["model"] << "]" << std::endl; return RTC::RTC_OK; } @@ -181,18 +177,11 @@ RTC::ReturnCode_t ModifiedServo::onDeactivated(RTC::UniqueId ec_id) RTC::ReturnCode_t ModifiedServo::onExecute(RTC::UniqueId ec_id) { - loop++; - if (m_tauRefIn.isNew()) m_tauRefIn.read(); - if (m_qIn.isNew()) { + if (m_qIn.isNew()) m_qIn.read(); - if (dof == 0) { - dof = m_q.data.length(); - readGainFile(); - } - } if (m_qRefIn.isNew()) { m_qRefIn.read(); @@ -226,6 +215,10 @@ RTC::ReturnCode_t ModifiedServo::onExecute(RTC::UniqueId ec_id) double tau_limit = m_robot->joint(i)->torqueConst * m_robot->joint(i)->climit * fabs(m_robot->joint(i)->gearRatio); m_tau.data[i] = std::max(std::min(tau, tau_limit), -tau_limit); + + // if (i == 11 || i == 21) + // std::cout << "Rafa, in ModifiedServo::onExecute, for i = " << i << ", q[i] = " << q << ", qRef[i] = " << qRef + // << ", tau[i] = " << tau << ", tau_limit[i] = " << tau_limit << ", m_tau[i] = " << m_tau.data[i] << std::endl; } step--; @@ -276,88 +269,34 @@ void ModifiedServo::readGainFile() coil::stringTo(gain_fname, prop["pdgains_sim_file_name"].c_str()); } - // initialize length of vectors - - q_old.resize(dof); - qRef_old.resize(dof); - - m_tauRef.data.length(dof); - m_qRef.data.length(dof); - m_torqueMode.data.length(dof); - - tau_limit_ratio.resize(dof); - - Pgain.resize(dof); - Dgain.resize(dof); - gain.open(gain_fname.c_str()); if (gain.is_open()) { double val; + Pgain.resize(dof); + Dgain.resize(dof); + for (unsigned int i = 0; i < dof; i++) { if (gain >> val) Pgain[i] = val; else - std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; + std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; if (gain >> val) Dgain[i] = val; else - std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; + std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; } gain.close(); - std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] successfully read" << std::endl; + std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] successfully read" << std::endl; } else - std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] could not be opened" << std::endl; - - // tau_limit_ratio initialize - RTC::Properties& prop = getProperties(); - - if (prop["pdcontrol_tau_limit_ratio"] != "") { - - coil::vstring tau_limit_ratio_str = coil::split(prop["pdcontrol_tau_limit_ratio"], ","); - - if (tau_limit_ratio_str.size() == dof) { - - for (size_t i = 0; i < dof; i++) - coil::stringTo(tau_limit_ratio[i], tau_limit_ratio_str[i].c_str()); - - std::cerr << "[" << m_profile.instance_name << "] tau_limit_ratio is set to " << prop["pdcontrol_tau_limit_ratio"] << std::endl; - } - else { - - for (size_t i = 0; i < dof; i++) - tau_limit_ratio[i] = 1.0; - - std::cerr << "[" << m_profile.instance_name << "] pdcontrol_tau_limit_ratio found, but invalid length (" << tau_limit_ratio_str.size() << " != " << dof << ")." << std::endl; - std::cerr << "[" << m_profile.instance_name << "] All tau_limit_ratio are set to 1.0." << std::endl; - } - } - else { - for (size_t i = 0; i < dof; i++) - tau_limit_ratio[i] = 1.0; - - std::cerr << "[" << m_profile.instance_name << "] No pdcontrol_tau_limit_ratio found." << std::endl; - std::cerr << "[" << m_profile.instance_name << "] All tau_limit_ratio are set to 1.0." << std::endl; - } - - // initialize angleRef, old_ref and old with angle - for(unsigned int i=0; i < dof; ++i) { - m_tauRef.data[i] = 0.0; - m_qRef.data[i] = qRef_old[i] = q_old[i] = m_q.data[i]; - m_torqueMode.data[i] = false; - } - - // Print loaded gain - std::cerr << "[" << m_profile.instance_name << "] loadGain" << std::endl; - for (unsigned int i=0; inumJoints(); i++) - std::cerr << "[" << m_profile.instance_name << "] " << m_robot->joint(i)->name << ", pgain = " << Pgain[i] << ", dgain = " << Dgain[i] << std::endl; + std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] could not be opened" << std::endl; } extern "C" diff --git a/rtc/ModifiedServo/ModifiedServo.h b/rtc/ModifiedServo/ModifiedServo.h index df9bb4afa12..287af997312 100644 --- a/rtc/ModifiedServo/ModifiedServo.h +++ b/rtc/ModifiedServo/ModifiedServo.h @@ -150,16 +150,13 @@ class ModifiedServo : public RTC::DataFlowComponentBase double step; // current interpolation step double nstep; // number of steps to interpolate references - size_t dof, loop; - unsigned int m_debugLevel; - int dummy; + size_t dof; std::string gain_fname; std::ifstream gain; hrp::dvector Pgain, Dgain; hrp::dvector q_old, qRef_old; - hrp::dvector tau_limit_ratio; }; From 0a39c1367d7154158ff20fc71b769414e3941bcc Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Tue, 25 Jun 2019 18:57:03 +0900 Subject: [PATCH 04/26] Added torqueControlMode to RobotHardware --- rtc/RobotHardware/RobotHardware.cpp | 9 +++++++++ rtc/RobotHardware/RobotHardware.h | 8 +++++++- 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/rtc/RobotHardware/RobotHardware.cpp b/rtc/RobotHardware/RobotHardware.cpp index 51abd91a656..fa9f3453ad0 100644 --- a/rtc/RobotHardware/RobotHardware.cpp +++ b/rtc/RobotHardware/RobotHardware.cpp @@ -51,6 +51,7 @@ RobotHardware::RobotHardware(RTC::Manager* manager) m_dqRefIn("dqRef", m_dqRef), m_ddqRefIn("ddqRef", m_ddqRef), m_tauRefIn("tauRef", m_tauRef), + m_torqueControlModeIn("torqueControlMode", m_torqueControlMode), m_qOut("q", m_q), m_dqOut("dq", m_dq), m_tauOut("tau", m_tau), @@ -79,6 +80,7 @@ RTC::ReturnCode_t RobotHardware::onInitialize() addInPort("dqRef", m_dqRefIn); addInPort("ddqRef", m_ddqRefIn); addInPort("tauRef", m_tauRefIn); + addInPort("torqueControlMode", m_torqueControlModeIn); addOutPort("q", m_qOut); addOutPort("dq", m_dqOut); @@ -145,6 +147,7 @@ RTC::ReturnCode_t RobotHardware::onInitialize() m_dqRef.data.length(m_robot->numJoints()); m_ddqRef.data.length(m_robot->numJoints()); m_tauRef.data.length(m_robot->numJoints()); + m_torqueControlMode.data.length(m_robot->numJoints()); int ngyro = m_robot->numSensors(Sensor::RATE_GYRO); std::cout << "the number of gyros = " << ngyro << std::endl; @@ -281,6 +284,12 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id) // output to iob m_robot->writeTorqueCommands(m_tauRef.data.get_buffer()); } + if (m_torqueControlModeIn.isNew()){ + m_torqueControlModeIn.read(); + for (unsigned int i=0; inumJoints(); ++i) { + m_robot->setJointControlMode(m_robot->joint(i)->name.c_str(), m_torqueControlMode.data[i] ? JCM_TORQUE : JCM_POSITION); + } + } // read from iob m_robot->readJointAngles(m_q.data.get_buffer()); diff --git a/rtc/RobotHardware/RobotHardware.h b/rtc/RobotHardware/RobotHardware.h index 1450cfd07c8..24d5118e3c6 100644 --- a/rtc/RobotHardware/RobotHardware.h +++ b/rtc/RobotHardware/RobotHardware.h @@ -23,6 +23,7 @@ #include #include +#include // Service implementation headers // @@ -140,7 +141,12 @@ class RobotHardware */ TimedDoubleSeq m_tauRef; InPort m_tauRefIn; - + /** + \brief array of booleans indicating which joint is in torque mode (1) or position mode (0) + */ + TimedBooleanSeq m_torqueControlMode; + InPort m_torqueControlModeIn; + // /** From 8846a8bb4fc38a3f14cc595a609b1454a14858b5 Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Wed, 14 Aug 2019 08:55:51 +0900 Subject: [PATCH 05/26] Excluded torque-controlled joints from the checkEmergency test for the maximum difference between angle and command, and added test for the torque --- rtc/RobotHardware/robot.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/rtc/RobotHardware/robot.cpp b/rtc/RobotHardware/robot.cpp index b79e2c49678..3f5fa7e8531 100644 --- a/rtc/RobotHardware/robot.cpp +++ b/rtc/RobotHardware/robot.cpp @@ -665,9 +665,11 @@ bool robot::checkJointCommands(const double *i_commands) bool robot::checkEmergency(emg_reason &o_reason, int &o_id) { int state; + joint_control_mode mode; // Added by Rafa for (unsigned int i=0; i torque_limit){ + std::cerr << time_string() + << ": servo error torque limit over: joint = " + << joint(i)->name + << ", tauRef = " << command_torque << "[Nm], tauMax = " + << torque_limit << "[Nm]" << std::endl; + } + } } if (m_rLegForceSensorId >= 0){ From 228ce6fc4ddd5b83d561d6e961c4f2795dcf0975 Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Wed, 14 Aug 2019 08:56:34 +0900 Subject: [PATCH 06/26] Removed test for the torque because at the end it gets saturated to the maximum values --- rtc/RobotHardware/robot.cpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/rtc/RobotHardware/robot.cpp b/rtc/RobotHardware/robot.cpp index 3f5fa7e8531..ef5415c5d8e 100644 --- a/rtc/RobotHardware/robot.cpp +++ b/rtc/RobotHardware/robot.cpp @@ -684,18 +684,6 @@ bool robot::checkEmergency(emg_reason &o_reason, int &o_id) return true; } } - else if (state == ON && mode == JCB_TORQUE){ // Added by Rafa - double command_torque, torque_limit; - read_command_torque(i, &command_torque); - read_torque_limit(i, &torque_limit); - if (fabs(command_torque) > torque_limit){ - std::cerr << time_string() - << ": servo error torque limit over: joint = " - << joint(i)->name - << ", tauRef = " << command_torque << "[Nm], tauMax = " - << torque_limit << "[Nm]" << std::endl; - } - } } if (m_rLegForceSensorId >= 0){ From ef3910b0e44b0c957b388c9db3087967fdba4d80 Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Wed, 14 Aug 2019 10:06:37 +0900 Subject: [PATCH 07/26] Corrected typo --- rtc/RobotHardware/robot.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rtc/RobotHardware/robot.cpp b/rtc/RobotHardware/robot.cpp index ef5415c5d8e..bf05343831b 100644 --- a/rtc/RobotHardware/robot.cpp +++ b/rtc/RobotHardware/robot.cpp @@ -669,7 +669,7 @@ bool robot::checkEmergency(emg_reason &o_reason, int &o_id) for (unsigned int i=0; i Date: Wed, 14 Aug 2019 15:00:24 +0900 Subject: [PATCH 08/26] Added check for the joint values to be inside of the joint range and a check if a new tauRef does not delay for more than a number of cycles --- lib/io/iob.h | 18 +++++++++++++++ rtc/RobotHardware/RobotHardware.cpp | 15 +++++++++++- rtc/RobotHardware/RobotHardware.h | 2 ++ rtc/RobotHardware/robot.cpp | 36 +++++++++++++++++++++++++++++ rtc/RobotHardware/robot.h | 14 ++++++++++- 5 files changed, 83 insertions(+), 2 deletions(-) diff --git a/lib/io/iob.h b/lib/io/iob.h index 0321beb7cab..5892fef6520 100644 --- a/lib/io/iob.h +++ b/lib/io/iob.h @@ -175,6 +175,24 @@ extern "C"{ */ int write_angle_offset(int id, double offset); + /** + * @brief read lower joint limit[rad] + * @param id joint id + * @param angle lower joint limit[rad] + * @retval TRUE this function is supported + * @retval FALSE otherwise + */ + int read_llimit_angle(int id, double *angle); + + /** + * @brief read upper joint limit[rad] + * @param id joint id + * @param angle upper joint limit[rad] + * @retval TRUE this function is supported + * @retval FALSE otherwise + */ + int read_ulimit_angle(int id, double *angle); + //@} // @name joint diff --git a/rtc/RobotHardware/RobotHardware.cpp b/rtc/RobotHardware/RobotHardware.cpp index fa9f3453ad0..42c1834ca3a 100644 --- a/rtc/RobotHardware/RobotHardware.cpp +++ b/rtc/RobotHardware/RobotHardware.cpp @@ -15,6 +15,8 @@ #include #include +#define MAX_CYCLES_WITHOUT_TAUREF 3 // Added by Rafa + using namespace OpenHRP; using namespace hrp; @@ -62,7 +64,8 @@ RobotHardware::RobotHardware(RTC::Manager* manager) m_rstate2Out("rstate2", m_rstate2), m_RobotHardwareServicePort("RobotHardwareService"), // - dummy(0) + m_count_noNewTauRef(0), + dummy(0) { } @@ -253,6 +256,15 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id) } } + if (!m_isDemoMode && m_robot->isJointTorqueControlModeUsed()){ // Added by Rafa + if (m_robot->checkJointActualValues() || m_count_noNewTauRef > MAX_CYCLES_WITHOUT_TAUREF){ + m_robot->servo("all", false); + m_emergencySignal.data = robot::EMG_SERVO_ERROR; + m_emergencySignalOut.write(); + } + m_count_noNewTauRef++; + } + if (m_qRefIn.isNew()){ m_qRefIn.read(); //std::cout << "RobotHardware: qRef[21] = " << m_qRef.data[21] << std::endl; @@ -280,6 +292,7 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id) } if (m_tauRefIn.isNew()){ m_tauRefIn.read(); + m_count_noNewTauRef = 0; //std::cout << "RobotHardware: tauRef[21] = " << m_tauRef.data[21] << std::endl; // output to iob m_robot->writeTorqueCommands(m_tauRef.data.get_buffer()); diff --git a/rtc/RobotHardware/RobotHardware.h b/rtc/RobotHardware/RobotHardware.h index 24d5118e3c6..04cd478b159 100644 --- a/rtc/RobotHardware/RobotHardware.h +++ b/rtc/RobotHardware/RobotHardware.h @@ -223,6 +223,8 @@ class RobotHardware private: void getStatus2(OpenHRP::RobotHardwareService::RobotState2 &rstate2); + int m_count_noNewTauRef; // Added by Rafa + int dummy; boost::shared_ptr m_robot; }; diff --git a/rtc/RobotHardware/robot.cpp b/rtc/RobotHardware/robot.cpp index bf05343831b..60715de1dae 100644 --- a/rtc/RobotHardware/robot.cpp +++ b/rtc/RobotHardware/robot.cpp @@ -627,6 +627,31 @@ char *time_string() return time; } +bool robot::checkJointActualValues() +{ + int state; + for (unsigned int i=0; i= ulimit){ + std::cerr << time_string() + << ": actual joint angle outside allowed range" + << ": joint = " << joint(i)->name + << ", llimit = " << llimit/M_PI*180 << "[deg]" + << ", angle = " << angle/M_PI*180 << "[deg]" + << ", ulimit = " << ulimit/M_PI*180 << "[deg]" + << std::endl; + return true; + } + } + } + return false; +} + bool robot::checkJointCommands(const double *i_commands) { if (!m_dt) return false; @@ -1065,3 +1090,14 @@ bool robot::setJointControlMode(const char *i_jname, joint_control_mode mode) } return true; } + +bool robot::isJointTorqueControlModeUsed() // Added by Rafa +{ + joint_control_mode mode; + for (unsigned int i=0; i Date: Wed, 14 Aug 2019 15:55:14 +0900 Subject: [PATCH 09/26] Added resetJointControlMode and deactivated the previous security protection --- rtc/RobotHardware/RobotHardware.cpp | 9 +++++++-- rtc/RobotHardware/RobotHardware.h | 4 ++-- rtc/RobotHardware/robot.cpp | 21 +++++++++++++++++++++ rtc/RobotHardware/robot.h | 6 ++++++ 4 files changed, 36 insertions(+), 4 deletions(-) diff --git a/rtc/RobotHardware/RobotHardware.cpp b/rtc/RobotHardware/RobotHardware.cpp index 42c1834ca3a..34d8d9ac0bf 100644 --- a/rtc/RobotHardware/RobotHardware.cpp +++ b/rtc/RobotHardware/RobotHardware.cpp @@ -15,7 +15,7 @@ #include #include -#define MAX_CYCLES_WITHOUT_TAUREF 3 // Added by Rafa +//#define MAX_CYCLES_WITHOUT_TAUREF 3 // Added by Rafa using namespace OpenHRP; using namespace hrp; @@ -64,7 +64,7 @@ RobotHardware::RobotHardware(RTC::Manager* manager) m_rstate2Out("rstate2", m_rstate2), m_RobotHardwareServicePort("RobotHardwareService"), // - m_count_noNewTauRef(0), + // m_count_noNewTauRef(0), dummy(0) { } @@ -256,6 +256,7 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id) } } + /* if (!m_isDemoMode && m_robot->isJointTorqueControlModeUsed()){ // Added by Rafa if (m_robot->checkJointActualValues() || m_count_noNewTauRef > MAX_CYCLES_WITHOUT_TAUREF){ m_robot->servo("all", false); @@ -264,6 +265,10 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id) } m_count_noNewTauRef++; } + */ + + if (!m_isDemoMode && m_robot->checkJointActualValues()) + resetJointControlMode(); if (m_qRefIn.isNew()){ m_qRefIn.read(); diff --git a/rtc/RobotHardware/RobotHardware.h b/rtc/RobotHardware/RobotHardware.h index 04cd478b159..0a983b626b4 100644 --- a/rtc/RobotHardware/RobotHardware.h +++ b/rtc/RobotHardware/RobotHardware.h @@ -222,8 +222,8 @@ class RobotHardware robot *robot_ptr(void) { return m_robot.get(); }; private: void getStatus2(OpenHRP::RobotHardwareService::RobotState2 &rstate2); - - int m_count_noNewTauRef; // Added by Rafa + + // int m_count_noNewTauRef; // Added by Rafa int dummy; boost::shared_ptr m_robot; diff --git a/rtc/RobotHardware/robot.cpp b/rtc/RobotHardware/robot.cpp index 60715de1dae..2b765f352b6 100644 --- a/rtc/RobotHardware/robot.cpp +++ b/rtc/RobotHardware/robot.cpp @@ -1101,3 +1101,24 @@ bool robot::isJointTorqueControlModeUsed() // Added by Rafa } return false; } + +bool robot::resetJointControlMode() // Added by Rafa +{ + bool res = false; + double angle; + joint_control_mode mode; + for (unsigned int=0; i Date: Wed, 14 Aug 2019 19:29:00 +0900 Subject: [PATCH 10/26] Added allowTorqueControlMode --- rtc/RobotHardware/RobotHardware.cpp | 13 +++++++++---- rtc/RobotHardware/RobotHardware.h | 1 + 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/rtc/RobotHardware/RobotHardware.cpp b/rtc/RobotHardware/RobotHardware.cpp index 34d8d9ac0bf..066470f7289 100644 --- a/rtc/RobotHardware/RobotHardware.cpp +++ b/rtc/RobotHardware/RobotHardware.cpp @@ -65,6 +65,7 @@ RobotHardware::RobotHardware(RTC::Manager* manager) m_RobotHardwareServicePort("RobotHardwareService"), // // m_count_noNewTauRef(0), + allowTorqueControlMode(true), dummy(0) { } @@ -267,8 +268,10 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id) } */ - if (!m_isDemoMode && m_robot->checkJointActualValues()) + if (!m_isDemoMode && m_robot->checkJointActualValues()){ resetJointControlMode(); + allowTorqueControlMode = false; + } if (m_qRefIn.isNew()){ m_qRefIn.read(); @@ -297,15 +300,17 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id) } if (m_tauRefIn.isNew()){ m_tauRefIn.read(); - m_count_noNewTauRef = 0; + // m_count_noNewTauRef = 0; //std::cout << "RobotHardware: tauRef[21] = " << m_tauRef.data[21] << std::endl; // output to iob m_robot->writeTorqueCommands(m_tauRef.data.get_buffer()); } if (m_torqueControlModeIn.isNew()){ m_torqueControlModeIn.read(); - for (unsigned int i=0; inumJoints(); ++i) { - m_robot->setJointControlMode(m_robot->joint(i)->name.c_str(), m_torqueControlMode.data[i] ? JCM_TORQUE : JCM_POSITION); + if (allowTorqueControlMode){ + for (unsigned int i=0; inumJoints(); ++i){ + m_robot->setJointControlMode(m_robot->joint(i)->name.c_str(), m_torqueControlMode.data[i] ? JCM_TORQUE : JCM_POSITION); + } } } diff --git a/rtc/RobotHardware/RobotHardware.h b/rtc/RobotHardware/RobotHardware.h index 0a983b626b4..ca0d7109573 100644 --- a/rtc/RobotHardware/RobotHardware.h +++ b/rtc/RobotHardware/RobotHardware.h @@ -224,6 +224,7 @@ class RobotHardware void getStatus2(OpenHRP::RobotHardwareService::RobotState2 &rstate2); // int m_count_noNewTauRef; // Added by Rafa + bool allowTorqueControlMode; // Added by Rafa int dummy; boost::shared_ptr m_robot; From 9c750dcfc3e1a1ad459d4a1a65e082687c179de4 Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Wed, 14 Aug 2019 23:20:09 +0900 Subject: [PATCH 11/26] Commented out the messages that are displayed when changing the joint control mode, and fixed some typos --- rtc/RobotHardware/RobotHardware.cpp | 2 +- rtc/RobotHardware/robot.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/rtc/RobotHardware/RobotHardware.cpp b/rtc/RobotHardware/RobotHardware.cpp index 066470f7289..a9369342f9c 100644 --- a/rtc/RobotHardware/RobotHardware.cpp +++ b/rtc/RobotHardware/RobotHardware.cpp @@ -269,7 +269,7 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id) */ if (!m_isDemoMode && m_robot->checkJointActualValues()){ - resetJointControlMode(); + m_robot->resetJointControlMode(); allowTorqueControlMode = false; } diff --git a/rtc/RobotHardware/robot.cpp b/rtc/RobotHardware/robot.cpp index 2b765f352b6..d6a8b087307 100644 --- a/rtc/RobotHardware/robot.cpp +++ b/rtc/RobotHardware/robot.cpp @@ -1075,10 +1075,10 @@ bool robot::setJointControlMode(const char *i_jname, joint_control_mode mode) for (int i=0; i < numJoints(); i++) { write_control_mode(i, mode); } - std::cerr << "[RobotHardware] setJointControlMode for all joints : " << mode << std::endl; - } else if ((l = link(i_jname)) && (l->jointId >= 0)) { + // std::cerr << "[RobotHardware] setJointControlMode for all joints : " << mode << std::endl; // Commented out by Rafa + } else if ((l = link(i_jname))) { write_control_mode(l->jointId, mode); - std::cerr << "[RobotHardware] setJointControlMode for " << i_jname << " : " << mode << std::endl; + // std::cerr << "[RobotHardware] setJointControlMode for " << i_jname << " : " << mode << std::endl; // Commented out by Rafa } else { char *s = (char *)i_jname; while(*s) { *s=toupper(*s); s++; } const std::vector jgroup = m_jointGroups[i_jname]; @@ -1086,7 +1086,7 @@ bool robot::setJointControlMode(const char *i_jname, joint_control_mode mode) for (unsigned int i=0; i Date: Thu, 15 Aug 2019 09:22:41 +0900 Subject: [PATCH 12/26] Deleted isJointTorqueControlModeUsed --- rtc/RobotHardware/robot.cpp | 13 +------------ rtc/RobotHardware/robot.h | 6 ------ 2 files changed, 1 insertion(+), 18 deletions(-) diff --git a/rtc/RobotHardware/robot.cpp b/rtc/RobotHardware/robot.cpp index d6a8b087307..555ff96ec8a 100644 --- a/rtc/RobotHardware/robot.cpp +++ b/rtc/RobotHardware/robot.cpp @@ -1091,17 +1091,6 @@ bool robot::setJointControlMode(const char *i_jname, joint_control_mode mode) return true; } -bool robot::isJointTorqueControlModeUsed() // Added by Rafa -{ - joint_control_mode mode; - for (unsigned int i=0; i Date: Thu, 15 Aug 2019 13:20:48 +0900 Subject: [PATCH 13/26] Added write_llimit_angle and write_ulimit_angle to iob.h --- lib/io/iob.cpp | 14 ++++++++++++++ lib/io/iob.h | 26 ++++++++++++++++++++++---- 2 files changed, 36 insertions(+), 4 deletions(-) diff --git a/lib/io/iob.cpp b/lib/io/iob.cpp index 37147b4e4d0..56d157d5fc2 100644 --- a/lib/io/iob.cpp +++ b/lib/io/iob.cpp @@ -527,18 +527,32 @@ int read_ulimit_angle(int id, double *angle) { return FALSE; } + int read_llimit_angle(int id, double *angle) { return FALSE; } + +int write_ulimit_angle(int id, double angle) +{ + return FALSE; +} + +int write_llimit_angle(int id, double angle) +{ + return FALSE; +} + int read_encoder_pulse(int id, double *ec) { return FALSE; } + int read_gear_ratio(int id, double *gr) { return FALSE; } + int read_torque_const(int id, double *tc) { return FALSE; diff --git a/lib/io/iob.h b/lib/io/iob.h index 5892fef6520..8a888ae5683 100644 --- a/lib/io/iob.h +++ b/lib/io/iob.h @@ -179,8 +179,8 @@ extern "C"{ * @brief read lower joint limit[rad] * @param id joint id * @param angle lower joint limit[rad] - * @retval TRUE this function is supported - * @retval FALSE otherwise + * @retval TRUE this function is supported + * @retval FALSE otherwise */ int read_llimit_angle(int id, double *angle); @@ -188,10 +188,28 @@ extern "C"{ * @brief read upper joint limit[rad] * @param id joint id * @param angle upper joint limit[rad] - * @retval TRUE this function is supported - * @retval FALSE otherwise + * @retval TRUE this function is supported + * @retval FALSE otherwise */ int read_ulimit_angle(int id, double *angle); + + /** + * @brief write lower joint limit[rad] + * @param id joint id + * @param angle lower joint limit[rad] + * @retval TRUE this function is supported + * @retval FALSE otherwise + */ + int write_llimit_angle(int id, double angle); + + /** + * @brief write upper joint limit[rad] + * @param id joint id + * @param angle upper joint limit[rad] + * @retval TRUE this function is supported + * @retval FALSE otherwise + */ + int write_ulimit_angle(int id, double angle); //@} From 6584025ebdf5054cbb9eb13b618b95c1a79c939c Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Fri, 16 Aug 2019 15:21:20 +0900 Subject: [PATCH 14/26] Used m_ for the member variables of ModifiedServo --- rtc/ModifiedServo/ModifiedServo.cpp | 83 +++++++++++++++-------------- rtc/ModifiedServo/ModifiedServo.h | 18 +++---- 2 files changed, 52 insertions(+), 49 deletions(-) diff --git a/rtc/ModifiedServo/ModifiedServo.cpp b/rtc/ModifiedServo/ModifiedServo.cpp index a4064b9d1ef..a24ffc49d4e 100644 --- a/rtc/ModifiedServo/ModifiedServo.cpp +++ b/rtc/ModifiedServo/ModifiedServo.cpp @@ -42,9 +42,9 @@ ModifiedServo::ModifiedServo(RTC::Manager* manager) m_pgainsOut("pgainsGet", m_pgains), m_dgainsOut("dgainsGet", m_dgains), // - gain_fname(""), - dt(0.005), - dof(0) + m_gain_fname(""), + m_dt(0.005), + m_dof(0) { } @@ -87,10 +87,10 @@ RTC::ReturnCode_t ModifiedServo::onInitialize() RTC::Properties & prop = getProperties(); - coil::stringTo(dt, prop["dt"].c_str()); - coil::stringTo(ref_dt, prop["ref_dt"].c_str()); - nstep = ref_dt/dt; - step = nstep; + coil::stringTo(m_dt, prop["dt"].c_str()); + coil::stringTo(m_ref_dt, prop["ref_dt"].c_str()); + m_nstep = m_ref_dt / m_dt; + m_step = m_nstep; m_robot = hrp::BodyPtr(new hrp::Body()); @@ -139,27 +139,30 @@ RTC::ReturnCode_t ModifiedServo::onActivated(RTC::UniqueId ec_id) if (m_qIn.isNew()) { m_qIn.read(); - if (dof == 0) { - dof = m_q.data.length(); + if (m_dof == 0) { + m_dof = m_q.data.length(); readGainFile(); } } - q_old.resize(dof); - qRef_old.resize(dof); + m_q_old.resize(m_dof); + m_qRef_old.resize(m_dof); - m_tauRef.data.length(dof); - m_qRef.data.length(dof); - m_torqueMode.data.length(dof); + m_tauRef.data.length(m_dof); + m_qRef.data.length(m_dof); + m_torqueMode.data.length(m_dof); - m_tau.data.length(dof); + m_tau.data.length(m_dof); m_pgains.data.length(dof); m_dgains.data.length(dof); for (size_t i = 0; i < dof; i++) { +======= + for (size_t i = 0; i < m_dof; i++) { +>>>>>>> Used m_ for the member variables of ModifiedServo m_tauRef.data[i] = 0.0; - m_qRef.data[i] = qRef_old[i] = q_old[i] = m_q.data[i]; + m_qRef.data[i] = m_qRef_old[i] = m_q_old[i] = m_q.data[i]; m_torqueMode.data[i] = false; m_pgains.data[i] = Pgain[i]; m_dgains.data[i] = Dgain[i]; @@ -185,7 +188,7 @@ RTC::ReturnCode_t ModifiedServo::onExecute(RTC::UniqueId ec_id) if (m_qRefIn.isNew()) { m_qRefIn.read(); - step = nstep; + m_step = m_nstep; } if(m_pgainsIn.isNew()){ m_pgainsIn.read(); @@ -202,15 +205,15 @@ RTC::ReturnCode_t ModifiedServo::onExecute(RTC::UniqueId ec_id) Dgain[i] = m_dgains.data[i]; double q = m_q.data[i]; - double qRef = step > 0 ? qRef_old[i] + (m_qRef.data[i] - qRef_old[i]) / step : qRef_old[i]; + double qRef = m_step > 0 ? m_qRef_old[i] + (m_qRef.data[i] - m_qRef_old[i]) / m_step : m_qRef_old[i]; - double dq = (q - q_old[i]) / dt; - double dqRef = (qRef - qRef_old[i]) / dt; + double dq = (q - m_q_old[i]) / m_dt; + double dqRef = (qRef - m_qRef_old[i]) / m_dt; - q_old[i] = q; - qRef_old[i] = qRef; + m_q_old[i] = q; + m_qRef_old[i] = qRef; - double tau = m_torqueMode.data[i] ? m_tauRef.data[i] : Pgain[i] * (qRef - q) + Dgain[i] * (dqRef - dq); + double tau = m_torqueMode.data[i] ? m_tauRef.data[i] : m_Pgain[i] * (qRef - q) + m_Dgain[i] * (dqRef - dq); double tau_limit = m_robot->joint(i)->torqueConst * m_robot->joint(i)->climit * fabs(m_robot->joint(i)->gearRatio); @@ -221,7 +224,7 @@ RTC::ReturnCode_t ModifiedServo::onExecute(RTC::UniqueId ec_id) // << ", tau[i] = " << tau << ", tau_limit[i] = " << tau_limit << ", m_tau[i] = " << m_tau.data[i] << std::endl; } - step--; + m_step--; m_tau.tm = m_q.tm; m_tauOut.write(); @@ -264,39 +267,39 @@ RTC::ReturnCode_t ModifiedServo::onRateChanged(RTC::UniqueId ec_id) void ModifiedServo::readGainFile() { - if (gain_fname == "") { + if (m_gain_fname == "") { RTC::Properties & prop = getProperties(); - coil::stringTo(gain_fname, prop["pdgains_sim_file_name"].c_str()); + coil::stringTo(m_gain_fname, prop["pdgains_sim_file_name"].c_str()); } - gain.open(gain_fname.c_str()); + m_gain.open(m_gain_fname.c_str()); - if (gain.is_open()) { + if (m_gain.is_open()) { double val; - Pgain.resize(dof); - Dgain.resize(dof); + m_Pgain.resize(m_dof); + m_Dgain.resize(m_dof); - for (unsigned int i = 0; i < dof; i++) { + for (unsigned int i = 0; i < m_dof; i++) { - if (gain >> val) - Pgain[i] = val; + if (m_gain >> val) + m_Pgain[i] = val; else - std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; + std::cout << "[" << m_profile.instance_name << "] Gain file [" << m_gain_fname << "] is too short" << std::endl; - if (gain >> val) - Dgain[i] = val; + if (m_gain >> val) + m_Dgain[i] = val; else - std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; + std::cout << "[" << m_profile.instance_name << "] Gain file [" << m_gain_fname << "] is too short" << std::endl; } - gain.close(); + m_gain.close(); - std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] successfully read" << std::endl; + std::cout << "[" << m_profile.instance_name << "] Gain file [" << m_gain_fname << "] successfully read" << std::endl; } else - std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] could not be opened" << std::endl; + std::cout << "[" << m_profile.instance_name << "] Gain file [" << m_gain_fname << "] could not be opened" << std::endl; } extern "C" diff --git a/rtc/ModifiedServo/ModifiedServo.h b/rtc/ModifiedServo/ModifiedServo.h index 287af997312..0c45eff9b75 100644 --- a/rtc/ModifiedServo/ModifiedServo.h +++ b/rtc/ModifiedServo/ModifiedServo.h @@ -145,18 +145,18 @@ class ModifiedServo : public RTC::DataFlowComponentBase hrp::BodyPtr m_robot; - double dt; // sampling time of the controller - double ref_dt; // sampling time of reference angles - double step; // current interpolation step - double nstep; // number of steps to interpolate references + double m_dt; // sampling time of the controller + double m_ref_dt; // sampling time of reference angles + double m_step; // current interpolation step + double m_nstep; // number of steps to interpolate references - size_t dof; + size_t m_dof; - std::string gain_fname; - std::ifstream gain; + std::string m_gain_fname; + std::ifstream m_gain; - hrp::dvector Pgain, Dgain; - hrp::dvector q_old, qRef_old; + hrp::dvector m_Pgain, m_Dgain; + hrp::dvector m_q_old, m_qRef_old; }; From b8d15168524380a12862d68abe82a1055c5426be Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Fri, 16 Aug 2019 15:27:24 +0900 Subject: [PATCH 15/26] Changed intermediate qRef into qCom in ModifiedServo --- rtc/ModifiedServo/ModifiedServo.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/rtc/ModifiedServo/ModifiedServo.cpp b/rtc/ModifiedServo/ModifiedServo.cpp index a24ffc49d4e..b91b5913ecf 100644 --- a/rtc/ModifiedServo/ModifiedServo.cpp +++ b/rtc/ModifiedServo/ModifiedServo.cpp @@ -205,23 +205,19 @@ RTC::ReturnCode_t ModifiedServo::onExecute(RTC::UniqueId ec_id) Dgain[i] = m_dgains.data[i]; double q = m_q.data[i]; - double qRef = m_step > 0 ? m_qRef_old[i] + (m_qRef.data[i] - m_qRef_old[i]) / m_step : m_qRef_old[i]; + double qCom = m_step > 0 ? m_qRef_old[i] + (m_qRef.data[i] - m_qRef_old[i]) / m_step : m_qRef_old[i]; double dq = (q - m_q_old[i]) / m_dt; - double dqRef = (qRef - m_qRef_old[i]) / m_dt; + double dqCom = (qCom - m_qRef_old[i]) / m_dt; m_q_old[i] = q; - m_qRef_old[i] = qRef; + m_qRef_old[i] = qCom; - double tau = m_torqueMode.data[i] ? m_tauRef.data[i] : m_Pgain[i] * (qRef - q) + m_Dgain[i] * (dqRef - dq); + double tau = m_torqueMode.data[i] ? m_tauRef.data[i] : m_Pgain[i] * (qCom - q) + m_Dgain[i] * (dqCom - dq); double tau_limit = m_robot->joint(i)->torqueConst * m_robot->joint(i)->climit * fabs(m_robot->joint(i)->gearRatio); m_tau.data[i] = std::max(std::min(tau, tau_limit), -tau_limit); - - // if (i == 11 || i == 21) - // std::cout << "Rafa, in ModifiedServo::onExecute, for i = " << i << ", q[i] = " << q << ", qRef[i] = " << qRef - // << ", tau[i] = " << tau << ", tau_limit[i] = " << tau_limit << ", m_tau[i] = " << m_tau.data[i] << std::endl; } m_step--; From cbed8f4e18047f00c873fbd9a22d57dd860ac5c0 Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Mon, 15 Feb 2021 16:52:41 +0900 Subject: [PATCH 16/26] Removed some test versions of ModifiedServo --- rtc/ModifiedServo/ModifiedServo-PDcopy.cpp | 305 ------------------ rtc/ModifiedServo/ModifiedServo-PDcopy.h | 151 --------- rtc/ModifiedServo/ModifiedServo-PDlike.cpp | 354 --------------------- rtc/ModifiedServo/ModifiedServo-PDlike.h | 164 ---------- 4 files changed, 974 deletions(-) delete mode 100644 rtc/ModifiedServo/ModifiedServo-PDcopy.cpp delete mode 100644 rtc/ModifiedServo/ModifiedServo-PDcopy.h delete mode 100644 rtc/ModifiedServo/ModifiedServo-PDlike.cpp delete mode 100644 rtc/ModifiedServo/ModifiedServo-PDlike.h diff --git a/rtc/ModifiedServo/ModifiedServo-PDcopy.cpp b/rtc/ModifiedServo/ModifiedServo-PDcopy.cpp deleted file mode 100644 index c7fea36d4a0..00000000000 --- a/rtc/ModifiedServo/ModifiedServo-PDcopy.cpp +++ /dev/null @@ -1,305 +0,0 @@ -// -*- mode: c++; -*- -/*! - * @file ModifiedServo.cpp - * @brief Sample PD component - * $Date$ - * - * $Id$ - */ - -#include "ModifiedServo.h" -#include -#include - -// Module specification -// -static const char* ModifiedServo_spec[] = - { - "implementation_id", "ModifiedServo", - "type_name", "ModifiedServo", - "description", "ModifiedServo component", - "version", HRPSYS_PACKAGE_VERSION, - "vendor", "AIST", - "category", "example", - "activity_type", "DataFlowComponent", - "max_instance", "10", - "language", "C++", - "lang_type", "compile", - // Configuration variables - "conf.default.pdgains_sim_file_name", "", - "conf.default.debugLevel", "0", - "" - }; -// - -ModifiedServo::ModifiedServo(RTC::Manager* manager) - : RTC::DataFlowComponentBase(manager), - // - m_angleIn("angle", m_angle), - m_angleRefIn("angleRef", m_angleRef), - m_torqueOut("torque", m_torque), - dt(0.005), - // - gain_fname(""), - dof(0), loop(0), - dummy(0) -{ -} - -ModifiedServo::~ModifiedServo() -{ -} - - -RTC::ReturnCode_t ModifiedServo::onInitialize() -{ - std::cout << m_profile.instance_name << ": onInitialize() " << std::endl; - - RTC::Properties& prop = getProperties(); - coil::stringTo(dt, prop["dt"].c_str()); - ref_dt = dt; - coil::stringTo(ref_dt, prop["ref_dt"].c_str()); - nstep = ref_dt/dt; - step = nstep; - - m_robot = hrp::BodyPtr(new hrp::Body()); - - RTC::Manager& rtcManager = RTC::Manager::instance(); - std::string nameServer = rtcManager.getConfig()["corba.nameservers"]; - int comPos = nameServer.find(","); - if (comPos < 0){ - comPos = nameServer.length(); - } - nameServer = nameServer.substr(0, comPos); - RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str()); - if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), - CosNaming::NamingContext::_duplicate(naming.getRootContext()) - )){ - std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" - << std::endl; - } - - // - // Bind variables and configuration variable - bindParameter("pdgains_sim_file_name", gain_fname, ""); - bindParameter("debugLevel", m_debugLevel, "0"); - - // Set InPort buffers - addInPort("angle", m_angleIn); - addInPort("angleRef", m_angleRefIn); - - // Set OutPort buffer - addOutPort("torque", m_torqueOut); - - // - - return RTC::RTC_OK; -} - - - -/* -RTC::ReturnCode_t ModifiedServo::onFinalize() -{ - return RTC::RTC_OK; -} -*/ - -/* -RTC::ReturnCode_t ModifiedServo::onStartup(RTC::UniqueId ec_id) -{ - return RTC::RTC_OK; -} -*/ - -/* -RTC::ReturnCode_t ModifiedServo::onShutdown(RTC::UniqueId ec_id) -{ - return RTC::RTC_OK; -} -*/ - -RTC::ReturnCode_t ModifiedServo::onActivated(RTC::UniqueId ec_id) -{ - std::cout << m_profile.instance_name << ": on Activated " << std::endl; - if(m_angleIn.isNew()){ - m_angleIn.read(); - if (dof == 0) { - dof = m_angle.data.length(); - readGainFile(); - } - } - return RTC::RTC_OK; -} - -RTC::ReturnCode_t ModifiedServo::onDeactivated(RTC::UniqueId ec_id) -{ - std::cout << m_profile.instance_name << ": on Deactivated " << std::endl; - return RTC::RTC_OK; -} - - -RTC::ReturnCode_t ModifiedServo::onExecute(RTC::UniqueId ec_id) -{ - loop++; - if(m_angleIn.isNew()){ - m_angleIn.read(); - if (dof == 0) { - dof = m_angle.data.length(); - readGainFile(); - } - } - if(m_angleRefIn.isNew()){ - m_angleRefIn.read(); - step = nstep; - } - - for(size_t i=0; i 0 ? qold_ref[i] + (m_angleRef.data[i] - qold_ref[i])/step : qold_ref[i]; - double dq = (q - qold[i]) / dt; - double dq_ref = (q_ref - qold_ref[i]) / dt; - qold[i] = q; - qold_ref[i] = q_ref; - m_torque.data[i] = -(q - q_ref) * Pgain[i] - (dq - dq_ref) * Dgain[i]; - double tlimit; - if (m_robot && m_robot->numJoints() == dof) { - tlimit = m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst * tlimit_ratio[i]; - } else { - tlimit = (std::numeric_limits::max)() * tlimit_ratio[i]; - if (i == 0 && loop % 500 == 0) { - std::cerr << "[" << m_profile.instance_name << "] m_robot is not set properly!! Maybe ModelLoader is missing?" << std::endl; - } - } - if (loop % 100 == 0 && m_debugLevel == 1) { - std::cerr << "[" << m_profile.instance_name << "] joint = " - << i << ", tq = " << m_torque.data[i] << ", q,qref = (" << q << ", " << q_ref << "), dq,dqref = (" << dq << ", " << dq_ref << "), pd = (" << Pgain[i] << ", " << Dgain[i] << "), tlimit = " << tlimit << std::endl; - } - m_torque.data[i] = std::max(std::min(m_torque.data[i], tlimit), -tlimit); - } - step--; - - m_torqueOut.write(); - - return RTC::RTC_OK; -} - -void ModifiedServo::readGainFile() -{ - if (gain_fname == "") { - RTC::Properties& prop = getProperties(); - coil::stringTo(gain_fname, prop["pdgains_sim_file_name"].c_str()); - } - // initialize length of vectors - qold.resize(dof); - qold_ref.resize(dof); - m_torque.data.length(dof); - m_angleRef.data.length(dof); - Pgain.resize(dof); - Dgain.resize(dof); - gain.open(gain_fname.c_str()); - tlimit_ratio.resize(dof); - if (gain.is_open()){ - double tmp; - for (unsigned int i=0; i> tmp) { - Pgain[i] = tmp; - } else { - std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; - } - if (gain >> tmp) { - Dgain[i] = tmp; - } else { - std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; - } - } - gain.close(); - std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] opened" << std::endl; - }else{ - std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] not opened" << std::endl; - } - // tlimit_ratio initialize - { - RTC::Properties& prop = getProperties(); - if (prop["pdcontrol_tlimit_ratio"] != "") { - coil::vstring tlimit_ratio_str = coil::split(prop["pdcontrol_tlimit_ratio"], ","); - if (tlimit_ratio_str.size() == dof) { - for (size_t i = 0; i < dof; i++) { - coil::stringTo(tlimit_ratio[i], tlimit_ratio_str[i].c_str()); - } - std::cerr << "[" << m_profile.instance_name << "] tlimit_ratio is set to " << prop["pdcontrol_tlimit_ratio"] << std::endl; - } else { - for (size_t i = 0; i < dof; i++) { - tlimit_ratio[i] = 1.0; - } - std::cerr << "[" << m_profile.instance_name << "] pdcontrol_tlimit_ratio found, but invalid length (" << tlimit_ratio_str.size() << " != " << dof << ")." << std::endl; - std::cerr << "[" << m_profile.instance_name << "] All tlimit_ratio are set to 1.0." << std::endl; - } - } else { - for (size_t i = 0; i < dof; i++) { - tlimit_ratio[i] = 1.0; - } - std::cerr << "[" << m_profile.instance_name << "] No pdcontrol_tlimit_ratio found." << std::endl; - std::cerr << "[" << m_profile.instance_name << "] All tlimit_ratio are set to 1.0." << std::endl; - } - } - // initialize angleRef, old_ref and old with angle - for(unsigned int i=0; i < dof; ++i){ - m_angleRef.data[i] = qold_ref[i] = qold[i] = m_angle.data[i]; - } - // Print loaded gain - std::cerr << "[" << m_profile.instance_name << "] loadGain" << std::endl; - for (unsigned int i=0; inumJoints(); i++) { - std::cerr << "[" << m_profile.instance_name << "] " << m_robot->joint(i)->name << ", pgain = " << Pgain[i] << ", dgain = " << Dgain[i] << std::endl; - } -} - -/* - RTC::ReturnCode_t ModifiedServo::onAborting(RTC::UniqueId ec_id) - { - return RTC::RTC_OK; - } -*/ - -/* - RTC::ReturnCode_t ModifiedServo::onError(RTC::UniqueId ec_id) - { - return RTC::RTC_OK; - } -*/ - -/* - RTC::ReturnCode_t ModifiedServo::onReset(RTC::UniqueId ec_id) - { - return RTC::RTC_OK; - } -*/ - -/* - RTC::ReturnCode_t ModifiedServo::onStateUpdate(RTC::UniqueId ec_id) - { - return RTC::RTC_OK; - } -*/ - -/* - RTC::ReturnCode_t ModifiedServo::onRateChanged(RTC::UniqueId ec_id) - { - return RTC::RTC_OK; - } -*/ - -extern "C" -{ - - void ModifiedServoInit(RTC::Manager* manager) - { - RTC::Properties profile(ModifiedServo_spec); - manager->registerFactory(profile, - RTC::Create, - RTC::Delete); - } - -}; - - diff --git a/rtc/ModifiedServo/ModifiedServo-PDcopy.h b/rtc/ModifiedServo/ModifiedServo-PDcopy.h deleted file mode 100644 index 75048ecc9cd..00000000000 --- a/rtc/ModifiedServo/ModifiedServo-PDcopy.h +++ /dev/null @@ -1,151 +0,0 @@ -// -*- mode: c++; -*- -/*! - * @file ModifiedServo.h - * @brief Sample PD component - * @date $Date$ - * - * $Id$ - */ - -#ifndef ModifiedServo_H -#define ModifiedServo_H - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -// Service implementation headers -// - -// - -// Service Consumer stub headers -// - -// - -using namespace RTC; - -class ModifiedServo - : public RTC::DataFlowComponentBase -{ - public: - ModifiedServo(RTC::Manager* manager); - ~ModifiedServo(); - - // The initialize action (on CREATED->ALIVE transition) - // formaer rtc_init_entry() - virtual RTC::ReturnCode_t onInitialize(); - - // The finalize action (on ALIVE->END transition) - // formaer rtc_exiting_entry() - // virtual RTC::ReturnCode_t onFinalize(); - - // The startup action when ExecutionContext startup - // former rtc_starting_entry() - // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id); - - // The shutdown action when ExecutionContext stop - // former rtc_stopping_entry() - // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id); - - // The activated action (Active state entry action) - // former rtc_active_entry() - virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id); - - // The deactivated action (Active state exit action) - // former rtc_active_exit() - virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); - - // The execution action that is invoked periodically - // former rtc_active_do() - virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id); - - // The aborting action when main logic error occurred. - // former rtc_aborting_entry() - // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id); - - // The error action in ERROR state - // former rtc_error_do() - // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id); - - // The reset action that is invoked resetting - // This is same but different the former rtc_init_entry() - // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id); - - // The state update action that is invoked after onExecute() action - // no corresponding operation exists in OpenRTm-aist-0.2.0 - // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id); - - // The action that is invoked when execution context's rate is changed - // no corresponding operation exists in OpenRTm-aist-0.2.0 - // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); - - protected: - // Configuration variable declaration - // - - // - - // DataInPort declaration - // - TimedDoubleSeq m_angle; - InPort m_angleIn; - TimedDoubleSeq m_angleRef; - InPort m_angleRefIn; - - // - - // DataOutPort declaration - // - TimedDoubleSeq m_torque; - OutPort m_torqueOut; - - // - - // CORBA Port declaration - // - - // - - // Service declaration - // - - // - - // Consumer declaration - // - - // - - private: - void readGainFile(); - hrp::BodyPtr m_robot; - double dt; // sampling time of pd control - double ref_dt; // sampling time of renference angles - int step; // current interpolation step - int nstep; // the number of steps to interpolate references - std::ifstream gain; - std::string gain_fname; - hrp::dvector qold, qold_ref, Pgain, Dgain, tlimit_ratio; - size_t dof, loop; - unsigned int m_debugLevel; - int dummy; -}; - -extern "C" -{ - void ModifiedServoInit(RTC::Manager* manager); -}; - -#endif // ModifiedServo_H diff --git a/rtc/ModifiedServo/ModifiedServo-PDlike.cpp b/rtc/ModifiedServo/ModifiedServo-PDlike.cpp deleted file mode 100644 index 14e087e2837..00000000000 --- a/rtc/ModifiedServo/ModifiedServo-PDlike.cpp +++ /dev/null @@ -1,354 +0,0 @@ -// -*- C++ -*- -/*! - * @file ModifiedServo.cpp - * @brief ModifiedServo component - * $Date$ - * - * $Id$ - */ - -#include "ModifiedServo.h" -// #include // Added by Rafa - -// Module specification -// -static const char* modifiedservo_spec[] = - { - "implementation_id", "ModifiedServo", - "type_name", "ModifiedServo", - "description", "ModifiedServo component", - "version", HRPSYS_PACKAGE_VERSION, - "vendor", "AIST", - "category", "example", - "activity_type", "DataFlowComponent", - "max_instance", "10", - "language", "C++", - "lang_type", "compile", - // Configuration variables - "conf.default.pdgains_sim_file_name", "", - "conf.default.debugLevel", "0", - "" - }; -// - -ModifiedServo::ModifiedServo(RTC::Manager* manager) - // - : RTC::DataFlowComponentBase(manager), - m_tauRefIn("tauRef", m_tauRef), - m_qRefIn("qRef", m_qRef), - m_qIn("q", m_q), - m_torqueModeIn("torqueMode", m_torqueMode), - m_tauOut("tau", m_tau), - // - gain_fname(""), - dt(0.005), - dof(0), loop(0), - dummy(0) -{ -} - -ModifiedServo::~ModifiedServo() -{ -} - - -RTC::ReturnCode_t ModifiedServo::onInitialize() -{ - // Registration: InPort/OutPort/Service - // - // Set InPort buffers - addInPort("tauRef", m_tauRefIn); - addInPort("qRef", m_qRefIn); - addInPort("q", m_qIn); - addInPort("torqueMode", m_torqueModeIn); - - // Set OutPort buffer - addOutPort("tau", m_tauOut); - - // Set service provider to Ports - - // Set service consumers to Ports - - // Set CORBA Service Ports - - // - - // - // Bind variables and configuration variable - - bindParameter("pdgains_sim_file_name", gain_fname, ""); - bindParameter("debugLevel", m_debugLevel, "0"); - - // - - std::cout << m_profile.instance_name << ": onInitialize() " << std::endl; - - RTC::Properties& prop = getProperties(); - coil::stringTo(dt, prop["dt"].c_str()); - ref_dt = dt; - coil::stringTo(ref_dt, prop["ref_dt"].c_str()); - nstep = ref_dt/dt; - step = nstep; - - m_robot = hrp::BodyPtr(new hrp::Body()); - - RTC::Manager& rtcManager = RTC::Manager::instance(); - std::string nameServer = rtcManager.getConfig()["corba.nameservers"]; - int comPos = nameServer.find(","); - - if (comPos < 0) - comPos = nameServer.length(); - - nameServer = nameServer.substr(0, comPos); - RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str()); - - if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), - CosNaming::NamingContext::_duplicate(naming.getRootContext()))) - std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] - << "]" << std::endl; - - return RTC::RTC_OK; -} - -/* -RTC::ReturnCode_t ModifiedServo::onFinalize() -{ - return RTC::RTC_OK; -} -*/ -/* -RTC::ReturnCode_t ModifiedServo::onStartup(RTC::UniqueId ec_id) -{ - return RTC::RTC_OK; -} -*/ -/* -RTC::ReturnCode_t ModifiedServo::onShutdown(RTC::UniqueId ec_id) -{ - return RTC::RTC_OK; -} -*/ - -RTC::ReturnCode_t ModifiedServo::onActivated(RTC::UniqueId ec_id) -{ - std::cout << m_profile.instance_name << ": on Activated" << std::endl; - - if (m_qIn.isNew()) { - m_qIn.read(); - if (dof == 0) { - dof = m_q.data.length(); - readGainFile(); - } - } - - return RTC::RTC_OK; -} - -RTC::ReturnCode_t ModifiedServo::onDeactivated(RTC::UniqueId ec_id) -{ - std::cout << m_profile.instance_name << ": on Deactivated" << std::endl; - - return RTC::RTC_OK; -} - -RTC::ReturnCode_t ModifiedServo::onExecute(RTC::UniqueId ec_id) -{ - loop++; - - if (m_tauRefIn.isNew()) - m_tauRefIn.read(); - - if (m_qIn.isNew()) { - m_qIn.read(); - if (dof == 0) { - dof = m_q.data.length(); - readGainFile(); - } - } - - if (m_qRefIn.isNew()) { - m_qRefIn.read(); - step = nstep; - } - - if (m_torqueModeIn.isNew()) - m_torqueModeIn.read(); - - for (size_t i = 0; i < dof; i++) { - - double q = m_q.data[i]; - double qRef = step > 0 ? qRef_old[i] + (m_qRef.data[i] - qRef_old[i]) / step : qRef_old[i]; - - double dq = (q - q_old[i]) / dt; - double dqRef = (qRef - qRef_old[i]) / dt; - - q_old[i] = q; - qRef_old[i] = qRef; - - double tau = m_torqueMode.data[i] ? m_tauRef.data[i] : Pgain[i] * (qRef - q) + Dgain[i] * (dqRef - dq); - - double tau_limit; - - if (m_robot && m_robot->numJoints() == dof) - - tau_limit = m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst * tau_limit_ratio[i]; - - else { - - tau_limit = (std::numeric_limits::max)() * tau_limit_ratio[i]; - - if (i == 0 && loop % 500 == 0) - std::cerr << "[" << m_profile.instance_name << "] m_robot is not set properly!! Maybe ModelLoader is missing?" << std::endl; - } - - if (loop % 100 == 0 && m_debugLevel == 1) - std::cerr << "[" << m_profile.instance_name << "] joint = " - << i << ", tau = " << tau << ", q , qRef = (" << q << ", " << qRef - << "), dq, dqRef = (" << dq << ", " << dqRef << "), pd = (" - << Pgain[i] << ", " << Dgain[i] << "), tau_limit = " << tau_limit << std::endl; - - m_tau.data[i] = std::max(std::min(tau, tau_limit), -tau_limit); - } - - step--; - - m_tau.tm = m_q.tm; - m_tauOut.write(); - - return RTC::RTC_OK; -} - -/* -RTC::ReturnCode_t ModifiedServo::onAborting(RTC::UniqueId ec_id) -{ - return RTC::RTC_OK; -} -*/ -/* -RTC::ReturnCode_t ModifiedServo::onError(RTC::UniqueId ec_id) -{ - return RTC::RTC_OK; -} -*/ -/* -RTC::ReturnCode_t ModifiedServo::onReset(RTC::UniqueId ec_id) -{ - return RTC::RTC_OK; -} -*/ -/* -RTC::ReturnCode_t ModifiedServo::onStateUpdate(RTC::UniqueId ec_id) -{ - return RTC::RTC_OK; -} -*/ -/* -RTC::ReturnCode_t ModifiedServo::onRateChanged(RTC::UniqueId ec_id) -{ - return RTC::RTC_OK; -} -*/ - -void ModifiedServo::readGainFile() -{ - if (gain_fname == "") { - RTC::Properties & prop = getProperties(); - coil::stringTo(gain_fname, prop["pdgains_sim_file_name"].c_str()); - } - - // initialize length of vectors - - q_old.resize(dof); - qRef_old.resize(dof); - - m_tauRef.data.length(dof); - m_qRef.data.length(dof); - m_torqueMode.data.length(dof); - - tau_limit_ratio.resize(dof); - - Pgain.resize(dof); - Dgain.resize(dof); - - gain.open(gain_fname.c_str()); - - if (gain.is_open()) { - - double val; - - for (unsigned int i = 0; i < dof; i++) { - - if (gain >> val) - Pgain[i] = val; - else - std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; - - if (gain >> val) - Dgain[i] = val; - else - std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; - } - - gain.close(); - - std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] successfully read" << std::endl; - } - else - std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] could not be opened" << std::endl; - - // tau_limit_ratio initialize - RTC::Properties& prop = getProperties(); - - if (prop["pdcontrol_tau_limit_ratio"] != "") { - - coil::vstring tau_limit_ratio_str = coil::split(prop["pdcontrol_tau_limit_ratio"], ","); - - if (tau_limit_ratio_str.size() == dof) { - - for (size_t i = 0; i < dof; i++) - coil::stringTo(tau_limit_ratio[i], tau_limit_ratio_str[i].c_str()); - - std::cerr << "[" << m_profile.instance_name << "] tau_limit_ratio is set to " << prop["pdcontrol_tau_limit_ratio"] << std::endl; - } - else { - - for (size_t i = 0; i < dof; i++) - tau_limit_ratio[i] = 1.0; - - std::cerr << "[" << m_profile.instance_name << "] pdcontrol_tau_limit_ratio found, but invalid length (" << tau_limit_ratio_str.size() << " != " << dof << ")." << std::endl; - std::cerr << "[" << m_profile.instance_name << "] All tau_limit_ratio are set to 1.0." << std::endl; - } - } - else { - for (size_t i = 0; i < dof; i++) - tau_limit_ratio[i] = 1.0; - - std::cerr << "[" << m_profile.instance_name << "] No pdcontrol_tau_limit_ratio found." << std::endl; - std::cerr << "[" << m_profile.instance_name << "] All tau_limit_ratio are set to 1.0." << std::endl; - } - - // initialize angleRef, old_ref and old with angle - for(unsigned int i=0; i < dof; ++i) { - m_tauRef.data[i] = 0.0; - m_qRef.data[i] = qRef_old[i] = q_old[i] = m_q.data[i]; - m_torqueMode.data[i] = false; - } - - // Print loaded gain - std::cerr << "[" << m_profile.instance_name << "] loadGain" << std::endl; - for (unsigned int i=0; inumJoints(); i++) - std::cerr << "[" << m_profile.instance_name << "] " << m_robot->joint(i)->name << ", pgain = " << Pgain[i] << ", dgain = " << Dgain[i] << std::endl; -} - -extern "C" -{ - - void ModifiedServoInit(RTC::Manager* manager) - { - coil::Properties profile(modifiedservo_spec); - manager->registerFactory(profile, - RTC::Create, - RTC::Delete); - } - -}; diff --git a/rtc/ModifiedServo/ModifiedServo-PDlike.h b/rtc/ModifiedServo/ModifiedServo-PDlike.h deleted file mode 100644 index ddfe72b709b..00000000000 --- a/rtc/ModifiedServo/ModifiedServo-PDlike.h +++ /dev/null @@ -1,164 +0,0 @@ -// -*- C++ -*- -/*! - * @file ModifiedServo.h - * @brief ModifiedServo component - * @date $Date$ - * - * $Id$ - */ - -#ifndef MODIFIEDSERVO_H -#define MODIFIEDSERVO_H - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -// Service implementation headers -// - -// - -// Service Consumer stub headers -// - -// - -using namespace RTC; - -class ModifiedServo : public RTC::DataFlowComponentBase -{ - public: - ModifiedServo(RTC::Manager* manager); - ~ModifiedServo(); - - // The initialize action (on CREATED->ALIVE transition) - // formaer rtc_init_entry() - virtual RTC::ReturnCode_t onInitialize(); - - // The finalize action (on ALIVE->END transition) - // formaer rtc_exiting_entry() - // virtual RTC::ReturnCode_t onFinalize(); - - // The startup action when ExecutionContext startup - // former rtc_starting_entry() - // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id); - - // The shutdown action when ExecutionContext stop - // former rtc_stopping_entry() - // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id); - - // The activated action (Active state entry action) - // former rtc_active_entry() - virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id); - - // The deactivated action (Active state exit action) - // former rtc_active_exit() - virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); - - // The execution action that is invoked periodically - // former rtc_active_do() - virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id); - - // The aborting action when main logic error occurred. - // former rtc_aborting_entry() - // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id); - - // The error action in ERROR state - // former rtc_error_do() - // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id); - - // The reset action that is invoked resetting - // This is same but different the former rtc_init_entry() - // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id); - - // The state update action that is invoked after onExecute() action - // no corresponding operation exists in OpenRTm-aist-0.2.0 - // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id); - - // The action that is invoked when execution context's rate is changed - // no corresponding operation exists in OpenRTm-aist-0.2.0 - // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); - - - protected: - // Configuration variable declaration - // - - // - - // DataInPort declaration - // - TimedDoubleSeq m_tauRef; - InPort m_tauRefIn; - TimedDoubleSeq m_qRef; - InPort m_qRefIn; - TimedDoubleSeq m_q; - InPort m_qIn; - TimedBooleanSeq m_torqueMode; - InPort m_torqueModeIn; - - // - - // DataOutPort declaration - // - TimedDoubleSeq m_tau; - OutPort m_tauOut; - - // - - // CORBA Port declaration - // - - // - - // Service declaration - // - - // - - // Consumer declaration - // - - // - - private: - - void readGainFile(); - - hrp::BodyPtr m_robot; - - double dt; // sampling time of the controller - double ref_dt; // sampling time of reference angles - double step; // current interpolation step - double nstep; // number of steps to interpolate references - - size_t dof, loop; - unsigned int m_debugLevel; - int dummy; - - std::string gain_fname; - std::ifstream gain; - - hrp::dvector Pgain, Dgain; - hrp::dvector q_old, qRef_old; - hrp::dvector tau_limit_ratio; -}; - - -extern "C" -{ - DLL_EXPORT void ModifiedServoInit(RTC::Manager* manager); -}; - -#endif // MODIFIEDSERVO_H - From 134ec6711b309fdfb5c91b29502e5f4db2df7844 Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Mon, 15 Feb 2021 18:13:06 +0900 Subject: [PATCH 17/26] Removed ModifiedServo-original --- rtc/ModifiedServo/ModifiedServo-original.cpp | 290 ------------------- rtc/ModifiedServo/ModifiedServo-original.h | 161 ---------- rtc/ModifiedServo/ModifiedServo.cpp | 4 + 3 files changed, 4 insertions(+), 451 deletions(-) delete mode 100644 rtc/ModifiedServo/ModifiedServo-original.cpp delete mode 100644 rtc/ModifiedServo/ModifiedServo-original.h diff --git a/rtc/ModifiedServo/ModifiedServo-original.cpp b/rtc/ModifiedServo/ModifiedServo-original.cpp deleted file mode 100644 index 00c991f5e46..00000000000 --- a/rtc/ModifiedServo/ModifiedServo-original.cpp +++ /dev/null @@ -1,290 +0,0 @@ -// -*- C++ -*- -/*! - * @file ModifiedServo.cpp - * @brief ModifiedServo component - * $Date$ - * - * $Id$ - */ - -#include "ModifiedServo.h" -// #include // Added by Rafa - -// Module specification -// -static const char* modifiedservo_spec[] = - { - "implementation_id", "ModifiedServo", - "type_name", "ModifiedServo", - "description", "ModifiedServo component", - "version", HRPSYS_PACKAGE_VERSION, - "vendor", "AIST", - "category", "example", - "activity_type", "DataFlowComponent", - "max_instance", "10", - "language", "C++", - "lang_type", "compile", - // Configuration variables - "" - }; -// - -ModifiedServo::ModifiedServo(RTC::Manager* manager) - // - : RTC::DataFlowComponentBase(manager), - m_tauRefIn("tauRef", m_tauRef), - m_qRefIn("qRef", m_qRef), - m_qIn("q", m_q), - m_torqueModeIn("torqueMode", m_torqueMode), - m_tauOut("tau", m_tau), - // - gain_fname(""), - dt(0.005), - dof(0) -{ -} - -ModifiedServo::~ModifiedServo() -{ -} - - -RTC::ReturnCode_t ModifiedServo::onInitialize() -{ - // Registration: InPort/OutPort/Service - // - // Set InPort buffers - addInPort("tauRef", m_tauRefIn); - addInPort("qRef", m_qRefIn); - addInPort("q", m_qIn); - addInPort("torqueMode", m_torqueModeIn); - - // Set OutPort buffer - addOutPort("tau", m_tauOut); - - // Set service provider to Ports - - // Set service consumers to Ports - - // Set CORBA Service Ports - - // - - // - // Bind variables and configuration variable - - // - - std::cout << m_profile.instance_name << ": onInitialize() " << std::endl; - - RTC::Properties & prop = getProperties(); - - coil::stringTo(dt, prop["dt"].c_str()); - coil::stringTo(ref_dt, prop["ref_dt"].c_str()); - nstep = ref_dt/dt; - step = nstep; - - m_robot = hrp::BodyPtr(new hrp::Body()); - - RTC::Manager & rtcManager = RTC::Manager::instance(); - std::string nameServer = rtcManager.getConfig()["corba.nameservers"]; - int comPos = nameServer.find(","); - - if (comPos < 0) - comPos = nameServer.length(); - - // In case there is more than one, retrieves only the first one - nameServer = nameServer.substr(0, comPos); - - RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str()); - - if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), - CosNaming::NamingContext::_duplicate(naming.getRootContext()))) - std::cerr << "[" << m_profile.instance_name << "] failed to load model " - << "[" << prop["model"] << "]" << std::endl; - - return RTC::RTC_OK; -} - -/* -RTC::ReturnCode_t ModifiedServo::onFinalize() -{ - return RTC::RTC_OK; -} -*/ -/* -RTC::ReturnCode_t ModifiedServo::onStartup(RTC::UniqueId ec_id) -{ - return RTC::RTC_OK; -} -*/ -/* -RTC::ReturnCode_t ModifiedServo::onShutdown(RTC::UniqueId ec_id) -{ - return RTC::RTC_OK; -} -*/ - -RTC::ReturnCode_t ModifiedServo::onActivated(RTC::UniqueId ec_id) -{ - std::cout << m_profile.instance_name << ": on Activated" << std::endl; - - if (m_qIn.isNew()) { - m_qIn.read(); - if (dof == 0) { - dof = m_q.data.length(); - readGainFile(); - } - } - - q_old.resize(dof); - qRef_old.resize(dof); - - m_tauRef.data.length(dof); - m_qRef.data.length(dof); - m_torqueMode.data.length(dof); - - m_tau.data.length(dof); - - for (size_t i = 0; i < dof; i++) { - m_tauRef.data[i] = 0.0; - m_qRef.data[i] = qRef_old[i] = q_old[i] = m_q.data[i]; - m_torqueMode.data[i] = false; - } - - return RTC::RTC_OK; -} - -RTC::ReturnCode_t ModifiedServo::onDeactivated(RTC::UniqueId ec_id) -{ - std::cout << m_profile.instance_name << ": on Deactivated" << std::endl; - - return RTC::RTC_OK; -} - -RTC::ReturnCode_t ModifiedServo::onExecute(RTC::UniqueId ec_id) -{ - if (m_tauRefIn.isNew()) - m_tauRefIn.read(); - - if (m_qIn.isNew()) - m_qIn.read(); - - if (m_qRefIn.isNew()) { - m_qRefIn.read(); - step = nstep; - } - - if (m_torqueModeIn.isNew()) - m_torqueModeIn.read(); - - for (size_t i = 0; i < dof; i++) { - - double q = m_q.data[i]; - double qRef = step > 0 ? qRef_old[i] + (m_qRef.data[i] - qRef_old[i]) / step : qRef_old[i]; - - double dq = (q - q_old[i]) / dt; - double dqRef = (qRef - qRef_old[i]) / dt; - - q_old[i] = q; - qRef_old[i] = qRef; - - double tau = m_torqueMode.data[i] ? m_tauRef.data[i] : Pgain[i] * (qRef - q) + Dgain[i] * (dqRef - dq); - - double tau_limit = m_robot->joint(i)->torqueConst * m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio; - - m_tau.data[i] = std::max(std::min(tau, tau_limit), -tau_limit); - - // if (i == 11 || i == 21) - // std::cout << "Rafa, in ModifiedServo::onExecute, for i = " << i << ", q[i] = " << q << ", qRef[i] = " << qRef - // << ", tau[i] = " << tau << ", tau_limit[i] = " << tau_limit << ", m_tau[i] = " << m_tau.data[i] << std::endl; - } - - step--; - - m_tau.tm = m_q.tm; - m_tauOut.write(); - - return RTC::RTC_OK; -} - -/* -RTC::ReturnCode_t ModifiedServo::onAborting(RTC::UniqueId ec_id) -{ - return RTC::RTC_OK; -} -*/ -/* -RTC::ReturnCode_t ModifiedServo::onError(RTC::UniqueId ec_id) -{ - return RTC::RTC_OK; -} -*/ -/* -RTC::ReturnCode_t ModifiedServo::onReset(RTC::UniqueId ec_id) -{ - return RTC::RTC_OK; -} -*/ -/* -RTC::ReturnCode_t ModifiedServo::onStateUpdate(RTC::UniqueId ec_id) -{ - return RTC::RTC_OK; -} -*/ -/* -RTC::ReturnCode_t ModifiedServo::onRateChanged(RTC::UniqueId ec_id) -{ - return RTC::RTC_OK; -} -*/ - -void ModifiedServo::readGainFile() -{ - if (gain_fname == "") { - RTC::Properties & prop = getProperties(); - coil::stringTo(gain_fname, prop["pdgains_sim_file_name"].c_str()); - } - - gain.open(gain_fname.c_str()); - - if (gain.is_open()) { - - double val; - - Pgain.resize(dof); - Dgain.resize(dof); - - for (unsigned int i = 0; i < dof; i++) { - - if (gain >> val) - Pgain[i] = val; - else - std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; - - if (gain >> val) - Dgain[i] = val; - else - std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; - } - - gain.close(); - - std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] successfully read" << std::endl; - } - else - std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] could not be opened" << std::endl; -} - -extern "C" -{ - - void ModifiedServoInit(RTC::Manager* manager) - { - coil::Properties profile(modifiedservo_spec); - manager->registerFactory(profile, - RTC::Create, - RTC::Delete); - } - -}; diff --git a/rtc/ModifiedServo/ModifiedServo-original.h b/rtc/ModifiedServo/ModifiedServo-original.h deleted file mode 100644 index a58ed0863a0..00000000000 --- a/rtc/ModifiedServo/ModifiedServo-original.h +++ /dev/null @@ -1,161 +0,0 @@ -// -*- C++ -*- -/*! - * @file ModifiedServo.h - * @brief ModifiedServo component - * @date $Date$ - * - * $Id$ - */ - -#ifndef MODIFIEDSERVO_H -#define MODIFIEDSERVO_H - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -// Service implementation headers -// - -// - -// Service Consumer stub headers -// - -// - -using namespace RTC; - -class ModifiedServo : public RTC::DataFlowComponentBase -{ - public: - ModifiedServo(RTC::Manager* manager); - ~ModifiedServo(); - - // The initialize action (on CREATED->ALIVE transition) - // formaer rtc_init_entry() - virtual RTC::ReturnCode_t onInitialize(); - - // The finalize action (on ALIVE->END transition) - // formaer rtc_exiting_entry() - // virtual RTC::ReturnCode_t onFinalize(); - - // The startup action when ExecutionContext startup - // former rtc_starting_entry() - // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id); - - // The shutdown action when ExecutionContext stop - // former rtc_stopping_entry() - // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id); - - // The activated action (Active state entry action) - // former rtc_active_entry() - virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id); - - // The deactivated action (Active state exit action) - // former rtc_active_exit() - virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); - - // The execution action that is invoked periodically - // former rtc_active_do() - virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id); - - // The aborting action when main logic error occurred. - // former rtc_aborting_entry() - // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id); - - // The error action in ERROR state - // former rtc_error_do() - // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id); - - // The reset action that is invoked resetting - // This is same but different the former rtc_init_entry() - // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id); - - // The state update action that is invoked after onExecute() action - // no corresponding operation exists in OpenRTm-aist-0.2.0 - // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id); - - // The action that is invoked when execution context's rate is changed - // no corresponding operation exists in OpenRTm-aist-0.2.0 - // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); - - - protected: - // Configuration variable declaration - // - - // - - // DataInPort declaration - // - TimedDoubleSeq m_tauRef; - InPort m_tauRefIn; - TimedDoubleSeq m_qRef; - InPort m_qRefIn; - TimedDoubleSeq m_q; - InPort m_qIn; - TimedBooleanSeq m_torqueMode; - InPort m_torqueModeIn; - - // - - // DataOutPort declaration - // - TimedDoubleSeq m_tau; - OutPort m_tauOut; - - // - - // CORBA Port declaration - // - - // - - // Service declaration - // - - // - - // Consumer declaration - // - - // - - private: - - void readGainFile(); - - hrp::BodyPtr m_robot; - - double dt; // sampling time of the controller - double ref_dt; // sampling time of reference angles - double step; // current interpolation step - double nstep; // number of steps to interpolate references - - size_t dof; - - std::string gain_fname; - std::ifstream gain; - - hrp::dvector Pgain, Dgain; - hrp::dvector q_old, qRef_old; -}; - - -extern "C" -{ - DLL_EXPORT void ModifiedServoInit(RTC::Manager* manager); -}; - -#endif // MODIFIEDSERVO_H - diff --git a/rtc/ModifiedServo/ModifiedServo.cpp b/rtc/ModifiedServo/ModifiedServo.cpp index b91b5913ecf..2a3c777bb2f 100644 --- a/rtc/ModifiedServo/ModifiedServo.cpp +++ b/rtc/ModifiedServo/ModifiedServo.cpp @@ -218,6 +218,10 @@ RTC::ReturnCode_t ModifiedServo::onExecute(RTC::UniqueId ec_id) double tau_limit = m_robot->joint(i)->torqueConst * m_robot->joint(i)->climit * fabs(m_robot->joint(i)->gearRatio); m_tau.data[i] = std::max(std::min(tau, tau_limit), -tau_limit); + + // if (i == 11 || i == 21) + // std::cout << "Rafa, in ModifiedServo::onExecute, for i = " << i << ", q[i] = " << q << ", qRef[i] = " << qRef + // << ", tau[i] = " << tau << ", tau_limit[i] = " << tau_limit << ", m_tau[i] = " << m_tau.data[i] << std::endl; } m_step--; From 84941fc475329383406e18aabedfaacdf82f93d9 Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Thu, 18 Feb 2021 10:42:13 +0900 Subject: [PATCH 18/26] Removed commented out functions that I had moved to iob level and commented out a protection for the current angles that causes problems --- rtc/RobotHardware/RobotHardware.cpp | 13 +------------ rtc/RobotHardware/RobotHardware.h | 1 - 2 files changed, 1 insertion(+), 13 deletions(-) diff --git a/rtc/RobotHardware/RobotHardware.cpp b/rtc/RobotHardware/RobotHardware.cpp index a9369342f9c..96dcc7f98e7 100644 --- a/rtc/RobotHardware/RobotHardware.cpp +++ b/rtc/RobotHardware/RobotHardware.cpp @@ -15,8 +15,6 @@ #include #include -//#define MAX_CYCLES_WITHOUT_TAUREF 3 // Added by Rafa - using namespace OpenHRP; using namespace hrp; @@ -258,20 +256,11 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id) } /* - if (!m_isDemoMode && m_robot->isJointTorqueControlModeUsed()){ // Added by Rafa - if (m_robot->checkJointActualValues() || m_count_noNewTauRef > MAX_CYCLES_WITHOUT_TAUREF){ - m_robot->servo("all", false); - m_emergencySignal.data = robot::EMG_SERVO_ERROR; - m_emergencySignalOut.write(); - } - m_count_noNewTauRef++; - } - */ - if (!m_isDemoMode && m_robot->checkJointActualValues()){ m_robot->resetJointControlMode(); allowTorqueControlMode = false; } + */ if (m_qRefIn.isNew()){ m_qRefIn.read(); diff --git a/rtc/RobotHardware/RobotHardware.h b/rtc/RobotHardware/RobotHardware.h index ca0d7109573..ff8c58d518d 100644 --- a/rtc/RobotHardware/RobotHardware.h +++ b/rtc/RobotHardware/RobotHardware.h @@ -223,7 +223,6 @@ class RobotHardware private: void getStatus2(OpenHRP::RobotHardwareService::RobotState2 &rstate2); - // int m_count_noNewTauRef; // Added by Rafa bool allowTorqueControlMode; // Added by Rafa int dummy; From ea83f7aceed7c965661914b423f166465d074737 Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Thu, 18 Feb 2021 10:43:29 +0900 Subject: [PATCH 19/26] Had missed to erase commented out code --- rtc/RobotHardware/RobotHardware.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rtc/RobotHardware/RobotHardware.cpp b/rtc/RobotHardware/RobotHardware.cpp index 96dcc7f98e7..760c44ec64f 100644 --- a/rtc/RobotHardware/RobotHardware.cpp +++ b/rtc/RobotHardware/RobotHardware.cpp @@ -62,7 +62,6 @@ RobotHardware::RobotHardware(RTC::Manager* manager) m_rstate2Out("rstate2", m_rstate2), m_RobotHardwareServicePort("RobotHardwareService"), // - // m_count_noNewTauRef(0), allowTorqueControlMode(true), dummy(0) { From 64ddc718bf8a02aa340ff59aebb20c7900f05d63 Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Thu, 18 Feb 2021 10:44:38 +0900 Subject: [PATCH 20/26] Had missed to erase more commented out code --- rtc/RobotHardware/RobotHardware.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rtc/RobotHardware/RobotHardware.cpp b/rtc/RobotHardware/RobotHardware.cpp index 760c44ec64f..d2e4294ac80 100644 --- a/rtc/RobotHardware/RobotHardware.cpp +++ b/rtc/RobotHardware/RobotHardware.cpp @@ -288,7 +288,6 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id) } if (m_tauRefIn.isNew()){ m_tauRefIn.read(); - // m_count_noNewTauRef = 0; //std::cout << "RobotHardware: tauRef[21] = " << m_tauRef.data[21] << std::endl; // output to iob m_robot->writeTorqueCommands(m_tauRef.data.get_buffer()); From 53e70f4b41a17789b50ca82eb7781289089a7d65 Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Thu, 15 Dec 2022 16:37:34 +0900 Subject: [PATCH 21/26] Brought ModifiedServo as it is from master --- rtc/ModifiedServo/ModifiedServo.cpp | 97 +++++++++++++---------------- rtc/ModifiedServo/ModifiedServo.h | 18 +++--- 2 files changed, 54 insertions(+), 61 deletions(-) diff --git a/rtc/ModifiedServo/ModifiedServo.cpp b/rtc/ModifiedServo/ModifiedServo.cpp index 2a3c777bb2f..544dd1653e6 100644 --- a/rtc/ModifiedServo/ModifiedServo.cpp +++ b/rtc/ModifiedServo/ModifiedServo.cpp @@ -8,7 +8,6 @@ */ #include "ModifiedServo.h" -// #include // Added by Rafa // Module specification // @@ -17,11 +16,12 @@ static const char* modifiedservo_spec[] = "implementation_id", "ModifiedServo", "type_name", "ModifiedServo", "description", "ModifiedServo component", - "version", HRPSYS_PACKAGE_VERSION, + "version", "0.1", "vendor", "AIST", "category", "example", - "activity_type", "DataFlowComponent", - "max_instance", "10", + "activity_type", "SPORADIC", + "kind", "DataFlowComponent", + "max_instance", "1", "language", "C++", "lang_type", "compile", // Configuration variables @@ -42,9 +42,9 @@ ModifiedServo::ModifiedServo(RTC::Manager* manager) m_pgainsOut("pgainsGet", m_pgains), m_dgainsOut("dgainsGet", m_dgains), // - m_gain_fname(""), - m_dt(0.005), - m_dof(0) + gain_fname(""), + dt(0.005), + dof(0) { } @@ -87,10 +87,10 @@ RTC::ReturnCode_t ModifiedServo::onInitialize() RTC::Properties & prop = getProperties(); - coil::stringTo(m_dt, prop["dt"].c_str()); - coil::stringTo(m_ref_dt, prop["ref_dt"].c_str()); - m_nstep = m_ref_dt / m_dt; - m_step = m_nstep; + coil::stringTo(dt, prop["dt"].c_str()); + coil::stringTo(ref_dt, prop["ref_dt"].c_str()); + nstep = ref_dt/dt; + step = nstep; m_robot = hrp::BodyPtr(new hrp::Body()); @@ -109,7 +109,7 @@ RTC::ReturnCode_t ModifiedServo::onInitialize() if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()))) std::cerr << "[" << m_profile.instance_name << "] failed to load model " - << "[" << prop["model"] << "]" << std::endl; + << "[" << prop["model"] << "]" << std::endl; return RTC::RTC_OK; } @@ -139,30 +139,27 @@ RTC::ReturnCode_t ModifiedServo::onActivated(RTC::UniqueId ec_id) if (m_qIn.isNew()) { m_qIn.read(); - if (m_dof == 0) { - m_dof = m_q.data.length(); + if (dof == 0) { + dof = m_q.data.length(); readGainFile(); } } - m_q_old.resize(m_dof); - m_qRef_old.resize(m_dof); + q_old.resize(dof); + qRef_old.resize(dof); - m_tauRef.data.length(m_dof); - m_qRef.data.length(m_dof); - m_torqueMode.data.length(m_dof); + m_tauRef.data.length(dof); + m_qRef.data.length(dof); + m_torqueMode.data.length(dof); - m_tau.data.length(m_dof); + m_tau.data.length(dof); m_pgains.data.length(dof); m_dgains.data.length(dof); for (size_t i = 0; i < dof; i++) { -======= - for (size_t i = 0; i < m_dof; i++) { ->>>>>>> Used m_ for the member variables of ModifiedServo m_tauRef.data[i] = 0.0; - m_qRef.data[i] = m_qRef_old[i] = m_q_old[i] = m_q.data[i]; + m_qRef.data[i] = qRef_old[i] = q_old[i] = m_q.data[i]; m_torqueMode.data[i] = false; m_pgains.data[i] = Pgain[i]; m_dgains.data[i] = Dgain[i]; @@ -188,7 +185,7 @@ RTC::ReturnCode_t ModifiedServo::onExecute(RTC::UniqueId ec_id) if (m_qRefIn.isNew()) { m_qRefIn.read(); - m_step = m_nstep; + step = nstep; } if(m_pgainsIn.isNew()){ m_pgainsIn.read(); @@ -205,26 +202,22 @@ RTC::ReturnCode_t ModifiedServo::onExecute(RTC::UniqueId ec_id) Dgain[i] = m_dgains.data[i]; double q = m_q.data[i]; - double qCom = m_step > 0 ? m_qRef_old[i] + (m_qRef.data[i] - m_qRef_old[i]) / m_step : m_qRef_old[i]; + double qRef = step > 0 ? qRef_old[i] + (m_qRef.data[i] - qRef_old[i]) / step : qRef_old[i]; - double dq = (q - m_q_old[i]) / m_dt; - double dqCom = (qCom - m_qRef_old[i]) / m_dt; + double dq = (q - q_old[i]) / dt; + double dqRef = (qRef - qRef_old[i]) / dt; - m_q_old[i] = q; - m_qRef_old[i] = qCom; + q_old[i] = q; + qRef_old[i] = qRef; - double tau = m_torqueMode.data[i] ? m_tauRef.data[i] : m_Pgain[i] * (qCom - q) + m_Dgain[i] * (dqCom - dq); + double tau = m_torqueMode.data[i] ? m_tauRef.data[i] : Pgain[i] * (qRef - q) + Dgain[i] * (dqRef - dq); double tau_limit = m_robot->joint(i)->torqueConst * m_robot->joint(i)->climit * fabs(m_robot->joint(i)->gearRatio); m_tau.data[i] = std::max(std::min(tau, tau_limit), -tau_limit); - - // if (i == 11 || i == 21) - // std::cout << "Rafa, in ModifiedServo::onExecute, for i = " << i << ", q[i] = " << q << ", qRef[i] = " << qRef - // << ", tau[i] = " << tau << ", tau_limit[i] = " << tau_limit << ", m_tau[i] = " << m_tau.data[i] << std::endl; } - m_step--; + step--; m_tau.tm = m_q.tm; m_tauOut.write(); @@ -267,39 +260,39 @@ RTC::ReturnCode_t ModifiedServo::onRateChanged(RTC::UniqueId ec_id) void ModifiedServo::readGainFile() { - if (m_gain_fname == "") { + if (gain_fname == "") { RTC::Properties & prop = getProperties(); - coil::stringTo(m_gain_fname, prop["pdgains_sim_file_name"].c_str()); + coil::stringTo(gain_fname, prop["pdgains_sim_file_name"].c_str()); } - m_gain.open(m_gain_fname.c_str()); + gain.open(gain_fname.c_str()); - if (m_gain.is_open()) { + if (gain.is_open()) { double val; - m_Pgain.resize(m_dof); - m_Dgain.resize(m_dof); + Pgain.resize(dof); + Dgain.resize(dof); - for (unsigned int i = 0; i < m_dof; i++) { + for (unsigned int i = 0; i < dof; i++) { - if (m_gain >> val) - m_Pgain[i] = val; + if (gain >> val) + Pgain[i] = val; else - std::cout << "[" << m_profile.instance_name << "] Gain file [" << m_gain_fname << "] is too short" << std::endl; + std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; - if (m_gain >> val) - m_Dgain[i] = val; + if (gain >> val) + Dgain[i] = val; else - std::cout << "[" << m_profile.instance_name << "] Gain file [" << m_gain_fname << "] is too short" << std::endl; + std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; } - m_gain.close(); + gain.close(); - std::cout << "[" << m_profile.instance_name << "] Gain file [" << m_gain_fname << "] successfully read" << std::endl; + std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] successfully read" << std::endl; } else - std::cout << "[" << m_profile.instance_name << "] Gain file [" << m_gain_fname << "] could not be opened" << std::endl; + std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] could not be opened" << std::endl; } extern "C" diff --git a/rtc/ModifiedServo/ModifiedServo.h b/rtc/ModifiedServo/ModifiedServo.h index 0c45eff9b75..287af997312 100644 --- a/rtc/ModifiedServo/ModifiedServo.h +++ b/rtc/ModifiedServo/ModifiedServo.h @@ -145,18 +145,18 @@ class ModifiedServo : public RTC::DataFlowComponentBase hrp::BodyPtr m_robot; - double m_dt; // sampling time of the controller - double m_ref_dt; // sampling time of reference angles - double m_step; // current interpolation step - double m_nstep; // number of steps to interpolate references + double dt; // sampling time of the controller + double ref_dt; // sampling time of reference angles + double step; // current interpolation step + double nstep; // number of steps to interpolate references - size_t m_dof; + size_t dof; - std::string m_gain_fname; - std::ifstream m_gain; + std::string gain_fname; + std::ifstream gain; - hrp::dvector m_Pgain, m_Dgain; - hrp::dvector m_q_old, m_qRef_old; + hrp::dvector Pgain, Dgain; + hrp::dvector q_old, qRef_old; }; From 34c8f9f10e28c9f28f02a35442c68a10a3df0849 Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Mon, 6 Feb 2023 16:59:24 +0900 Subject: [PATCH 22/26] Deleted unused functions: checkJointActualValues and resetJointControlMode --- rtc/RobotHardware/RobotHardware.cpp | 7 ----- rtc/RobotHardware/robot.cpp | 48 +---------------------------- rtc/RobotHardware/robot.h | 12 -------- 3 files changed, 1 insertion(+), 66 deletions(-) diff --git a/rtc/RobotHardware/RobotHardware.cpp b/rtc/RobotHardware/RobotHardware.cpp index d2e4294ac80..93126336b7e 100644 --- a/rtc/RobotHardware/RobotHardware.cpp +++ b/rtc/RobotHardware/RobotHardware.cpp @@ -252,14 +252,7 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id) m_emergencySignalOut.write(); } } - } - - /* - if (!m_isDemoMode && m_robot->checkJointActualValues()){ - m_robot->resetJointControlMode(); - allowTorqueControlMode = false; } - */ if (m_qRefIn.isNew()){ m_qRefIn.read(); diff --git a/rtc/RobotHardware/robot.cpp b/rtc/RobotHardware/robot.cpp index 555ff96ec8a..2966737e693 100644 --- a/rtc/RobotHardware/robot.cpp +++ b/rtc/RobotHardware/robot.cpp @@ -627,31 +627,6 @@ char *time_string() return time; } -bool robot::checkJointActualValues() -{ - int state; - for (unsigned int i=0; i= ulimit){ - std::cerr << time_string() - << ": actual joint angle outside allowed range" - << ": joint = " << joint(i)->name - << ", llimit = " << llimit/M_PI*180 << "[deg]" - << ", angle = " << angle/M_PI*180 << "[deg]" - << ", ulimit = " << ulimit/M_PI*180 << "[deg]" - << std::endl; - return true; - } - } - } - return false; -} - bool robot::checkJointCommands(const double *i_commands) { if (!m_dt) return false; @@ -1076,7 +1051,7 @@ bool robot::setJointControlMode(const char *i_jname, joint_control_mode mode) write_control_mode(i, mode); } // std::cerr << "[RobotHardware] setJointControlMode for all joints : " << mode << std::endl; // Commented out by Rafa - } else if ((l = link(i_jname))) { + } else if ((l = link(i_jname)) && (l->jointId >= 0)) { write_control_mode(l->jointId, mode); // std::cerr << "[RobotHardware] setJointControlMode for " << i_jname << " : " << mode << std::endl; // Commented out by Rafa } else { @@ -1090,24 +1065,3 @@ bool robot::setJointControlMode(const char *i_jname, joint_control_mode mode) } return true; } - -bool robot::resetJointControlMode() // Added by Rafa -{ - bool res = false; - double angle; - joint_control_mode mode; - for (unsigned int i=0; i Date: Tue, 7 Feb 2023 14:37:13 +0900 Subject: [PATCH 23/26] Removed the flag allowTorqueControlMode as it is not used anymore --- rtc/RobotHardware/RobotHardware.cpp | 7 ++----- rtc/RobotHardware/RobotHardware.h | 2 -- 2 files changed, 2 insertions(+), 7 deletions(-) diff --git a/rtc/RobotHardware/RobotHardware.cpp b/rtc/RobotHardware/RobotHardware.cpp index 93126336b7e..e0247c1adde 100644 --- a/rtc/RobotHardware/RobotHardware.cpp +++ b/rtc/RobotHardware/RobotHardware.cpp @@ -62,7 +62,6 @@ RobotHardware::RobotHardware(RTC::Manager* manager) m_rstate2Out("rstate2", m_rstate2), m_RobotHardwareServicePort("RobotHardwareService"), // - allowTorqueControlMode(true), dummy(0) { } @@ -287,10 +286,8 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id) } if (m_torqueControlModeIn.isNew()){ m_torqueControlModeIn.read(); - if (allowTorqueControlMode){ - for (unsigned int i=0; inumJoints(); ++i){ - m_robot->setJointControlMode(m_robot->joint(i)->name.c_str(), m_torqueControlMode.data[i] ? JCM_TORQUE : JCM_POSITION); - } + for (unsigned int i=0; inumJoints(); ++i){ + m_robot->setJointControlMode(m_robot->joint(i)->name.c_str(), m_torqueControlMode.data[i] ? JCM_TORQUE : JCM_POSITION); } } diff --git a/rtc/RobotHardware/RobotHardware.h b/rtc/RobotHardware/RobotHardware.h index ff8c58d518d..65e5457f12d 100644 --- a/rtc/RobotHardware/RobotHardware.h +++ b/rtc/RobotHardware/RobotHardware.h @@ -223,8 +223,6 @@ class RobotHardware private: void getStatus2(OpenHRP::RobotHardwareService::RobotState2 &rstate2); - bool allowTorqueControlMode; // Added by Rafa - int dummy; boost::shared_ptr m_robot; }; From 42f6519de857244f19e3ef210b58c87346cc1f53 Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Tue, 7 Feb 2023 14:43:02 +0900 Subject: [PATCH 24/26] Reduced diff, removed unnecessary comments and uncommented messages to avoid changing current behavior --- lib/io/iob.cpp | 3 --- rtc/RobotHardware/robot.cpp | 12 ++++++------ 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/lib/io/iob.cpp b/lib/io/iob.cpp index 56d157d5fc2..c1e9e861172 100644 --- a/lib/io/iob.cpp +++ b/lib/io/iob.cpp @@ -527,7 +527,6 @@ int read_ulimit_angle(int id, double *angle) { return FALSE; } - int read_llimit_angle(int id, double *angle) { return FALSE; @@ -547,12 +546,10 @@ int read_encoder_pulse(int id, double *ec) { return FALSE; } - int read_gear_ratio(int id, double *gr) { return FALSE; } - int read_torque_const(int id, double *tc) { return FALSE; diff --git a/rtc/RobotHardware/robot.cpp b/rtc/RobotHardware/robot.cpp index 2966737e693..49d2b95fdd0 100644 --- a/rtc/RobotHardware/robot.cpp +++ b/rtc/RobotHardware/robot.cpp @@ -665,11 +665,11 @@ bool robot::checkJointCommands(const double *i_commands) bool robot::checkEmergency(emg_reason &o_reason, int &o_id) { int state; - joint_control_mode mode; // Added by Rafa + joint_control_mode mode; for (unsigned int i=0; ijointId >= 0)) { write_control_mode(l->jointId, mode); - // std::cerr << "[RobotHardware] setJointControlMode for " << i_jname << " : " << mode << std::endl; // Commented out by Rafa + std::cerr << "[RobotHardware] setJointControlMode for " << i_jname << " : " << mode << std::endl; } else { char *s = (char *)i_jname; while(*s) { *s=toupper(*s); s++; } const std::vector jgroup = m_jointGroups[i_jname]; @@ -1061,7 +1061,7 @@ bool robot::setJointControlMode(const char *i_jname, joint_control_mode mode) for (unsigned int i=0; i Date: Tue, 7 Feb 2023 15:43:23 +0900 Subject: [PATCH 25/26] Removed functions write_llimit_angle and write_ulimit_angle as they are not safe --- lib/io/iob.cpp | 11 ----------- lib/io/iob.h | 18 ------------------ 2 files changed, 29 deletions(-) diff --git a/lib/io/iob.cpp b/lib/io/iob.cpp index c1e9e861172..37147b4e4d0 100644 --- a/lib/io/iob.cpp +++ b/lib/io/iob.cpp @@ -531,17 +531,6 @@ int read_llimit_angle(int id, double *angle) { return FALSE; } - -int write_ulimit_angle(int id, double angle) -{ - return FALSE; -} - -int write_llimit_angle(int id, double angle) -{ - return FALSE; -} - int read_encoder_pulse(int id, double *ec) { return FALSE; diff --git a/lib/io/iob.h b/lib/io/iob.h index 8a888ae5683..b03da4b5022 100644 --- a/lib/io/iob.h +++ b/lib/io/iob.h @@ -192,24 +192,6 @@ extern "C"{ * @retval FALSE otherwise */ int read_ulimit_angle(int id, double *angle); - - /** - * @brief write lower joint limit[rad] - * @param id joint id - * @param angle lower joint limit[rad] - * @retval TRUE this function is supported - * @retval FALSE otherwise - */ - int write_llimit_angle(int id, double angle); - - /** - * @brief write upper joint limit[rad] - * @param id joint id - * @param angle upper joint limit[rad] - * @retval TRUE this function is supported - * @retval FALSE otherwise - */ - int write_ulimit_angle(int id, double angle); //@} From b4394937df9c3db470cda51954b28df58021411f Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Mon, 20 Feb 2023 13:58:24 +0900 Subject: [PATCH 26/26] Further minimized diff --- rtc/RobotHardware/robot.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rtc/RobotHardware/robot.h b/rtc/RobotHardware/robot.h index 3e04f98f401..aa2d36aa345 100644 --- a/rtc/RobotHardware/robot.h +++ b/rtc/RobotHardware/robot.h @@ -261,6 +261,7 @@ class robot : public hrp::Body */ bool checkEmergency(emg_reason &o_reason, int &o_id); + /** \brief check joint commands are valid or not \return true if the joint command is invalid, false otherwise @@ -355,7 +356,6 @@ class robot : public hrp::Body \return true if set successfully, false otherwise */ bool setJointControlMode(const char *i_jname, joint_control_mode mode); - private: /** \brief calibrate inertia sensor for one sampling period