(policy, noise)
return msg
Convert a policy object to a ROS ControllerParams message.
Convert a policy object to a ROS ControllerParams message.
Convert a policy object to a ROS ControllerParams message .
def policy_to_msg(policy, noise):
    """
    Convert a policy object to a ROS ControllerParams message.
    """
    msg = ControllerParams()
    if isinstance(policy, LinearGaussianPolicy):
        msg.controller_to_execute = LIN_GAUSS_CONTROLLER
        msg.lingauss = LinGaussParams()
        msg.lingauss.dX = policy.dX
        msg.lingauss.dU = policy.dU
        msg.lingauss.K_t = \
                policy.K.reshape(policy.T * policy.dX * policy.dU).tolist()
        msg.lingauss.k_t = \
                policy.fold_k(noise).reshape(policy.T * policy.dU).tolist()
    elif NO_CAFFE is False and isinstance(policy, CaffePolicy):
        msg.controller_to_execute = CAFFE_CONTROLLER
        msg.caffe = CaffeParams()
        msg.caffe.net_param = policy.get_net_param()
        msg.caffe.bias = policy.bias.tolist()
        msg.caffe.dU = policy.dU
        scale_shape = policy.scale.shape
        msg.caffe.scale = policy.scale.reshape(scale_shape[0] * scale_shape[1]).tolist()
        msg.caffe.dim_bias = scale_shape[0]
        scaled_noise = np.zeros_like(noise)
        for i in range(noise.shape[0]):
            scaled_noise[i] = policy.chol_pol_covar.T.dot(noise[i])
        msg.caffe.noise = scaled_noise.reshape(-1).tolist()
    elif isinstance(policy, TfPolicy):
        msg.controller_to_execute = TF_CONTROLLER
        msg.tf = TfParams()
        msg.tf.dU = policy.dU
    else:
        raise NotImplementedError("Caffe not imported or Unknown policy object: %s" % policy)
    return msg
def policy_to_msg ( policy , noise ) : msg = ControllerParams ( ) if isinstance ( policy , LinearGaussianPolicy ) : msg . controller_to_execute = LIN_GAUSS_CONTROLLER msg . lingauss = LinGaussParams ( ) msg . lingauss . dX = policy . dX msg . lingauss . dU = policy . dU msg . lingauss . K_t = policy . K . reshape ( policy . T * policy . dX * policy . dU ) . tolist ( ) msg . lingauss . k_t = policy . fold_k ( noise ) . reshape ( policy . T * policy . dU ) . tolist ( ) elif NO_CAFFE is False and isinstance ( policy , CaffePolicy ) : msg . controller_to_execute = CAFFE_CONTROLLER msg . caffe = CaffeParams ( ) msg . caffe . net_param = policy . get_net_param ( ) msg . caffe . bias = policy . bias . tolist ( ) msg . caffe . dU = policy . dU scale_shape = policy . scale . shape msg . caffe . scale = policy . scale . reshape ( scale_shape [ 0 ] * scale_shape [ 1 ] ) . tolist ( ) msg . caffe . dim_bias = scale_shape [ 0 ] scaled_noise = np . zeros_like ( noise ) for i in range ( noise . shape [ 0 ] ) : scaled_noise [ i ] = policy . chol_pol_covar . T . dot ( noise [ i ] ) msg . caffe . noise = scaled_noise . reshape ( - 1 ) . tolist ( ) elif isinstance ( policy , TfPolicy ) : msg . controller_to_execute = TF_CONTROLLER msg . tf = TfParams ( ) msg . tf . dU = policy . dU else : raise NotImplementedError ( "Caffe not imported or Unknown policy object: %s" % policy ) return msg