I have the following code in C++ that uses the Eigen library, need help to translate to python (numpy)
Initialization
double b = 20.0;
Eigen::Vector3d C(1.0/10.2, 1.0/10.2, 1/30);
Eigen::MatrixXd U(5200, 3);
int i = 0;
for (double x = 10.2/2.0; x < 200; x += 10) {
for (double y = 10.2/2.0; y < 200; y += 10) {
for (double t = 0; t <= 360; t += 30) {
U(i, 0) = x;
U(i, 1) = y;
U(i, 2) = psi;
i += 1;
}
}
}
Function:
Eigen::VectorXd operator()(const Eigen::VectorXd& s) {
Eigen::VectorXd p(length());
p(0) = s[0];
p(1) = s[1];
p(2) = s[2];
p(3) = s[3];
for (int i = 0; i < U.rows(); i++) {
p(i + 4) = b*exp(-0.5*(s.tail(U.cols()) - U.row(i).transpose()).dot(C*(s.tail(U.cols())
- U.row(i).transpose())));
if (p(i + 4) < 0.1) {
p(i + 4) = 0;
}
}
return p;
}
Python version
Initialization:
my_x = 10.2/2.0
my_y = 10.2/2.0
my_p = 0
xx = []
while my_x < 200:
xx.append(my_x)
my_x += 10
yy = []
while my_y < 200:
yy.append(my_y)
my_y += 10
pps = []
while my_psi <= 360:
pps.append(my_p)
my_p+=30
U =[]
for x in xx:
for y in yy:
for p in pps:
U.append([x,y,p])
U = numpy.matrix(U)
C = numpy.array([1.0/10.2, 1.0/10.2, 1.0/30.0])
b = 20.0
The Function
Instead of operator()
I will call the function doSomething()
def doSomething(s): # Where s is a numpy array (1-d vector)
p[0:4] = s[0:4]
for i in range (U.shape[0]):
s_dash = -0.5*(s - U[i].T)
s_ddash = C*s
s_dddash = s_dash.dot(s_ddash) - U[i].T
p[i+4] = b * numpy.exp(s_dddash)
if p[i+4] < 0.1: p[i+4] = 0
What I am confused:
- In the C++ implementation, I think
p[i+4]
is supposed to be a single value - In my python version, I get a
p[i+4]
as square matrix - Each
p[i+4]
is a zero matrix.
I am unable to decipher my mistake. Please help!