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

Jump to bottom

Allow to change to torque control mode #1326

Open
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 25 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
18 changes: 18 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,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);

Copy link
Owner

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

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

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

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
13 changes: 11 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,7 @@ RobotHardware::RobotHardware(RTC::Manager* manager)
m_rstate2Out("rstate2", m_rstate2),
m_RobotHardwareServicePort("RobotHardwareService"),
// </rtc-template>
dummy(0)
dummy(0)
{
}

Expand All @@ -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);
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -248,7 +251,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 +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; 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
10 changes: 8 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,7 @@ class RobotHardware
robot *robot_ptr(void) { return m_robot.get(); };
private:
void getStatus2(OpenHRP::RobotHardwareService::RobotState2 &rstate2);

int dummy;
boost::shared_ptr<robot> m_robot;
};
Expand Down
4 changes: 3 additions & 1 deletion 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;
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);
if (state == ON && m_servoErrorLimit[i] != 0 && mode != JCM_TORQUE){
Comment on lines 665 to +672
Copy link
Contributor

Choose a reason for hiding this comment

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

In current JSK JAXON control board's imprementation,

  • JCM_POSITION means position control.
  • JCM_TORQUE means position control + command_torque.

While the robot is hanged by the crane, we use JCM_TORQUE with high position pdgain. After the robot is put on the ground, we reduce position pdgain (or set position pdgain to zero).

If JCM_TORQUE with high position pdgain, servoErrorLimitCheck is necessary.
If JCM_TORQUE with low (or zero) position pdgain, servoErrorLimitCheck should be deactivated.

We currently modify read_servo_state function in our iob.cpp to switch servoErrorLimitCheck.

int read_servo_state(int id, int *s)
{
  *s = 0;
  if (servo_state[id] == ON &&
      pgain[id] > THRESHOLD)
    *s = 1;
  return TRUE;
}

After this PR is merged, JCM_TORQUE mode always disables servoErrorLimitCheck. So we have to change our software. @kindsenior

double angle, command;
read_actual_angle(i, &angle);
read_command_angle(i, &command);
Expand Down
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