1022 lines
41 KiB
Python
1022 lines
41 KiB
Python
"""
|
|
约束和关节系统模块
|
|
负责各种物理约束和关节的创建和管理
|
|
"""
|
|
|
|
from panda3d.core import Point3, Vec3, TransformState, LVector3f
|
|
from panda3d.bullet import BulletConstraint
|
|
from panda3d.bullet import BulletPoint2PointConstraint, BulletHingeConstraint
|
|
from panda3d.bullet import BulletSliderConstraint, BulletConeTwistConstraint
|
|
from panda3d.bullet import BulletGeneric6DofConstraint, BulletGeneric6DofSpringConstraint
|
|
from panda3d.bullet import BulletGearConstraint, BulletFixedConstraint
|
|
import math
|
|
import time
|
|
from collections import deque
|
|
|
|
|
|
class ConstraintManager:
|
|
"""
|
|
约束管理器类
|
|
管理各种类型的物理约束和关节
|
|
"""
|
|
|
|
CONSTRAINT_POINT2POINT = 'point2point'
|
|
CONSTRAINT_HINGE = 'hinge'
|
|
CONSTRAINT_SLIDER = 'slider'
|
|
CONSTRAINT_CONETWIST = 'conetwist'
|
|
CONSTRAINT_6DOF = '6dof'
|
|
CONSTRAINT_6DOF_SPRING = '6dof_spring'
|
|
CONSTRAINT_GEAR = 'gear'
|
|
CONSTRAINT_FIXED = 'fixed'
|
|
|
|
def __init__(self):
|
|
"""
|
|
初始化约束管理器
|
|
"""
|
|
self.constraints = []
|
|
self.constraint_groups = {} # 约束组
|
|
self.constraint_limits = {} # 约束限制
|
|
self.constraint_motors = {} # 约束马达
|
|
self.constraint_springs = {} # 约束弹簧
|
|
|
|
print("约束管理器初始化完成")
|
|
|
|
def create_point2point_constraint(self, body_a, body_b, pivot_a, pivot_b,
|
|
collision=True, breaking_threshold=0.0,
|
|
disable_collision_between_linked_bodies=True):
|
|
"""
|
|
创建点对点约束
|
|
|
|
Args:
|
|
body_a (RigidBody): 第一个刚体
|
|
body_b (RigidBody): 第二个刚体
|
|
pivot_a (Point3): 第一个刚体上的连接点
|
|
pivot_b (Point3): 第二个刚体上的连接点
|
|
collision (bool): 是否允许两个刚体碰撞
|
|
breaking_threshold (float): 破裂阈值
|
|
disable_collision_between_linked_bodies (bool): 是否禁用连接刚体间的碰撞
|
|
|
|
Returns:
|
|
BulletPoint2PointConstraint: 点对点约束对象
|
|
"""
|
|
constraint = BulletPoint2PointConstraint(
|
|
body_a.bullet_body, body_b.bullet_body, pivot_a, pivot_b)
|
|
|
|
# 设置约束属性
|
|
constraint.setOverrideEnabled(not collision)
|
|
if breaking_threshold > 0:
|
|
constraint.setBreakingImpulseThreshold(breaking_threshold)
|
|
|
|
# 禁用连接刚体间的碰撞
|
|
if disable_collision_between_linked_bodies:
|
|
body_a.bullet_body.setIgnoreCollisionCheck(body_b.bullet_body, True)
|
|
|
|
# 保存约束信息
|
|
constraint_info = {
|
|
'type': self.CONSTRAINT_POINT2POINT,
|
|
'constraint': constraint,
|
|
'body_a': body_a,
|
|
'body_b': body_b,
|
|
'properties': {
|
|
'pivot_a': pivot_a,
|
|
'pivot_b': pivot_b,
|
|
'collision': collision,
|
|
'breaking_threshold': breaking_threshold,
|
|
'disable_collision': disable_collision_between_linked_bodies
|
|
},
|
|
'creation_time': time.time(),
|
|
'is_enabled': True,
|
|
'group': None,
|
|
'tags': set(),
|
|
'feedback': None
|
|
}
|
|
|
|
self.constraints.append(constraint_info)
|
|
return constraint
|
|
|
|
def create_hinge_constraint(self, body_a, body_b, pivot_a, pivot_b, axis_a, axis_b,
|
|
collision=True, breaking_threshold=0.0, limits=None,
|
|
motor=None, disable_collision_between_linked_bodies=True):
|
|
"""
|
|
创建铰链约束
|
|
|
|
Args:
|
|
body_a (RigidBody): 第一个刚体
|
|
body_b (RigidBody): 第二个刚体
|
|
pivot_a (Point3): 第一个刚体上的连接点
|
|
pivot_b (Point3): 第二个刚体上的连接点
|
|
axis_a (Vec3): 第一个刚体上的轴向
|
|
axis_b (Vec3): 第二个刚体上的轴向
|
|
collision (bool): 是否允许两个刚体碰撞
|
|
breaking_threshold (float): 破裂阈值
|
|
limits (tuple): 旋转限制 (min_angle, max_angle) 或 None
|
|
motor (dict): 马达参数 {'target_velocity': float, 'max_impulse': float}
|
|
disable_collision_between_linked_bodies (bool): 是否禁用连接刚体间的碰撞
|
|
|
|
Returns:
|
|
BulletHingeConstraint: 铰链约束对象
|
|
"""
|
|
constraint = BulletHingeConstraint(
|
|
body_a.bullet_body, body_b.bullet_body, pivot_a, pivot_b, axis_a, axis_b)
|
|
|
|
# 设置约束属性
|
|
constraint.setOverrideEnabled(not collision)
|
|
if breaking_threshold > 0:
|
|
constraint.setBreakingImpulseThreshold(breaking_threshold)
|
|
|
|
# 设置旋转限制
|
|
if limits:
|
|
min_angle, max_angle = limits
|
|
constraint.setLimit(min_angle, max_angle)
|
|
|
|
# 设置马达
|
|
if motor:
|
|
target_velocity = motor.get('target_velocity', 0.0)
|
|
max_impulse = motor.get('max_impulse', 1.0)
|
|
constraint.enableAngularMotor(True, target_velocity, max_impulse)
|
|
|
|
# 禁用连接刚体间的碰撞
|
|
if disable_collision_between_linked_bodies:
|
|
body_a.bullet_body.setIgnoreCollisionCheck(body_b.bullet_body, True)
|
|
|
|
# 保存约束信息
|
|
constraint_info = {
|
|
'type': self.CONSTRAINT_HINGE,
|
|
'constraint': constraint,
|
|
'body_a': body_a,
|
|
'body_b': body_b,
|
|
'properties': {
|
|
'pivot_a': pivot_a,
|
|
'pivot_b': pivot_b,
|
|
'axis_a': axis_a,
|
|
'axis_b': axis_b,
|
|
'collision': collision,
|
|
'breaking_threshold': breaking_threshold,
|
|
'limits': limits,
|
|
'motor': motor,
|
|
'disable_collision': disable_collision_between_linked_bodies
|
|
},
|
|
'creation_time': time.time(),
|
|
'is_enabled': True,
|
|
'group': None,
|
|
'tags': set(),
|
|
'feedback': None,
|
|
'motor_enabled': motor is not None,
|
|
'current_angle': 0.0,
|
|
'angular_velocity': 0.0
|
|
}
|
|
|
|
self.constraints.append(constraint_info)
|
|
return constraint
|
|
|
|
def create_slider_constraint(self, body_a, body_b, frame_a=None, frame_b=None,
|
|
collision=True, breaking_threshold=0.0, limits=None,
|
|
motor=None, disable_collision_between_linked_bodies=True):
|
|
"""
|
|
创建滑动约束
|
|
|
|
Args:
|
|
body_a (RigidBody): 第一个刚体
|
|
body_b (RigidBody): 第二个刚体
|
|
frame_a (TransformState): 第一个刚体上的参考帧
|
|
frame_b (TransformState): 第二个刚体上的参考帧
|
|
collision (bool): 是否允许两个刚体碰撞
|
|
breaking_threshold (float): 破裂阈值
|
|
limits (tuple): 滑动限制 (linear_min, linear_max, angular_min, angular_max) 或 None
|
|
motor (dict): 马达参数 {'target_linear_velocity': float, 'max_linear_force': float,
|
|
'target_angular_velocity': float, 'max_angular_force': float}
|
|
disable_collision_between_linked_bodies (bool): 是否禁用连接刚体间的碰撞
|
|
|
|
Returns:
|
|
BulletSliderConstraint: 滑动约束对象
|
|
"""
|
|
# 如果没有提供参考帧，创建默认的
|
|
if frame_a is None:
|
|
frame_a = TransformState.makePos(Point3(0, 0, 0))
|
|
if frame_b is None:
|
|
frame_b = TransformState.makePos(Point3(0, 0, 0))
|
|
|
|
constraint = BulletSliderConstraint(
|
|
body_a.bullet_body, body_b.bullet_body, frame_a, frame_b, useLinearReferenceFrameA=True)
|
|
|
|
# 设置约束属性
|
|
constraint.setOverrideEnabled(not collision)
|
|
if breaking_threshold > 0:
|
|
constraint.setBreakingImpulseThreshold(breaking_threshold)
|
|
|
|
# 设置滑动限制
|
|
if limits:
|
|
linear_min, linear_max, angular_min, angular_max = limits
|
|
constraint.setLowerLinLimit(linear_min)
|
|
constraint.setUpperLinLimit(linear_max)
|
|
constraint.setLowerAngLimit(angular_min)
|
|
constraint.setUpperAngLimit(angular_max)
|
|
|
|
# 设置马达
|
|
if motor:
|
|
# 滑动约束的马达设置比较复杂，需要分别设置线性和角马达
|
|
pass
|
|
|
|
# 禁用连接刚体间的碰撞
|
|
if disable_collision_between_linked_bodies:
|
|
body_a.bullet_body.setIgnoreCollisionCheck(body_b.bullet_body, True)
|
|
|
|
# 保存约束信息
|
|
constraint_info = {
|
|
'type': self.CONSTRAINT_SLIDER,
|
|
'constraint': constraint,
|
|
'body_a': body_a,
|
|
'body_b': body_b,
|
|
'properties': {
|
|
'frame_a': frame_a,
|
|
'frame_b': frame_b,
|
|
'collision': collision,
|
|
'breaking_threshold': breaking_threshold,
|
|
'limits': limits,
|
|
'motor': motor,
|
|
'disable_collision': disable_collision_between_linked_bodies
|
|
},
|
|
'creation_time': time.time(),
|
|
'is_enabled': True,
|
|
'group': None,
|
|
'tags': set(),
|
|
'feedback': None,
|
|
'motor_enabled': motor is not None,
|
|
'current_linear_position': 0.0,
|
|
'current_angular_position': 0.0
|
|
}
|
|
|
|
self.constraints.append(constraint_info)
|
|
return constraint
|
|
|
|
def create_cone_twist_constraint(self, body_a, body_b, frame_a=None, frame_b=None,
|
|
collision=True, breaking_threshold=0.0, limits=None,
|
|
motor=None, disable_collision_between_linked_bodies=True):
|
|
"""
|
|
创建锥形扭转约束
|
|
|
|
Args:
|
|
body_a (RigidBody): 第一个刚体
|
|
body_b (RigidBody): 第二个刚体
|
|
frame_a (TransformState): 第一个刚体上的参考帧
|
|
frame_b (TransformState): 第二个刚体上的参考帧
|
|
collision (bool): 是否允许两个刚体碰撞
|
|
breaking_threshold (float): 破裂阈值
|
|
limits (tuple): 限制 (swing_span1, swing_span2, twist_span) 或 None
|
|
motor (dict): 马达参数
|
|
disable_collision_between_linked_bodies (bool): 是否禁用连接刚体间的碰撞
|
|
|
|
Returns:
|
|
BulletConeTwistConstraint: 锥形扭转约束对象
|
|
"""
|
|
# 如果没有提供参考帧，创建默认的
|
|
if frame_a is None:
|
|
frame_a = TransformState.makePos(Point3(0, 0, 0))
|
|
if frame_b is None:
|
|
frame_b = TransformState.makePos(Point3(0, 0, 0))
|
|
|
|
constraint = BulletConeTwistConstraint(
|
|
body_a.bullet_body, body_b.bullet_body, frame_a, frame_b)
|
|
|
|
# 设置约束属性
|
|
constraint.setOverrideEnabled(not collision)
|
|
if breaking_threshold > 0:
|
|
constraint.setBreakingImpulseThreshold(breaking_threshold)
|
|
|
|
# 设置限制
|
|
if limits:
|
|
swing_span1, swing_span2, twist_span = limits
|
|
constraint.setLimit(swing_span1, swing_span2, twist_span)
|
|
|
|
# 禁用连接刚体间的碰撞
|
|
if disable_collision_between_linked_bodies:
|
|
body_a.bullet_body.setIgnoreCollisionCheck(body_b.bullet_body, True)
|
|
|
|
# 保存约束信息
|
|
constraint_info = {
|
|
'type': self.CONSTRAINT_CONETWIST,
|
|
'constraint': constraint,
|
|
'body_a': body_a,
|
|
'body_b': body_b,
|
|
'properties': {
|
|
'frame_a': frame_a,
|
|
'frame_b': frame_b,
|
|
'collision': collision,
|
|
'breaking_threshold': breaking_threshold,
|
|
'limits': limits,
|
|
'motor': motor,
|
|
'disable_collision': disable_collision_between_linked_bodies
|
|
},
|
|
'creation_time': time.time(),
|
|
'is_enabled': True,
|
|
'group': None,
|
|
'tags': set(),
|
|
'feedback': None
|
|
}
|
|
|
|
self.constraints.append(constraint_info)
|
|
return constraint
|
|
|
|
def create_6dof_constraint(self, body_a, body_b, frame_a=None, frame_b=None,
|
|
collision=True, breaking_threshold=0.0, linear_limits=None,
|
|
angular_limits=None, motors=None, springs=None,
|
|
disable_collision_between_linked_bodies=True):
|
|
"""
|
|
创建6自由度约束
|
|
|
|
Args:
|
|
body_a (RigidBody): 第一个刚体
|
|
body_b (RigidBody): 第二个刚体
|
|
frame_a (TransformState): 第一个刚体上的参考帧
|
|
frame_b (TransformState): 第二个刚体上的参考帧
|
|
collision (bool): 是否允许两个刚体碰撞
|
|
breaking_threshold (float): 破裂阈值
|
|
linear_limits (tuple): 线性限制 (x_min, x_max, y_min, y_max, z_min, z_max) 或 None
|
|
angular_limits (tuple): 角度限制 (x_min, x_max, y_min, y_max, z_min, z_max) 或 None
|
|
motors (dict): 马达参数 {'linear': {axis: {'target_velocity', 'max_force'}},
|
|
'angular': {axis: {'target_velocity', 'max_force'}}}
|
|
springs (dict): 弹簧参数 {'linear': {axis: {'stiffness', 'damping', 'equilibrium'}},
|
|
'angular': {axis: {'stiffness', 'damping', 'equilibrium'}}}
|
|
disable_collision_between_linked_bodies (bool): 是否禁用连接刚体间的碰撞
|
|
|
|
Returns:
|
|
BulletGeneric6DofConstraint: 6自由度约束对象
|
|
"""
|
|
# 如果没有提供参考帧，创建默认的
|
|
if frame_a is None:
|
|
frame_a = TransformState.makePos(Point3(0, 0, 0))
|
|
if frame_b is None:
|
|
frame_b = TransformState.makePos(Point3(0, 0, 0))
|
|
|
|
constraint = BulletGeneric6DofConstraint(
|
|
body_a.bullet_body, body_b.bullet_body, frame_a, frame_b, useLinearReferenceFrameA=True)
|
|
|
|
# 设置约束属性
|
|
constraint.setOverrideEnabled(not collision)
|
|
if breaking_threshold > 0:
|
|
constraint.setBreakingImpulseThreshold(breaking_threshold)
|
|
|
|
# 设置线性限制
|
|
if linear_limits:
|
|
x_min, x_max, y_min, y_max, z_min, z_max = linear_limits
|
|
constraint.setLinearLowerLimit(Vec3(x_min, y_min, z_min))
|
|
constraint.setLinearUpperLimit(Vec3(x_max, y_max, z_max))
|
|
|
|
# 设置角度限制
|
|
if angular_limits:
|
|
x_min, x_max, y_min, y_max, z_min, z_max = angular_limits
|
|
constraint.setAngularLowerLimit(Vec3(x_min, y_min, z_min))
|
|
constraint.setAngularUpperLimit(Vec3(x_max, y_max, z_max))
|
|
|
|
# 设置马达
|
|
if motors:
|
|
linear_motors = motors.get('linear', {})
|
|
angular_motors = motors.get('angular', {})
|
|
|
|
for axis in range(3):
|
|
# 线性马达
|
|
if axis in linear_motors:
|
|
motor_info = linear_motors[axis]
|
|
constraint.setTargetVelocity(axis, motor_info.get('target_velocity', 0.0))
|
|
constraint.setMaxMotorForce(axis, motor_info.get('max_force', 1.0))
|
|
constraint.setParam(axis, 1) # 启用马达
|
|
|
|
# 角马达
|
|
if axis in angular_motors:
|
|
motor_info = angular_motors[axis]
|
|
constraint.setTargetVelocity(axis + 3, motor_info.get('target_velocity', 0.0))
|
|
constraint.setMaxMotorForce(axis + 3, motor_info.get('max_force', 1.0))
|
|
constraint.setParam(axis + 3, 1) # 启用马达
|
|
|
|
# 禁用连接刚体间的碰撞
|
|
if disable_collision_between_linked_bodies:
|
|
body_a.bullet_body.setIgnoreCollisionCheck(body_b.bullet_body, True)
|
|
|
|
# 保存约束信息
|
|
constraint_info = {
|
|
'type': self.CONSTRAINT_6DOF,
|
|
'constraint': constraint,
|
|
'body_a': body_a,
|
|
'body_b': body_b,
|
|
'properties': {
|
|
'frame_a': frame_a,
|
|
'frame_b': frame_b,
|
|
'collision': collision,
|
|
'breaking_threshold': breaking_threshold,
|
|
'linear_limits': linear_limits,
|
|
'angular_limits': angular_limits,
|
|
'motors': motors,
|
|
'springs': springs,
|
|
'disable_collision': disable_collision_between_linked_bodies
|
|
},
|
|
'creation_time': time.time(),
|
|
'is_enabled': True,
|
|
'group': None,
|
|
'tags': set(),
|
|
'feedback': None,
|
|
'motor_enabled': motors is not None,
|
|
'spring_enabled': springs is not None
|
|
}
|
|
|
|
self.constraints.append(constraint_info)
|
|
return constraint
|
|
|
|
def create_6dof_spring_constraint(self, body_a, body_b, frame_a=None, frame_b=None,
|
|
collision=True, breaking_threshold=0.0, linear_limits=None,
|
|
angular_limits=None, springs=None, motors=None,
|
|
disable_collision_between_linked_bodies=True):
|
|
"""
|
|
创建6自由度弹簧约束
|
|
|
|
Args:
|
|
body_a (RigidBody): 第一个刚体
|
|
body_b (RigidBody): 第二个刚体
|
|
frame_a (TransformState): 第一个刚体上的参考帧
|
|
frame_b (TransformState): 第二个刚体上的参考帧
|
|
collision (bool): 是否允许两个刚体碰撞
|
|
breaking_threshold (float): 破裂阈值
|
|
linear_limits (tuple): 线性限制 (x_min, x_max, y_min, y_max, z_min, z_max) 或 None
|
|
angular_limits (tuple): 角度限制 (x_min, x_max, y_min, y_max, z_min, z_max) 或 None
|
|
springs (dict): 弹簧参数 {'linear': {axis: {'stiffness', 'damping', 'equilibrium'}},
|
|
'angular': {axis: {'stiffness', 'damping', 'equilibrium'}}}
|
|
motors (dict): 马达参数
|
|
disable_collision_between_linked_bodies (bool): 是否禁用连接刚体间的碰撞
|
|
|
|
Returns:
|
|
BulletGeneric6DofSpringConstraint: 6自由度弹簧约束对象
|
|
"""
|
|
# 如果没有提供参考帧，创建默认的
|
|
if frame_a is None:
|
|
frame_a = TransformState.makePos(Point3(0, 0, 0))
|
|
if frame_b is None:
|
|
frame_b = TransformState.makePos(Point3(0, 0, 0))
|
|
|
|
constraint = BulletGeneric6DofSpringConstraint(
|
|
body_a.bullet_body, body_b.bullet_body, frame_a, frame_b, useLinearReferenceFrameA=True)
|
|
|
|
# 设置约束属性
|
|
constraint.setOverrideEnabled(not collision)
|
|
if breaking_threshold > 0:
|
|
constraint.setBreakingImpulseThreshold(breaking_threshold)
|
|
|
|
# 设置线性限制
|
|
if linear_limits:
|
|
x_min, x_max, y_min, y_max, z_min, z_max = linear_limits
|
|
constraint.setLinearLowerLimit(Vec3(x_min, y_min, z_min))
|
|
constraint.setLinearUpperLimit(Vec3(x_max, y_max, z_max))
|
|
|
|
# 设置角度限制
|
|
if angular_limits:
|
|
x_min, x_max, y_min, y_max, z_min, z_max = angular_limits
|
|
constraint.setAngularLowerLimit(Vec3(x_min, y_min, z_min))
|
|
constraint.setAngularUpperLimit(Vec3(x_max, y_max, z_max))
|
|
|
|
# 设置弹簧
|
|
if springs:
|
|
linear_springs = springs.get('linear', {})
|
|
angular_springs = springs.get('angular', {})
|
|
|
|
for axis in range(3):
|
|
# 线性弹簧
|
|
if axis in linear_springs:
|
|
spring_info = linear_springs[axis]
|
|
constraint.setStiffness(axis, spring_info.get('stiffness', 0.0))
|
|
constraint.setDamping(axis, spring_info.get('damping', 0.0))
|
|
constraint.setEquilibriumPoint(axis, spring_info.get('equilibrium', 0.0))
|
|
constraint.enableSpring(axis, True)
|
|
|
|
# 角弹簧
|
|
if axis in angular_springs:
|
|
spring_info = angular_springs[axis]
|
|
constraint.setStiffness(axis + 3, spring_info.get('stiffness', 0.0))
|
|
constraint.setDamping(axis + 3, spring_info.get('damping', 0.0))
|
|
constraint.setEquilibriumPoint(axis + 3, spring_info.get('equilibrium', 0.0))
|
|
constraint.enableSpring(axis + 3, True)
|
|
|
|
# 禁用连接刚体间的碰撞
|
|
if disable_collision_between_linked_bodies:
|
|
body_a.bullet_body.setIgnoreCollisionCheck(body_b.bullet_body, True)
|
|
|
|
# 保存约束信息
|
|
constraint_info = {
|
|
'type': self.CONSTRAINT_6DOF_SPRING,
|
|
'constraint': constraint,
|
|
'body_a': body_a,
|
|
'body_b': body_b,
|
|
'properties': {
|
|
'frame_a': frame_a,
|
|
'frame_b': frame_b,
|
|
'collision': collision,
|
|
'breaking_threshold': breaking_threshold,
|
|
'linear_limits': linear_limits,
|
|
'angular_limits': angular_limits,
|
|
'springs': springs,
|
|
'motors': motors,
|
|
'disable_collision': disable_collision_between_linked_bodies
|
|
},
|
|
'creation_time': time.time(),
|
|
'is_enabled': True,
|
|
'group': None,
|
|
'tags': set(),
|
|
'feedback': None,
|
|
'spring_enabled': springs is not None,
|
|
'motor_enabled': motors is not None
|
|
}
|
|
|
|
self.constraints.append(constraint_info)
|
|
return constraint
|
|
|
|
def create_gear_constraint(self, body_a, body_b, axis_a, axis_b, ratio=1.0,
|
|
collision=True, breaking_threshold=0.0):
|
|
"""
|
|
创建齿轮约束
|
|
|
|
Args:
|
|
body_a (RigidBody): 第一个刚体
|
|
body_b (RigidBody): 第二个刚体
|
|
axis_a (Vec3): 第一个刚体上的轴向
|
|
axis_b (Vec3): 第二个刚体上的轴向
|
|
ratio (float): 齿轮比
|
|
collision (bool): 是否允许两个刚体碰撞
|
|
breaking_threshold (float): 破裂阈值
|
|
|
|
Returns:
|
|
BulletGearConstraint: 齿轮约束对象
|
|
"""
|
|
constraint = BulletGearConstraint(
|
|
body_a.bullet_body, body_b.bullet_body, axis_a, axis_b, ratio)
|
|
|
|
# 设置约束属性
|
|
constraint.setOverrideEnabled(not collision)
|
|
if breaking_threshold > 0:
|
|
constraint.setBreakingImpulseThreshold(breaking_threshold)
|
|
|
|
# 保存约束信息
|
|
constraint_info = {
|
|
'type': self.CONSTRAINT_GEAR,
|
|
'constraint': constraint,
|
|
'body_a': body_a,
|
|
'body_b': body_b,
|
|
'properties': {
|
|
'axis_a': axis_a,
|
|
'axis_b': axis_b,
|
|
'ratio': ratio,
|
|
'collision': collision,
|
|
'breaking_threshold': breaking_threshold
|
|
},
|
|
'creation_time': time.time(),
|
|
'is_enabled': True,
|
|
'group': None,
|
|
'tags': set(),
|
|
'feedback': None
|
|
}
|
|
|
|
self.constraints.append(constraint_info)
|
|
return constraint
|
|
|
|
def create_fixed_constraint(self, body_a, body_b, frame_a=None, frame_b=None,
|
|
collision=True, breaking_threshold=0.0,
|
|
disable_collision_between_linked_bodies=True):
|
|
"""
|
|
创建固定约束
|
|
|
|
Args:
|
|
body_a (RigidBody): 第一个刚体
|
|
body_b (RigidBody): 第二个刚体
|
|
frame_a (TransformState): 第一个刚体上的参考帧
|
|
frame_b (TransformState): 第二个刚体上的参考帧
|
|
collision (bool): 是否允许两个刚体碰撞
|
|
breaking_threshold (float): 破裂阈值
|
|
disable_collision_between_linked_bodies (bool): 是否禁用连接刚体间的碰撞
|
|
|
|
Returns:
|
|
BulletFixedConstraint: 固定约束对象
|
|
"""
|
|
# 如果没有提供参考帧，创建默认的
|
|
if frame_a is None:
|
|
frame_a = TransformState.makePos(Point3(0, 0, 0))
|
|
if frame_b is None:
|
|
frame_b = TransformState.makePos(Point3(0, 0, 0))
|
|
|
|
constraint = BulletFixedConstraint(
|
|
body_a.bullet_body, body_b.bullet_body, frame_a, frame_b)
|
|
|
|
# 设置约束属性
|
|
constraint.setOverrideEnabled(not collision)
|
|
if breaking_threshold > 0:
|
|
constraint.setBreakingImpulseThreshold(breaking_threshold)
|
|
|
|
# 禁用连接刚体间的碰撞
|
|
if disable_collision_between_linked_bodies:
|
|
body_a.bullet_body.setIgnoreCollisionCheck(body_b.bullet_body, True)
|
|
|
|
# 保存约束信息
|
|
constraint_info = {
|
|
'type': self.CONSTRAINT_FIXED,
|
|
'constraint': constraint,
|
|
'body_a': body_a,
|
|
'body_b': body_b,
|
|
'properties': {
|
|
'frame_a': frame_a,
|
|
'frame_b': frame_b,
|
|
'collision': collision,
|
|
'breaking_threshold': breaking_threshold,
|
|
'disable_collision': disable_collision_between_linked_bodies
|
|
},
|
|
'creation_time': time.time(),
|
|
'is_enabled': True,
|
|
'group': None,
|
|
'tags': set(),
|
|
'feedback': None
|
|
}
|
|
|
|
self.constraints.append(constraint_info)
|
|
return constraint
|
|
|
|
def remove_constraint(self, constraint):
|
|
"""
|
|
移除约束
|
|
|
|
Args:
|
|
constraint: 约束对象
|
|
"""
|
|
for constraint_info in self.constraints:
|
|
if constraint_info['constraint'] == constraint:
|
|
# 重新启用刚体间的碰撞
|
|
if constraint_info['properties'].get('disable_collision', False):
|
|
body_a = constraint_info['body_a']
|
|
body_b = constraint_info['body_b']
|
|
body_a.bullet_body.setIgnoreCollisionCheck(body_b.bullet_body, False)
|
|
|
|
self.constraints.remove(constraint_info)
|
|
return True
|
|
return False
|
|
|
|
def get_constraint_info(self, constraint):
|
|
"""
|
|
获取约束信息
|
|
|
|
Args:
|
|
constraint: 约束对象
|
|
|
|
Returns:
|
|
dict: 约束信息字典
|
|
"""
|
|
for constraint_info in self.constraints:
|
|
if constraint_info['constraint'] == constraint:
|
|
return constraint_info
|
|
return None
|
|
|
|
def set_motor(self, constraint, axis, target_velocity, max_impulse):
|
|
"""
|
|
设置约束马达
|
|
|
|
Args:
|
|
constraint: 约束对象
|
|
axis (int): 轴向 (0=X, 1=Y, 2=Z for linear, 3=X, 4=Y, 5=Z for angular)
|
|
target_velocity (float): 目标速度
|
|
max_impulse (float): 最大冲量
|
|
"""
|
|
constraint_info = self.get_constraint_info(constraint)
|
|
if not constraint_info:
|
|
return
|
|
|
|
constraint_type = constraint_info['type']
|
|
if constraint_type == self.CONSTRAINT_HINGE:
|
|
constraint.setMotorTargetVelocity(target_velocity)
|
|
constraint.setMaxMotorImpulse(max_impulse)
|
|
constraint.enableAngularMotor(True)
|
|
constraint_info['motor_enabled'] = True
|
|
elif constraint_type == self.CONSTRAINT_6DOF:
|
|
constraint.setTargetVelocity(axis, target_velocity)
|
|
constraint.setMaxMotorForce(axis, max_impulse)
|
|
constraint.setParam(axis, 1) # 启用马达
|
|
constraint_info['motor_enabled'] = True
|
|
elif constraint_type == self.CONSTRAINT_6DOF_SPRING:
|
|
constraint.setTargetVelocity(axis, target_velocity)
|
|
constraint.setMaxMotorForce(axis, max_impulse)
|
|
constraint.setParam(axis, 1) # 启用马达
|
|
constraint_info['motor_enabled'] = True
|
|
|
|
def disable_motor(self, constraint):
|
|
"""
|
|
禁用约束马达
|
|
|
|
Args:
|
|
constraint: 约束对象
|
|
"""
|
|
constraint_info = self.get_constraint_info(constraint)
|
|
if not constraint_info:
|
|
return
|
|
|
|
constraint_type = constraint_info['type']
|
|
if constraint_type == self.CONSTRAINT_HINGE:
|
|
constraint.enableAngularMotor(False)
|
|
constraint_info['motor_enabled'] = False
|
|
elif constraint_type == self.CONSTRAINT_6DOF:
|
|
# 禁用所有轴的马达
|
|
for axis in range(6):
|
|
constraint.setParam(axis, 0)
|
|
constraint_info['motor_enabled'] = False
|
|
elif constraint_type == self.CONSTRAINT_6DOF_SPRING:
|
|
# 禁用所有轴的马达
|
|
for axis in range(6):
|
|
constraint.setParam(axis, 0)
|
|
constraint_info['motor_enabled'] = False
|
|
|
|
def set_spring(self, constraint, axis, stiffness, damping, equilibrium=0.0):
|
|
"""
|
|
设置约束弹簧
|
|
|
|
Args:
|
|
constraint: 约束对象
|
|
axis (int): 轴向 (0=X, 1=Y, 2=Z for linear, 3=X, 4=Y, 5=Z for angular)
|
|
stiffness (float): 刚度
|
|
damping (float): 阻尼
|
|
equilibrium (float): 平衡点
|
|
"""
|
|
constraint_info = self.get_constraint_info(constraint)
|
|
if not constraint_info:
|
|
return
|
|
|
|
constraint_type = constraint_info['type']
|
|
if constraint_type == self.CONSTRAINT_6DOF_SPRING:
|
|
constraint.setStiffness(axis, stiffness)
|
|
constraint.setDamping(axis, damping)
|
|
constraint.setEquilibriumPoint(axis, equilibrium)
|
|
constraint.enableSpring(axis, True)
|
|
constraint_info['spring_enabled'] = True
|
|
|
|
def disable_spring(self, constraint, axis):
|
|
"""
|
|
禁用约束弹簧
|
|
|
|
Args:
|
|
constraint: 约束对象
|
|
axis (int): 轴向
|
|
"""
|
|
constraint_info = self.get_constraint_info(constraint)
|
|
if not constraint_info:
|
|
return
|
|
|
|
constraint_type = constraint_info['type']
|
|
if constraint_type == self.CONSTRAINT_6DOF_SPRING:
|
|
constraint.enableSpring(axis, False)
|
|
constraint_info['spring_enabled'] = False
|
|
|
|
def set_limit(self, constraint, axis, lower_limit, upper_limit):
|
|
"""
|
|
设置约束限制
|
|
|
|
Args:
|
|
constraint: 约束对象
|
|
axis (int): 轴向
|
|
lower_limit (float): 下限
|
|
upper_limit (float): 上限
|
|
"""
|
|
constraint_info = self.get_constraint_info(constraint)
|
|
if not constraint_info:
|
|
return
|
|
|
|
constraint_type = constraint_info['type']
|
|
if constraint_type == self.CONSTRAINT_6DOF:
|
|
if axis < 3: # 线性轴
|
|
current_lower = constraint.getLinearLowerLimit()
|
|
current_upper = constraint.getLinearUpperLimit()
|
|
if axis == 0:
|
|
current_lower.setX(lower_limit)
|
|
current_upper.setX(upper_limit)
|
|
elif axis == 1:
|
|
current_lower.setY(lower_limit)
|
|
current_upper.setY(upper_limit)
|
|
elif axis == 2:
|
|
current_lower.setZ(lower_limit)
|
|
current_upper.setZ(upper_limit)
|
|
constraint.setLinearLowerLimit(current_lower)
|
|
constraint.setLinearUpperLimit(current_upper)
|
|
else: # 角轴
|
|
current_lower = constraint.getAngularLowerLimit()
|
|
current_upper = constraint.getAngularUpperLimit()
|
|
axis_index = axis - 3
|
|
if axis_index == 0:
|
|
current_lower.setX(lower_limit)
|
|
current_upper.setX(upper_limit)
|
|
elif axis_index == 1:
|
|
current_lower.setY(lower_limit)
|
|
current_upper.setY(upper_limit)
|
|
elif axis_index == 2:
|
|
current_lower.setZ(lower_limit)
|
|
current_upper.setZ(upper_limit)
|
|
constraint.setAngularLowerLimit(current_lower)
|
|
constraint.setAngularUpperLimit(current_upper)
|
|
|
|
def enable_constraint(self, constraint, enabled=True):
|
|
"""
|
|
启用或禁用约束
|
|
|
|
Args:
|
|
constraint: 约束对象
|
|
enabled (bool): 是否启用
|
|
"""
|
|
constraint_info = self.get_constraint_info(constraint)
|
|
if not constraint_info:
|
|
return
|
|
|
|
constraint_info['is_enabled'] = enabled
|
|
|
|
def add_constraint_to_group(self, constraint, group_name):
|
|
"""
|
|
将约束添加到组
|
|
|
|
Args:
|
|
constraint: 约束对象
|
|
group_name (str): 组名
|
|
"""
|
|
constraint_info = self.get_constraint_info(constraint)
|
|
if not constraint_info:
|
|
return
|
|
|
|
constraint_info['group'] = group_name
|
|
if group_name not in self.constraint_groups:
|
|
self.constraint_groups[group_name] = []
|
|
if constraint not in self.constraint_groups[group_name]:
|
|
self.constraint_groups[group_name].append(constraint)
|
|
|
|
def remove_constraint_from_group(self, constraint, group_name):
|
|
"""
|
|
将约束从组中移除
|
|
|
|
Args:
|
|
constraint: 约束对象
|
|
group_name (str): 组名
|
|
"""
|
|
constraint_info = self.get_constraint_info(constraint)
|
|
if not constraint_info:
|
|
return
|
|
|
|
constraint_info['group'] = None
|
|
if group_name in self.constraint_groups and constraint in self.constraint_groups[group_name]:
|
|
self.constraint_groups[group_name].remove(constraint)
|
|
|
|
def add_constraint_tag(self, constraint, tag):
|
|
"""
|
|
为约束添加标签
|
|
|
|
Args:
|
|
constraint: 约束对象
|
|
tag (str): 标签
|
|
"""
|
|
constraint_info = self.get_constraint_info(constraint)
|
|
if not constraint_info:
|
|
return
|
|
|
|
constraint_info['tags'].add(tag)
|
|
|
|
def remove_constraint_tag(self, constraint, tag):
|
|
"""
|
|
为约束移除标签
|
|
|
|
Args:
|
|
constraint: 约束对象
|
|
tag (str): 标签
|
|
"""
|
|
constraint_info = self.get_constraint_info(constraint)
|
|
if not constraint_info:
|
|
return
|
|
|
|
constraint_info['tags'].discard(tag)
|
|
|
|
def get_constraints_by_tag(self, tag):
|
|
"""
|
|
根据标签获取约束
|
|
|
|
Args:
|
|
tag (str): 标签
|
|
|
|
Returns:
|
|
list: 约束列表
|
|
"""
|
|
result = []
|
|
for constraint_info in self.constraints:
|
|
if tag in constraint_info['tags']:
|
|
result.append(constraint_info['constraint'])
|
|
return result
|
|
|
|
def get_constraints_by_group(self, group_name):
|
|
"""
|
|
根据组名获取约束
|
|
|
|
Args:
|
|
group_name (str): 组名
|
|
|
|
Returns:
|
|
list: 约束列表
|
|
"""
|
|
return self.constraint_groups.get(group_name, [])
|
|
|
|
def get_constraint_state(self, constraint):
|
|
"""
|
|
获取约束状态
|
|
|
|
Args:
|
|
constraint: 约束对象
|
|
|
|
Returns:
|
|
dict: 约束状态
|
|
"""
|
|
constraint_info = self.get_constraint_info(constraint)
|
|
if not constraint_info:
|
|
return None
|
|
|
|
state = {
|
|
'type': constraint_info['type'],
|
|
'is_enabled': constraint_info['is_enabled'],
|
|
'group': constraint_info['group'],
|
|
'tags': list(constraint_info['tags']),
|
|
'creation_time': constraint_info['creation_time'],
|
|
'age': time.time() - constraint_info['creation_time']
|
|
}
|
|
|
|
# 添加特定于约束类型的状态信息
|
|
if constraint_info['type'] == self.CONSTRAINT_HINGE:
|
|
state['current_angle'] = constraint.getHingeAngle()
|
|
state['motor_enabled'] = constraint_info.get('motor_enabled', False)
|
|
elif constraint_info['type'] == self.CONSTRAINT_6DOF:
|
|
state['motor_enabled'] = constraint_info.get('motor_enabled', False)
|
|
state['spring_enabled'] = constraint_info.get('spring_enabled', False)
|
|
elif constraint_info['type'] == self.CONSTRAINT_6DOF_SPRING:
|
|
state['motor_enabled'] = constraint_info.get('motor_enabled', False)
|
|
state['spring_enabled'] = constraint_info.get('spring_enabled', False)
|
|
|
|
return state
|
|
|
|
def clear_all_constraints(self):
|
|
"""
|
|
清除所有约束
|
|
"""
|
|
# 重新启用所有刚体间的碰撞
|
|
for constraint_info in self.constraints:
|
|
if constraint_info['properties'].get('disable_collision', False):
|
|
body_a = constraint_info['body_a']
|
|
body_b = constraint_info['body_b']
|
|
body_a.bullet_body.setIgnoreCollisionCheck(body_b.bullet_body, False)
|
|
|
|
self.constraints.clear()
|
|
self.constraint_groups.clear()
|
|
self.constraint_limits.clear()
|
|
self.constraint_motors.clear()
|
|
self.constraint_springs.clear()
|
|
|
|
print("所有约束已清除")
|
|
|
|
def update_constraint_feedback(self, time_delta):
|
|
"""
|
|
更新约束反馈信息
|
|
|
|
Args:
|
|
time_delta (float): 时间增量
|
|
"""
|
|
for constraint_info in self.constraints:
|
|
constraint = constraint_info['constraint']
|
|
constraint_type = constraint_info['type']
|
|
|
|
# 更新约束反馈信息
|
|
if constraint_type == self.CONSTRAINT_HINGE:
|
|
constraint_info['current_angle'] = constraint.getHingeAngle()
|
|
# 可以添加更多反馈信息
|
|
elif constraint_type == self.CONSTRAINT_SLIDER:
|
|
constraint_info['current_linear_position'] = constraint.getLinearPos()
|
|
constraint_info['current_angular_position'] = constraint.getAngularPos()
|
|
|
|
def get_constraint_statistics(self):
|
|
"""
|
|
获取约束统计信息
|
|
|
|
Returns:
|
|
dict: 统计信息
|
|
"""
|
|
type_counts = {}
|
|
group_counts = {}
|
|
tag_counts = {}
|
|
|
|
for constraint_info in self.constraints:
|
|
# 统计类型
|
|
constraint_type = constraint_info['type']
|
|
type_counts[constraint_type] = type_counts.get(constraint_type, 0) + 1
|
|
|
|
# 统计组
|
|
group = constraint_info['group']
|
|
if group:
|
|
group_counts[group] = group_counts.get(group, 0) + 1
|
|
|
|
# 统计标签
|
|
for tag in constraint_info['tags']:
|
|
tag_counts[tag] = tag_counts.get(tag, 0) + 1
|
|
|
|
return {
|
|
'total_constraints': len(self.constraints),
|
|
'type_distribution': type_counts,
|
|
'group_distribution': group_counts,
|
|
'tag_distribution': tag_counts,
|
|
'groups': list(self.constraint_groups.keys())
|
|
} |