在drake的刚体工厂中添加扭转弹簧

import numpy as np
from pydrake.all import (
    RigidBodyFrame, RigidBodyTree, SpatialInertia, UnitInertia, TorsionalSpring
)

# 创建刚体树
tree = RigidBodyTree()

# 创建两个刚体
body1 = tree.AddBody(RigidBodyFrame("body1", np.array([0, 0, 0]), UnitInertia(), SpatialInertia()))
body2 = tree.AddBody(RigidBodyFrame("body2", np.array([1, 0, 0]), UnitInertia(), SpatialInertia()))

# 创建一个连接两个刚体的框架
frame1 = RigidBodyFrame("frame1", np.array([0, 0, 0]), np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), np.eye(6))
frame2 = RigidBodyFrame("frame2", np.array([1, 0, 0]), np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), np.eye(6))

# 将框架添加到刚体
tree.AddFrameToBody(body1, frame1)
tree.AddFrameToBody(body2, frame2)

# 创建扭转弹簧
spring = TorsionalSpring(
    body1.body_frame(), frame1.frame_bcm,
    body2.body_frame(), frame2.frame_bcm,
    k=100.0,  # 刚度系数
    c=10.0    # 阻尼系数
)

# 将扭转弹簧添加到刚体树
tree.AddTorsionalSpring(spring)