DRAKE水弹性模型接触扭矩与力可视化异常问题问询

我正在使用DRAKE进行仿真测试，针对Contact_F=results.hydroelastic_contact_info().F_Ac_W().get_coeffs()函数验证水弹性接触力获取功能。初始测试中，记录的接触力与施加力不符，排查发现是包导入问题（疑似来自"manipulation"包），新建虚拟环境并安装最新版DRAKE后，接触力恢复正常，相关代码已更新至GitHub仓库。

但目前仍存在两个问题：

• 接触扭矩不符合预期：在该简单演示场景中，预期仅y轴存在接触扭矩（τ_y=1N*0.05m/2=0.025 Nm，τ_x=τ_z=0 Nm），但实际记录的τ_x、τ_y、τ_z均异常，即使更换不同施加力，结果仍不符合预期，相关截图：
  • τ_x异常截图
  • τ_y异常截图
  • τ_z异常截图
• 力可视化异常：力箭头中的蓝色箭头指向错误。

相关演示代码如下：

def Addfloor(plant):
    parser = Parser(plant)
    floor_path= "./verification_floor.sdf"
    floor=parser.AddModelFromFile(floor_path)
    plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("paddle2", floor),  
                   RigidTransform(RotationMatrix.MakeXRotation(np.pi*0/180),
                                    [0,0,0]))
    return floor

def AddBox(plant):
    parser = Parser(plant)
    box_path = "./verification_box.sdf"
    my_box = parser.AddModelFromFile(box_path)
                                           
    false_body1 = plant.AddRigidBody(
        "false_body1", my_box, SpatialInertia(0, [0, 0, 0],
                                              UnitInertia(0, 0, 0)))
    false_body2 = plant.AddRigidBody(
        "false_body2", my_box, SpatialInertia(0, [0, 0, 0],
                                              UnitInertia(0, 0, 0)))
    joint_x = plant.AddJoint(
        PrismaticJoint("joint_x", plant.world_frame(),
                       plant.GetFrameByName("false_body1"), [1, 0, 0], -5, 5))
    plant.AddJointActuator("joint_x", joint_x)
    joint_z = plant.AddJoint(
        PrismaticJoint("joint_z", plant.GetFrameByName("false_body1"),
                       plant.GetFrameByName("false_body2"), [0, 0, 1], -5, 5))
    plant.AddJointActuator("joint_z", joint_z)
    
    joint_theta = plant.AddJoint(
        RevoluteJoint("joint_theta", plant.GetFrameByName("false_body2"),
                       plant.GetFrameByName("paddleh"), [0, 1, 0], 0.5))
    plant.AddJointActuator("joint_theta", joint_theta)
    return my_box


class Box_on_floor(LeafSystem):

    def __init__(self, plant):
        LeafSystem.__init__(self)
        self._plant = plant
        self.contF_box=np.array([0,0,0,0,0,0])
        self.all_command=np.array([0,0,0])
        self.count=0
        self.DeclareAbstractInputPort("contact_results",
                                      AbstractValue.Make(ContactResults()))
        self.DeclareVectorOutputPort("actuation", 3, self.CalcOutput)

    def CalcOutput(self, context, output):
        self.count=self.count+1
        g = self._plant.gravity_field().gravity_vector()[[0,2]]
        results = self.get_input_port(0).Eval(context)
        if results.num_hydroelastic_contacts() == 0:
            self.contF_box = np.vstack((self.contF_box, [0,0,0,0,0,0]))
        for i in range(results.num_hydroelastic_contacts()):
            info = results.hydroelastic_contact_info(i)
            this_F=info.F_Ac_W().get_coeffs()
            self.contF_box = np.vstack((self.contF_box, this_F))    
        k=1/1000
        if self.count < 1500:
            F=[0,0,0]  
        elif self.count < 2500:
            F=[k*(self.count-1500), 0, 0]
        else:
            F=[k*(2500-1500), 0, 0]
        output.SetFromVector(F) 
        self.all_command = np.vstack((self.all_command, F))

# before plant finish 
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001)
box = Addfloor(plant)
my_box = AddBox(plant)
plant.GetJointByName("joint_x").set_default_translation(0)
plant.GetJointByName("joint_z").set_default_translation(0.2)
plant.Finalize()

MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)
ContactVisualizer.AddToBuilder(
    builder, plant, meshcat,
    ContactVisualizerParams(radius=0.005))

# add system
controller = builder.AddSystem(Box_on_floor(plant))
zoh = builder.AddSystem(ZeroOrderHold(0.001,AbstractValue.Make(ContactResults())))
builder.Connect(plant.get_contact_results_output_port(), zoh.get_input_port(0))
builder.Connect(zoh.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), plant.get_actuation_input_port())
logger = LogVectorOutput(plant.get_state_output_port(my_box), builder)


# start diagarm
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
simulator.set_target_realtime_rate(0.5) # it defines the time step
simulator.AdvanceTo(6); # total time

我的仿真场景为：1kg的箱体静置在地面，通过x轴关节施加线性递增的外力（最大外力不超过最大静摩擦力），箱体处于静止状态。相关SDF文件已上传至上述GitHub仓库，期待得到问题解决的技术指导。

内容的提问来源于stack exchange，提问作者Lin YANG