informed_sampling.py

import numpy as np

class InformedSampler:
    def __init__(self, goal_q, joint_lower_limits, joint_upper_limits, goal_bias=0.1, s_max=0.15):
        """
        Handles APF-Biased Gaussian sampling for the configuration space.
        """
        self.goal_q = np.array(goal_q)
        self.joint_lower_limits = np.array(joint_lower_limits)
        self.joint_upper_limits = np.array(joint_upper_limits)
        self.goal_bias = goal_bias
        self.s_max = s_max
        
        # Standard deviation is 10% of the joint limits (focusing on local search)
        self.std_dev = (self.joint_upper_limits - self.joint_lower_limits) * 0.1

    def sample(self, q_near, v_apf_dir):
        """
        Generates a random configuration biased by the APF gradient.
        """
        if np.random.rand() < self.goal_bias:
            return self.goal_q
            
        # Shifting the mean of our random distribution in the direction of the APF pull
        mean = q_near + (v_apf_dir * self.s_max * 2) 
        
        # Sampling from the Gaussian distribution
        q_rand = np.random.normal(mean, self.std_dev)
        
        # Enforcing strict physical joint limits
        return np.clip(q_rand, self.joint_lower_limits, self.joint_upper_limits)