Skip to content

Allow to change to torque control mode #1326

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

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

Allow to change to torque control mode #1326

Show file tree
Hide file tree
Changes from 22 commits
Commits
Show all changes
26 commits
Select commit Hold shift + click to select a range
91ec2f2
Still trying to fix ModifiedServo [WIP]
rafaelxero May 31, 2019
108fb6b
Changed ModifiedServo to look more like PDcontroller [WIP]
rafaelxero Jun 2, 2019
3eb9a0f
Several versions of ModifiedServo [WIP]
rafaelxero Jun 3, 2019
0a39c13
Added torqueControlMode to RobotHardware
rafaelxero Jun 25, 2019
8846a8b
Excluded torque-controlled joints from the checkEmergency test for th…
rafaelxero Aug 13, 2019
228ce6f
Removed test for the torque because at the end it gets saturated to t…
rafaelxero Aug 13, 2019
ef3910b
Corrected typo
rafaelxero Aug 14, 2019
e3adcb7
Added check for the joint values to be inside of the joint range and …
rafaelxero Aug 14, 2019
e827887
Added resetJointControlMode and deactivated the previous security pro…
rafaelxero Aug 14, 2019
4ede167
Added allowTorqueControlMode
rafaelxero Aug 14, 2019
9c750dc
Commented out the messages that are displayed when changing the joint…
rafaelxero Aug 14, 2019
81d2f79
Deleted isJointTorqueControlModeUsed
rafaelxero Aug 15, 2019
cee9a70
Added write_llimit_angle and write_ulimit_angle to iob.h
rafaelxero Aug 15, 2019
6584025
Used m_ for the member variables of ModifiedServo
rafaelxero Aug 16, 2019
b8d1516
Changed intermediate qRef into qCom in ModifiedServo
rafaelxero Aug 16, 2019
cbed8f4
Removed some test versions of ModifiedServo
rafaelxero Feb 15, 2021
134ec67
Removed ModifiedServo-original
rafaelxero Feb 15, 2021
84941fc
Removed commented out functions that I had moved to iob level and com…
rafaelxero Feb 18, 2021
ea83f7a
Had missed to erase commented out code
rafaelxero Feb 18, 2021
64ddc71
Had missed to erase more commented out code
rafaelxero Feb 18, 2021
53e70f4
Brought ModifiedServo as it is from master
rafaelxero Dec 15, 2022
34c8f9f
Deleted unused functions: checkJointActualValues and resetJointContro…
rafaelxero Feb 6, 2023
d068033
Removed the flag allowTorqueControlMode as it is not used anymore
rafaelxero Feb 7, 2023
42f6519
Reduced diff, removed unnecessary comments and uncommented messages t…
rafaelxero Feb 7, 2023
d8d245f
Removed functions write_llimit_angle and write_ulimit_angle as they a…
rafaelxero Feb 7, 2023
b439493
Further minimized diff
rafaelxero Feb 20, 2023
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
14 changes: 14 additions & 0 deletions lib/io/iob.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -527,18 +527,32 @@ int read_ulimit_angle(int id, double *angle)
{
return FALSE;
}

Copy link
Owner

@fkanehiro fkanehiro Feb 6, 2023

Choose a reason for hiding this comment

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

It is better to minimize diff.

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;
Expand Down
36 changes: 36 additions & 0 deletions lib/io/iob.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -175,6 +175,42 @@ 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);

