Closed24
モータに電流を入力して位置が出力されるまでの過程を C++ でモデル化する
ピン留めされたアイテム
薄田達哉 / tatsuyasusukidaGitHub リポジトリ
ソースコードを GitHub リポジトリで公開しました。
MIT ライセンスでご利用いただけます。
薄田達哉 / tatsuyasusukidaこのスクラップについて
このスクラップではロボットアーム VR コンテンツを開発するにあたり、モータに電流を入力してから位置が出力されるブロック図を C++ で実装する過程を記録する。
薄田達哉 / tatsuyasusukidaブロック図
薄田達哉 / tatsuyasusukidaプロトタイプを作る準備
コマンド
mkdir ~/workspace/motor-model
cd ~/workspace/motor-model
touch main.cpp
gcc -o main main.cpp
プロトタイプの作成
main.cpp
#include <iostream>
class MotorModel
{
public:
MotorModel() {}
/**
* 現在の時刻と入力電流から位置を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 電流入力、単位は [A] です。
* @return 位置出力、単位は [rad] です。
*/
double calculatePosition(double currentTime, double inputCurrent)
{
return 0;
}
};
int main()
{
MotorModel motorModel;
double currentTime = 0;
double inputCurrent = 0;
double outputPosition = motorModel.calculatePosition(currentTime, inputCurrent);
std::cout << "outputPosition = " << outputPosition << std::endl;
return 0;
}
コマンド
g++ -o main main.cpp && ./main
コンソール出力
outputPosition = 0
トルクの計算
トルクは電流入力にトルク定数を乗算するだけ。
main.cpp
#include <iostream>
class MotorModel
{
private:
/** トルク定数 Kt、単位は [Nm/A] です。*/
double _torqueConstant;
public:
MotorModel(double torqueConstant) : _torqueConstant(torqueConstant) {}
/**
* 現在の時刻と入力電流から位置を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 電流入力、単位は [A] です。
* @return 位置出力、単位は [rad] です。
*/
double calculatePosition(double currentTime, double inputCurrent)
{
return 0;
}
/**
* 現在の時刻と入力電流からトルク τ [Nm] を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 電流入力、単位は [A] です。
* @return トルク τ、単位は [Nm] です。
*/
double calculateTorque(double currentTime, double inputCurrent)
{
return _torqueConstant * inputCurrent;
}
};
int main()
{
double torqueConstant = 1;
MotorModel motorModel(torqueConstant);
double currentTime = 2;
double inputCurrent = 3;
double torque = motorModel.calculateTorque(currentTime, inputCurrent);
std::cout << "torque = " << torque << std::endl;
return 0;
}
コマンド
g++ -o main main.cpp && ./main
コンソール出力
torque = 3
角加速度の計算
トルクから負荷トルクと摩擦トルクを引かなければならないがこれらは後から加えるとしよう。
トルクから角加速度を計算するのは慣性モーメント
main.cpp
#include <iostream>
class MotorModel
{
private:
/** トルク定数 Kt、単位は [Nm/A] です。*/
double _torqueConstant;
/** 慣性モーメント J、単位は [kgm²] です。*/
double _momentOfInertia;
/** 直前の角加速度 α、単位は [rad/s²] です。*/
double _lastAngularAcceleration;
public:
MotorModel(double torqueConstant, double momentOfInertia)
: _torqueConstant(torqueConstant),
_momentOfInertia(momentOfInertia) {}
/**
* 現在の時刻と入力電流から位置を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 電流入力、単位は [A] です。
* @return 位置出力、単位は [rad] です。
*/
double calculatePosition(double currentTime, double inputCurrent)
{
return 0;
}
/**
* 現在の時刻と入力電流から角加速度 α [rad/s²] を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 電流入力、単位は [A] です。
* @return 角加速度 α、単位は [rad/s²] です。
*/
double calculateAngularAcceleration(double currentTime, double inputCurrent)
{
double torque = calculateTorque(currentTime, inputCurrent);
double angularAcceleration = torque / _momentOfInertia;
_lastAngularAcceleration = angularAcceleration;
return angularAcceleration;
}
/**
* 現在の時刻と入力電流からトルク τ [Nm] を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 電流入力、単位は [A] です。
* @return トルク τ、単位は [Nm] です。
*/
double calculateTorque(double currentTime, double inputCurrent)
{
return _torqueConstant * inputCurrent;
}
};
int main()
{
double torqueConstant = 1;
double momentOfInertia = 2;
MotorModel motorModel(torqueConstant, momentOfInertia);
double currentTime = 3;
double inputCurrent = 4;
double angularAcceleration = motorModel.calculateAngularAcceleration(currentTime, inputCurrent);
std::cout << "angularAcceleration = " << angularAcceleration << std::endl;
return 0;
}
コマンド
g++ -o main main.cpp && ./main
コンソール出力
angularAcceleration = 2
角速度 ω [rad/s] の計算
こちらは角加速度を積分すれば良いだけ。
定積分値を計算するのには台形公式を使う。
main.cpp
#include <iostream>
class MotorModel
{
private:
/** トルク定数 Kt、単位は [Nm/A] です。*/
double _torqueConstant;
/** 慣性モーメント J、単位は [kgm²] です。*/
double _momentOfInertia;
/** 直前の角加速度 α、単位は [rad/s²] です。*/
double _lastAngularAcceleration;
/** 前回の時刻、単位は [s] です。*/
double _previousTime;
/** 角速度 ω、単位は [rad/s] です。*/
double _angularVelocity;
public:
MotorModel(double torqueConstant, double momentOfInertia)
: _torqueConstant(torqueConstant),
_momentOfInertia(momentOfInertia),
_lastAngularAcceleration(0),
_previousTime(0),
_angularVelocity(0) {}
/**
* 現在の時刻を保存します。
*
* @param previousTime 現在の時刻、単位は [s] です。
*/
void savePreviousTime(double previousTime)
{
_previousTime = previousTime;
}
/**
* 現在の時刻と入力電流から位置を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 電流入力、単位は [A] です。
* @return 位置出力、単位は [rad] です。
*/
double calculatePosition(double currentTime, double inputCurrent)
{
return 0;
}
/**
* 現在の時刻と入力電流から角速度 ω [rad/s] を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 電流入力、単位は [A] です。
* @return 角速度 ω、単位は [rad/s] です。
*/
double calculateAngularVelocity(double currentTime, double inputCurrent)
{
double previousAngularAcceleration = _lastAngularAcceleration;
double currentAngularAcceleration = calculateAngularAcceleration(currentTime, inputCurrent);
_lastAngularAcceleration = currentAngularAcceleration;
double angularVelocityDelta = calculateDefiniteIntegralValue(
_previousTime,
previousAngularAcceleration,
currentTime,
currentAngularAcceleration);
_angularVelocity += angularVelocityDelta;
return _angularVelocity;
}
/**
* 現在の時刻と入力電流から角加速度 α [rad/s²] を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 電流入力、単位は [A] です。
* @return 角加速度 α、単位は [rad/s²] です。
*/
double calculateAngularAcceleration(double currentTime, double inputCurrent)
{
double torque = calculateTorque(currentTime, inputCurrent);
double angularAcceleration = torque / _momentOfInertia;
return angularAcceleration;
}
/**
* 現在の時刻と入力電流からトルク τ [Nm] を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 電流入力、単位は [A] です。
* @return トルク τ、単位は [Nm] です。
*/
double calculateTorque(double currentTime, double inputCurrent)
{
return _torqueConstant * inputCurrent;
}
/**
* 定積分値を台形公式を使って計算します。
*
* @param previousTime 直前の時刻、単位は [s] です。
* @param previousValue 直前の値、単位は任意です。
* @param currentTime 現在の時刻、単位は [s] です。
* @param currentValue 現在の値、単位は任意です。
* @return 定積分値です。
*/
double calculateDefiniteIntegralValue(
double previousTime,
double previousValue,
double currentTime,
double currentValue)
{
double upperBase = previousValue;
double lowerBase = currentValue;
double height = currentTime - previousTime;
return (upperBase + lowerBase) * height / 2;
}
};
int main()
{
double torqueConstant = 1;
double momentOfInertia = 2;
MotorModel motorModel(torqueConstant, momentOfInertia);
double previousTime = 3;
motorModel.savePreviousTime(previousTime);
double currentTime = 4;
double inputCurrent = 5;
double angularVelocity = motorModel.calculateAngularVelocity(currentTime, inputCurrent);
std::cout << "angularVelocity = " << angularVelocity << std::endl;
return 0;
}
コマンド
g++ -o main main.cpp && ./main
コンソール出力
angularVelocity = 1.25
位置出力 θ [rad] の計算
こちらも角速度 ω [rad/s] を積分するだけ。
main.cpp
#include <iostream>
class MotorModel
{
private:
/** トルク定数 Kt、単位は [Nm/A] です。*/
double _torqueConstant;
/** 慣性モーメント J、単位は [kgm²] です。*/
double _momentOfInertia;
/** 直前の角加速度 α、単位は [rad/s²] です。*/
double _lastAngularAcceleration;
/** 直前の角速度 ω、単位は [rad/s] です。*/
double _lastAngularVelocity;
/** 前回の時刻、単位は [s] です。*/
double _previousTime;
/** 角速度 ω、単位は [rad/s] です。*/
double _currentAngularVelocity;
/** 位置出力 θ、単位は [rad] です。*/
double _currentPosition;
public:
MotorModel(double torqueConstant, double momentOfInertia)
: _torqueConstant(torqueConstant),
_momentOfInertia(momentOfInertia),
_lastAngularAcceleration(0),
_lastAngularVelocity(0),
_previousTime(0),
_currentAngularVelocity(0),
_currentPosition(0) {}
/**
* 現在の時刻を保存します。
*
* @param previousTime 現在の時刻、単位は [s] です。
*/
void savePreviousTime(double previousTime)
{
_previousTime = previousTime;
}
/**
* 現在の時刻と入力電流から位置を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 電流入力 i、単位は [A] です。
* @return 位置出力 θ、単位は [rad] です。
*/
double calculatePosition(double currentTime, double inputCurrent)
{
double previousAngularVelocity = _lastAngularVelocity;
double currentAngularVelocity = calculateAngularVelocity(currentTime, inputCurrent);
_lastAngularVelocity = currentAngularVelocity;
double positionDelta = calculateDefiniteIntegralValue(
_previousTime,
previousAngularVelocity,
currentTime,
currentAngularVelocity);
_currentPosition += positionDelta;
return _currentPosition;
}
/**
* 現在の時刻と入力電流から角速度 ω [rad/s] を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 電流入力、単位は [A] です。
* @return 角速度 ω、単位は [rad/s] です。
*/
double calculateAngularVelocity(double currentTime, double inputCurrent)
{
double previousAngularAcceleration = _lastAngularAcceleration;
double currentAngularAcceleration = calculateAngularAcceleration(currentTime, inputCurrent);
_lastAngularAcceleration = currentAngularAcceleration;
double angularVelocityDelta = calculateDefiniteIntegralValue(
_previousTime,
previousAngularAcceleration,
currentTime,
currentAngularAcceleration);
_currentAngularVelocity += angularVelocityDelta;
return _currentAngularVelocity;
}
/**
* 現在の時刻と入力電流から角加速度 α [rad/s²] を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 電流入力、単位は [A] です。
* @return 角加速度 α、単位は [rad/s²] です。
*/
double calculateAngularAcceleration(double currentTime, double inputCurrent)
{
double torque = calculateTorque(currentTime, inputCurrent);
double angularAcceleration = torque / _momentOfInertia;
return angularAcceleration;
}
/**
* 現在の時刻と入力電流からトルク τ [Nm] を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 電流入力、単位は [A] です。
* @return トルク τ、単位は [Nm] です。
*/
double calculateTorque(double currentTime, double inputCurrent)
{
return _torqueConstant * inputCurrent;
}
/**
* 定積分値を台形公式を使って計算します。
*
* @param previousTime 直前の時刻、単位は [s] です。
* @param previousValue 直前の値、単位は任意です。
* @param currentTime 現在の時刻、単位は [s] です。
* @param currentValue 現在の値、単位は任意です。
* @return 定積分値です。
*/
double calculateDefiniteIntegralValue(
double previousTime,
double previousValue,
double currentTime,
double currentValue)
{
double upperBase = previousValue;
double lowerBase = currentValue;
double height = currentTime - previousTime;
return (upperBase + lowerBase) * height / 2;
}
};
int main()
{
double torqueConstant = 1;
double momentOfInertia = 2;
MotorModel motorModel(torqueConstant, momentOfInertia);
double previousTime = 3;
motorModel.savePreviousTime(previousTime);
double currentTime = 4;
double inputCurrent = 5;
double position = motorModel.calculatePosition(currentTime, inputCurrent);
std::cout << "position = " << position << std::endl;
return 0;
}
コマンド
g++ -o main main.cpp && ./main
コンソール出力
position = 0.625
1/1 (月) はここまで
他のこともやっていたが今日は 2.5 時間ほど費やしたようだ。
薄田達哉 / tatsuyasusukida1/2 (火) はここから
開始宣言を忘れたが 10:45 頃から始めた。
薄田達哉 / tatsuyasusukidaフィードバックの実装
角加速度を計算する時にトルクから摩擦トルクと負荷トルクを引く。
摩擦トルクは角速度に粘性摩擦係数を引いて求める。
main.cpp
#include <iostream>
class MotorModel
{
private:
/** トルク定数 Kt、単位は [Nm/A] です。*/
double _torqueConstant;
/** 慣性モーメント J、単位は [kgm²] です。*/
double _momentOfInertia;
/** 粘性摩擦係数 D、単位は [Nm/(rad/s)] です。*/
double _viscousFrictionCoefficient;
/** 直前の角加速度 α、単位は [rad/s²] です。*/
double _lastAngularAcceleration;
/** 直前の角速度 ω、単位は [rad/s] です。*/
double _lastAngularVelocity;
/** 前回の時刻、単位は [s] です。*/
double _previousTime;
/** 角速度 ω、単位は [rad/s] です。*/
double _currentAngularVelocity;
/** 位置出力 θ、単位は [rad] です。*/
double _currentPosition;
public:
MotorModel(
double torqueConstant,
double momentOfInertia,
double viscousFrictionCoefficient)
: _torqueConstant(torqueConstant),
_momentOfInertia(momentOfInertia),
_viscousFrictionCoefficient(viscousFrictionCoefficient),
_lastAngularAcceleration(0),
_lastAngularVelocity(0),
_previousTime(0),
_currentAngularVelocity(0),
_currentPosition(0) {}
/**
* 現在の時刻を保存します。
*
* @param previousTime 現在の時刻、単位は [s] です。
*/
void setPreviousTime(double previousTime)
{
_previousTime = previousTime;
}
/**
* 現在の時刻と入力電流から位置を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 電流入力 i、単位は [A] です。
* @return 位置出力 θ、単位は [rad] です。
*/
double calculatePosition(
double currentTime,
double inputCurrent,
bool savePreviousTime = true)
{
double previousAngularVelocity = _lastAngularVelocity;
double currentAngularVelocity = calculateAngularVelocity(currentTime, inputCurrent);
_lastAngularVelocity = currentAngularVelocity;
double positionDelta = calculateDefiniteIntegralValue(
_previousTime,
previousAngularVelocity,
currentTime,
currentAngularVelocity);
_currentPosition += positionDelta;
if (savePreviousTime)
{
setPreviousTime(currentTime);
}
return _currentPosition;
}
/**
* 現在の時刻と入力電流から角速度 ω [rad/s] を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 電流入力、単位は [A] です。
* @return 角速度 ω、単位は [rad/s] です。
*/
double calculateAngularVelocity(double currentTime, double inputCurrent)
{
double previousAngularAcceleration = _lastAngularAcceleration;
double currentAngularAcceleration = calculateAngularAcceleration(currentTime, inputCurrent);
_lastAngularAcceleration = currentAngularAcceleration;
double angularVelocityDelta = calculateDefiniteIntegralValue(
_previousTime,
previousAngularAcceleration,
currentTime,
currentAngularAcceleration);
_currentAngularVelocity += angularVelocityDelta;
return _currentAngularVelocity;
}
/**
* 現在の時刻と入力電流から角加速度 α [rad/s²] を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 電流入力、単位は [A] です。
* @return 角加速度 α、単位は [rad/s²] です。
*/
double calculateAngularAcceleration(double currentTime, double inputCurrent)
{
double torque = calculateTorque(currentTime, inputCurrent);
double frictionalTorque = _viscousFrictionCoefficient * _currentAngularVelocity;
double loadTorque = getLoadTorque(currentTime);
double sumOfTorque = torque - (frictionalTorque + loadTorque);
double angularAcceleration = sumOfTorque / _momentOfInertia;
return angularAcceleration;
}
/**
* 指定時刻における負荷トルクです。
* このメソッドはオーバーライドして使用されることを想定しています。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @return 角加速度 α、単位は [rad/s²] です。
*/
virtual double getLoadTorque(double currentTime)
{
return 0;
}
/**
* 現在の時刻と入力電流からトルク τ [Nm] を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 電流入力、単位は [A] です。
* @return トルク τ、単位は [Nm] です。
*/
double calculateTorque(double currentTime, double inputCurrent)
{
return _torqueConstant * inputCurrent;
}
/**
* 定積分値を台形公式を使って計算します。
*
* @param previousTime 直前の時刻、単位は [s] です。
* @param previousValue 直前の値、単位は任意です。
* @param currentTime 現在の時刻、単位は [s] です。
* @param currentValue 現在の値、単位は任意です。
* @return 定積分値です。
*/
double calculateDefiniteIntegralValue(
double previousTime,
double previousValue,
double currentTime,
double currentValue)
{
double upperBase = previousValue;
double lowerBase = currentValue;
double height = currentTime - previousTime;
return (upperBase + lowerBase) * height / 2;
}
};
int main()
{
double torqueConstant = 1;
double momentOfInertia = 2;
double viscousFrictionCoefficient = 3;
MotorModel motorModel(torqueConstant, momentOfInertia, viscousFrictionCoefficient);
double previousTime = 4;
motorModel.setPreviousTime(previousTime);
double currentTime = 5;
double inputCurrent = 6;
double position = motorModel.calculatePosition(currentTime, inputCurrent);
std::cout << "position = " << position << std::endl;
return 0;
}
コマンド
g++ -o main main.cpp && ./main
コンソール出力
position = 0.75
動かしてみる
main.cpp（main関数のみ）
int main()
{
double torqueConstant = 1;
double momentOfInertia = 2;
double viscousFrictionCoefficient = 3;
MotorModel motorModel(torqueConstant, momentOfInertia, viscousFrictionCoefficient);
printf("時刻,電流入力[A],位置出力[rad]\n");
for (int msec = 0; msec <= 10000; msec += 100)
{
double currentTime = msec * 1.0e-3;
double inputCurrent = msec <= 1000 ? 1.0 : 0;
double position = motorModel.calculatePosition(currentTime, inputCurrent);
printf("%f,%f,%f\n", currentTime, inputCurrent, position);
}
return 0;
}
コマンド
g++ -o main main.cpp && ./main > main.csv
main.csv（0.0〜2.0秒まで）
時刻,電流入力[A],位置出力[rad]
0.000000,1.000000,0.000000
0.100000,1.000000,0.002500
0.200000,1.000000,0.009813
0.300000,1.000000,0.021389
0.400000,1.000000,0.036549
0.500000,1.000000,0.054704
0.600000,1.000000,0.075360
0.700000,1.000000,0.098105
0.800000,1.000000,0.122595
0.900000,1.000000,0.148543
1.000000,1.000000,0.175707
1.100000,0.000000,0.202639
1.200000,0.000000,0.226763
1.300000,0.000000,0.247057
1.400000,0.000000,0.264021
1.500000,0.000000,0.278190
1.600000,0.000000,0.290024
1.700000,0.000000,0.299908
1.800000,0.000000,0.308163
1.900000,0.000000,0.315058
2.000000,0.000000,0.320816
位置出力のグラフ｜それらしい感じになっている気がする
薄田達哉 / tatsuyasusukida電流入力と位置出力
今更だけどそれぞれ入力電流と位置に置換しよう。
薄田達哉 / tatsuyasusukida他のグラフも見てみたい
せっかくなのでゲッターを設けて内部状態を見れるようにしよう。
main.cpp
#include <iostream>
class MotorModel
{
private:
/** トルク定数 Kt、単位は [Nm/A] です。*/
double _torqueConstant;
/** 慣性モーメント J、単位は [kgm²] です。*/
double _momentOfInertia;
/** 粘性摩擦係数 D、単位は [Nm/(rad/s)] です。*/
double _viscousFrictionCoefficient;
/** 直前の角加速度 α、単位は [rad/s²] です。*/
double _lastAngularAcceleration;
/** 直前の角速度 ω、単位は [rad/s] です。*/
double _lastAngularVelocity;
/** 前回の時刻、単位は [s] です。*/
double _previousTime;
/** 角速度 ω、単位は [rad/s] です。*/
double _currentAngularVelocity;
/** 位置 θ、単位は [rad] です。*/
double _currentPosition;
public:
MotorModel(
double torqueConstant,
double momentOfInertia,
double viscousFrictionCoefficient)
: _torqueConstant(torqueConstant),
_momentOfInertia(momentOfInertia),
_viscousFrictionCoefficient(viscousFrictionCoefficient),
_lastAngularAcceleration(0),
_lastAngularVelocity(0),
_previousTime(0),
_currentAngularVelocity(0),
_currentPosition(0) {}
/**
* 現在の時刻を保存します。
*
* @param previousTime 現在の時刻、単位は [s] です。
*/
void setPreviousTime(double previousTime)
{
_previousTime = previousTime;
}
/**
* 現在の時刻と入力電流から位置を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 入力電流 i、単位は [A] です。
* @return 位置 θ、単位は [rad] です。
*/
double calculatePosition(
double currentTime,
double inputCurrent,
bool savePreviousTime = true)
{
double previousAngularVelocity = _lastAngularVelocity;
double currentAngularVelocity = calculateAngularVelocity(currentTime, inputCurrent);
_lastAngularVelocity = currentAngularVelocity;
double positionDelta = calculateDefiniteIntegralValue(
_previousTime,
previousAngularVelocity,
currentTime,
currentAngularVelocity);
_currentPosition += positionDelta;
if (savePreviousTime)
{
setPreviousTime(currentTime);
}
return _currentPosition;
}
/**
* 現在の時刻と入力電流から角速度 ω [rad/s] を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 入力電流、単位は [A] です。
* @return 角速度 ω、単位は [rad/s] です。
*/
double calculateAngularVelocity(double currentTime, double inputCurrent)
{
double previousAngularAcceleration = _lastAngularAcceleration;
double currentAngularAcceleration = calculateAngularAcceleration(currentTime, inputCurrent);
_lastAngularAcceleration = currentAngularAcceleration;
double angularVelocityDelta = calculateDefiniteIntegralValue(
_previousTime,
previousAngularAcceleration,
currentTime,
currentAngularAcceleration);
_currentAngularVelocity += angularVelocityDelta;
return _currentAngularVelocity;
}
/**
* 現在の時刻と入力電流から角加速度 α [rad/s²] を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 入力電流、単位は [A] です。
* @return 角加速度 α、単位は [rad/s²] です。
*/
double calculateAngularAcceleration(double currentTime, double inputCurrent)
{
double torque = calculateTorque(currentTime, inputCurrent);
double frictionalTorque = _viscousFrictionCoefficient * _currentAngularVelocity;
double loadTorque = getLoadTorque(currentTime);
double sumOfTorque = torque - (frictionalTorque + loadTorque);
double angularAcceleration = sumOfTorque / _momentOfInertia;
return angularAcceleration;
}
/**
* 指定時刻における負荷トルクです。
* このメソッドはオーバーライドして使用されることを想定しています。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @return 角加速度 α、単位は [rad/s²] です。
*/
virtual double getLoadTorque(double currentTime)
{
return 0;
}
/**
* 現在の時刻と入力電流からトルク τ [Nm] を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 入力電流、単位は [A] です。
* @return トルク τ、単位は [Nm] です。
*/
double calculateTorque(double currentTime, double inputCurrent)
{
return _torqueConstant * inputCurrent;
}
/**
* 定積分値を台形公式を使って計算します。
*
* @param previousTime 直前の時刻、単位は [s] です。
* @param previousValue 直前の値、単位は任意です。
* @param currentTime 現在の時刻、単位は [s] です。
* @param currentValue 現在の値、単位は任意です。
* @return 定積分値です。
*/
double calculateDefiniteIntegralValue(
double previousTime,
double previousValue,
double currentTime,
double currentValue)
{
double upperBase = previousValue;
double lowerBase = currentValue;
double height = currentTime - previousTime;
return (upperBase + lowerBase) * height / 2;
}
double getTorqueConstant() const
{
return _torqueConstant;
}
double getMomentOfInertia() const
{
return _momentOfInertia;
}
double getViscousFrictionCoefficient() const
{
return _viscousFrictionCoefficient;
}
double getLastAngularAcceleration() const
{
return _lastAngularAcceleration;
}
double getLastAngularVelocity() const
{
return _lastAngularVelocity;
}
double getPreviousTime() const
{
return _previousTime;
}
double getCurrentAngularVelocity() const
{
return _currentAngularVelocity;
}
double getCurrentPosition() const
{
return _currentPosition;
}
};
int main()
{
double torqueConstant = 1;
double momentOfInertia = 2;
double viscousFrictionCoefficient = 3;
MotorModel motorModel(torqueConstant, momentOfInertia, viscousFrictionCoefficient);
printf("時刻,入力電流[A],位置[rad],角速度[rad/s],角加速度[rad/s²],\n");
for (int msec = 0; msec <= 10000; msec += 100)
{
double currentTime = msec * 1.0e-3;
double inputCurrent = msec <= 1000 ? 1.0 : 0;
double position = motorModel.calculatePosition(currentTime, inputCurrent);
double angularVelocity = motorModel.getLastAngularVelocity();
double angularAcceleration = motorModel.getLastAngularAcceleration();
printf(
"%f,%f,%f,%f,%f\n",
currentTime,
inputCurrent,
position,
angularVelocity,
angularAcceleration);
}
return 0;
}
コマンド
g++ -o main main.cpp && ./main > main.csv
main.csv（0.0〜2.0秒まで）
時刻,入力電流[A],位置[rad],角速度[rad/s],角加速度[rad/s²],
0.000000,1.000000,0.000000,0.000000,0.500000
0.100000,1.000000,0.002500,0.050000,0.500000
0.200000,1.000000,0.009813,0.096250,0.425000
0.300000,1.000000,0.021389,0.135281,0.355625
0.400000,1.000000,0.036549,0.167916,0.297078
0.500000,1.000000,0.054704,0.195177,0.248125
0.600000,1.000000,0.075360,0.217945,0.207235
0.700000,1.000000,0.098105,0.236961,0.173083
0.800000,1.000000,0.122595,0.252843,0.144559
0.900000,1.000000,0.148543,0.266107,0.120736
1.000000,1.000000,0.175707,0.277186,0.100839
1.100000,0.000000,0.202639,0.261439,-0.415779
1.200000,0.000000,0.226763,0.221042,-0.392159
1.300000,0.000000,0.247057,0.184856,-0.331563
1.400000,0.000000,0.264021,0.154414,-0.277284
1.500000,0.000000,0.278190,0.128969,-0.231621
1.600000,0.000000,0.290024,0.107715,-0.193453
1.700000,0.000000,0.299908,0.089964,-0.161572
1.800000,0.000000,0.308163,0.075138,-0.134945
1.900000,0.000000,0.315058,0.062755,-0.112707
2.000000,0.000000,0.320816,0.052413,-0.094133
それぞれの値のグラフ｜最初の時刻は 0.1 秒の方が良い気がしてきた
薄田達哉 / tatsuyasusukida1〜2 秒にかけて電流を流すことにする
main.cpp（main関数のみ）
int main()
{
double torqueConstant = 1;
double momentOfInertia = 2;
double viscousFrictionCoefficient = 3;
MotorModel motorModel(torqueConstant, momentOfInertia, viscousFrictionCoefficient);
printf("時刻,入力電流[A],位置[rad],角速度[rad/s],角加速度[rad/s²],\n");
for (int msec = 0; msec <= 10000; msec += 100)
{
double currentTime = msec * 1.0e-3;
double inputCurrent = 1000 <= msec && msec <= 2000 ? 1.0 : 0;
motorModel.calculatePosition(currentTime, inputCurrent);
double position = motorModel.getCurrentPosition();
double angularVelocity = motorModel.getLastAngularVelocity();
double angularAcceleration = motorModel.getLastAngularAcceleration();
printf(
"%f,%f,%f,%f,%f\n",
currentTime,
inputCurrent,
position,
angularVelocity,
angularAcceleration);
}
return 0;
}
それぞれの値のグラフ｜これで時刻 0〜0.1 秒における加速度が 0.5 のもやもやが少し解決した
薄田達哉 / tatsuyasusukida冗長なメンバー変数を削除したい
_lastAngularVelocity と _currentAngularVelocity は同じ値になるなのでいらなそう。
いずれかを削除しよう。
current か last のどちらを残すかが悩ましいが last の方に統一しようかな。
薄田達哉 / tatsuyasusukidaリファクタリング
main.cpp
#include <iostream>
class MotorModel
{
private:
/** トルク定数 Kt、単位は [Nm/A] です。*/
double _torqueConstant;
/** 慣性モーメント J、単位は [kgm²] です。*/
double _momentOfInertia;
/** 粘性摩擦係数 D、単位は [Nm/(rad/s)] です。*/
double _viscousFrictionCoefficient;
/** 直前のトルク τ、単位は [Nm] です。*/
double _lastTorque;
/** 直前の角加速度 α、単位は [rad/s²] です。*/
double _lastAngularAcceleration;
/** 直前の角速度 ω、単位は [rad/s] です。*/
double _lastAngularVelocity;
/** 直前の位置 θ、単位は [rad] です。*/
double _lastPosition;
/** 前回の時刻、単位は [s] です。*/
double _previousTime;
public:
/**
* MotorModel のコンストラクターです。
*
* @param torqueConstant トルク定数 Kt、単位は [Nm/A] です。
* @param momentOfInertia 慣性モーメント J、単位は [kgm²] です。
* @param viscousFrictionCoefficient 粘性摩擦係数 D、単位は [Nm/(rad/s)] です。
*/
MotorModel(
double torqueConstant,
double momentOfInertia,
double viscousFrictionCoefficient)
: _torqueConstant(torqueConstant),
_momentOfInertia(momentOfInertia),
_viscousFrictionCoefficient(viscousFrictionCoefficient),
_lastTorque(0),
_lastAngularAcceleration(0),
_lastAngularVelocity(0),
_lastPosition(0),
_previousTime(0) {}
/**
* 現在の時刻と入力電流から位置を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 入力電流 i、単位は [A] です。
* @param savePreviousTime true の場合、現在の時刻を保存します。
* @return 位置 θ、単位は [rad] です。
*/
double calculatePosition(
double currentTime,
double inputCurrent,
bool savePreviousTime = true)
{
double previousAngularVelocity = _lastAngularVelocity;
double currentAngularVelocity = calculateAngularVelocity(currentTime, inputCurrent);
double positionDelta = calculateDefiniteIntegralValue(
_previousTime,
previousAngularVelocity,
currentTime,
currentAngularVelocity);
if (savePreviousTime)
{
setPreviousTime(currentTime);
}
_lastPosition += positionDelta;
return _lastPosition;
}
/**
* 現在の時刻と入力電流から角速度 ω [rad/s] を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 入力電流、単位は [A] です。
* @return 角速度 ω、単位は [rad/s] です。
*/
double calculateAngularVelocity(double currentTime, double inputCurrent)
{
double previousAngularAcceleration = _lastAngularAcceleration;
double currentAngularAcceleration = calculateAngularAcceleration(currentTime, inputCurrent);
double angularVelocityDelta = calculateDefiniteIntegralValue(
_previousTime,
previousAngularAcceleration,
currentTime,
currentAngularAcceleration);
_lastAngularVelocity += angularVelocityDelta;
return _lastAngularVelocity;
}
/**
* 現在の時刻と入力電流から角加速度 α [rad/s²] を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 入力電流、単位は [A] です。
* @return 角加速度 α、単位は [rad/s²] です。
*/
double calculateAngularAcceleration(double currentTime, double inputCurrent)
{
double torque = calculateTorque(currentTime, inputCurrent);
double frictionalTorque = _viscousFrictionCoefficient * _lastAngularVelocity;
double loadTorque = getLoadTorque(currentTime);
double sumOfTorque = torque - (frictionalTorque + loadTorque);
_lastAngularAcceleration = sumOfTorque / _momentOfInertia;
return _lastAngularAcceleration;
}
/**
* 入力電流からトルク τ [Nm] を計算します。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @param inputCurrent 入力電流、単位は [A] です。
* @return トルク τ、単位は [Nm] です。
*/
double calculateTorque(double currentTime, double inputCurrent)
{
_lastTorque = _torqueConstant * inputCurrent;
return _lastTorque;
}
/**
* 指定時刻における負荷トルクです。
* このメソッドはオーバーライドして使用されることを想定しています。
*
* @param currentTime 現在の時刻、単位は [s] です。
* @return 角加速度 α、単位は [rad/s²] です。
*/
virtual double getLoadTorque(double currentTime)
{
return 0;
}
/**
* 定積分値を台形公式を使って計算します。
*
* @param previousTime 直前の時刻、単位は [s] です。
* @param previousValue 直前の値、単位は任意です。
* @param currentTime 現在の時刻、単位は [s] です。
* @param currentValue 現在の値、単位は任意です。
* @return 定積分値です。
*/
double calculateDefiniteIntegralValue(
double previousTime,
double previousValue,
double currentTime,
double currentValue)
{
double upperBase = previousValue;
double lowerBase = currentValue;
double height = currentTime - previousTime;
return (upperBase + lowerBase) * height / 2;
}
double getTorqueConstant() const
{
return _torqueConstant;
}
double getMomentOfInertia() const
{
return _momentOfInertia;
}
double getViscousFrictionCoefficient() const
{
return _viscousFrictionCoefficient;
}
double getLastTorque() const
{
return _lastTorque;
}
double getLastAngularAcceleration() const
{
return _lastAngularAcceleration;
}
double getLastAngularVelocity() const
{
return _lastAngularVelocity;
}
double getLastPosition() const
{
return _lastPosition;
}
double getPreviousTime() const
{
return _previousTime;
}
void setPreviousTime(double previousTime)
{
_previousTime = previousTime;
}
};
int main()
{
double torqueConstant = 1;
double momentOfInertia = 2;
double viscousFrictionCoefficient = 3;
MotorModel motorModel(torqueConstant, momentOfInertia, viscousFrictionCoefficient);
printf("時刻,入力電流[A],位置[rad],角速度[rad/s],角加速度[rad/s²]\n");
for (int msec = 0; msec <= 10000; msec += 100)
{
double currentTime = msec * 1.0e-3;
double inputCurrent = 1000 <= msec && msec <= 2000 ? 1.0 : 0;
motorModel.calculatePosition(currentTime, inputCurrent);
double position = motorModel.getLastPosition();
double angularVelocity = motorModel.getLastAngularVelocity();
double angularAcceleration = motorModel.getLastAngularAcceleration();
printf(
"%f,%f,%f,%f,%f\n",
currentTime,
inputCurrent,
position,
angularVelocity,
angularAcceleration);
}
return 0;
}
テストは再実行して CSV に変化が無いかどうかでチェックした。
薄田達哉 / tatsuyasusukidaファイルを分ける
コマンド
touch MotorModel.hh
MotorModel.hh には MotorModel クラスをカット＆ペーストする。
main.cpp
#include <iostream>
#include "MotorModel.hh"
int main()
{
double torqueConstant = 1;
double momentOfInertia = 2;
double viscousFrictionCoefficient = 3;
MotorModel motorModel(torqueConstant, momentOfInertia, viscousFrictionCoefficient);
printf("時刻,入力電流[A],位置[rad],角速度[rad/s],角加速度[rad/s²]\n");
for (int msec = 0; msec <= 10000; msec += 100)
{
double currentTime = msec * 1.0e-3;
double inputCurrent = 1000 <= msec && msec <= 2000 ? 1.0 : 0;
motorModel.calculatePosition(currentTime, inputCurrent);
double position = motorModel.getLastPosition();
double angularVelocity = motorModel.getLastAngularVelocity();
double angularAcceleration = motorModel.getLastAngularAcceleration();
printf(
"%f,%f,%f,%f,%f\n",
currentTime,
inputCurrent,
position,
angularVelocity,
angularAcceleration);
}
return 0;
}
main.cpp の方には #include "MotorModel.hh" を追加しただけ。
薄田達哉 / tatsuyasusukidacalculateDefiniteIntegralValue() のテスト
コマンド
touch unittest.cpp
unittest.cpp
#include <cassert>
#include <cstdio>
#include "MotorModel.hh"
bool g_debug = false;
#define ASSERT(actual, expected) \
if (g_debug) \
printf("DEBUG: actual = %f, expected = %f\n", actual, expected); \
assert(actual == expected);
void testCalculateDefiniteIntegralValue()
{
double torqueConstant = 1;
double momentOfInertia = 2;
double viscousFrictionCoefficient = 3;
MotorModel motorModel(torqueConstant, momentOfInertia, viscousFrictionCoefficient);
double previousTime = 1;
double previousValue = 2;
double currentTime = 3;
double currentValue = 4;
double actual = motorModel.calculateDefiniteIntegralValue(
previousTime,
previousValue,
currentTime,
currentValue);
double expected = (previousValue + currentValue) * (currentTime - previousTime) / 2;
ASSERT(actual, expected)
}
int main()
{
testCalculateDefiniteIntegralValue();
return 0;
}
コマンド
g++ -o unittest unittest.cpp && ./unittest
テストに成功した場合は何も表示されない。
薄田達哉 / tatsuyasusukidacalculateTorque() のテスト
unittest.cpp
#include <cassert>
#include <cstdio>
#include "MotorModel.hh"
bool g_debug = false;
#define ASSERT(actual, expected) \
if (g_debug) \
printf("DEBUG: actual = %f, expected = %f\n", actual, expected); \
assert(actual == expected);
#define TEST(fn) \
printf("%s\n", #fn); \
fn();
void testCalculateDefiniteIntegralValue()
{
double torqueConstant = 1;
double momentOfInertia = 2;
double viscousFrictionCoefficient = 3;
MotorModel motorModel(torqueConstant, momentOfInertia, viscousFrictionCoefficient);
double previousTime = 1;
double previousValue = 2;
double currentTime = 3;
double currentValue = 4;
double actual = motorModel.calculateDefiniteIntegralValue(
previousTime,
previousValue,
currentTime,
currentValue);
double expected = (previousValue + currentValue) * (currentTime - previousTime) / 2;
ASSERT(actual, expected)
}
void testCalculateTorque()
{
double torqueConstant = 1;
double momentOfInertia = 2;
double viscousFrictionCoefficient = 3;
MotorModel motorModel(torqueConstant, momentOfInertia, viscousFrictionCoefficient);
double previousTime = 4;
motorModel.setPreviousTime(previousTime);
double currentTime = 5;
double inputCurrent = 6;
double actual = motorModel.calculateTorque(currentTime, inputCurrent);
double expected = torqueConstant * inputCurrent;
ASSERT(actual, expected)
}
int main()
{
TEST(testCalculateDefiniteIntegralValue);
TEST(testCalculateTorque);
return 0;
}
テスト実行の追加忘れが無いように TEST マクロを設けた。
薄田達哉 / tatsuyasusukidacalculateAngularAcceleration() のテスト
unittest.cpp（追加分）
void testCalculateAngularAcceleration()
{
double torqueConstant = 1;
double momentOfInertia = 2;
double viscousFrictionCoefficient = 3;
MotorModel motorModel(torqueConstant, momentOfInertia, viscousFrictionCoefficient);
double previousTime = 4;
motorModel.setPreviousTime(previousTime);
double currentTime = 5;
double inputCurrent = 6;
double actual = motorModel.calculateAngularAcceleration(currentTime, inputCurrent);
double torque = motorModel.getLastTorque();
double frictionalTorque = motorModel.getViscousFrictionCoefficient() * motorModel.getLastAngularVelocity();
double loadTorque = motorModel.getLoadTorque(currentTime);
double expected = (torque - (frictionalTorque + loadTorque)) / motorModel.getMomentOfInertia();
ASSERT(actual, expected)
}
最終的なテストコード
unittest.cpp
#include <cassert>
#include <cstdio>
#include "MotorModel.hh"
bool g_debug = false;
#define ASSERT(actual, expected) \
if (g_debug) \
printf("DEBUG: actual = %f, expected = %f\n", actual, expected); \
assert(actual == expected);
#define TEST(fn) \
printf("%s\n", #fn); \
fn();
void testCalculateDefiniteIntegralValue()
{
double torqueConstant = 1;
double momentOfInertia = 2;
double viscousFrictionCoefficient = 3;
MotorModel motorModel(torqueConstant, momentOfInertia, viscousFrictionCoefficient);
double previousTime = 1;
double previousValue = 2;
double currentTime = 3;
double currentValue = 4;
double actual = motorModel.calculateDefiniteIntegralValue(
previousTime,
previousValue,
currentTime,
currentValue);
double expected = (previousValue + currentValue) * (currentTime - previousTime) / 2;
ASSERT(actual, expected)
}
void testCalculateTorque()
{
double torqueConstant = 1;
double momentOfInertia = 2;
double viscousFrictionCoefficient = 3;
MotorModel motorModel(torqueConstant, momentOfInertia, viscousFrictionCoefficient);
double previousTime = 4;
motorModel.setPreviousTime(previousTime);
double currentTime = 5;
double inputCurrent = 6;
double actual = motorModel.calculateTorque(currentTime, inputCurrent);
double expected = torqueConstant * inputCurrent;
ASSERT(actual, expected)
}
void testCalculateAngularAcceleration()
{
double torqueConstant = 1;
double momentOfInertia = 2;
double viscousFrictionCoefficient = 3;
MotorModel motorModel(torqueConstant, momentOfInertia, viscousFrictionCoefficient);
double previousTime = 4;
motorModel.setPreviousTime(previousTime);
double currentTime = 5;
double inputCurrent = 6;
double actual = motorModel.calculateAngularAcceleration(currentTime, inputCurrent);
double torque = motorModel.getLastTorque();
double frictionalTorque = motorModel.getViscousFrictionCoefficient() * motorModel.getLastAngularVelocity();
double loadTorque = motorModel.getLoadTorque(currentTime);
double expected = (torque - (frictionalTorque + loadTorque)) / motorModel.getMomentOfInertia();
ASSERT(actual, expected)
}
void testCalculateAngularVelocity()
{
double torqueConstant = 1;
double momentOfInertia = 2;
double viscousFrictionCoefficient = 3;
MotorModel motorModel(torqueConstant, momentOfInertia, viscousFrictionCoefficient);
double previousTime = 4;
motorModel.setPreviousTime(previousTime);
double currentTime = 5;
double inputCurrent = 6;
double previousAngularAcceleration = motorModel.getLastAngularAcceleration();
double actual = motorModel.calculateAngularVelocity(currentTime, inputCurrent);
double currentAngularAcceleration = motorModel.getLastAngularAcceleration();
double expected = motorModel.calculateDefiniteIntegralValue(
previousTime,
previousAngularAcceleration,
currentTime,
currentAngularAcceleration);
ASSERT(actual, expected)
}
void testCalculatePosition()
{
double torqueConstant = 1;
double momentOfInertia = 2;
double viscousFrictionCoefficient = 3;
MotorModel motorModel(torqueConstant, momentOfInertia, viscousFrictionCoefficient);
double previousTime = 4;
motorModel.setPreviousTime(previousTime);
double currentTime = 5;
double inputCurrent = 6;
double previousAngularVelocity = motorModel.getLastAngularVelocity();
double actual = motorModel.calculatePosition(currentTime, inputCurrent);
double currentAngularVelocity = motorModel.getLastAngularVelocity();
double expected = motorModel.calculateDefiniteIntegralValue(
previousTime,
previousAngularVelocity,
currentTime,
currentAngularVelocity);
ASSERT(actual, expected)
}
int main()
{
TEST(testCalculateDefiniteIntegralValue);
TEST(testCalculateTorque);
TEST(testCalculateAngularAcceleration);
TEST(testCalculateAngularVelocity);
TEST(testCalculatePosition);
return 0;
}
おわりに
次はロボットアーム VR コンテンツに組み込んでいこう。
このスクラップは2024/01/02にクローズされました