Skip to content

Commit

Permalink
Merge pull request #1483 from rdiankov/updateForTorqueLimit
Browse files Browse the repository at this point in the history 
Remove back-emf code for torque limit calculation and improving torque limit calculation codes.
  • Loading branch information
@rdiankov
rdiankov authored Dec 27, 2024
2 parents 522410a + b2391da commit 0edd958
Show file tree
Hide file tree
Showing 4 changed files with 76 additions and 166 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@ set( CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS TRUE )

# Define here the needed parameters
set (OPENRAVE_VERSION_MAJOR 0)
set (OPENRAVE_VERSION_MINOR 160)
set (OPENRAVE_VERSION_MINOR 161)
set (OPENRAVE_VERSION_PATCH 0)
set (OPENRAVE_VERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR}.${OPENRAVE_VERSION_PATCH})
set (OPENRAVE_SOVERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR})
Expand Down
6 changes: 6 additions & 0 deletions docs/source/changelog.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,12 @@
ChangeLog
#########

Version 0.161.0
===============

- Remove the code for back electromotive force from torque limit calculation APIs.
- Add common utility function for torque limit calculation.

Version 0.160.0
===============

Expand Down
10 changes: 10 additions & 0 deletions include/openrave/kinbody.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2200,6 +2200,16 @@ class OPENRAVE_API KinBody : public InterfaceBase
/// \brief Return the velocity of the specified joint axis only.
dReal _GetVelocity(int axis, const std::pair<Vector,Vector>&linkparentvelocity, const std::pair<Vector,Vector>&linkchildvelocity) const;

/// \brief compute torque limit from speed torque points.
/// \param[in] iaxis : index of axis.
/// \param[in] electricMotorActuatorInfo : electrical motor actuator info.
/// \param[in] vSpeedTorquePoints : vector of pairs of speed and torque, which is coming from electrical motor actuator info.
/// \param[in] fDefaultTorqueLimit : if vSpeedTorquePoints is empty, this value is used.
dReal _GetTorqueLimitFromSpeedTorquePoints(const int iaxis,
const ElectricMotorActuatorInfo& electricMotorActuatorInfo,
const std::vector<std::pair<dReal, dReal> >& vSpeedTorquePoints,
const dReal fDefaultTorqueLimit) const;

boost::array<dReal,3> _doflastsetvalues; ///< the last set value by the kinbody (_voffsets not applied). For revolute joints that have a range greater than 2*pi, it is only possible to recover the joint value from the link positions mod 2*pi. In order to recover the branch, multiplies of 2*pi are added/subtracted to this value that is closest to _doflastsetvalues. For circular joints, the last set value can be ignored since they always return a value from [-pi,pi)

private:
Expand Down
224 changes: 59 additions & 165 deletions src/libopenrave/kinbodyjoint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1907,49 +1907,63 @@ void KinBody::Joint::AddTorque(const std::vector<dReal>& pTorques)
GetParent()->GetEnv()->GetPhysicsEngine()->AddJointTorque(shared_from_this(), pTorques);
}

