こんにちは。はじめて質問させていただきます。
簡易的なシミュレーションの計算精度確認として単振り子（質量1 kg、ロッド長1.5 m）の力学的エネルギーを計算したのですが、下図のように大きく散逸していく結果となりました。
（シミュレーション画面でも徐々に振幅が小さくなっていく様子が見られました。）
エネルギーの計算はSimpleControllerアイテム（下記コード）を使って行っています。
摩擦等は設定していませんのでエネルギーは保存されるはずですが、何か非保存力に関係する設定があるのか、計算方法の間違いでしょうか。あるいはAISTSimulatorの仕様なのでしょうか。
なお使用環境は Choreonoid-1.7.0、Ubuntu18.04です。
ご回答いただけると助かります。よろしくお願いいたします。
#include <cnoid/SimpleController>
#include <cnoid/EigenTypes>
#include <cnoid/SimulatorItem>
#include <cnoid/RootItem>
#include <iostream>
#include <fstream>
namespace cnoid
{
class CalcSysEnergy : public SimpleController
{
private:
BodyPtr BodyPtr_;
double E_kinetic_;
double E_pot_;
double E_total_;
const double g_ = 9.80665;
std::string body_name_;
SimulatorItem *SimulatorItem_;
std::string logfile_path_ = "/path/to/directory/";
std::string logfile_extension_ = ".txt";
public:
virtual bool initialize(SimpleControllerIO *IoPtr) override
{
BodyPtr_ = IoPtr->body();
body_name_ = BodyPtr_->name();
for (int i = 0; i < BodyPtr_->numLinks(); ++i)
{
Link *LinkPtr = BodyPtr_->link(i);
IoPtr->enableInput(LinkPtr, LINK_POSITION);
}
for (int i = 0; i < BodyPtr_->numJoints(); ++i)
{
Link *jointPtr = BodyPtr_->joint(i);
IoPtr->enableInput(jointPtr, JOINT_VELOCITY);
}
std::ofstream logfile(logfile_path_+body_name_+logfile_extension_, std::ofstream::out); // clear log file
if (!logfile)
{
std::cout << "logfile did not opend." << std::endl;
}
else {
std::cout << "logfile opend." << std::endl;
logfile << "time,Total_Energy,Kinetic_Energy,Potential_Energy" << std::endl;
}
return true;
}
virtual bool start() override
{
Item *BodyItem = RootItem::instance()->findItem(body_name_);
SimulatorItem_ = SimulatorItem::findActiveSimulatorItemFor(BodyItem);
return true;
}
virtual bool control() override
{
Vector3 com_W = BodyPtr_->calcCenterOfMass();
BodyPtr_->calcForwardKinematics(true, true);
for (int id = 0; id < BodyPtr_->numLinks(); ++id)
{
Link *LinkPtr = BodyPtr_->link(id);
double mass = LinkPtr->mass();
Matrix3 inertia = LinkPtr->I();
Vector3 com_L = LinkPtr->centerOfMass();
Vector3 vel_W = LinkPtr->v();
Vector3 ang_vel_W = LinkPtr->w();
Vector3 com_W = LinkPtr->centerOfMassGlobal();
E_kinetic_ += 0.5 * (mass * vel_W.dot(vel_W) + 2.0 * mass * vel_W.dot(ang_vel_W.cross(com_L)) + ang_vel_W.dot(inertia * ang_vel_W));
E_pot_ += mass * g_ * com_W[2];
E_total_ = (E_kinetic_ + E_pot_);
}
double sim_time = SimulatorItem_->simulationTime();
std::ofstream logfile(logfile_path_+body_name_+logfile_extension_, std::ofstream::app);
if (!logfile)
{
std::cout << "logfile did not opend." << std::endl;
}
else {
logfile << sim_time << "," << E_total_ << "," << E_kinetic_ << "," << E_pot_ << std::endl;
}
E_kinetic_ = E_pot_ = E_total_ = 0;
return true;
}
CalcSysEnergy();
~CalcSysEnergy();
};
CNOID_IMPLEMENT_SIMPLE_CONTROLLER_FACTORY(CalcSysEnergy)
CalcSysEnergy::CalcSysEnergy()
{
E_kinetic_ = 0.0;
E_pot_ = 0.0;
E_total_ = 0.0;
}
CalcSysEnergy::~CalcSysEnergy()
{
}
} // namespace cnoid