/**
* @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);

Copy link
Owner

@fkanehiro fkanehiro Feb 6, 2023

Choose a reason for hiding this comment

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

Are these necessary? They are not called in this PR.

Copy link
Contributor Author

@rafaelxero rafaelxero Feb 7, 2023

Choose a reason for hiding this comment

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

@fkanehiro In this PR they are not used, but in the PosTorqueCtrl-rebase branch of hrp5p-iob, that I will try to merge soon, I have a standalone program called modify_joint_limits (https://github.com/isri-aist/hrp5p-iob/blob/PosTorqueCtrl-rebase/iob/modify_joint_limits.cpp) that uses those functions

Copy link
Owner

@fkanehiro fkanehiro Feb 7, 2023

Choose a reason for hiding this comment

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

If they are not relevant to this PR, I think they should be removed from this PR.

In addition, as adding APIs will break compatibility, you need to use ROBOT_IOB_VERSION to add APIs.
Moreover, I prefer not to add these APIs. Because they will enable normal users to implement dangerous programs.

Copy link
Contributor Author

@rafaelxero rafaelxero Feb 7, 2023

Choose a reason for hiding this comment

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

Got it... I have removed these APIs in d8d245f

//@}

// @name joint
Expand Down
16 changes: 14 additions & 2 deletions rtc/RobotHardware/RobotHardware.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand All @@ -61,7 +62,8 @@ RobotHardware::RobotHardware(RTC::Manager* manager)
m_rstate2Out("rstate2", m_rstate2),
m_RobotHardwareServicePort("RobotHardwareService"),
// </rtc-template>
dummy(0)
allowTorqueControlMode(true),
Copy link
Owner

@fkanehiro fkanehiro Feb 6, 2023

Choose a reason for hiding this comment

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

Is this necessary? There is no way to change its value.

Copy link
Contributor Author

@rafaelxero rafaelxero Feb 7, 2023

Choose a reason for hiding this comment

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

Sorry for this, it remained after being used by code which was not ultimately used and that I erased
Solved by d068033

dummy(0)
{
}

Expand All @@ -79,6 +81,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);
Expand Down Expand Up @@ -145,6 +148,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;
Expand Down Expand Up @@ -248,7 +252,7 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id)
m_emergencySignalOut.write();
}
}
}
}

if (m_qRefIn.isNew()){
m_qRefIn.read();
Expand Down Expand Up @@ -281,6 +285,14 @@ 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();
if (allowTorqueControlMode){
for (unsigned int i=0; i<m_robot->numJoints(); ++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());
Expand Down
12 changes: 10 additions & 2 deletions rtc/RobotHardware/RobotHardware.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
#include <rtm/idl/ExtendedDataTypesSkel.h>

#include <hrpModel/Body.h>
#include <hrpModel/Link.h>

// Service implementation headers
// <rtc-template block="service_impl_h">
Expand Down Expand Up @@ -140,7 +141,12 @@ class RobotHardware
*/
TimedDoubleSeq m_tauRef;
InPort<TimedDoubleSeq> m_tauRefIn;

/**
\brief array of booleans indicating which joint is in torque mode (1) or position mode (0)
*/
TimedBooleanSeq m_torqueControlMode;
InPort<TimedBooleanSeq> m_torqueControlModeIn;

// </rtc-template>

/**
Expand Down Expand Up @@ -216,7 +222,9 @@ class RobotHardware
robot *robot_ptr(void) { return m_robot.get(); };
private:
void getStatus2(OpenHRP::RobotHardwareService::RobotState2 &rstate2);


bool allowTorqueControlMode; // Added by Rafa

Copy link
Owner

@fkanehiro fkanehiro Feb 6, 2023

Choose a reason for hiding this comment

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

Please remove "... by Rafa" :-)

int dummy;
boost::shared_ptr<robot> m_robot;
};
Expand Down
10 changes: 6 additions & 4 deletions rtc/RobotHardware/robot.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<numJoints(); i++){
read_servo_state(i, &state);
if (state == ON && m_servoErrorLimit[i] != 0){
read_control_mode(i, &mode); // Added by Rafa
if (state == ON && m_servoErrorLimit[i] != 0 && mode != JCM_TORQUE){ // Modified by Rafa
double angle, command;
read_actual_angle(i, &angle);
read_command_angle(i, &command);
Expand Down Expand Up @@ -1048,18 +1050,18 @@ 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;
// std::cerr << "[RobotHardware] setJointControlMode for all joints : " << mode << std::endl; // Commented out by Rafa
Copy link
Owner

@fkanehiro fkanehiro Feb 6, 2023

Choose a reason for hiding this comment

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

It is better not to change the current behavior.

} 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;
// 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<int> jgroup = m_jointGroups[i_jname];
if (jgroup.size()==0) return false;
for (unsigned int i=0; i<jgroup.size(); i++) {
write_control_mode(jgroup[i], 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
}
return true;
}
2 changes: 1 addition & 1 deletion rtc/RobotHardware/robot.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -261,7 +261,6 @@ 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
Expand Down Expand Up @@ -356,6 +355,7 @@ 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
Expand Down