dReal KinBody::Joint::_GetTorqueLimitFromSpeedTorquePoints(const int iaxis,
const ElectricMotorActuatorInfo& electricMotorActuatorInfo,
const std::vector<std::pair<dReal, dReal> >& vSpeedTorquePoints,
const dReal fDefaultTorqueLimit) const
{
if( vSpeedTorquePoints.size() > 0 ) {
const std::pair<dReal, dReal>& firstSpeedTorquePoint = vSpeedTorquePoints[0];
if( vSpeedTorquePoints.size() == 1 ) {
// doesn't matter what the velocity is
return firstSpeedTorquePoint.second*electricMotorActuatorInfo.gear_ratio;
}

const dReal rawvelocity = GetVelocity(iaxis);
const dReal velocity = RaveFabs(rawvelocity);
dReal revolutionsPerSecond = electricMotorActuatorInfo.gear_ratio * velocity;
if( IsRevolute(iaxis) ) {
revolutionsPerSecond /= 2*M_PI;
}

if( revolutionsPerSecond <= firstSpeedTorquePoint.first ) {
return firstSpeedTorquePoint.second*electricMotorActuatorInfo.gear_ratio;
}

for(size_t i = 1; i < vSpeedTorquePoints.size(); ++i) {
const std::pair<dReal, dReal>& speedTorquePoint1 = vSpeedTorquePoints[i];
if( revolutionsPerSecond <= speedTorquePoint1.first ) {
// linearly interpolate to get the desired torque
const std::pair<dReal, dReal>& speedTorquePoint0 = vSpeedTorquePoints[i-1];
const dReal rps0 = speedTorquePoint0.first;
const dReal torque0 = speedTorquePoint0.second;
const dReal rps1 = speedTorquePoint1.first;
const dReal torque1 = speedTorquePoint1.second;
if( rps1 - rps0 <= g_fEpsilonLinear ) {
return torque1*electricMotorActuatorInfo.gear_ratio;
}
else {
return ((revolutionsPerSecond - rps0)/(rps1 - rps0)*(torque1-torque0) + torque0)*electricMotorActuatorInfo.gear_ratio;
}
}
}

// revolutionsPerSecond is huge, return the last point
return vSpeedTorquePoints.back().second*electricMotorActuatorInfo.gear_ratio;
}
else {
return fDefaultTorqueLimit*electricMotorActuatorInfo.gear_ratio;
}
}

dReal KinBody::Joint::GetMaxTorque(int iaxis) const
{
if( !_info._infoElectricMotor ) {
return _info._vmaxtorque.at(iaxis);
}
else {
if( _info._infoElectricMotor->max_speed_torque_points.size() > 0 ) {
if( _info._infoElectricMotor->max_speed_torque_points.size() == 1 ) {
// doesn't matter what the velocity is
return _info._infoElectricMotor->max_speed_torque_points.at(0).second*_info._infoElectricMotor->gear_ratio;
}

dReal velocity = RaveFabs(GetVelocity(iaxis));
dReal revolutionsPerSecond = _info._infoElectricMotor->gear_ratio * velocity;
if( IsRevolute(iaxis) ) {
revolutionsPerSecond /= 2*M_PI;
}

if( revolutionsPerSecond <= _info._infoElectricMotor->max_speed_torque_points.at(0).first ) {
return _info._infoElectricMotor->max_speed_torque_points.at(0).second*_info._infoElectricMotor->gear_ratio;
}

for(size_t i = 1; i < _info._infoElectricMotor->max_speed_torque_points.size(); ++i) {
if( revolutionsPerSecond <= _info._infoElectricMotor->max_speed_torque_points.at(i).first ) {
// linearly interpolate to get the desired torque
dReal rps0 = _info._infoElectricMotor->max_speed_torque_points.at(i-1).first;
dReal torque0 = _info._infoElectricMotor->max_speed_torque_points.at(i-1).second;
dReal rps1 = _info._infoElectricMotor->max_speed_torque_points.at(i).first;
dReal torque1 = _info._infoElectricMotor->max_speed_torque_points.at(i).second;
if( rps1 - rps0 <= g_fEpsilonLinear ) {
return torque1*_info._infoElectricMotor->gear_ratio;
}

return ((revolutionsPerSecond - rps0)/(rps1 - rps0)*(torque1-torque0) + torque0)*_info._infoElectricMotor->gear_ratio;
}
}

// revolutionsPerSecond is huge, return the last point
return _info._infoElectricMotor->max_speed_torque_points.back().second*_info._infoElectricMotor->gear_ratio;
}
else {
return _info._infoElectricMotor->max_instantaneous_torque*_info._infoElectricMotor->gear_ratio;
}
const ElectricMotorActuatorInfo& electricMotorActuatorInfo = *_info._infoElectricMotor;
return _GetTorqueLimitFromSpeedTorquePoints(iaxis, electricMotorActuatorInfo, electricMotorActuatorInfo.max_speed_torque_points, electricMotorActuatorInfo.max_instantaneous_torque);
}
}

