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