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.
113 lines
3.4 KiB
113 lines
3.4 KiB
#pragma once
|
|
|
|
#include <variant>
|
|
|
|
#include "drake/common/name_value.h"
|
|
#include "drake/common/schema/stochastic.h"
|
|
#include "drake/math/roll_pitch_yaw.h"
|
|
#include "drake/math/rotation_matrix.h"
|
|
|
|
namespace drake {
|
|
namespace schema {
|
|
|
|
/// A specification for an SO(3) rotation, to be used for serialization
|
|
/// purposes, e.g., to define stochastic scenarios. This structure specifies
|
|
/// either one specific rotation or else a distribution of possible rotations.
|
|
/// It does not provide mathematical operators to compose or mutate rotations.
|
|
/// Instead, users should call either GetDeterministicValue() or ToSymbolic()
|
|
/// to obtain a RotationMatrix value that can be operated on.
|
|
///
|
|
/// For an overview of configuring stochastic transforms, see
|
|
/// @ref schema_transform and @ref schema_stochastic.
|
|
///
|
|
/// See @ref implementing_serialize "Implementing Serialize" for implementation
|
|
/// details, especially the unusually public member fields.
|
|
class Rotation {
|
|
public:
|
|
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Rotation)
|
|
|
|
/// Constructs the Identity rotation.
|
|
Rotation() = default;
|
|
|
|
/// Constructs an Rpy rotation with the given value.
|
|
explicit Rotation(const math::RotationMatrix<double>&);
|
|
|
|
/// Constructs an Rpy rotation with the given value.
|
|
explicit Rotation(const math::RollPitchYaw<double>&);
|
|
|
|
/// No-op rotation.
|
|
struct Identity {
|
|
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Identity)
|
|
Identity() = default;
|
|
|
|
template <typename Archive>
|
|
void Serialize(Archive*) {}
|
|
};
|
|
|
|
/// A roll-pitch-yaw rotation, using the angle conventions of Drake's
|
|
/// RollPitchYaw.
|
|
struct Rpy {
|
|
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Rpy)
|
|
Rpy() = default;
|
|
|
|
template <typename Archive>
|
|
void Serialize(Archive* a) {
|
|
a->Visit(DRAKE_NVP(deg));
|
|
}
|
|
|
|
DistributionVectorVariant<3> deg{Eigen::Vector3d::Zero()};
|
|
};
|
|
|
|
/// Rotation constructed from a fixed axis and an angle.
|
|
struct AngleAxis {
|
|
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(AngleAxis)
|
|
AngleAxis() = default;
|
|
|
|
template <typename Archive>
|
|
void Serialize(Archive* a) {
|
|
a->Visit(DRAKE_NVP(angle_deg));
|
|
a->Visit(DRAKE_NVP(axis));
|
|
}
|
|
|
|
DistributionVariant angle_deg;
|
|
DistributionVectorVariant<3> axis{Eigen::Vector3d::UnitZ()};
|
|
};
|
|
|
|
/// Rotation sampled from a uniform distribution over SO(3).
|
|
struct Uniform {
|
|
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Uniform)
|
|
Uniform() = default;
|
|
|
|
template <typename Archive>
|
|
void Serialize(Archive*) {}
|
|
};
|
|
|
|
/// Returns true iff this is fully deterministic.
|
|
bool IsDeterministic() const;
|
|
|
|
/// If this is deterministic, retrieves its value.
|
|
/// @throws std::exception if this is not fully deterministic.
|
|
math::RotationMatrixd GetDeterministicValue() const;
|
|
|
|
/// Returns the symbolic form of this rotation. If this is deterministic,
|
|
/// the result will contain no variables. If this is random, the result will
|
|
/// contain one or more random variables, based on the distributions in use.
|
|
math::RotationMatrix<symbolic::Expression> ToSymbolic() const;
|
|
|
|
/// Sets this value to the given deterministic RPY, in degrees.
|
|
void set_rpy_deg(const Eigen::Vector3d& rpy_deg) {
|
|
value.emplace<Rotation::Rpy>().deg = rpy_deg;
|
|
}
|
|
|
|
template <typename Archive>
|
|
void Serialize(Archive* a) {
|
|
a->Visit(DRAKE_NVP(value));
|
|
}
|
|
|
|
using Variant = std::variant<Identity, Rpy, AngleAxis, Uniform>;
|
|
Variant value;
|
|
};
|
|
|
|
} // namespace schema
|
|
} // namespace drake
|