Skip to content

Commit

Permalink
Add velocityThres param for torque control
Browse files Browse the repository at this point in the history
  • Loading branch information
@sgiraz
sgiraz committed Aug 31, 2023
1 parent ae55d5c commit 81b7f3c
Show file tree
Hide file tree
Showing 12 changed files with 80 additions and 13 deletions.
5 changes: 5 additions & 0 deletions doc/release/yarp_3_9_master/added_velocityThres.md
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
#### `lib_yarp_dev`

* ITorqueControl supports four a new friction parameters: velocityThresh.
This parameters is used to enable the new torque control law and fine tune the the friction compensation at FW level.

6 changes: 4 additions & 2 deletions src/devices/RemoteControlBoard/RemoteControlBoard.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2215,6 +2215,7 @@ bool RemoteControlBoard::setMotorTorqueParams(int j, const MotorTorqueParameters
b.addFloat64(params.viscousNeg);
b.addFloat64(params.coulombPos);
b.addFloat64(params.coulombNeg);
b.addFloat64(params.velocityThres);
bool ok = rpc_p.write(cmd, response);
return CHECK_FAIL(ok, response);
}
Expand All @@ -2233,9 +2234,9 @@ bool RemoteControlBoard::getMotorTorqueParams(int j, MotorTorqueParameters *para
return false;
}
Bottle& l = *lp;
if (l.size() != 8)
if (l.size() != 9)

Check warning on line 2237 in src/devices/RemoteControlBoard/RemoteControlBoard.cpp

View check run for this annotation

Codacy Production / Codacy Static Code Analysis

src/devices/RemoteControlBoard/RemoteControlBoard.cpp#L2237 

9 is a magic number; consider replacing it with a named constant
{
yCError(REMOTECONTROLBOARD, "getMotorTorqueParams return value not understood, size != 8");
yCError(REMOTECONTROLBOARD, "getMotorTorqueParams return value not understood, size != 9");
return false;
}
params->bemf = l.get(0).asFloat64();
Expand All @@ -2246,6 +2247,7 @@ bool RemoteControlBoard::getMotorTorqueParams(int j, MotorTorqueParameters *para
params->viscousNeg = l.get(5).asFloat64();
params->coulombPos = l.get(6).asFloat64();
params->coulombNeg = l.get(7).asFloat64();
params->velocityThres = l.get(8).asFloat64();

Check warning on line 2250 in src/devices/RemoteControlBoard/RemoteControlBoard.cpp

View check run for this annotation

Codacy Production / Codacy Static Code Analysis

src/devices/RemoteControlBoard/RemoteControlBoard.cpp#L2250 

8 is a magic number; consider replacing it with a named constant
return true;
}
return false;
Expand Down
6 changes: 4 additions & 2 deletions src/devices/controlBoard_nws_yarp/RPCMessagesParser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -477,8 +477,8 @@ void RPCMessagesParser::handleTorqueMsg(const yarp::os::Bottle& cmd,
break;
}

