Allow to change to torque control mode #1326
Conversation
…e maximum difference between angle and command, and added test for the torque
…he maximum values
…a check if a new tauRef does not delay for more than a number of cycles
… control mode, and fixed some typos
…mented out a protection for the current angles that causes problems
lib/io/iob.cpp
Outdated
| @@ -527,18 +527,32 @@ int read_ulimit_angle(int id, double *angle) | |||
| { | |||
| return FALSE; | |||
| } | |||
|
|
|||
| * @retval FALSE otherwise | ||
| */ | ||
| int write_ulimit_angle(int id, double angle); | ||
|
|
There was a problem hiding this comment.
Are these necessary? They are not called in this PR.
There was a problem hiding this comment.
@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
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
Got it... I have removed these APIs in d8d245f
rtc/RobotHardware/RobotHardware.h
Outdated
|
|
||
|
|
||
| bool allowTorqueControlMode; // Added by Rafa | ||
|
|
There was a problem hiding this comment.
Please remove "... by Rafa" :-)
rtc/RobotHardware/RobotHardware.cpp
Outdated
| @@ -61,7 +62,8 @@ RobotHardware::RobotHardware(RTC::Manager* manager) | |||
| m_rstate2Out("rstate2", m_rstate2), | |||
| m_RobotHardwareServicePort("RobotHardwareService"), | |||
| // </rtc-template> | |||
| dummy(0) | |||
| allowTorqueControlMode(true), | |||
There was a problem hiding this comment.
Is this necessary? There is no way to change its value.
There was a problem hiding this comment.
Sorry for this, it remained after being used by code which was not ultimately used and that I erased
Solved by d068033
rtc/RobotHardware/robot.cpp
Outdated
| @@ -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 | |||
There was a problem hiding this comment.
It is better not to change the current behavior.
| 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){ |
There was a problem hiding this comment.
In current JSK JAXON control board's imprementation,
JCM_POSITIONmeansposition control.JCM_TORQUEmeansposition 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
This PR is to bring the modifications of https://github.com/rafaelxero/hrpsys-base/tree/PosTorqueCtrl-rebase (which I used for my torque control experiments) to master branch.
It basically allows to change the joint control mode to torque and adds some functions to iob.h.
I have removed code that was not ultimately used.