Expand All @@ -1959,70 +1973,10 @@ std::pair<dReal, dReal> KinBody::Joint::GetInstantaneousTorqueLimits(int iaxis)
return std::make_pair(-_info._vmaxtorque.at(iaxis), _info._vmaxtorque.at(iaxis));
}
else {
if( _info._infoElectricMotor->max_speed_torque_points.size() > 0 ) {
dReal fMaxTorqueAtZeroSpeed = _info._infoElectricMotor->max_speed_torque_points.at(0).second*_info._infoElectricMotor->gear_ratio;
if( _info._infoElectricMotor->max_speed_torque_points.size() == 1 ) {
// doesn't matter what the velocity is
return std::make_pair(-fMaxTorqueAtZeroSpeed, fMaxTorqueAtZeroSpeed);
}

dReal rawvelocity = GetVelocity(iaxis);
dReal velocity = RaveFabs(rawvelocity);
dReal revolutionsPerSecond = _info._infoElectricMotor->gear_ratio * velocity;
if( IsRevolute(iaxis) ) {
revolutionsPerSecond /= 2*M_PI;
}

if( revolutionsPerSecond <= _info._infoElectricMotor->max_speed_torque_points.at(0).first ) {
return std::make_pair(-fMaxTorqueAtZeroSpeed, fMaxTorqueAtZeroSpeed);
}

for(size_t i = 1; i < _info._infoElectricMotor->max_speed_torque_points.size(); ++i) {
if( revolutionsPerSecond <= _info._infoElectricMotor->max_speed_torque_points.at(i).first ) {
// linearly interpolate to get the desired torque
dReal rps0 = _info._infoElectricMotor->max_speed_torque_points.at(i-1).first;
dReal torque0 = _info._infoElectricMotor->max_speed_torque_points.at(i-1).second;
dReal rps1 = _info._infoElectricMotor->max_speed_torque_points.at(i).first;
dReal torque1 = _info._infoElectricMotor->max_speed_torque_points.at(i).second;

dReal finterpolatedtorque;
if( rps1 - rps0 <= g_fEpsilonLinear ) {
finterpolatedtorque = torque1*_info._infoElectricMotor->gear_ratio;
}
else {
finterpolatedtorque = ((revolutionsPerSecond - rps0)/(rps1 - rps0)*(torque1-torque0) + torque0)*_info._infoElectricMotor->gear_ratio;
}

// due to back emf, the deceleration magnitude is less than acceleration?
if (abs(rawvelocity) < 1.0/360) {
return std::make_pair(-finterpolatedtorque, finterpolatedtorque);
}
else if( rawvelocity > 0 ) {
return std::make_pair(-0.9*finterpolatedtorque, finterpolatedtorque);
}
else {
return std::make_pair(-finterpolatedtorque, 0.9*finterpolatedtorque);
}
}
}

// due to back emf, the deceleration magnitude is less than acceleration?
// revolutionsPerSecond is huge, return the last point
dReal f = _info._infoElectricMotor->max_speed_torque_points.back().second*_info._infoElectricMotor->gear_ratio;
if (abs(rawvelocity) < 1.0/360) {
return std::make_pair(-f, f);
}
else if( rawvelocity > 0 ) {
return std::make_pair(-0.9*f, f);
}
else {
return std::make_pair(-f, 0.9*f);
}
}
else {
dReal f = _info._infoElectricMotor->max_instantaneous_torque*_info._infoElectricMotor->gear_ratio;
return std::make_pair(-f, f);
}
const ElectricMotorActuatorInfo& electricMotorActuatorInfo = *_info._infoElectricMotor;
const dReal fLimit = _GetTorqueLimitFromSpeedTorquePoints(iaxis, electricMotorActuatorInfo, electricMotorActuatorInfo.max_speed_torque_points, electricMotorActuatorInfo.max_instantaneous_torque);
// TODO : if we'll need to consider the back electromotive force (emf), re-think the formulation and then add the corrsponding parameters to ElectricMotorActuatorInfo
return std::make_pair(-fLimit, fLimit);
}
}

