Corpo extra com nans no contexto de um MultiBodyPlant causando erro durante a simulação
Depois de importar um arquivo urdf e criar um MultiBodyPlant, vejo um grupo de parâmetros extra com nans quando imprimo o contexto. O contexto impresso completo pode ser visto aqui . O grupo de parâmetros que não consigo explicar / entender é este:
18 numeric parameter groups with
10 parameters
nan nan nan nan nan nan nan nan nan nan
Se eu obtiver a topologia da planta usando: pydot.graph_from_dot_data(plant.GetTopologyGraphvizString())[0].write_svg("robot_topology.svg")
Eu vejo a topologia da planta como eu esperava ao fazer o urdf (com um WorldBody extra dentro do WorldBodyInstance ). A imagem da topologia pode ser vista aqui .
Este corpo extra parece estar causando o seguinte erro durante uma simulação em tempo contínuo (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.
Tentei remover os 3 primeiros pseudo-links, mas o primeiro grupo de parâmetros no contexto ainda está com nans. Todos os outros grupos de parâmetros correspondem corretamente aos corpos / juntas no arquivo urdf.
Este erro pode estar em como um arquivo urdf é escrito?
O arquivo urdf pode ser encontrado aqui . O código usado para imprimir o contexto:
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)
Se eu executar uma simulação em tempo discreto (time_step = 0,001), a simulação será executada com sucesso, mas não vejo nenhuma mudança no visualizador meshcat ou no contexto impresso pós-simulação após aplicar um torque de junta à junta base_roll usando as seguintes linhas:
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)
Conseqüentemente, ambas as simulações de tempo contínuo e discreto parecem estar falhando (provavelmente) devido aos nans que não posso explicar a partir do modelo.
Respostas
Os NaNs que você está vendo no grupo de parâmetros correspondem ao corpo do mundo e, embora pareçam suspeitos, não acho que sejam a raiz do seu problema. O corpo do mundo não tem um conjunto válido de parâmetros inerciais (portanto, eles são definidos como NaN) e é tratado como um caso especial no código. Como está escrito na implementação atual, o parâmetro API do corpo é onipresente para todos os corpos. Cada corpo possui um conjunto de parâmetros no contexto, independentemente de serem válidos. Este pode ser um ponto de discórdia para órgãos especiais ('mundo'), então abri uma questão para discutir.
O problema que você está tendo agora é porque seu base_mainlink (e, portanto, todo o seu robô) é um corpo flutuante livre. Você não pode ter uma árvore flutuante livre de links de massa zero anexada a uma outra árvore flutuante de links com massa diferente de zero porque qualquer torque aplicado na junta que os conecta ( Joint_base_yawno seu caso) causaria uma aceleração infinita nos corpos internos. Para resolver isso:
(Opção 1): Adicione uma junta fixa entre
base_maineworldem seu arquivo URDF.<joint name="base_main" type="fixed"> <parent link="world"/> <child link="base_main"/> </joint>(Opção 2): Solde o
base_mainquadro do corpo do link ao quadro mundial no código.plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base_main"))