if (b->size() != 8) {
yCError(CONTROLBOARD, "received a SET VOCAB_MOTOR_PARAMS command not understood, size != 8");
if (b->size() != 9) {
yCError(CONTROLBOARD, "received a SET VOCAB_MOTOR_PARAMS command not understood, size != 9");
break;
}

Expand All @@ -490,6 +490,7 @@ void RPCMessagesParser::handleTorqueMsg(const yarp::os::Bottle& cmd,
params.viscousNeg = b->get(5).asFloat64();
params.coulombPos = b->get(6).asFloat64();
params.coulombNeg = b->get(7).asFloat64();
params.velocityThres = b->get(8).asFloat64();

Check warning on line 493 in src/devices/controlBoard_nws_yarp/RPCMessagesParser.cpp

View check run for this annotation

Codacy Production / Codacy Static Code Analysis

src/devices/controlBoard_nws_yarp/RPCMessagesParser.cpp#L493 

8 is a magic number; consider replacing it with a named constant

*ok = rpc_ITorque->setMotorTorqueParams(joint, params);
} break;
Expand Down Expand Up @@ -564,6 +565,7 @@ void RPCMessagesParser::handleTorqueMsg(const yarp::os::Bottle& cmd,
b.addFloat64(params.viscousNeg);
b.addFloat64(params.coulombPos);
b.addFloat64(params.coulombNeg);
b.addFloat64(params.velocityThres);
} break;
case VOCAB_RANGE: {
*ok = rpc_ITorque->getTorqueRange(cmd.get(3).asInt32(), &dtmp, &dtmp2);
Expand Down
21 changes: 18 additions & 3 deletions src/devices/fakeMotionControl/fakeMotionControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -268,6 +268,7 @@ bool FakeMotionControl::alloc(int nj)
_viscousNeg=allocAndCheck<double>(nj);
_coulombPos=allocAndCheck<double>(nj);
_coulombNeg=allocAndCheck<double>(nj);
_velocityThres=allocAndCheck<double>(nj);

// Reserve space for data stored locally. values are initialized to 0
_posCtrl_references = allocAndCheck<double>(nj);
Expand Down Expand Up @@ -344,6 +345,7 @@ bool FakeMotionControl::dealloc()
checkAndDestroy(_viscousNeg);
checkAndDestroy(_coulombPos);
checkAndDestroy(_coulombNeg);
checkAndDestroy(_velocityThres);
checkAndDestroy(_posCtrl_references);
checkAndDestroy(_posDir_references);
checkAndDestroy(_command_speeds);
Expand Down Expand Up @@ -446,6 +448,7 @@ FakeMotionControl::FakeMotionControl() :
_viscousNeg (nullptr),
_coulombPos (nullptr),
_coulombNeg (nullptr),
_velocityThres (nullptr),
_filterType (nullptr),
_torqueSensorId (nullptr),
_torqueSensorChan (nullptr),
Expand Down Expand Up @@ -744,7 +747,7 @@ bool FakeMotionControl::parsePositionPidsGroup(Bottle& pidsGroup, Pid myPid[])
return true;
}

bool FakeMotionControl::parseTorquePidsGroup(Bottle& pidsGroup, Pid myPid[], double kbemf[], double ktau[], int filterType[], double viscousPos[], double viscousNeg[], double coulombPos[], double coulombNeg[])
bool FakeMotionControl::parseTorquePidsGroup(Bottle& pidsGroup, Pid myPid[], double kbemf[], double ktau[], int filterType[], double viscousPos[], double viscousNeg[], double coulombPos[], double coulombNeg[], double velocityThres[])
{
int j=0;
Bottle xtmp;
Expand Down Expand Up @@ -867,6 +870,14 @@ bool FakeMotionControl::parseTorquePidsGroup(Bottle& pidsGroup, Pid myPid[], dou
coulombNeg[j] = xtmp.get(j+1).asFloat64();
}

if (!extractGroup(pidsGroup, xtmp, "velocityThres", "velocity threshold parameter", _njoints)) {
return false;
}
for (j=0; j<_njoints; j++) {
velocityThres[j] = xtmp.get(j+1).asFloat64();
}


//conversion from metric to machine units (if applicable)
// for (j=0; j<_njoints; j++)
// {
Expand Down Expand Up @@ -2909,14 +2920,16 @@ bool FakeMotionControl::getMotorTorqueParamsRaw(int j, MotorTorqueParameters *pa
params->viscousNeg = _viscousNeg[j];
params->coulombPos = _coulombPos[j];
params->coulombNeg = _coulombNeg[j];
params->velocityThres = _velocityThres[j];
yCDebug(FAKEMOTIONCONTROL) << "getMotorTorqueParamsRaw" << params->bemf
<< params->bemf_scale
<< params->ktau
<< params->ktau_scale
<< params->viscousPos
<< params->viscousNeg
<< params->coulombPos
<< params->coulombNeg;
<< params->coulombNeg
<< params->velocityThres;
return true;
}

Expand All @@ -2930,14 +2943,16 @@ bool FakeMotionControl::setMotorTorqueParamsRaw(int j, const MotorTorqueParamete
_viscousNeg[j] = params.viscousNeg;
_coulombPos[j] = params.coulombPos;
_coulombNeg[j] = params.coulombNeg;
_velocityThres[j] = params.velocityThres;
yCDebug(FAKEMOTIONCONTROL) << "setMotorTorqueParamsRaw" << params.bemf
<< params.bemf_scale
<< params.ktau
<< params.ktau_scale
<< params.viscousPos
<< params.viscousNeg
<< params.coulombPos
<< params.coulombNeg;
<< params.coulombNeg
<< params.velocityThres;
return true;
}

Expand Down
11 changes: 6 additions & 5 deletions src/devices/fakeMotionControl/fakeMotionControl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -193,10 +193,11 @@ class FakeMotionControl :
double *_ktau; /** motor torque constant */
int *_kbemf_scale; /** back-emf compensation parameter */
int *_ktau_scale; /** motor torque constant */
double *_viscousPos; /** viscous pos friction */
double *_viscousNeg; /** viscous neg friction */
double *_coulombPos; /** coulomb up friction */
double *_coulombNeg; /** coulomb neg friction */
double *_viscousPos; /** viscous pos friction */
double *_viscousNeg; /** viscous neg friction */
double *_coulombPos; /** coulomb up friction */
double *_coulombNeg; /** coulomb neg friction */
double *_velocityThres; /** velocity threshold for torque control */
int * _filterType; /** the filter type (int value) used by the force control algorithm */
int *_torqueSensorId; /** Id of associated Joint Torque Sensor */
int *_torqueSensorChan; /** Channel of associated Joint Torque Sensor */
Expand Down Expand Up @@ -508,7 +509,7 @@ class FakeMotionControl :
void cleanup();
bool dealloc();
bool parsePositionPidsGroup(yarp::os::Bottle& pidsGroup, yarp::dev::Pid myPid[]);
bool parseTorquePidsGroup(yarp::os::Bottle& pidsGroup, yarp::dev::Pid myPid[], double kbemf[], double ktau[], int filterType[], double viscousPos[], double viscousNeg[], double coulombPos[], double coulombNeg[]);
bool parseTorquePidsGroup(yarp::os::Bottle& pidsGroup, yarp::dev::Pid myPid[], double kbemf[], double ktau[], int filterType[], double viscousPos[], double viscousNeg[], double coulombPos[], double coulombNeg[], double velocityThres[]);

bool parseImpedanceGroup_NewFormat(yarp::os::Bottle& pidsGroup, ImpedanceParameters vals[]);

Expand Down
24 changes: 24 additions & 0 deletions src/libYARP_dev/src/yarp/dev/ControlBoardHelper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@ class yarp::dev::ControlBoardHelper::PrivateUnitsHandler
double *viscousNegToRaws;
double *coulombPosToRaws;
double *coulombNegToRaws;
double *velocityThresToRaws;

PidUnits* PosPid_units;
PidUnits* VelPid_units;
Expand All @@ -68,6 +69,7 @@ class yarp::dev::ControlBoardHelper::PrivateUnitsHandler
viscousNegToRaws(nullptr),
coulombPosToRaws(nullptr),
coulombNegToRaws(nullptr),
velocityThresToRaws(nullptr),
PosPid_units(nullptr),
VelPid_units(nullptr),
CurPid_units(nullptr),
Expand All @@ -87,6 +89,7 @@ class yarp::dev::ControlBoardHelper::PrivateUnitsHandler
std::fill_n(viscousNegToRaws, size, 1.0);
std::fill_n(coulombPosToRaws, size, 1.0);
std::fill_n(coulombNegToRaws, size, 1.0);
std::fill_n(velocityThresToRaws, size, 1.0);
}

~PrivateUnitsHandler()
Expand All @@ -110,6 +113,7 @@ class yarp::dev::ControlBoardHelper::PrivateUnitsHandler
checkAndDestroy<double>(viscousNegToRaws);
checkAndDestroy<double>(coulombPosToRaws);
checkAndDestroy<double>(coulombNegToRaws);
checkAndDestroy<double>(velocityThresToRaws);
}

