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")