def add_com_target(self,
weight: float = 1.0,
as_constraint: bool = False,
constraint_tolerance: float = 1E-8) -> None:
# Add the target
self._ik.setCOMTarget(idt.Position_Zero(), weight)
# Configure it either as target or constraint
self._ik.setCOMAsConstraint(asConstraint=as_constraint)
self._ik.setCOMAsConstraintTolerance(tolerance=constraint_tolerance)
# Initialize the target data buffers
assert "com" not in self._targets_data.keys()
self._targets_data["com"] = TargetData(type=TargetType.POSITION,
weight=weight,
data=np.array([0.0, 0, 0]))
def add_target(
self,
frame_name: str,
target_type: TargetType,
weight: Union[float, Tuple[float, float]] = None,
as_constraint: bool = False,
) -> None:
# Check the type of the 'weight' argument
float_target_types = {TargetType.ROTATION, TargetType.POSITION}
weight_type = float if target_type in float_target_types else tuple
# Backward compatibility: if the target type is POSE and the weight is a float,
# we apply the same weight to both target components
if target_type is TargetType.POSE and isinstance(weight, float):
weight = (weight, weight)
# Set the default weight if not specified
default_weight = 1.0 if target_type in float_target_types else (1.0, 1.0)
weight = weight if weight is not None else default_weight
if not isinstance(weight, weight_type):
raise ValueError(f"The weight must be {weight_type} for this target")
if target_type == TargetType.ROTATION:
# Add the target
ok_target = self._ik.addRotationTarget(
frame_name, idt.Rotation.Identity(), weight
)
# Initialize the target data buffers
self._targets_data[frame_name] = TargetData(
type=TargetType.ROTATION, weight=weight, data=np.array([1.0, 0, 0, 0])
)
elif target_type == TargetType.POSITION:
# Add the target
ok_target = self._ik.addPositionTarget(
frame_name, idt.Position_Zero(), weight
)
# Initialize the target data buffers
self._targets_data[frame_name] = TargetData(
type=TargetType.POSITION, weight=weight, data=np.array([0.0, 0, 0])
)
elif target_type == TargetType.POSE:
# Add the target
ok_target = self._ik.addTarget(
frame_name, idt.Transform.Identity(), weight[0], weight[1]
)
# Create the transform target data
target_data = TransformTargetData(
position=np.array([0.0, 0, 0]), quaternion=np.array([1.0, 0, 0, 0])
)
# Initialize the target data buffers
self._targets_data[frame_name] = TargetData(
type=TargetType.POSE, weight=weight, data=target_data
)
else:
raise ValueError(target_type)
if not ok_target:
raise RuntimeError(f"Failed to add target for frame '{frame_name}'")
if as_constraint:
if target_type == TargetType.ROTATION:
constraint = idt.InverseKinematicsTreatTargetAsConstraintRotationOnly
elif target_type == TargetType.POSITION:
constraint = idt.InverseKinematicsTreatTargetAsConstraintPositionOnly
else:
assert target_type == TargetType.POSE
constraint = idt.InverseKinematicsTreatTargetAsConstraintFull
if not self._ik.setTargetResolutionMode(frame_name, constraint):
raise RuntimeError(f"Failed to set target '{frame_name}' as constraint")
def add_target(self,
frame_name: str,
target_type: TargetType,
weight: float = 1.0,
as_constraint: bool = False) -> None:
if target_type == TargetType.ROTATION:
# Add the target
ok_target = self._ik.addRotationTarget(frame_name,
idt.Rotation.Identity(),
weight)
# Initialize the target data buffers
self._targets_data[frame_name] = TargetData(
type=TargetType.ROTATION,
weight=weight,
data=np.array([1.0, 0, 0, 0]))
elif target_type == TargetType.POSITION:
# Add the target
ok_target = self._ik.addPositionTarget(frame_name,
idt.Position_Zero(), weight)
# Initialize the target data buffers
self._targets_data[frame_name] = TargetData(
type=TargetType.POSITION,
weight=weight,
data=np.array([0.0, 0, 0]))
elif target_type == TargetType.POSE:
# Add the target
ok_target = self._ik.addTarget(frame_name,
idt.Transform.Identity(), weight,
weight)
# Create the transform target data
target_data = TransformTargetData(position=np.array([0.0, 0, 0]),
quaternion=np.array(
[1., 0, 0, 0]))
# Initialize the target data buffers
self._targets_data[frame_name] = TargetData(type=TargetType.POSE,
weight=weight,
data=target_data)
else:
raise ValueError(target_type)
if not ok_target:
raise RuntimeError(
f"Failed to add target for frame '{frame_name}'")
if as_constraint:
if target_type == TargetType.ROTATION:
constraint = idt.InverseKinematicsTreatTargetAsConstraintRotationOnly
elif target_type == TargetType.POSITION:
constraint = idt.InverseKinematicsTreatTargetAsConstraintPositionOnly
else:
assert target_type == TargetType.POSE
constraint = idt.InverseKinematicsTreatTargetAsConstraintFull
if not self._ik.setTargetResolutionMode(frame_name, constraint):
raise RuntimeError(
f"Failed to set target '{frame_name}' as constraint")