def rot2euler(matrices):
"""Calculate euler angles from rotation matrices
Args:
matrices (:class `~chainer.Variable` or :ref:`ndarray`):
Rotation matrices. A 3-D array of shape `(B, 3, 3)`
Returns:
~chainer.Variable:
A 2-D array of shape `(B, 3)`
Euler angles along with x, y, and z axis respectively.
"""
xs = F.arctan2(matrices[:,2,1], matrices[:,2,2])
ys = F.arcsin(-matrices[:,2,0])
zs = F.arctan2(matrices[:,1,0], matrices[:,0,0])
return F.stack([xs, ys, zs], axis=1)
def check_forward(self, x1_data, x2_data):
y = F.arctan2(x1_data, x2_data)
numpy.testing.assert_array_less(
cuda.to_cpu(y.data),
numpy.full(y.shape, numpy.pi))
numpy.testing.assert_array_less(
numpy.full(y.shape, -numpy.pi),
cuda.to_cpu(y.data))
testing.assert_allclose(
numpy.arctan2(self.x1, self.x2), y.data, atol=1e-4, rtol=1e-4)
def check_forward(self, x1_data, x2_data):
y = F.arctan2(x1_data, x2_data)
numpy.testing.assert_array_less(
cuda.to_cpu(y.data),
numpy.full(y.shape, numpy.pi))
numpy.testing.assert_array_less(
numpy.full(y.shape, -numpy.pi),
cuda.to_cpu(y.data))
testing.assert_allclose(
numpy.arctan2(self.x1, self.x2), y.data, atol=1e-4, rtol=1e-4)
def forward(self, x, u):
# x, u = inputs
squeeze = len(x.shape) == 1
if squeeze:
# print("squeeze is true")
x = F.expand_dims(x, 0)
u = F.expand_dims(u, 0)
assert len(x.shape) == 2
assert x.shape[0] == u.shape[0]
assert x.shape[1] == self.n_state, str(x.shape[1])
assert u.shape[1] == self.n_ctrl
# x (n_batch, n_state)
# u (n_batch, n_ctrl)
assert len(u.shape) == 2, str(u.shape)
if not hasattr(self, 'simple') or self.simple:
g, m, l = F.separate(self.params)
else:
g, m, l, d, b = F.separate(self.params)
u = F.clip(u, -self.max_torque, self.max_torque)[:, 0]
# cos, sin, angular velocity
cos_th, sin_th, dth = F.separate(x, axis=1)
# theta
th = F.arctan2(sin_th, cos_th)
# calculate new angular velocity
if not hasattr(self, 'simple') or self.simple:
newdth = dth
newdth += self.dt * (-3. * g / (2. * l) * (-sin_th) + 3. * u / (m * l ** 2))
else:
sin_th_bias = F.sin(th + b)
newdth = dth
newdth += self.dt * (-3. * g / (2. * l) * (-sin_th_bias) + 3. * u / (m * l ** 2) - d * th)
newth = th + newdth * self.dt
state = F.stack((F.cos(newth), F.sin(newth), newdth), axis=1)
if squeeze:
state = F.squeeze(state, axis=0)
return state