From cf23a772865b1773355b8447ed2ad08a3bfa91f2 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Sat, 21 Dec 2024 16:51:23 +0900 Subject: [PATCH 1/8] store const reference first --- src/libopenrave/kinbodyjoint.cpp | 64 +++++++++++++++++--------------- 1 file changed, 34 insertions(+), 30 deletions(-) diff --git a/src/libopenrave/kinbodyjoint.cpp b/src/libopenrave/kinbodyjoint.cpp index 519529abce..7c9c4d6ab2 100644 --- a/src/libopenrave/kinbodyjoint.cpp +++ b/src/libopenrave/kinbodyjoint.cpp @@ -1959,38 +1959,40 @@ std::pair 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 ) { + const ElectricMotorActuatorInfo& electricMotorActuatorInfo = *_info._infoElectricMotor; + const std::vector >& vSpeedTorquePoints = electricMotorActuatorInfo.max_speed_torque_points; + if( vSpeedTorquePoints.size() > 0 ) { + dReal fMaxTorqueAtZeroSpeed = vSpeedTorquePoints.at(0).second*electricMotorActuatorInfo.gear_ratio; + if( vSpeedTorquePoints.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; + dReal revolutionsPerSecond = electricMotorActuatorInfo.gear_ratio * velocity; if( IsRevolute(iaxis) ) { revolutionsPerSecond /= 2*M_PI; } - if( revolutionsPerSecond <= _info._infoElectricMotor->max_speed_torque_points.at(0).first ) { + if( revolutionsPerSecond <= vSpeedTorquePoints.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 ) { + for(size_t i = 1; i < vSpeedTorquePoints.size(); ++i) { + if( revolutionsPerSecond <= vSpeedTorquePoints.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 rps0 = vSpeedTorquePoints.at(i-1).first; + dReal torque0 = vSpeedTorquePoints.at(i-1).second; + dReal rps1 = vSpeedTorquePoints.at(i).first; + dReal torque1 = vSpeedTorquePoints.at(i).second; dReal finterpolatedtorque; if( rps1 - rps0 <= g_fEpsilonLinear ) { - finterpolatedtorque = torque1*_info._infoElectricMotor->gear_ratio; + finterpolatedtorque = torque1*electricMotorActuatorInfo.gear_ratio; } else { - finterpolatedtorque = ((revolutionsPerSecond - rps0)/(rps1 - rps0)*(torque1-torque0) + torque0)*_info._infoElectricMotor->gear_ratio; + finterpolatedtorque = ((revolutionsPerSecond - rps0)/(rps1 - rps0)*(torque1-torque0) + torque0)*electricMotorActuatorInfo.gear_ratio; } // due to back emf, the deceleration magnitude is less than acceleration? @@ -2008,7 +2010,7 @@ std::pair KinBody::Joint::GetInstantaneousTorqueLimits(int iaxis) // 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; + dReal f = vSpeedTorquePoints.back().second*electricMotorActuatorInfo.gear_ratio; if (abs(rawvelocity) < 1.0/360) { return std::make_pair(-f, f); } @@ -2020,7 +2022,7 @@ std::pair KinBody::Joint::GetInstantaneousTorqueLimits(int iaxis) } } else { - dReal f = _info._infoElectricMotor->max_instantaneous_torque*_info._infoElectricMotor->gear_ratio; + dReal f = electricMotorActuatorInfo.max_instantaneous_torque*electricMotorActuatorInfo.gear_ratio; return std::make_pair(-f, f); } } @@ -2032,38 +2034,40 @@ std::pair 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 ) { + const ElectricMotorActuatorInfo& electricMotorActuatorInfo = *_info._infoElectricMotor; + const std::vector >& vSpeedTorquePoints = electricMotorActuatorInfo.nominal_speed_torque_points; + if( vSpeedTorquePoints.size() > 0 ) { + dReal fMaxTorqueAtZeroSpeed = vSpeedTorquePoints.at(0).second*electricMotorActuatorInfo.gear_ratio; + if( vSpeedTorquePoints.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; + dReal revolutionsPerSecond = electricMotorActuatorInfo.gear_ratio * velocity; if( IsRevolute(iaxis) ) { revolutionsPerSecond /= 2*M_PI; } - if( revolutionsPerSecond <= _info._infoElectricMotor->nominal_speed_torque_points.at(0).first ) { + if( revolutionsPerSecond <= vSpeedTorquePoints.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 ) { + for(size_t i = 1; i < vSpeedTorquePoints.size(); ++i) { + if( revolutionsPerSecond <= vSpeedTorquePoints.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 rps0 = vSpeedTorquePoints.at(i-1).first; + dReal torque0 = vSpeedTorquePoints.at(i-1).second; + dReal rps1 = vSpeedTorquePoints.at(i).first; + dReal torque1 = vSpeedTorquePoints.at(i).second; dReal finterpolatedtorque; if( rps1 - rps0 <= g_fEpsilonLinear ) { - finterpolatedtorque = torque1*_info._infoElectricMotor->gear_ratio; + finterpolatedtorque = torque1*electricMotorActuatorInfo.gear_ratio; } else { - finterpolatedtorque = ((revolutionsPerSecond - rps0)/(rps1 - rps0)*(torque1-torque0) + torque0)*_info._infoElectricMotor->gear_ratio; + finterpolatedtorque = ((revolutionsPerSecond - rps0)/(rps1 - rps0)*(torque1-torque0) + torque0)*electricMotorActuatorInfo.gear_ratio; } // due to back emf, the deceleration magnitude is less than acceleration? @@ -2081,7 +2085,7 @@ std::pair KinBody::Joint::GetNominalTorqueLimits(int iaxis) const // 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; + dReal f = vSpeedTorquePoints.back().second*electricMotorActuatorInfo.gear_ratio; if (abs(rawvelocity) < 1.0/360) { return std::make_pair(-f, f); } @@ -2093,7 +2097,7 @@ std::pair KinBody::Joint::GetNominalTorqueLimits(int iaxis) const } } else { - dReal f = _info._infoElectricMotor->nominal_torque*_info._infoElectricMotor->gear_ratio; + dReal f = electricMotorActuatorInfo.nominal_torque*electricMotorActuatorInfo.gear_ratio; return std::make_pair(-f, f); } } From c44e1d9481a6230560f1d556ef0b543f41365b9e Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Sat, 21 Dec 2024 17:36:24 +0900 Subject: [PATCH 2/8] instead of computing abs again, use 'velocity' instead of 'rawvelocity'. in addition, use const as much as possible --- src/libopenrave/kinbodyjoint.cpp | 44 ++++++++++++++++---------------- 1 file changed, 22 insertions(+), 22 deletions(-) diff --git a/src/libopenrave/kinbodyjoint.cpp b/src/libopenrave/kinbodyjoint.cpp index 7c9c4d6ab2..e64c92c6b5 100644 --- a/src/libopenrave/kinbodyjoint.cpp +++ b/src/libopenrave/kinbodyjoint.cpp @@ -1962,14 +1962,14 @@ std::pair KinBody::Joint::GetInstantaneousTorqueLimits(int iaxis) const ElectricMotorActuatorInfo& electricMotorActuatorInfo = *_info._infoElectricMotor; const std::vector >& vSpeedTorquePoints = electricMotorActuatorInfo.max_speed_torque_points; if( vSpeedTorquePoints.size() > 0 ) { - dReal fMaxTorqueAtZeroSpeed = vSpeedTorquePoints.at(0).second*electricMotorActuatorInfo.gear_ratio; + const dReal fMaxTorqueAtZeroSpeed = vSpeedTorquePoints.at(0).second*electricMotorActuatorInfo.gear_ratio; if( vSpeedTorquePoints.size() == 1 ) { // doesn't matter what the velocity is return std::make_pair(-fMaxTorqueAtZeroSpeed, fMaxTorqueAtZeroSpeed); } - dReal rawvelocity = GetVelocity(iaxis); - dReal velocity = RaveFabs(rawvelocity); + const dReal rawvelocity = GetVelocity(iaxis); + const dReal velocity = RaveFabs(rawvelocity); dReal revolutionsPerSecond = electricMotorActuatorInfo.gear_ratio * velocity; if( IsRevolute(iaxis) ) { revolutionsPerSecond /= 2*M_PI; @@ -1982,10 +1982,10 @@ std::pair KinBody::Joint::GetInstantaneousTorqueLimits(int iaxis) for(size_t i = 1; i < vSpeedTorquePoints.size(); ++i) { if( revolutionsPerSecond <= vSpeedTorquePoints.at(i).first ) { // linearly interpolate to get the desired torque - dReal rps0 = vSpeedTorquePoints.at(i-1).first; - dReal torque0 = vSpeedTorquePoints.at(i-1).second; - dReal rps1 = vSpeedTorquePoints.at(i).first; - dReal torque1 = vSpeedTorquePoints.at(i).second; + const dReal rps0 = vSpeedTorquePoints.at(i-1).first; + const dReal torque0 = vSpeedTorquePoints.at(i-1).second; + const dReal rps1 = vSpeedTorquePoints.at(i).first; + const dReal torque1 = vSpeedTorquePoints.at(i).second; dReal finterpolatedtorque; if( rps1 - rps0 <= g_fEpsilonLinear ) { @@ -1996,7 +1996,7 @@ std::pair KinBody::Joint::GetInstantaneousTorqueLimits(int iaxis) } // due to back emf, the deceleration magnitude is less than acceleration? - if (abs(rawvelocity) < 1.0/360) { + if (velocity < 1.0/360) { return std::make_pair(-finterpolatedtorque, finterpolatedtorque); } else if( rawvelocity > 0 ) { @@ -2010,8 +2010,8 @@ std::pair KinBody::Joint::GetInstantaneousTorqueLimits(int iaxis) // due to back emf, the deceleration magnitude is less than acceleration? // revolutionsPerSecond is huge, return the last point - dReal f = vSpeedTorquePoints.back().second*electricMotorActuatorInfo.gear_ratio; - if (abs(rawvelocity) < 1.0/360) { + const dReal f = vSpeedTorquePoints.back().second*electricMotorActuatorInfo.gear_ratio; + if (velocity < 1.0/360) { return std::make_pair(-f, f); } else if( rawvelocity > 0 ) { @@ -2022,7 +2022,7 @@ std::pair KinBody::Joint::GetInstantaneousTorqueLimits(int iaxis) } } else { - dReal f = electricMotorActuatorInfo.max_instantaneous_torque*electricMotorActuatorInfo.gear_ratio; + const dReal f = electricMotorActuatorInfo.max_instantaneous_torque*electricMotorActuatorInfo.gear_ratio; return std::make_pair(-f, f); } } @@ -2037,14 +2037,14 @@ std::pair KinBody::Joint::GetNominalTorqueLimits(int iaxis) const const ElectricMotorActuatorInfo& electricMotorActuatorInfo = *_info._infoElectricMotor; const std::vector >& vSpeedTorquePoints = electricMotorActuatorInfo.nominal_speed_torque_points; if( vSpeedTorquePoints.size() > 0 ) { - dReal fMaxTorqueAtZeroSpeed = vSpeedTorquePoints.at(0).second*electricMotorActuatorInfo.gear_ratio; + const dReal fMaxTorqueAtZeroSpeed = vSpeedTorquePoints.at(0).second*electricMotorActuatorInfo.gear_ratio; if( vSpeedTorquePoints.size() == 1 ) { // doesn't matter what the velocity is return std::make_pair(-fMaxTorqueAtZeroSpeed, fMaxTorqueAtZeroSpeed); } - dReal rawvelocity = GetVelocity(iaxis); - dReal velocity = RaveFabs(rawvelocity); + const dReal rawvelocity = GetVelocity(iaxis); + const dReal velocity = RaveFabs(rawvelocity); dReal revolutionsPerSecond = electricMotorActuatorInfo.gear_ratio * velocity; if( IsRevolute(iaxis) ) { revolutionsPerSecond /= 2*M_PI; @@ -2057,10 +2057,10 @@ std::pair KinBody::Joint::GetNominalTorqueLimits(int iaxis) const for(size_t i = 1; i < vSpeedTorquePoints.size(); ++i) { if( revolutionsPerSecond <= vSpeedTorquePoints.at(i).first ) { // linearly interpolate to get the desired torque - dReal rps0 = vSpeedTorquePoints.at(i-1).first; - dReal torque0 = vSpeedTorquePoints.at(i-1).second; - dReal rps1 = vSpeedTorquePoints.at(i).first; - dReal torque1 = vSpeedTorquePoints.at(i).second; + const dReal rps0 = vSpeedTorquePoints.at(i-1).first; + const dReal torque0 = vSpeedTorquePoints.at(i-1).second; + const dReal rps1 = vSpeedTorquePoints.at(i).first; + const dReal torque1 = vSpeedTorquePoints.at(i).second; dReal finterpolatedtorque; if( rps1 - rps0 <= g_fEpsilonLinear ) { @@ -2071,7 +2071,7 @@ std::pair KinBody::Joint::GetNominalTorqueLimits(int iaxis) const } // due to back emf, the deceleration magnitude is less than acceleration? - if (abs(rawvelocity) < 1.0/360) { + if (velocity < 1.0/360) { return std::make_pair(-finterpolatedtorque, finterpolatedtorque); } else if( rawvelocity > 0 ) { @@ -2085,8 +2085,8 @@ std::pair KinBody::Joint::GetNominalTorqueLimits(int iaxis) const // due to back emf, the deceleration magnitude is less than acceleration? // revolutionsPerSecond is huge, return the last point - dReal f = vSpeedTorquePoints.back().second*electricMotorActuatorInfo.gear_ratio; - if (abs(rawvelocity) < 1.0/360) { + const dReal f = vSpeedTorquePoints.back().second*electricMotorActuatorInfo.gear_ratio; + if (velocity < 1.0/360) { return std::make_pair(-f, f); } else if( rawvelocity > 0 ) { @@ -2097,7 +2097,7 @@ std::pair KinBody::Joint::GetNominalTorqueLimits(int iaxis) const } } else { - dReal f = electricMotorActuatorInfo.nominal_torque*electricMotorActuatorInfo.gear_ratio; + const dReal f = electricMotorActuatorInfo.nominal_torque*electricMotorActuatorInfo.gear_ratio; return std::make_pair(-f, f); } } From 63674294cafd4146f33f408477aa5ad5ad614dc9 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Wed, 25 Dec 2024 20:51:17 +0900 Subject: [PATCH 3/8] Remove back electromotive force (EMF) implemetation for now. - The values are hardcoded. - The implemented formula and theoretical background are not clear. - If we'll need it in future, clarify the theorecial formulation and add the parameter for it in electricMotorInfo. --- src/libopenrave/kinbodyjoint.cpp | 40 ++++---------------------------- 1 file changed, 4 insertions(+), 36 deletions(-) diff --git a/src/libopenrave/kinbodyjoint.cpp b/src/libopenrave/kinbodyjoint.cpp index e64c92c6b5..60477c8fcf 100644 --- a/src/libopenrave/kinbodyjoint.cpp +++ b/src/libopenrave/kinbodyjoint.cpp @@ -1996,30 +1996,14 @@ std::pair KinBody::Joint::GetInstantaneousTorqueLimits(int iaxis) } // due to back emf, the deceleration magnitude is less than acceleration? - if (velocity < 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); - } + return std::make_pair(-finterpolatedtorque, finterpolatedtorque); } } // due to back emf, the deceleration magnitude is less than acceleration? // revolutionsPerSecond is huge, return the last point const dReal f = vSpeedTorquePoints.back().second*electricMotorActuatorInfo.gear_ratio; - if (velocity < 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); - } + return std::make_pair(-f, f); } else { const dReal f = electricMotorActuatorInfo.max_instantaneous_torque*electricMotorActuatorInfo.gear_ratio; @@ -2071,30 +2055,14 @@ std::pair KinBody::Joint::GetNominalTorqueLimits(int iaxis) const } // due to back emf, the deceleration magnitude is less than acceleration? - if (velocity < 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); - } + return std::make_pair(-finterpolatedtorque, finterpolatedtorque); } } // due to back emf, the deceleration magnitude is less than acceleration? // revolutionsPerSecond is huge, return the last point const dReal f = vSpeedTorquePoints.back().second*electricMotorActuatorInfo.gear_ratio; - if (velocity < 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); - } + return std::make_pair(-f, f); } else { const dReal f = electricMotorActuatorInfo.nominal_torque*electricMotorActuatorInfo.gear_ratio; From 91431dee6fa1030ac9d5ed2191920dc9d9ee2d9f Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Wed, 25 Dec 2024 22:07:52 +0900 Subject: [PATCH 4/8] Store const reference, use const as much as possible --- src/libopenrave/kinbodyjoint.cpp | 40 ++++++++++++++++++-------------- 1 file changed, 22 insertions(+), 18 deletions(-) diff --git a/src/libopenrave/kinbodyjoint.cpp b/src/libopenrave/kinbodyjoint.cpp index 60477c8fcf..69e3bea55a 100644 --- a/src/libopenrave/kinbodyjoint.cpp +++ b/src/libopenrave/kinbodyjoint.cpp @@ -1913,42 +1913,46 @@ dReal KinBody::Joint::GetMaxTorque(int iaxis) const return _info._vmaxtorque.at(iaxis); } else { - if( _info._infoElectricMotor->max_speed_torque_points.size() > 0 ) { - if( _info._infoElectricMotor->max_speed_torque_points.size() == 1 ) { + const ElectricMotorActuatorInfo& electricMotorActuatorInfo = *_info._infoElectricMotor; + const std::vector >& vSpeedTorquePoints = electricMotorActuatorInfo.max_speed_torque_points; + if( vSpeedTorquePoints.size() > 0 ) { + if( vSpeedTorquePoints.size() == 1 ) { // doesn't matter what the velocity is - return _info._infoElectricMotor->max_speed_torque_points.at(0).second*_info._infoElectricMotor->gear_ratio; + return vSpeedTorquePoints.at(0).second*electricMotorActuatorInfo.gear_ratio; } - dReal velocity = RaveFabs(GetVelocity(iaxis)); - dReal revolutionsPerSecond = _info._infoElectricMotor->gear_ratio * velocity; + 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 <= _info._infoElectricMotor->max_speed_torque_points.at(0).first ) { - return _info._infoElectricMotor->max_speed_torque_points.at(0).second*_info._infoElectricMotor->gear_ratio; + if( revolutionsPerSecond <= vSpeedTorquePoints.at(0).first ) { + return vSpeedTorquePoints.at(0).second*electricMotorActuatorInfo.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 ) { + for(size_t i = 1; i < vSpeedTorquePoints.size(); ++i) { + if( revolutionsPerSecond <= vSpeedTorquePoints.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; + const dReal rps0 = vSpeedTorquePoints.at(i-1).first; + const dReal torque0 = vSpeedTorquePoints.at(i-1).second; + const dReal rps1 = vSpeedTorquePoints.at(i).first; + const dReal torque1 = vSpeedTorquePoints.at(i).second; if( rps1 - rps0 <= g_fEpsilonLinear ) { - return torque1*_info._infoElectricMotor->gear_ratio; + return torque1*electricMotorActuatorInfo.gear_ratio; + } + else { + return ((revolutionsPerSecond - rps0)/(rps1 - rps0)*(torque1-torque0) + torque0)*electricMotorActuatorInfo.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; + return vSpeedTorquePoints.back().second*electricMotorActuatorInfo.gear_ratio; } else { - return _info._infoElectricMotor->max_instantaneous_torque*_info._infoElectricMotor->gear_ratio; + return electricMotorActuatorInfo.max_instantaneous_torque*electricMotorActuatorInfo.gear_ratio; } } } From 2a15bdc98fe73a9f3c78c6562f4b986934b79265 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Wed, 25 Dec 2024 23:38:41 +0900 Subject: [PATCH 5/8] add utility function to reduce the duplicated torque limit calculation. --- include/openrave/kinbody.h | 10 ++ src/libopenrave/kinbodyjoint.cpp | 189 +++++++++---------------------- 2 files changed, 61 insertions(+), 138 deletions(-) diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h index f8791b02e9..b9156a1243 100644 --- a/include/openrave/kinbody.h +++ b/include/openrave/kinbody.h @@ -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&linkparentvelocity, const std::pair&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 >& vSpeedTorquePoints, + const dReal fDefaultTorqueLimit) const; + boost::array _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: diff --git a/src/libopenrave/kinbodyjoint.cpp b/src/libopenrave/kinbodyjoint.cpp index 69e3bea55a..3168d1e4eb 100644 --- a/src/libopenrave/kinbodyjoint.cpp +++ b/src/libopenrave/kinbodyjoint.cpp @@ -1907,53 +1907,60 @@ void KinBody::Joint::AddTorque(const std::vector& pTorques) GetParent()->GetEnv()->GetPhysicsEngine()->AddJointTorque(shared_from_this(), pTorques); } -dReal KinBody::Joint::GetMaxTorque(int iaxis) const +dReal KinBody::Joint::_GetTorqueLimitFromSpeedTorquePoints(const int iaxis, + const ElectricMotorActuatorInfo& electricMotorActuatorInfo, + const std::vector >& vSpeedTorquePoints, + const dReal fDefaultTorqueLimit) const { - if( !_info._infoElectricMotor ) { - return _info._vmaxtorque.at(iaxis); - } - else { - const ElectricMotorActuatorInfo& electricMotorActuatorInfo = *_info._infoElectricMotor; - const std::vector >& vSpeedTorquePoints = electricMotorActuatorInfo.max_speed_torque_points; - if( vSpeedTorquePoints.size() > 0 ) { - if( vSpeedTorquePoints.size() == 1 ) { - // doesn't matter what the velocity is - return vSpeedTorquePoints.at(0).second*electricMotorActuatorInfo.gear_ratio; - } + if( vSpeedTorquePoints.size() > 0 ) { + if( vSpeedTorquePoints.size() == 1 ) { + // doesn't matter what the velocity is + return vSpeedTorquePoints.at(0).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; - } + 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 <= vSpeedTorquePoints.at(0).first ) { - return vSpeedTorquePoints.at(0).second*electricMotorActuatorInfo.gear_ratio; - } + if( revolutionsPerSecond <= vSpeedTorquePoints.at(0).first ) { + return vSpeedTorquePoints.at(0).second*electricMotorActuatorInfo.gear_ratio; + } - for(size_t i = 1; i < vSpeedTorquePoints.size(); ++i) { - if( revolutionsPerSecond <= vSpeedTorquePoints.at(i).first ) { - // linearly interpolate to get the desired torque - const dReal rps0 = vSpeedTorquePoints.at(i-1).first; - const dReal torque0 = vSpeedTorquePoints.at(i-1).second; - const dReal rps1 = vSpeedTorquePoints.at(i).first; - const dReal torque1 = vSpeedTorquePoints.at(i).second; - if( rps1 - rps0 <= g_fEpsilonLinear ) { - return torque1*electricMotorActuatorInfo.gear_ratio; - } - else { - return ((revolutionsPerSecond - rps0)/(rps1 - rps0)*(torque1-torque0) + torque0)*electricMotorActuatorInfo.gear_ratio; - } + for(size_t i = 1; i < vSpeedTorquePoints.size(); ++i) { + if( revolutionsPerSecond <= vSpeedTorquePoints.at(i).first ) { + // linearly interpolate to get the desired torque + const dReal rps0 = vSpeedTorquePoints.at(i-1).first; + const dReal torque0 = vSpeedTorquePoints.at(i-1).second; + const dReal rps1 = vSpeedTorquePoints.at(i).first; + const dReal torque1 = vSpeedTorquePoints.at(i).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 electricMotorActuatorInfo.max_instantaneous_torque*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 { + const ElectricMotorActuatorInfo& electricMotorActuatorInfo = *_info._infoElectricMotor; + return _GetTorqueLimitFromSpeedTorquePoints(iaxis, electricMotorActuatorInfo, electricMotorActuatorInfo.max_speed_torque_points, electricMotorActuatorInfo.max_instantaneous_torque); } } @@ -1964,55 +1971,8 @@ std::pair KinBody::Joint::GetInstantaneousTorqueLimits(int iaxis) } else { const ElectricMotorActuatorInfo& electricMotorActuatorInfo = *_info._infoElectricMotor; - const std::vector >& vSpeedTorquePoints = electricMotorActuatorInfo.max_speed_torque_points; - if( vSpeedTorquePoints.size() > 0 ) { - const dReal fMaxTorqueAtZeroSpeed = vSpeedTorquePoints.at(0).second*electricMotorActuatorInfo.gear_ratio; - if( vSpeedTorquePoints.size() == 1 ) { - // doesn't matter what the velocity is - return std::make_pair(-fMaxTorqueAtZeroSpeed, fMaxTorqueAtZeroSpeed); - } - - 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 <= vSpeedTorquePoints.at(0).first ) { - return std::make_pair(-fMaxTorqueAtZeroSpeed, fMaxTorqueAtZeroSpeed); - } - - for(size_t i = 1; i < vSpeedTorquePoints.size(); ++i) { - if( revolutionsPerSecond <= vSpeedTorquePoints.at(i).first ) { - // linearly interpolate to get the desired torque - const dReal rps0 = vSpeedTorquePoints.at(i-1).first; - const dReal torque0 = vSpeedTorquePoints.at(i-1).second; - const dReal rps1 = vSpeedTorquePoints.at(i).first; - const dReal torque1 = vSpeedTorquePoints.at(i).second; - - dReal finterpolatedtorque; - if( rps1 - rps0 <= g_fEpsilonLinear ) { - finterpolatedtorque = torque1*electricMotorActuatorInfo.gear_ratio; - } - else { - finterpolatedtorque = ((revolutionsPerSecond - rps0)/(rps1 - rps0)*(torque1-torque0) + torque0)*electricMotorActuatorInfo.gear_ratio; - } - - // due to back emf, the deceleration magnitude is less than acceleration? - return std::make_pair(-finterpolatedtorque, finterpolatedtorque); - } - } - - // due to back emf, the deceleration magnitude is less than acceleration? - // revolutionsPerSecond is huge, return the last point - const dReal f = vSpeedTorquePoints.back().second*electricMotorActuatorInfo.gear_ratio; - return std::make_pair(-f, f); - } - else { - const dReal f = electricMotorActuatorInfo.max_instantaneous_torque*electricMotorActuatorInfo.gear_ratio; - return std::make_pair(-f, f); - } + const dReal fLimit = _GetTorqueLimitFromSpeedTorquePoints(iaxis, electricMotorActuatorInfo, electricMotorActuatorInfo.max_speed_torque_points, electricMotorActuatorInfo.max_instantaneous_torque); + return std::make_pair(-fLimit, fLimit); } } @@ -2023,55 +1983,8 @@ std::pair KinBody::Joint::GetNominalTorqueLimits(int iaxis) const } else { const ElectricMotorActuatorInfo& electricMotorActuatorInfo = *_info._infoElectricMotor; - const std::vector >& vSpeedTorquePoints = electricMotorActuatorInfo.nominal_speed_torque_points; - if( vSpeedTorquePoints.size() > 0 ) { - const dReal fMaxTorqueAtZeroSpeed = vSpeedTorquePoints.at(0).second*electricMotorActuatorInfo.gear_ratio; - if( vSpeedTorquePoints.size() == 1 ) { - // doesn't matter what the velocity is - return std::make_pair(-fMaxTorqueAtZeroSpeed, fMaxTorqueAtZeroSpeed); - } - - 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 <= vSpeedTorquePoints.at(0).first ) { - return std::make_pair(-fMaxTorqueAtZeroSpeed, fMaxTorqueAtZeroSpeed); - } - - for(size_t i = 1; i < vSpeedTorquePoints.size(); ++i) { - if( revolutionsPerSecond <= vSpeedTorquePoints.at(i).first ) { - // linearly interpolate to get the desired torque - const dReal rps0 = vSpeedTorquePoints.at(i-1).first; - const dReal torque0 = vSpeedTorquePoints.at(i-1).second; - const dReal rps1 = vSpeedTorquePoints.at(i).first; - const dReal torque1 = vSpeedTorquePoints.at(i).second; - - dReal finterpolatedtorque; - if( rps1 - rps0 <= g_fEpsilonLinear ) { - finterpolatedtorque = torque1*electricMotorActuatorInfo.gear_ratio; - } - else { - finterpolatedtorque = ((revolutionsPerSecond - rps0)/(rps1 - rps0)*(torque1-torque0) + torque0)*electricMotorActuatorInfo.gear_ratio; - } - - // due to back emf, the deceleration magnitude is less than acceleration? - return std::make_pair(-finterpolatedtorque, finterpolatedtorque); - } - } - - // due to back emf, the deceleration magnitude is less than acceleration? - // revolutionsPerSecond is huge, return the last point - const dReal f = vSpeedTorquePoints.back().second*electricMotorActuatorInfo.gear_ratio; - return std::make_pair(-f, f); - } - else { - const dReal f = electricMotorActuatorInfo.nominal_torque*electricMotorActuatorInfo.gear_ratio; - return std::make_pair(-f, f); - } + const dReal fLimit = _GetTorqueLimitFromSpeedTorquePoints(iaxis, electricMotorActuatorInfo, electricMotorActuatorInfo.nominal_speed_torque_points, electricMotorActuatorInfo.nominal_torque); + return std::make_pair(-fLimit, fLimit); } } From 0d2da29b2c2b7639d70164e9b0872e619d843af2 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Wed, 25 Dec 2024 23:45:56 +0900 Subject: [PATCH 6/8] Do not use vector at operator repeatedly --- src/libopenrave/kinbodyjoint.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/libopenrave/kinbodyjoint.cpp b/src/libopenrave/kinbodyjoint.cpp index 3168d1e4eb..76944e398e 100644 --- a/src/libopenrave/kinbodyjoint.cpp +++ b/src/libopenrave/kinbodyjoint.cpp @@ -1913,9 +1913,10 @@ dReal KinBody::Joint::_GetTorqueLimitFromSpeedTorquePoints(const int iaxis, const dReal fDefaultTorqueLimit) const { if( vSpeedTorquePoints.size() > 0 ) { + const std::pair& firstSpeedTorquePoint = vSpeedTorquePoints[0]; if( vSpeedTorquePoints.size() == 1 ) { // doesn't matter what the velocity is - return vSpeedTorquePoints.at(0).second*electricMotorActuatorInfo.gear_ratio; + return firstSpeedTorquePoint.second*electricMotorActuatorInfo.gear_ratio; } const dReal rawvelocity = GetVelocity(iaxis); @@ -1925,17 +1926,19 @@ dReal KinBody::Joint::_GetTorqueLimitFromSpeedTorquePoints(const int iaxis, revolutionsPerSecond /= 2*M_PI; } - if( revolutionsPerSecond <= vSpeedTorquePoints.at(0).first ) { - return vSpeedTorquePoints.at(0).second*electricMotorActuatorInfo.gear_ratio; + if( revolutionsPerSecond <= firstSpeedTorquePoint.first ) { + return firstSpeedTorquePoint.second*electricMotorActuatorInfo.gear_ratio; } for(size_t i = 1; i < vSpeedTorquePoints.size(); ++i) { - if( revolutionsPerSecond <= vSpeedTorquePoints.at(i).first ) { + const std::pair& speedTorquePoint1 = vSpeedTorquePoints[i]; + if( revolutionsPerSecond <= speedTorquePoint1.first ) { // linearly interpolate to get the desired torque - const dReal rps0 = vSpeedTorquePoints.at(i-1).first; - const dReal torque0 = vSpeedTorquePoints.at(i-1).second; - const dReal rps1 = vSpeedTorquePoints.at(i).first; - const dReal torque1 = vSpeedTorquePoints.at(i).second; + const std::pair& 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; } From 77566430b035a77938067a29b3762241eb256d40 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Wed, 25 Dec 2024 23:47:52 +0900 Subject: [PATCH 7/8] Add todo comment toward back emf --- src/libopenrave/kinbodyjoint.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/libopenrave/kinbodyjoint.cpp b/src/libopenrave/kinbodyjoint.cpp index 76944e398e..8fcb98e3fd 100644 --- a/src/libopenrave/kinbodyjoint.cpp +++ b/src/libopenrave/kinbodyjoint.cpp @@ -1975,6 +1975,7 @@ std::pair KinBody::Joint::GetInstantaneousTorqueLimits(int iaxis) else { 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); } } @@ -1987,6 +1988,7 @@ std::pair KinBody::Joint::GetNominalTorqueLimits(int iaxis) const else { 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); } } From b2391dac8d576eef936a7330a6988070330de1c6 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Thu, 26 Dec 2024 00:04:28 +0900 Subject: [PATCH 8/8] Bump minor version: remove back emf code. add torque limit calculation utility --- CMakeLists.txt | 2 +- docs/source/changelog.rst | 6 ++++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 87500074b0..714d241598 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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}) diff --git a/docs/source/changelog.rst b/docs/source/changelog.rst index 495070c65b..861e2ba4c9 100644 --- a/docs/source/changelog.rst +++ b/docs/source/changelog.rst @@ -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 ===============