forked from pz4kybsvg/Conception
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
123 lines
4.1 KiB
123 lines
4.1 KiB
#include "drake/systems/sensors/rotary_encoders.h"
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <iterator>
|
|
#include <numeric>
|
|
#include <vector>
|
|
|
|
#include "drake/common/default_scalars.h"
|
|
#include "drake/common/eigen_types.h"
|
|
#include "drake/common/unused.h"
|
|
#include "drake/systems/framework/basic_vector.h"
|
|
|
|
namespace drake {
|
|
namespace systems {
|
|
namespace sensors {
|
|
|
|
namespace {
|
|
// Return a vector of the given size, with result[i] == i.
|
|
std::vector<int> vector_iota(int size) {
|
|
std::vector<int> result(size);
|
|
std::iota(std::begin(result), std::end(result), 0);
|
|
return result;
|
|
}
|
|
} // namespace
|
|
|
|
template <typename T>
|
|
RotaryEncoders<T>::RotaryEncoders(const std::vector<int>& ticks_per_revolution)
|
|
: RotaryEncoders(
|
|
ticks_per_revolution.size() /* input_port_size */,
|
|
vector_iota(ticks_per_revolution.size()) /* input_vector_indices */,
|
|
ticks_per_revolution) {}
|
|
|
|
template <typename T>
|
|
RotaryEncoders<T>::RotaryEncoders(int input_port_size,
|
|
const std::vector<int>& input_vector_indices)
|
|
: RotaryEncoders(input_port_size,
|
|
input_vector_indices,
|
|
std::vector<int>() /* ticks_per_revolution */) {}
|
|
|
|
template <typename T>
|
|
RotaryEncoders<T>::RotaryEncoders(int input_port_size,
|
|
const std::vector<int>& input_vector_indices,
|
|
const std::vector<int>& ticks_per_revolution)
|
|
: VectorSystem<T>(
|
|
SystemTypeTag<RotaryEncoders>{},
|
|
input_port_size,
|
|
input_vector_indices.size() /* output_port_size */),
|
|
num_encoders_(input_vector_indices.size()),
|
|
indices_(input_vector_indices),
|
|
ticks_per_revolution_(ticks_per_revolution) {
|
|
DRAKE_DEMAND(input_port_size >= 0);
|
|
DRAKE_ASSERT(*std::min_element(indices_.begin(), indices_.end()) >= 0);
|
|
DRAKE_ASSERT(*std::max_element(indices_.begin(), indices_.end()) <
|
|
input_port_size);
|
|
DRAKE_DEMAND(ticks_per_revolution_.empty() ||
|
|
indices_.size() == ticks_per_revolution_.size());
|
|
DRAKE_ASSERT(ticks_per_revolution_.empty() ||
|
|
*std::min_element(ticks_per_revolution_.begin(),
|
|
ticks_per_revolution_.end()) >= 0);
|
|
|
|
// This vector is numeric parameter 0.
|
|
this->DeclareNumericParameter(
|
|
BasicVector<T>(VectorX<T>::Zero(num_encoders_)));
|
|
}
|
|
|
|
template <typename T>
|
|
template <typename U>
|
|
RotaryEncoders<T>::RotaryEncoders(const RotaryEncoders<U>& other)
|
|
: RotaryEncoders(other.get_input_port().size(),
|
|
other.indices_,
|
|
other.ticks_per_revolution_) {}
|
|
|
|
template <typename T>
|
|
void RotaryEncoders<T>::DoCalcVectorOutput(
|
|
const Context<T>& context,
|
|
const Eigen::VectorBlock<const VectorX<T>>& input,
|
|
const Eigen::VectorBlock<const VectorX<T>>& state,
|
|
Eigen::VectorBlock<VectorX<T>>* output) const {
|
|
unused(state);
|
|
|
|
const VectorX<T>& calibration_offsets =
|
|
context.get_numeric_parameter(0).value();
|
|
DRAKE_ASSERT(calibration_offsets.size() == num_encoders_);
|
|
|
|
// Loop through the outputs.
|
|
Eigen::VectorBlock<VectorX<T>>& y = *output;
|
|
for (int i = 0; i < num_encoders_; i++) {
|
|
const int index = indices_.empty() ? i : indices_[i];
|
|
|
|
// Calibration.
|
|
y(i) = input(index) - calibration_offsets(i);
|
|
|
|
// Quantization.
|
|
if (!ticks_per_revolution_.empty()) {
|
|
using std::floor;
|
|
const T ticks_per_radian = ticks_per_revolution_[i] / (2.0 * M_PI);
|
|
y(i) = floor(y(i) * ticks_per_radian) / ticks_per_radian;
|
|
}
|
|
}
|
|
}
|
|
|
|
template <typename T>
|
|
void RotaryEncoders<T>::set_calibration_offsets(
|
|
Context<T>* context,
|
|
const Eigen::Ref<VectorX<T>>& calibration_offsets) const {
|
|
DRAKE_DEMAND(calibration_offsets.rows() == num_encoders_);
|
|
context->get_mutable_numeric_parameter(0).set_value(calibration_offsets);
|
|
}
|
|
|
|
template <typename T>
|
|
const VectorX<T>& RotaryEncoders<T>::get_calibration_offsets(
|
|
const Context<T>& context) const {
|
|
return context.get_numeric_parameter(0).value();
|
|
}
|
|
|
|
} // namespace sensors
|
|
} // namespace systems
|
|
} // namespace drake
|
|
|
|
DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
|
|
class ::drake::systems::sensors::RotaryEncoders)
|