1#include "dynamical_systems/PointAttractor.hpp"
3#include "dynamical_systems/exceptions/NotImplementedException.hpp"
4#include "dynamical_systems/exceptions/EmptyAttractorException.hpp"
5#include "state_representation/exceptions/InvalidParameterException.hpp"
6#include "dynamical_systems/exceptions/IncompatibleSizeException.hpp"
7#include "state_representation/exceptions/EmptyStateException.hpp"
8#include "state_representation/exceptions/IncompatibleReferenceFramesException.hpp"
9#include "state_representation/space/cartesian/CartesianPose.hpp"
10#include "state_representation/space/cartesian/CartesianState.hpp"
11#include "state_representation/space/joint/JointPositions.hpp"
12#include "state_representation/space/joint/JointState.hpp"
21 gain_(make_shared_parameter<Eigen::MatrixXd>(
"gain", Eigen::MatrixXd::Identity(6, 6))) {
22 this->parameters_.insert(std::make_pair(
"attractor", attractor_));
23 this->parameters_.insert(std::make_pair(
"gain", gain_));
27PointAttractor<JointState>::PointAttractor() :
29 gain_(std::make_shared<
Parameter<Eigen::MatrixXd>>(
"gain")) {
30 this->parameters_.insert(std::make_pair(
"attractor", attractor_));
31 this->parameters_.insert(std::make_pair(
"gain", gain_));
43 auto attractor = this->get_parameter_value<JointState>(
"attractor");
44 if (attractor.is_empty()) {
47 bool compatible = (attractor.get_size() == state.get_size());
49 for (
unsigned int i = 0; i < attractor.get_size(); ++i) {
50 compatible = (compatible && attractor.get_names()[i] == state.get_names()[i]);
57void PointAttractor<S>::set_gain(
const std::shared_ptr<ParameterInterface>& parameter,
unsigned int expected_size) {
58 if (parameter->get_parameter_type() == ParameterType::DOUBLE) {
59 auto gain = parameter->get_parameter_value<
double>();
60 this->gain_->set_value(gain * Eigen::MatrixXd::Identity(expected_size, expected_size));
61 }
else if (parameter->get_parameter_type() == ParameterType::DOUBLE_ARRAY) {
62 auto gain = parameter->get_parameter_value<std::vector<double>>();
63 if (gain.size() != expected_size) {
65 "The provided diagonal coefficients do not correspond to the expected size of the attractor");
67 Eigen::VectorXd diagonal = Eigen::VectorXd::Map(gain.data(), expected_size);
68 this->gain_->set_value(diagonal.asDiagonal());
69 }
else if (parameter->get_parameter_type() == ParameterType::MATRIX) {
70 auto gain = parameter->get_parameter_value<Eigen::MatrixXd>();
71 if (gain.rows() != expected_size && gain.cols() != expected_size) {
73 "The provided gain matrix do not have the expected size (" + std::to_string(expected_size) +
"x"
74 + std::to_string(expected_size) +
")");
76 this->gain_->set_value(gain);
83void PointAttractor<S>::set_attractor(
const S&) {
88void PointAttractor<CartesianState>::set_attractor(
const CartesianState& attractor) {
92 if (this->get_base_frame().is_empty()) {
93 IDynamicalSystem<CartesianState>::set_base_frame(
100 "The reference frame of the attractor " + attractor.
get_name() +
" in frame "
101 + attractor.
get_reference_frame() +
" is incompatible with the base frame of the dynamical system "
102 + this->get_base_frame().get_name() +
" in frame " + this->get_base_frame().get_reference_frame() +
".");
104 this->attractor_->set_value(this->get_base_frame().inverse() * attractor);
106 this->attractor_->set_value(attractor);
111void PointAttractor<JointState>::set_attractor(
const JointState& attractor) {
115 this->attractor_->set_value(attractor);
116 if (this->gain_->is_empty()) {
117 this->gain_->set_value(Eigen::MatrixXd::Identity(attractor.
get_size(), attractor.
get_size()));
133 IDynamicalSystem<CartesianState>::set_base_frame(base_frame);
134 if (!this->attractor_->get_value().is_empty()) {
136 auto attractor = this->attractor_->get_value();
137 attractor.set_reference_frame(base_frame.
get_name());
138 this->set_attractor(attractor);
143void PointAttractor<CartesianState>::validate_and_set_parameter(
const std::shared_ptr<ParameterInterface>& parameter) {
144 if (parameter->get_name() ==
"attractor") {
145 if (parameter->get_parameter_state_type() == StateType::CARTESIAN_STATE) {
146 this->set_attractor(parameter->get_parameter_value<
CartesianState>());
147 }
else if (parameter->get_parameter_state_type() == StateType::CARTESIAN_POSE) {
148 this->set_attractor(parameter->get_parameter_value<
CartesianPose>());
150 }
else if (parameter->get_name() ==
"gain") {
151 this->set_gain(parameter, 6);
154 "No parameter with name '" + parameter->get_name() +
"' found");
159void PointAttractor<JointState>::validate_and_set_parameter(
const std::shared_ptr<ParameterInterface>& parameter) {
160 if (parameter->get_name() ==
"attractor") {
161 if (parameter->get_parameter_state_type() == StateType::JOINT_STATE) {
162 this->set_attractor(parameter->get_parameter_value<
JointState>());
163 }
else if (parameter->get_parameter_state_type() == StateType::JOINT_POSITIONS) {
164 this->set_attractor(parameter->get_parameter_value<
JointPositions>());
166 }
else if (parameter->get_name() ==
"gain") {
167 this->set_gain(parameter, this->attractor_->get_value().get_size());
170 "No parameter with name '" + parameter->get_name() +
"' found");
181 if (this->attractor_->get_value().is_empty()) {
185 twist = this->gain_->get_value() * twist;
186 return CartesianTwist(state.get_name(), twist.
get_twist(), this->attractor_->get_value().get_reference_frame());
192 velocities = this->gain_->get_value() * velocities;
Abstract class for a dynamical system.
Represents a dynamical system to move towards an attractor.
PointAttractor()
Empty constructor.
Class to define Cartesian pose in Cartesian space as 3D position and quaternion based orientation.
Class to represent a state in Cartesian space.
Eigen::Matrix< double, 6, 1 > get_twist() const
Getter of the 6d twist from linear and angular velocity attributes.
static CartesianState Identity(const std::string &name, const std::string &reference="world")
Constructor for the identity Cartesian state (identity pose and 0 for the rest)
Class to define twist in Cartesian space as 3D linear and angular velocity vectors.
Class to define positions of the joints.
Class to define a state in joint space.
const Eigen::VectorXd & get_velocities() const
Getter of the velocities attribute.
unsigned int get_size() const
Getter of the size from the attributes.
Class to define velocities of the joints.
Class to represent name-value pairs of different types.
const std::string & get_reference_frame() const
Getter of the reference frame as const reference.
const std::string & get_name() const
Getter of the name attribute.
bool is_empty() const
Getter of the empty attribute.
Systems of equations relating state variables to their derivatives.
Core state variables and objects.