From 96f95bde19e3a53c89eb5aee8c4ca90cc23c355c Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Wed, 11 Nov 2020 15:22:32 +0900 Subject: [PATCH 1/2] [RobotHardware] add OutPort for gain --- rtc/RobotHardware/RobotHardware.cpp | 29 +++++++++++++++++++++++++++++ rtc/RobotHardware/RobotHardware.h | 20 ++++++++++++++++++++ rtc/RobotHardware/robot.cpp | 28 ++++++++++++++++++++++++++++ rtc/RobotHardware/robot.h | 28 ++++++++++++++++++++++++++++ 4 files changed, 105 insertions(+) diff --git a/rtc/RobotHardware/RobotHardware.cpp b/rtc/RobotHardware/RobotHardware.cpp index 51abd91a656..0265415119f 100644 --- a/rtc/RobotHardware/RobotHardware.cpp +++ b/rtc/RobotHardware/RobotHardware.cpp @@ -57,6 +57,10 @@ RobotHardware::RobotHardware(RTC::Manager* manager) m_ctauOut("ctau", m_ctau), m_pdtauOut("pdtau", m_pdtau), m_servoStateOut("servoState", m_servoState), + m_pgainOut("pgain", m_pgain), + m_dgainOut("dgain", m_dgain), + m_torquePgainOut("torquePgain", m_torquePgain), + m_torqueDgainOut("torqueDgain", m_torqueDgain), m_emergencySignalOut("emergencySignal", m_emergencySignal), m_rstate2Out("rstate2", m_rstate2), m_RobotHardwareServicePort("RobotHardwareService"), @@ -86,6 +90,10 @@ RTC::ReturnCode_t RobotHardware::onInitialize() addOutPort("ctau", m_ctauOut); addOutPort("pdtau", m_pdtauOut); addOutPort("servoState", m_servoStateOut); + addOutPort("pgain", m_pgainOut); + addOutPort("dgain", m_dgainOut); + addOutPort("torquePgain", m_torquePgainOut); + addOutPort("torqueDgain", m_torqueDgainOut); addOutPort("emergencySignal", m_emergencySignalOut); addOutPort("rstate2", m_rstate2Out); @@ -141,6 +149,12 @@ RTC::ReturnCode_t RobotHardware::onInitialize() m_ctau.data.length(m_robot->numJoints()); m_pdtau.data.length(m_robot->numJoints()); m_servoState.data.length(m_robot->numJoints()); + m_pgain.data.length(m_robot->numJoints()); + m_dgain.data.length(m_robot->numJoints()); +#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 4 + m_torquePgain.data.length(m_robot->numJoints()); + m_torqueDgain.data.length(m_robot->numJoints()); +#endif m_qRef.data.length(m_robot->numJoints()); m_dqRef.data.length(m_robot->numJoints()); m_ddqRef.data.length(m_robot->numJoints()); @@ -335,6 +349,17 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id) } m_servoState.tm = tm; + for (unsigned int i=0; ireadPgain(i, m_pgain.data[i]); + m_pgain.tm = tm; + for (unsigned int i=0; ireadDgain(i, m_dgain.data[i]); + m_dgain.tm = tm; +#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 4 + for (unsigned int i=0; ireadTorquePgain(i, m_torquePgain.data[i]); + m_torquePgain.tm = tm; + for (unsigned int i=0; ireadTorqueDgain(i, m_torqueDgain.data[i]); + m_torqueDgain.tm = tm; +#endif + getStatus2(m_rstate2.data); m_rstate2.tm = tm; @@ -346,6 +371,10 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id) m_ctauOut.write(); m_pdtauOut.write(); m_servoStateOut.write(); + m_pgainOut.write(); + m_dgainOut.write(); + m_torquePgainOut.write(); + m_torqueDgainOut.write(); for (unsigned int i=0; iwrite(); } diff --git a/rtc/RobotHardware/RobotHardware.h b/rtc/RobotHardware/RobotHardware.h index 1450cfd07c8..17a2c4baf9f 100644 --- a/rtc/RobotHardware/RobotHardware.h +++ b/rtc/RobotHardware/RobotHardware.h @@ -177,6 +177,22 @@ class RobotHardware */ std::vector m_force; OpenHRP::TimedLongSeqSeq m_servoState; + /** + \brief array of pgain of joint with jointId + */ + TimedDoubleSeq m_pgain; + /** + \brief array of dgain of joint with jointId + */ + TimedDoubleSeq m_dgain; + /** + \brief array of torque pgain of joint with jointId + */ + TimedDoubleSeq m_torquePgain; + /** + \brief array of torque dgain of joint with jointId + */ + TimedDoubleSeq m_torqueDgain; TimedLong m_emergencySignal; OpenHRP::RobotHardwareService::TimedRobotState2 m_rstate2; @@ -191,6 +207,10 @@ class RobotHardware std::vector *> m_rateOut; std::vector *> m_forceOut; OutPort m_servoStateOut; + OutPort m_pgainOut; + OutPort m_dgainOut; + OutPort m_torquePgainOut; + OutPort m_torqueDgainOut; OutPort m_emergencySignalOut; OutPort m_rstate2Out; diff --git a/rtc/RobotHardware/robot.cpp b/rtc/RobotHardware/robot.cpp index 2027dd405db..a6c895c7113 100644 --- a/rtc/RobotHardware/robot.cpp +++ b/rtc/RobotHardware/robot.cpp @@ -609,6 +609,34 @@ int robot::readDriverTemperature(int i) return temp; } +int robot::readPgain(int i, double &o_pgain) +{ + return read_pgain(i, &o_pgain); +} + +int robot::readDgain(int i, double &o_dgain) +{ + return read_dgain(i, &o_dgain); +} + +int robot::readTorquePgain(int i, double &o_torquepgain) +{ +#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 4 + return read_torque_pgain(i, &o_torquepgain); +#else + return E_ID; +#endif +} + +int robot::readTorqueDgain(int i, double &o_torquedgain) +{ +#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 4 + return read_torque_dgain(i, &o_torquedgain); +#else + return E_ID; +#endif +} + char *time_string() { struct timeval tv; diff --git a/rtc/RobotHardware/robot.h b/rtc/RobotHardware/robot.h index aa2d36aa345..7290c202f51 100644 --- a/rtc/RobotHardware/robot.h +++ b/rtc/RobotHardware/robot.h @@ -125,6 +125,34 @@ class robot : public hrp::Body */ int readDriverTemperature(int i); + /** + \brief read pgain of motor driver[Nm/rad] + \param i joint id + \param TRUE if read successfully, E_ID otherwise + */ + int readPgain(int i, double &o_pgain); + + /** + \brief read dgain of motor driver[Nm/(rad/s)] + \param i joint id + \param TRUE if read successfully, E_ID otherwise + */ + int readDgain(int i, double &o_dgain); + + /** + \brief read pgain of motor driver[Nm/Nm] + \param i joint id + \param TRUE if read successfully, E_ID otherwise + */ + int readTorquePgain(int i, double &o_torquepgain); + + /** + \brief read dgain of motor driver[Nm/(Nm/s)] + \param i joint id + \param TRUE if read successfully, E_ID otherwise + */ + int readTorqueDgain(int i, double &o_torquedgain); + /** \brief read voltage and current of the robot power source \param o_voltage voltage From d78c52e3dbc40a7c83ebdf2f1fe7d13caaeb447d Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Sun, 25 Apr 2021 16:56:16 +0900 Subject: [PATCH 2/2] [RobotHardware] fix previous commit --- rtc/RobotHardware/RobotHardware.cpp | 4 ---- rtc/RobotHardware/robot.cpp | 6 ++++-- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/rtc/RobotHardware/RobotHardware.cpp b/rtc/RobotHardware/RobotHardware.cpp index 0265415119f..6ffd8ae9717 100644 --- a/rtc/RobotHardware/RobotHardware.cpp +++ b/rtc/RobotHardware/RobotHardware.cpp @@ -151,10 +151,8 @@ RTC::ReturnCode_t RobotHardware::onInitialize() m_servoState.data.length(m_robot->numJoints()); m_pgain.data.length(m_robot->numJoints()); m_dgain.data.length(m_robot->numJoints()); -#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 4 m_torquePgain.data.length(m_robot->numJoints()); m_torqueDgain.data.length(m_robot->numJoints()); -#endif m_qRef.data.length(m_robot->numJoints()); m_dqRef.data.length(m_robot->numJoints()); m_ddqRef.data.length(m_robot->numJoints()); @@ -353,12 +351,10 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id) m_pgain.tm = tm; for (unsigned int i=0; ireadDgain(i, m_dgain.data[i]); m_dgain.tm = tm; -#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 4 for (unsigned int i=0; ireadTorquePgain(i, m_torquePgain.data[i]); m_torquePgain.tm = tm; for (unsigned int i=0; ireadTorqueDgain(i, m_torqueDgain.data[i]); m_torqueDgain.tm = tm; -#endif getStatus2(m_rstate2.data); m_rstate2.tm = tm; diff --git a/rtc/RobotHardware/robot.cpp b/rtc/RobotHardware/robot.cpp index a6c895c7113..61362b4628a 100644 --- a/rtc/RobotHardware/robot.cpp +++ b/rtc/RobotHardware/robot.cpp @@ -624,7 +624,8 @@ int robot::readTorquePgain(int i, double &o_torquepgain) #if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 4 return read_torque_pgain(i, &o_torquepgain); #else - return E_ID; + o_torquepgain = 0.0; + return TRUE; #endif } @@ -633,7 +634,8 @@ int robot::readTorqueDgain(int i, double &o_torquedgain) #if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 4 return read_torque_dgain(i, &o_torquedgain); #else - return E_ID; + o_torquedgain = 0.0; + return TRUE; #endif }