シミュレーション中にエラーを引き起こすMultiBodyPlantのコンテキストでnansを含む余分なボディ
urdfファイルをインポートしてMultiBodyPlantを作成した後、コンテキストを出力すると、nansを含む追加のパラメーターグループが表示されます。完全に印刷されたコンテキストはここで見ることができます。説明/理解できないパラメータグループは次のとおりです。
18 numeric parameter groups with
10 parameters
nan nan nan nan nan nan nan nan nan nan
以下を使用してプラントトポロジを取得する場合： pydot.graph_from_dot_data(plant.GetTopologyGraphvizString())[0].write_svg("robot_topology.svg")
urdfを作成しているときに期待どおりにプラントのトポロジが表示されます（WorldBodyInstance内に追加のWorldBodyがあります）。トポロジーイメージはここで見ることができます。
この余分なボディは、連続時間シミュレーション（time_step = 0.0）中に次のエラーを引き起こしているようです。
RuntimeError: Encountered singular articulated body hinge inertia for body node index 1. Please ensure that this body has non-zero inertia along all axes of motion.
最初の3つの疑似リンクを削除しようとしましたが、コンテキストの最初のパラメーターグループはまだnansを使用しています。他のすべてのパラメータグループは、urdfファイルのbody / jointに正しく対応しています。
このエラーは、urdfファイルの書き込み方法にある可能性がありますか？
urdfファイルはここにあります。コンテキストの印刷に使用されるコード：
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
Parser(plant, scene_graph).AddModelFromFile(SCpath)
# Remove gravity
plantGravityField = plant.gravity_field()
plantGravityField.set_gravity_vector([0,0,0])
plant.Finalize()
# Adds the MeshcatVisualizer and wires it to the SceneGraph.
meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url, delete_prefix_on_load=True)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant_context = plant.GetMyMutableContextFromRoot(context)
print(plant_context)
離散時間シミュレーション（time_step = 0.001）を実行すると、シミュレーションは正常に実行されますが、次の行を使用してbase_rollジョイントにジョイントトルクを適用した後、meshcatビジュアライザーまたはシミュレーション後の印刷コンテキストに変化は見られません。
jointAcutation = np.zeros((plant.num_actuators(),1))
jointAcutation[0] = 10
plant.get_actuation_input_port().FixValue(plant_context, jointAcutation)
simulator = Simulator(diagram, context)
meshcat.load()
meshcat.start_recording()
simulator.AdvanceTo(30.0)
meshcat.stop_recording()
meshcat.publish_recording()
print(plant_context)
したがって、連続時間シミュレーションと離散時間シミュレーションの両方が、モデルから説明できないnansが原因で（おそらく）失敗しているように見えます。
回答
NaNを使用すると、世界のボディにパラメータグループの対応に見ている、と彼らは不審に見えるが、私は彼らがあなたの問題の根本とは思わないこと。ワールドボディには有効な慣性パラメーターのセットがなく（したがって、NaNに設定されています）、コードでは特殊なケースとして処理されます。現在の実装で記述されているように、bodyパラメーターAPIはすべてのbodyに遍在しています。各ボディには、有効かどうかに関係なく、コンテキスト内に一連のパラメーターがあります。これは特別な団体（「世界」）の論点かもしれないので、私は議論する問題を開いた。
あなたが今抱えている問題は、あなたのbase_mainリンク（そしてあなたのロボット全体）が自由に浮かぶ物体だからです。ゼロ質量のリンクのフリーフローティングツリーを、ゼロ以外の質量のリンクの別のフリーフローティングツリーにアタッチすることは許可されていません。これらを接続するジョイントにトルクを加えるJoint_base_yawと、無限の加速が発生するためです。船内のボディに。これを解決するには：
（オプション1）：URDFファイル
base_mainとの間に固定ジョイントを追加しworldます。<joint name="base_main" type="fixed"> <parent link="world"/> <child link="base_main"/> </joint>（オプション2）：
base_mainリンクのボディフレームをコードのワールドフレームに溶接します。plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base_main"))