ir-lab/intprim

Interaction Primitives library

intprim's Issues

active_dofs in Kalman filter

In the Extended Kalman Filter I saw that during the filtering process, the entries in the Kalman Gain matrix corresponding to the inactive DoFs are fixed to zero.

intprim/intprim/filter/spatiotemporal/ekf.py

Lines 162 to 163 in 8994b15

	# Zero out the Kalman gain entries for the non-active DoFs. Since we aren't considering them we don't want them to affect the update process.
	kalman_gain[:, nonactive_dofs] = 0.0;

However, this is not being done in the Kalman Filter where the active_dofs argument is used only in the align_observations function and the entries for the inactive DoFs in the Kalman Gain matrix are not fixed to zero.

While using the KF, I did not find it giving any issues though, and I haven't tried the EKF yet. I would think that since the measurement noise is set very high for the inactive DoFs, their values would already be low enough in the Kalman Gain matrix to be negligible in such a case, which could be why they work fine in the KF without zeroing them out.

Is epsilon_y and correspondingly Sigma_y used?

I was going through the code and I saw that no where is Sigma_y or epsilon_y used. In the paper it's just mentioned in Eq. 2 and while defining x_d but after that it's not mentioned anywhere else.
Is it the observation_noise parameter that is used during the inference?
If not, can you please help me understand if and how it is modelled or is there a trick that allows to do away with it altogether?

Derivative issue

Hi,

thanks for the greate work. I checked your code, for function BayesianInteractionPrimitive. basis_inverse_transform, if we require derivate and scaling the observation, the derivate should not be unscaled as same as observation. Observation is: obsscale+mu, while derivative only need obsscale. I guess this is a bug. Is it right?

Thanks

intprim/intprim/bayesian_interaction_primitives.py

Lines 244 to 254 in 8994b15

	def basis_inverse_transform(self, x, weights, deriv = False):
	trajectory = np.zeros((self.basis_model.num_observed_dof, x.shape[0]), dtype = intprim.constants.DTYPE)
	for idx in range(x.shape[0]):
	trajectory[:, idx] = self.basis_model.apply_coefficients(x[idx], weights, deriv)
	if(self.scaling_groups is not None):
	for group, scaler in zip(self.scaling_groups, self.scalers):
	trajectory[group, :] = scaler.inverse_transform(trajectory[group, :].reshape(-1, 1)).reshape(trajectory[group, :].shape)
	return trajectory

Errors while using EKF

TL;DR

I encountered some errors while using the EKF filtering for performing BIP inference both from my code and from the minimal.py example that is provided. The corresponding fixes can be seen in the commit: souljaboy764/intprim@4ab692c

Long Version:

I was trying to use the EKF filter for performing the performing inference with the BIP, but was getting the following error:

  File "build/bdist.linux-x86_64/egg/intprim/bayesian_interaction_primitives.py", line 298, in generate_probable_trajectory_recursive
  File "build/bdist.linux-x86_64/egg/intprim/filter/spatiotemporal/ekf.py", line 157, in localize
  File "build/bdist.linux-x86_64/egg/intprim/filter/spatiotemporal/nonlinear_system.py", line 109, in get_measurement_model
  File "build/bdist.linux-x86_64/egg/intprim/basis/basis_model.py", line 91, in get_weighted_vector_derivative
IndexError: invalid index to scalar variable.

I thought it might have been an issue in my code but I was getting this issue even when I was running minimal.py with the EKF filter. I am getting the issue in both python2 and python3.

The stack trace shows the error in basis_model.py in the get_weighted_vector_derivative function at the following line:

intprim/intprim/basis/basis_model.py

Line 91 in 8994b15

basis_func_derivs = self.get_basis_function_derivatives(x[0])

On reading the docstring of the function, it says:that x should be a scalar value:

intprim/intprim/basis/basis_model.py

Line 78 in 8994b15

# @param x Scalar containing the phase value to use in the creation of the block diagonal matrix.

I am able to resolve this issue with dropping the index used for x and the line now looks like:

        basis_func_derivs = self.get_basis_function_derivatives(x)

After fixing the above, I get the next error:

  File "build/bdist.linux-x86_64/egg/intprim/bayesian_interaction_primitives.py", line 298, in generate_probable_trajectory_recursive
  File "build/bdist.linux-x86_64/egg/intprim/filter/spatiotemporal/ekf.py", line 157, in localize
  File "build/bdist.linux-x86_64/egg/intprim/filter/spatiotemporal/nonlinear_system.py", line 110, in get_measurement_model
  File "build/bdist.linux-x86_64/egg/intprim/basis/basis_model.py", line 50, in get_block_diagonal_basis_matrix
ValueError: could not broadcast input array from shape (8) into shape (8,1)

On looking further, I saw that the phase value x passed to the get_block_diagonal_basis_matrix function in basis_model.py is a scalar value, which is used here:

intprim/intprim/basis/basis_model.py

Line 48 in 8994b15

basis_funcs = self.get_basis_functions(x)

The docstrings of the get_basis_functions function for all the different basis functions state that the shape of the return value would be shape(degree, ) if x is a scalar. so I am able to fix this by expanding the dimensions of the value returned by the function:

        basis_funcs = self.get_basis_functions(x)
        if np.isscalar(x):
            basis_funcs = basis_funcs[:, None]

Since this is the same case in the get_block_diagonal_basis_matrix_derivative function, I felt a similar change there would fit.

The next error I get is:

  File "build/bdist.linux-x86_64/egg/intprim/bayesian_interaction_primitives.py", line 298, in generate_probable_trajectory_recursive
  File "build/bdist.linux-x86_64/egg/intprim/filter/spatiotemporal/ekf.py", line 157, in localize
AttributeError: 'GaussianModel' object has no attribute 'inverse_transform'

which is basically this line:

intprim/intprim/filter/spatiotemporal/ekf.py

Line 157 in 8994b15

predicted_measurement = np.dot(measurement_model[:,self.system_size:], self.basis_model.inverse_transform(self.state_mean[self.system_size:]))

After digging further, I saw that the only use of inverse_transform is for the scalers and not for any basis functions. Maybe it is from a previous version? I changed that part of the code to how it is in kf.py:

            predicted_measurement = np.dot(measurement_model[:,self.system_size:], self.state_mean[self.system_size:])

Where can I find IP(2014)'s Source Code?

Hello,I met a problem when I reading your code. I can't find the source code about IP which proposed in 2014 by Heni Ben Amor? could you provide it? Great thanks!