Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[RobotHardware] add OutPort for gain #1295

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

[RobotHardware] add OutPort for gain #1295

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
96f95bd
[RobotHardware] add OutPort for gain
Naoki-Hiraoka Nov 11, 2020
d78c52e
[RobotHardware] fix previous commit
Naoki-Hiraoka Apr 25, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
29 changes: 29 additions & 0 deletions rtc/RobotHardware/RobotHardware.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"),
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -335,6 +349,17 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id)
}
m_servoState.tm = tm;

for (unsigned int i=0; i<m_pgain.data.length(); i++) m_robot->readPgain(i, m_pgain.data[i]);
m_pgain.tm = tm;
for (unsigned int i=0; i<m_dgain.data.length(); i++) m_robot->readDgain(i, m_dgain.data[i]);
m_dgain.tm = tm;
#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 4
for (unsigned int i=0; i<m_torquePgain.data.length(); i++) m_robot->readTorquePgain(i, m_torquePgain.data[i]);
m_torquePgain.tm = tm;
for (unsigned int i=0; i<m_torqueDgain.data.length(); i++) m_robot->readTorqueDgain(i, m_torqueDgain.data[i]);
m_torqueDgain.tm = tm;
#endif

getStatus2(m_rstate2.data);
m_rstate2.tm = tm;

Expand All @@ -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; i<m_rateOut.size(); i++){
m_rateOut[i]->write();
}
Expand Down
20 changes: 20 additions & 0 deletions rtc/RobotHardware/RobotHardware.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -177,6 +177,22 @@ class RobotHardware
*/
std::vector<TimedDoubleSeq> 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;

Expand All @@ -191,6 +207,10 @@ class RobotHardware
std::vector<OutPort<TimedAngularVelocity3D> *> m_rateOut;
std::vector<OutPort<TimedDoubleSeq> *> m_forceOut;
OutPort<OpenHRP::TimedLongSeqSeq> m_servoStateOut;
OutPort<TimedDoubleSeq> m_pgainOut;
OutPort<TimedDoubleSeq> m_dgainOut;
OutPort<TimedDoubleSeq> m_torquePgainOut;
OutPort<TimedDoubleSeq> m_torqueDgainOut;
OutPort<TimedLong> m_emergencySignalOut;
OutPort<OpenHRP::RobotHardwareService::TimedRobotState2> m_rstate2Out;

Expand Down
28 changes: 28 additions & 0 deletions rtc/RobotHardware/robot.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ヘッダファイルと共に #ifマクロの外に関数宣言と定義を書いた方がコンパイルすら通らないのでそちらの方がより安全ということはないだろうか？

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

#ifマクロ による分岐箇所が可能な限り少ない方が可読性が高く保守もしやすいと考え、robot.cppのiobの関数を直接呼ぶ部分でのみ#ifマクロ を使う形にしました。

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

すみません　遅くなりました
確かに可読性が高い方がいいかもしれませんね
それでいいと思います

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;
Expand Down
28 changes: 28 additions & 0 deletions rtc/RobotHardware/robot.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down