Expand All @@ -2032,70 +1986,10 @@ std::pair<dReal, dReal> KinBody::Joint::GetNominalTorqueLimits(int iaxis) const
return std::make_pair(-_info._vmaxtorque.at(iaxis), _info._vmaxtorque.at(iaxis));
}
else {
if( _info._infoElectricMotor->nominal_speed_torque_points.size() > 0 ) {
dReal fMaxTorqueAtZeroSpeed = _info._infoElectricMotor->nominal_speed_torque_points.at(0).second*_info._infoElectricMotor->gear_ratio;
if( _info._infoElectricMotor->nominal_speed_torque_points.size() == 1 ) {
// doesn't matter what the velocity is
return std::make_pair(-fMaxTorqueAtZeroSpeed, fMaxTorqueAtZeroSpeed);
}

dReal rawvelocity = GetVelocity(iaxis);
dReal velocity = RaveFabs(rawvelocity);
dReal revolutionsPerSecond = _info._infoElectricMotor->gear_ratio * velocity;
if( IsRevolute(iaxis) ) {
revolutionsPerSecond /= 2*M_PI;
}

if( revolutionsPerSecond <= _info._infoElectricMotor->nominal_speed_torque_points.at(0).first ) {
return std::make_pair(-fMaxTorqueAtZeroSpeed, fMaxTorqueAtZeroSpeed);
}

for(size_t i = 1; i < _info._infoElectricMotor->nominal_speed_torque_points.size(); ++i) {
if( revolutionsPerSecond <= _info._infoElectricMotor->nominal_speed_torque_points.at(i).first ) {
// linearly interpolate to get the desired torque
dReal rps0 = _info._infoElectricMotor->nominal_speed_torque_points.at(i-1).first;
dReal torque0 = _info._infoElectricMotor->nominal_speed_torque_points.at(i-1).second;
dReal rps1 = _info._infoElectricMotor->nominal_speed_torque_points.at(i).first;
dReal torque1 = _info._infoElectricMotor->nominal_speed_torque_points.at(i).second;

dReal finterpolatedtorque;
if( rps1 - rps0 <= g_fEpsilonLinear ) {
finterpolatedtorque = torque1*_info._infoElectricMotor->gear_ratio;
}
else {
finterpolatedtorque = ((revolutionsPerSecond - rps0)/(rps1 - rps0)*(torque1-torque0) + torque0)*_info._infoElectricMotor->gear_ratio;
}

// due to back emf, the deceleration magnitude is less than acceleration?
if (abs(rawvelocity) < 1.0/360) {
return std::make_pair(-finterpolatedtorque, finterpolatedtorque);
}
else if( rawvelocity > 0 ) {
return std::make_pair(-0.9*finterpolatedtorque, finterpolatedtorque);
}
else {
return std::make_pair(-finterpolatedtorque, 0.9*finterpolatedtorque);
}
}
}

// due to back emf, the deceleration magnitude is less than acceleration?
// revolutionsPerSecond is huge, return the last point
dReal f = _info._infoElectricMotor->nominal_speed_torque_points.back().second*_info._infoElectricMotor->gear_ratio;
if (abs(rawvelocity) < 1.0/360) {
return std::make_pair(-f, f);
}
else if( rawvelocity > 0 ) {
return std::make_pair(-0.9*f, f);
}
else {
return std::make_pair(-f, 0.9*f);
}
}
else {
dReal f = _info._infoElectricMotor->nominal_torque*_info._infoElectricMotor->gear_ratio;
return std::make_pair(-f, f);
}
const ElectricMotorActuatorInfo& electricMotorActuatorInfo = *_info._infoElectricMotor;
const dReal fLimit = _GetTorqueLimitFromSpeedTorquePoints(iaxis, electricMotorActuatorInfo, electricMotorActuatorInfo.nominal_speed_torque_points, electricMotorActuatorInfo.nominal_torque);
// TODO : if we'll need to consider the back electromotive force (emf), re-think the formulation and then add the corrsponding parameters to ElectricMotorActuatorInfo
return std::make_pair(-fLimit, fLimit);
}
}

Expand Down

0 comments on commit 0edd958

Please sign in to comment.