void alloc(int n)
Expand All @@ -134,6 +138,7 @@ class yarp::dev::ControlBoardHelper::PrivateUnitsHandler
viscousNegToRaws = new double[nj];
coulombPosToRaws = new double[nj];
coulombNegToRaws = new double[nj];
velocityThresToRaws = new double[nj];

yAssert(position_zeros != nullptr);
yAssert(helper_ones != nullptr);
Expand All @@ -154,6 +159,7 @@ class yarp::dev::ControlBoardHelper::PrivateUnitsHandler
yAssert(viscousNegToRaws != nullptr);
yAssert(coulombPosToRaws != nullptr);
yAssert(coulombNegToRaws != nullptr);
yAssert(velocityThresToRaws != nullptr);

pid_units[VOCAB_PIDTYPE_POSITION] = PosPid_units;
pid_units[VOCAB_PIDTYPE_VELOCITY] = VelPid_units;
Expand Down Expand Up @@ -183,6 +189,7 @@ class yarp::dev::ControlBoardHelper::PrivateUnitsHandler
memcpy(this->viscousNegToRaws, other.viscousNegToRaws, sizeof(*other.viscousNegToRaws)*nj);
memcpy(this->coulombPosToRaws, other.coulombPosToRaws, sizeof(*other.coulombPosToRaws)*nj);
memcpy(this->coulombNegToRaws, other.coulombNegToRaws, sizeof(*other.coulombNegToRaws)*nj);
memcpy(this->velocityThresToRaws, other.velocityThresToRaws, sizeof(*other.velocityThresToRaws)*nj);
}
};

