0

情况

我意识到我一直在编写很多LeafSystem类，它们对输入数据进行非常轻量级的操作并将其作为输出数据输出。它们都是无状态的，唯一的区别是转换函数。

这方面的一个例子是简单地重新排序输入数据的类或简单地去除不相关的输入数据的类。

因此，我想编写一个LeafSystem将这个“转换函数”作为参数的类，以跳过每次我想以稍微不同的方式处理输入数据时都必须创建一个全新的类的麻烦。

这个想法是一个构造函数，它采用 astd::function的形式，以及输入和输出向量的大小。

std::function<void(const Eigen::VectorBlock<const drake::VectorX<T>>&, Eigen::VectorBlock<drake::VectorX<T>>& )>;

“转换函数”可以在这个函子中实现。

一个示例用例是，如果我想将 3D 状态数据（x、y、z、roll、pitch、yaw）转换为 2D 状态数据（x、y、yaw），我可以编写一个仿函数，例如

void 3Dto2D(const Eigen::VectorBlock<const drake::VectorX<T>>& input, Eigen::VectorBlock<drake::VectorX<T>>& output)
{
    output[0] = input[0]; //x
    output[1] = input[1]; //y
    output[2] = input[5]; //yaw
    output[3] = input[6]; //x_dot
    output[4] = input[7]; //y_dot
    output[5] = input[11]; //yaw_dot
}

并将这个函子传递给这个“StateConverter”。

问题

我面临的问题是关于标量转换复制构造函数。当类具有特定于类型的成员对象时，如何实现它？

这个类的主体如下（为了完整性）

using ConversionFunc = std::function<void(const Eigen::VectorBlock<const drake::VectorX<T>>&, Eigen::VectorBlock<drake::VectorX<T>>& )>;;

// Some black magic to handle alias explicit template instantiation

template <typename T>
StateConverter<T>::StateConverter(ConversionFunc func, const unsigned int input_size, const unsigned int output_size) :
    systems::LeafSystem<T>(systems::SystemTypeTag<StateConverter>{}),
    input_idx(this->DeclareVectorInputPort("input_port", systems::BasicVector<T>(input_size)).get_index()),
    output_idx(this->DeclareVectorOutputPort("output_port", systems::BasicVector<T>(output_size), &StateConverter::convert).get_index())
{
    convert_func = func;
}

template <typename T>
void StateConverter<T>::convert(const drake::systems::Context<T>& context, systems::BasicVector<T>* output) const
{
    const auto state = this->EvalVectorInput(context, input_idx)->get_value();
    auto mutable_output = output->get_mutable_value();
    convert_func(state, mutable_output);
}
4

1 回答 1

1

Partial Solution

I ended up creating a struct that holds all 3 required instances of the templated function

struct ConversionFunc
{
    std::function<void(const Eigen::VectorBlock<const drake::VectorX<double>>&, Eigen::VectorBlock<drake::VectorX<double>>& )> double_impl;
    std::function<void(const Eigen::VectorBlock<const drake::VectorX<drake::AutoDiffXd>>&, Eigen::VectorBlock<drake::VectorX<drake::AutoDiffXd>>& )> autodiff_impl;
    std::function<void(const Eigen::VectorBlock<const drake::VectorX<drake::symbolic::Expression>>&, Eigen::VectorBlock<drake::VectorX<drake::symbolic::Expression>>& )> symbolic_impl;
};

Which is what gets passed in the copy constructor

I create the struct as follows

template <typename T>
void convert_func(const Eigen::VectorBlock<const VectorX<T>>& state, Eigen::VectorBlock<VectorX<T>>& output)
{
    // Some example conversions
    output[0] = state[4];
    output[1] = state[6];
    output[2] = state[12];
    output[3] = state[14];
}

// I'm sure there's some way to automatically create these instantiations in the constructor of ConversionFunc...
ConversionFunc func;
func.double_impl = convert_func<double>;
func.autodiff_impl = convert_func<drake::AutoDiffXd>;
func.symbolic_impl = convert_func<drake::symbolic::Expression>;

And pass it to my Converter as follows

auto converter = builder.AddSystem(std::make_unique<StateConverter<double>>(func, 16, 4));

Problem

Unfortunately, this method seems to be leaking memory causing std::bad_alloc to be thrown...

于 2019-05-10T09:29:33.990 回答