Expand Down Expand Up @@ -806,6 +813,12 @@ void ControlBoardHelper::coulombNeg_user2raw(double coulombNeg_user, int j, doub
k = toHw(j);
}

void ControlBoardHelper::velocityThres_user2raw(double velocityThres_user, int j, double &velocityThres_raw, int &k)
{
velocityThres_raw = velocityThres_user * mPriv->velocityThresToRaws[j];
k = toHw(j);
}

void ControlBoardHelper::bemf_raw2user(double bemf_raw, int k_raw, double &bemf_user, int &j_user)
{
j_user = toUser(k_raw);
Expand Down Expand Up @@ -848,6 +861,11 @@ double ControlBoardHelper::coulombNeg_user2raw(double coulombNeg_user, int j)
return coulombNeg_user * mPriv->coulombNegToRaws[j];
}

double ControlBoardHelper::velocityThres_user2raw(double velocityThres_user, int j)
{
return velocityThres_user * mPriv->velocityThresToRaws[j];
}

void ControlBoardHelper::viscousPos_raw2user(double viscousPos_raw, int k_raw, double &viscousPos_user, int &j_user)
{
j_user = toUser(k_raw);
Expand All @@ -872,6 +890,12 @@ void ControlBoardHelper::coulombNeg_raw2user(double coulombNeg_raw, int k_raw, d
coulombNeg_user = coulombNeg_raw / mPriv->coulombNegToRaws[j_user];
}

void ControlBoardHelper::velocityThres_raw2user(double velocityThres_raw, int k_raw, double &velocityThres_user, int &j_user)
{
j_user = toUser(k_raw);
velocityThres_user = velocityThres_raw / mPriv->velocityThresToRaws[j_user];
}


// *******************************************//

Expand Down
3 changes: 3 additions & 0 deletions src/libYARP_dev/src/yarp/dev/ControlBoardHelper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -149,19 +149,22 @@ class YARP_dev_API yarp::dev::ControlBoardHelper
double viscousNeg_user2raw(double viscousNeg_user, int j);
double coulombPos_user2raw(double coulombPos_user, int j);
double coulombNeg_user2raw(double coulombNeg_user, int j);
double velocityThres_user2raw(double velocityThres_user, int j);

void bemf_user2raw(double bemf_user, int j, double &bemf_raw, int &k);
void ktau_user2raw(double ktau_user, int j, double &ktau_raw, int &k);
void viscousPos_user2raw(double viscousPos_user, int j, double &viscousPos_raw, int &k);
void viscousNeg_user2raw(double viscousNeg_user, int j, double &viscousNeg_raw, int &k);
void coulombPos_user2raw(double coulombPos_user, int j, double &coulombPos_raw, int &k);
void coulombNeg_user2raw(double coulombNeg_user, int j, double &coulombNeg_raw, int &k);
void velocityThres_user2raw(double velocityThres_user, int j, double &velocityThres_raw, int &k);
void bemf_raw2user(double bemf_raw, int k_raw, double &bemf_user, int &j_user);
void ktau_raw2user(double ktau_raw, int k_raw, double &ktau_user, int &j_user);
void viscousPos_raw2user(double viscousPos_raw, int k_raw, double &viscousPos_user, int &j_user);
void viscousNeg_raw2user(double viscousNeg_raw, int k_raw, double &viscousNeg_user, int &j_user);
void coulombPos_raw2user(double coulombPos_raw, int k_raw, double &coulombPos_user, int &j_user);
void coulombNeg_raw2user(double coulombNeg_raw, int k_raw, double &coulombNeg_user, int &j_user);
void velocityThres_raw2user(double threshold_raw, int k_raw, double &velocityThres_user, int &j_user);

int axes();

Expand Down
3 changes: 2 additions & 1 deletion src/libYARP_dev/src/yarp/dev/ITorqueControl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,8 @@ class YARP_dev_API yarp::dev::MotorTorqueParameters
double viscousNeg;
double coulombPos;
double coulombNeg;
MotorTorqueParameters() : bemf(0), bemf_scale(0), ktau(0), ktau_scale(0), viscousPos(0), viscousNeg(0), coulombPos(0), coulombNeg(0) {};
double velocityThres;
MotorTorqueParameters() : bemf(0), bemf_scale(0), ktau(0), ktau_scale(0), viscousPos(0), viscousNeg(0), coulombPos(0), coulombNeg(0), velocityThres(0){};
};

/**
Expand Down
2 changes: 2 additions & 0 deletions src/libYARP_dev/src/yarp/dev/ImplementTorqueControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,6 +98,7 @@ bool ImplementTorqueControl::setMotorTorqueParams(int j, const yarp::dev::Motor
castToMapper(helper)->viscousNeg_user2raw(params.viscousNeg, j, params_raw.viscousNeg, k);
castToMapper(helper)->coulombPos_user2raw(params.coulombPos, j, params_raw.coulombPos, k);
castToMapper(helper)->coulombNeg_user2raw(params.coulombNeg, j, params_raw.coulombNeg, k);
castToMapper(helper)->velocityThres_user2raw(params.velocityThres, j, params_raw.velocityThres, k);

return iTorqueRaw->setMotorTorqueParamsRaw(k, params_raw);
}
Expand All @@ -122,6 +123,7 @@ bool ImplementTorqueControl::getMotorTorqueParams(int j, yarp::dev::MotorTorque
castToMapper(helper)->viscousNeg_raw2user(params_raw.viscousNeg, k, (*params).viscousNeg, tmp_j);
castToMapper(helper)->coulombPos_raw2user(params_raw.coulombPos, k, (*params).coulombPos, tmp_j);
castToMapper(helper)->coulombNeg_raw2user(params_raw.coulombNeg, k, (*params).coulombNeg, tmp_j);
castToMapper(helper)->velocityThres_raw2user(params_raw.velocityThres, k, (*params).velocityThres, tmp_j);
}
return b;
}
Expand Down
2 changes: 2 additions & 0 deletions src/libYARP_dev/src/yarp/dev/tests/ITorqueControlTest.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@ namespace yarp::dev::tests
params.viscousNeg = 0.6;
params.coulombPos = 0.7;
params.coulombNeg = 0.8;
params.velocityThres = 0.9;

Check warning on line 37 in src/libYARP_dev/src/yarp/dev/tests/ITorqueControlTest.h

View check run for this annotation

Codacy Production / Codacy Static Code Analysis

src/libYARP_dev/src/yarp/dev/tests/ITorqueControlTest.h#L37 

0.9 is a magic number; consider replacing it with a named constant
b= itrq->setMotorTorqueParams(0, params);
CHECK(b);

Expand All @@ -48,6 +49,7 @@ namespace yarp::dev::tests
CHECK(res.viscousNeg == 0.6); // interface seems functional
CHECK(res.coulombPos == 0.7); // interface seems functional
CHECK(res.coulombNeg == 0.8); // interface seems functional
CHECK(res.velocityThres == 0.9); // interface seems functional

Check warning on line 52 in src/libYARP_dev/src/yarp/dev/tests/ITorqueControlTest.h

View check run for this annotation

Codacy Production / Codacy Static Code Analysis

src/libYARP_dev/src/yarp/dev/tests/ITorqueControlTest.h#L52 

0.9 is a magic number; consider replacing it with a named constant
}
}

Expand Down
5 changes: 5 additions & 0 deletions src/yarpmotorgui/piddlg.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,7 @@
#define TORQUE_VISCOUSNEG 15
#define TORQUE_COULOMBPOS 16
#define TORQUE_COULOMBNEG 17
#define VELOCITY_THRESHOLD 18

#define CURRENT_KP 0
#define CURRENT_KD 1
Expand Down Expand Up @@ -200,6 +201,9 @@ void PidDlg::initTorque(Pid myPid, MotorTorqueParameters TrqParam)
ui->tableTorque->item(TORQUE_COULOMBNEG, 0)->setText(QString("%1").arg((double)TrqParam.coulombNeg));
ui->tableTorque->item(TORQUE_COULOMBNEG, 1)->setText(QString("%1").arg((double)TrqParam.coulombNeg));

ui->tableTorque->item(VELOCITY_THRESHOLD, 0)->setText(QString("%1").arg((double)TrqParam.velocityThres));
ui->tableTorque->item(VELOCITY_THRESHOLD, 1)->setText(QString("%1").arg((double)TrqParam.velocityThres));

ui->tableTorque->item(TORQUE_KI,0)->setText(QString("%1").arg((double)myPid.ki));
ui->tableTorque->item(TORQUE_KI,1)->setText(QString("%1").arg((double)myPid.ki));

Expand Down Expand Up @@ -410,6 +414,7 @@ void PidDlg::onSend()
newMotorTorqueParams.viscousNeg = ui->tableTorque->item(TORQUE_VISCOUSNEG,1)->text().toDouble();
newMotorTorqueParams.coulombPos = ui->tableTorque->item(TORQUE_COULOMBPOS,1)->text().toDouble();
newMotorTorqueParams.coulombNeg = ui->tableTorque->item(TORQUE_COULOMBNEG,1)->text().toDouble();
newMotorTorqueParams.velocityThres = ui->tableTorque->item(VELOCITY_THRESHOLD,1)->text().toDouble();
newPid.ki = ui->tableTorque->item(TORQUE_KI,1)->text().toDouble();
newPid.scale = ui->tableTorque->item(TORQUE_SCALE,1)->text().toDouble();
newPid.offset = ui->tableTorque->item(TORQUE_OFFSET,1)->text().toDouble();
Expand Down
5 changes: 5 additions & 0 deletions src/yarpmotorgui/piddlg.ui
View file
Original file line number Diff line number Diff line change
Expand Up @@ -936,6 +936,11 @@
<string>Torque CoulombNeg</string>
</property>
</row>
<row>
<property name="text">
<string>Velocity Threshold</string>
</property>
</row>
<column>
<property name="text">
<string>Current</string>
Expand Down

0 comments on commit 81b7f3c

Please sign in to comment.