Control Libraries 7.4.0
|
Class to define positions of the joints. More...
#include <JointPositions.hpp>
Public Member Functions | |
const Eigen::VectorXd & | get_velocities () const =delete |
double | get_velocity (unsigned int joint_index) const =delete |
double | get_velocity (const std::string &joint_name) const =delete |
void | set_velocities (const Eigen::VectorXd &velocities)=delete |
void | set_velocities (const std::vector< double > &velocities)=delete |
void | set_velocity (double velocity, unsigned int joint_index) const =delete |
void | set_velocity (double velocity, const std::string &joint_name) const =delete |
const Eigen::VectorXd & | get_accelerations () const =delete |
double | get_acceleration (unsigned int joint_index) const =delete |
double | get_acceleration (const std::string &joint_name) const =delete |
void | set_accelerations (const Eigen::VectorXd &accelerations)=delete |
void | set_accelerations (const std::vector< double > &accelerations)=delete |
void | set_acceleration (double acceleration, unsigned int joint_index) const =delete |
void | set_acceleration (double acceleration, const std::string &joint_name) const =delete |
const Eigen::VectorXd & | get_torques () const =delete |
double | get_torque (unsigned int joint_index) const =delete |
double | get_torque (const std::string &joint_name) const =delete |
void | set_torques (const Eigen::VectorXd &torques)=delete |
void | set_torques (const std::vector< double > &torques)=delete |
void | set_torque (double torque, unsigned int joint_index) const =delete |
void | set_torque (double torque, const std::string &joint_name) const =delete |
JointState & | operator+= (const JointVelocities &velocities)=delete |
JointState & | operator+= (const JointAccelerations &accelerations)=delete |
JointState & | operator+= (const JointTorques &torques)=delete |
JointState | operator+ (const JointVelocities &velocities) const =delete |
JointState | operator+ (const JointAccelerations &accelerations) const =delete |
JointState | operator+ (const JointTorques &torques) const =delete |
JointState & | operator-= (const JointVelocities &velocities)=delete |
JointState & | operator-= (const JointAccelerations &accelerations)=delete |
JointState & | operator-= (const JointTorques &torques)=delete |
JointState | operator- (const JointVelocities &velocities) const =delete |
JointState | operator- (const JointAccelerations &accelerations) const =delete |
JointState | operator- (const JointTorques &torques) const =delete |
JointPositions () | |
Empty constructor. | |
JointPositions (const std::string &robot_name, unsigned int nb_joints=0) | |
Constructor with name and number of joints provided. | |
JointPositions (const std::string &robot_name, const std::vector< std::string > &joint_names) | |
Constructor with name and list of joint names provided. | |
JointPositions (const std::string &robot_name, const Eigen::VectorXd &positions) | |
Constructor with name and position values provided. | |
JointPositions (const std::string &robot_name, const std::vector< std::string > &joint_names, const Eigen::VectorXd &positions) | |
Constructor with name, a list of joint names and position values provided. | |
JointPositions (const JointPositions &positions) | |
Copy constructor. | |
JointPositions (const JointState &state) | |
Copy constructor from a JointState. | |
JointPositions (const JointVelocities &velocities) | |
Integration constructor from joint velocities by considering that it is equivalent to multiplying the velocities by 1 second. | |
JointPositions & | operator= (const JointPositions &positions)=default |
Copy assignment operator that has to be defined to the custom assignment operator. | |
Eigen::VectorXd | data () const override |
Returns the positions data as an Eigen vector. | |
virtual void | set_data (const Eigen::VectorXd &data) override |
Set the positions data from an Eigen vector. | |
virtual void | set_data (const std::vector< double > &data) override |
Set the positions data from a std vector. | |
void | clamp (double max_absolute_value, double noise_ratio=0.) |
Clamp inplace the magnitude of the positions to the value in argument. | |
void | clamp (const Eigen::ArrayXd &max_absolute_value_array, const Eigen::ArrayXd &noise_ratio_array) |
Clamp inplace the magnitude of the positions to the values in argument. | |
JointPositions | clamped (double max_absolute_value, double noise_ratio=0.) const |
Return the position clamped to the value in argument. | |
JointPositions | clamped (const Eigen::ArrayXd &max_absolute_value_array, const Eigen::ArrayXd &noise_ratio_array) const |
Return the position clamped to the values in argument. | |
JointPositions | copy () const |
Return a copy of the joint positions. | |
JointPositions & | operator*= (double lambda) |
Scale inplace by a scalar. | |
JointPositions | operator* (double lambda) const |
Scale joint positions by a scalar. | |
JointPositions & | operator/= (double lambda) |
Scale inplace by a scalar. | |
JointPositions | operator/ (double lambda) const |
Scale joint positions by a scalar. | |
JointVelocities | operator/ (const std::chrono::nanoseconds &dt) const |
Differentiate joint positions pose over a time period. | |
JointPositions & | operator+= (const JointPositions &positions) |
Add inplace other joint positions. | |
JointPositions & | operator+= (const JointState &state) |
Add inplace other joint positions from a joint state. | |
JointPositions | operator+ (const JointPositions &positions) const |
Add other joint positions. | |
JointState | operator+ (const JointState &state) const |
Add another joint sate. | |
JointPositions | operator- () const |
Negate joint positions. | |
JointPositions & | operator-= (const JointPositions &positions) |
Compute inplace the difference with other joint positions. | |
JointPositions & | operator-= (const JointState &state) |
Compute inplace the difference with a joint state. | |
JointPositions | operator- (const JointPositions &positions) const |
Compute the difference with other joint positions. | |
JointState | operator- (const JointState &state) const |
Compute the difference with a joint state. | |
Public Member Functions inherited from state_representation::JointState | |
JointState () | |
Empty constructor for a joint state. | |
JointState (const std::string &robot_name, unsigned int nb_joints=0) | |
Constructor with name and number of joints provided. | |
JointState (const std::string &robot_name, const std::vector< std::string > &joint_names) | |
Constructor with name and list of joint names provided. | |
JointState (const JointState &state) | |
Copy constructor of a joint state. | |
JointState & | operator= (const JointState &state) |
Copy assignment operator that has to be defined to the custom assignment operator. | |
unsigned int | get_size () const |
Getter of the size from the attributes. | |
const std::vector< std::string > & | get_names () const |
Getter of the names attribute. | |
unsigned int | get_joint_index (const std::string &joint_name) const |
Get joint index by the name of the joint, if it exists. | |
const Eigen::VectorXd & | get_positions () const |
Getter of the positions attribute. | |
double | get_position (const std::string &joint_name) const |
Get the position of a joint by its name, if it exists. | |
double | get_position (unsigned int joint_index) const |
Get the position of a joint by its index, if it exists. | |
const Eigen::VectorXd & | get_velocities () const |
Getter of the velocities attribute. | |
double | get_velocity (const std::string &joint_name) const |
Get the velocity of a joint by its name, if it exists. | |
double | get_velocity (unsigned int joint_index) const |
Get the velocity of a joint by its index, if it exists. | |
const Eigen::VectorXd & | get_accelerations () const |
Getter of the accelerations attribute. | |
double | get_acceleration (const std::string &joint_name) const |
Get the acceleration of a joint by its name, if it exists. | |
double | get_acceleration (unsigned int joint_index) const |
Get the acceleration of a joint by its index, if it exists. | |
const Eigen::VectorXd & | get_torques () const |
Getter of the torques attribute. | |
double | get_torque (const std::string &joint_name) const |
Get the torque of a joint by its name, if it exists. | |
double | get_torque (unsigned int joint_index) const |
Get the torque of a joint by its index, if it exists. | |
Eigen::ArrayXd | array () const |
Returns the data vector as an Eigen array. | |
void | set_names (unsigned int nb_joints) |
Setter of the names attribute from the number of joints. | |
void | set_names (const std::vector< std::string > &names) |
Setter of the names attribute. | |
void | set_positions (const Eigen::VectorXd &positions) |
Setter of the positions attribute. | |
void | set_positions (const std::vector< double > &positions) |
Setter of the positions from std vector. | |
void | set_position (double position, const std::string &joint_name) |
Set the position of a joint by its name. | |
void | set_position (double position, unsigned int joint_index) |
Set the position of a joint by its index. | |
void | set_velocities (const Eigen::VectorXd &velocities) |
Setter of the velocities attribute. | |
void | set_velocities (const std::vector< double > &velocities) |
Setter of the velocities from std vector. | |
void | set_velocity (double velocity, const std::string &joint_name) |
Set the velocity of a joint by its name. | |
void | set_velocity (double velocity, unsigned int joint_index) |
Set the velocity of a joint by its index. | |
void | set_accelerations (const Eigen::VectorXd &accelerations) |
Setter of the accelerations attribute. | |
void | set_accelerations (const std::vector< double > &accelerations) |
Setter of the accelerations from std vector. | |
void | set_acceleration (double acceleration, const std::string &joint_name) |
Set the acceleration of a joint by its name. | |
void | set_acceleration (double acceleration, unsigned int joint_index) |
Set the acceleration of a joint by its index. | |
void | set_torques (const Eigen::VectorXd &torques) |
Setter of the torques attribute. | |
void | set_torques (const std::vector< double > &torques) |
Setter of the torques attributes. | |
void | set_torque (double torque, const std::string &joint_name) |
Set the torque of a joint by its name. | |
void | set_torque (double torque, unsigned int joint_index) |
Set the torque of a joint by its index. | |
void | clamp_state_variable (double max_absolute_value, const JointStateVariable &state_variable_type, double noise_ratio=0) |
Clamp inplace the magnitude of the a specific joint state variable. | |
void | clamp_state_variable (const Eigen::ArrayXd &max_absolute_value_array, const JointStateVariable &state_variable_type, const Eigen::ArrayXd &noise_ratio_array) |
Clamp inplace the magnitude of the a specific joint state variable for each individual joint. | |
JointState | copy () const |
Return a copy of the joint state. | |
double | dist (const JointState &state, const JointStateVariable &state_variable_type=JointStateVariable::ALL) const |
Compute the distance to another state as the sum of distances between each attribute. | |
void | reset () override |
Reset the object to a post-construction state. | |
bool | is_incompatible (const State &state) const override |
Check if the joint state is incompatible for operations with the state given as argument. | |
void | set_zero () |
Set the joint state to a zero value. | |
std::vector< double > | to_std_vector () const |
Return the joint state as a std vector. | |
JointState & | operator*= (double lambda) |
Scale inplace by a scalar. | |
JointState | operator* (double lambda) const |
Scale a joint state by a scalar. | |
JointState & | operator/= (double lambda) |
Scale inplace by a scalar. | |
JointState | operator/ (double lambda) const |
Scale a joint state by a scalar. | |
JointState & | operator+= (const JointState &state) |
Add inplace another joint state. | |
JointState | operator+ (const JointState &state) const |
Add another joint sate. | |
JointState | operator- () const |
Negate a joint state. | |
JointState & | operator-= (const JointState &state) |
Compute inplace the difference with another joint state. | |
JointState | operator- (const JointState &state) const |
Compute the difference with another joint state. | |
Public Member Functions inherited from state_representation::State | |
State () | |
Empty constructor. | |
State (const std::string &name) | |
Constructor with name specification. | |
State (const State &state) | |
Copy constructor from another state. | |
virtual | ~State ()=default |
Virtual destructor. | |
State & | operator= (const State &state) |
Copy assignment operator that has to be defined to the custom assignment operator. | |
const StateType & | get_type () const |
Getter of the type attribute. | |
const std::string & | get_name () const |
Getter of the name attribute. | |
bool | is_empty () const |
Getter of the empty attribute. | |
const std::chrono::time_point< std::chrono::steady_clock > & | get_timestamp () const |
Getter of the timestamp attribute. | |
virtual void | set_name (const std::string &name) |
Setter of the name attribute. | |
void | reset_timestamp () |
Reset the timestamp attribute to now. | |
virtual void | set_data (const Eigen::MatrixXd &data) |
Set the data of the state from an Eigen matrix. | |
double | get_age () const |
Get the age of the state, i.e. the time since the last modification. | |
bool | is_deprecated (double time_delay) const |
Check if the state is deprecated given a certain time delay. | |
template<typename DurationT > | |
bool | is_deprecated (const std::chrono::duration< int64_t, DurationT > &time_delay) const |
Check if the state is deprecated given a certain time delay. | |
operator bool () const noexcept | |
Boolean operator for the truthiness of a state. | |
Static Public Member Functions | |
static JointPositions | Zero (const std::string &robot_name, unsigned int nb_joints) |
Constructor for zero joint positions. | |
static JointPositions | Zero (const std::string &robot_name, const std::vector< std::string > &joint_names) |
Constructor for zero joint positions. | |
static JointPositions | Random (const std::string &robot_name, unsigned int nb_joints) |
Constructor for random joint positions. | |
static JointPositions | Random (const std::string &robot_name, const std::vector< std::string > &joint_names) |
Constructor for the random joint positions. | |
Static Public Member Functions inherited from state_representation::JointState | |
static JointState | Zero (const std::string &robot_name, unsigned int nb_joints) |
Constructor for a zero joint state. | |
static JointState | Zero (const std::string &robot_name, const std::vector< std::string > &joint_names) |
Constructor for a zero joint state. | |
static JointState | Random (const std::string &robot_name, unsigned int nb_joints) |
Constructor for a random joint state. | |
static JointState | Random (const std::string &robot_name, const std::vector< std::string > &joint_names) |
Constructor for a random joint state. | |
Friends | |
JointPositions | operator* (double lambda, const JointPositions &positions) |
Scale joint positions by a scalar. | |
JointPositions | operator* (const Eigen::MatrixXd &lambda, const JointPositions &positions) |
Scale joint positions by a matrix. | |
std::ostream & | operator<< (std::ostream &os, const JointPositions &positions) |
Overload the ostream operator for printing. | |
Additional Inherited Members | |
Protected Member Functions inherited from state_representation::JointState | |
void | multiply_state_variable (const Eigen::MatrixXd &lambda, const JointStateVariable &state_variable_type) |
Proxy function that scale the specified state variable by a matrix. | |
Eigen::VectorXd | get_state_variable (const JointStateVariable &state_variable_type) const |
Getter of the variable value corresponding to the input. | |
void | set_state_variable (const Eigen::VectorXd &new_value, const JointStateVariable &state_variable_type) |
Setter of the variable value corresponding to the input. | |
void | set_state_variable (const std::vector< double > &new_value, const JointStateVariable &state_variable_type) |
Setter of the variable value corresponding to the input. | |
void | set_state_variable (double new_value, unsigned int joint_index, const JointStateVariable &state_variable_type) |
Setter of the variable value corresponding to the input. | |
std::string | to_string () const override |
Convert the state to its string representation. | |
Protected Member Functions inherited from state_representation::State | |
void | set_type (const StateType &type) |
Setter of the state type attribute. | |
void | set_empty (bool empty=true) |
Setter of the empty attribute. | |
void | assert_not_empty () const |
Throw an exception if the state is empty. | |
Class to define positions of the joints.
Definition at line 18 of file JointPositions.hpp.
|
explicit |
Empty constructor.
Definition at line 9 of file JointPositions.cpp.
|
explicit |
Constructor with name and number of joints provided.
name The name of the state
nb_joints The number of joints for initialization
Definition at line 13 of file JointPositions.cpp.
|
explicit |
Constructor with name and list of joint names provided.
name The name of the state
joint_names List of joint names
Definition at line 24 of file JointPositions.cpp.
|
explicit |
Constructor with name and position values provided.
name The name of the state
positions The vector of positions
Definition at line 18 of file JointPositions.cpp.
|
explicit |
Constructor with name, a list of joint names and position values provided.
name The name of the state
joint_names List of joint names
positions The vector of positions
Definition at line 29 of file JointPositions.cpp.
state_representation::JointPositions::JointPositions | ( | const JointPositions & | positions | ) |
Copy constructor.
Definition at line 44 of file JointPositions.cpp.
state_representation::JointPositions::JointPositions | ( | const JointState & | state | ) |
Copy constructor from a JointState.
Definition at line 36 of file JointPositions.cpp.
state_representation::JointPositions::JointPositions | ( | const JointVelocities & | velocities | ) |
Integration constructor from joint velocities by considering that it is equivalent to multiplying the velocities by 1 second.
Definition at line 47 of file JointPositions.cpp.
void state_representation::JointPositions::clamp | ( | const Eigen::ArrayXd & | max_absolute_value_array, |
const Eigen::ArrayXd & | noise_ratio_array | ||
) |
Clamp inplace the magnitude of the positions to the values in argument.
max_absolute_value_array | The maximum value of position for each joint |
noise_ratio_array | Those values will be used to apply a dead zone relative to the maximum absolute value under which the position will be set to 0 for each individual joint |
Definition at line 82 of file JointPositions.cpp.
void state_representation::JointPositions::clamp | ( | double | max_absolute_value, |
double | noise_ratio = 0. |
||
) |
Clamp inplace the magnitude of the positions to the value in argument.
max_absolute_value | The maximum value of position for all the joints |
noise_ratio | If provided, this value will be used to apply a dead zone relative to the maximum absolute value under which the position will be set to 0 |
Definition at line 78 of file JointPositions.cpp.
JointPositions state_representation::JointPositions::clamped | ( | const Eigen::ArrayXd & | max_absolute_value_array, |
const Eigen::ArrayXd & | noise_ratio_array | ||
) | const |
Return the position clamped to the values in argument.
max_absolute_value_array | The maximum value of position for each joint |
noise_ratio_array | Those values will be used to apply a dead zone relative to the maximum absolute value under which the position will be set to 0 for each individual joint |
Definition at line 92 of file JointPositions.cpp.
JointPositions state_representation::JointPositions::clamped | ( | double | max_absolute_value, |
double | noise_ratio = 0. |
||
) | const |
Return the position clamped to the value in argument.
max_absolute_value | The maximum value of position for all the joints |
noise_ratio | If provided, this value will be used to apply a dead zone relative to the maximum absolute value under which the position will be set to 0 |
Definition at line 86 of file JointPositions.cpp.
JointPositions state_representation::JointPositions::copy | ( | ) | const |
Return a copy of the joint positions.
Definition at line 100 of file JointPositions.cpp.
|
overridevirtual |
Returns the positions data as an Eigen vector.
Reimplemented from state_representation::JointState.
Definition at line 66 of file JointPositions.cpp.
JointPositions state_representation::JointPositions::operator* | ( | double | lambda | ) | const |
Scale joint positions by a scalar.
All joints in all the state variables are scaled by the same factor.
lambda | The scaling factor |
lambda | The scaling factor |
Definition at line 110 of file JointPositions.cpp.
JointPositions & state_representation::JointPositions::operator*= | ( | double | lambda | ) |
Scale inplace by a scalar.
All joints in all the state variables are scaled by the same factor.
lambda | The scaling factor |
lambda | The scaling factor |
Definition at line 105 of file JointPositions.cpp.
JointPositions state_representation::JointPositions::operator+ | ( | const JointPositions & | positions | ) | const |
Add other joint positions.
positions | Joint positions with same name and same joint names |
Definition at line 156 of file JointPositions.cpp.
JointState state_representation::JointPositions::operator+ | ( | const JointState & | state | ) | const |
Add another joint sate.
state | A joint state with same name and same joint names |
Definition at line 160 of file JointPositions.cpp.
JointPositions & state_representation::JointPositions::operator+= | ( | const JointPositions & | positions | ) |
Add inplace other joint positions.
positions | Joint positions with same name and same joint names |
Definition at line 146 of file JointPositions.cpp.
JointPositions & state_representation::JointPositions::operator+= | ( | const JointState & | state | ) |
Add inplace other joint positions from a joint state.
state | A joint state with same name and same joint names |
Definition at line 151 of file JointPositions.cpp.
JointPositions state_representation::JointPositions::operator- | ( | ) | const |
Negate joint positions.
Definition at line 164 of file JointPositions.cpp.
JointPositions state_representation::JointPositions::operator- | ( | const JointPositions & | positions | ) | const |
Compute the difference with other joint positions.
positions | Joint positions with same name and same joint names |
Definition at line 178 of file JointPositions.cpp.
JointState state_representation::JointPositions::operator- | ( | const JointState & | state | ) | const |
Compute the difference with a joint state.
state | A joint state with same name and same joint names |
Definition at line 182 of file JointPositions.cpp.
JointPositions & state_representation::JointPositions::operator-= | ( | const JointPositions & | positions | ) |
Compute inplace the difference with other joint positions.
positions | Joint positions with same name and same joint names |
Definition at line 168 of file JointPositions.cpp.
JointPositions & state_representation::JointPositions::operator-= | ( | const JointState & | state | ) |
Compute inplace the difference with a joint state.
state | A joint state with same name and same joint names |
Definition at line 173 of file JointPositions.cpp.
JointVelocities state_representation::JointPositions::operator/ | ( | const std::chrono::nanoseconds & | dt | ) | const |
Differentiate joint positions pose over a time period.
dt | The time period used for derivation |
Definition at line 135 of file JointPositions.cpp.
JointPositions state_representation::JointPositions::operator/ | ( | double | lambda | ) | const |
Scale joint positions by a scalar.
All joints in all the state variables are scaled by the same factor.
lambda | The scaling factor |
lambda | The scaling factor |
Definition at line 131 of file JointPositions.cpp.
JointPositions & state_representation::JointPositions::operator/= | ( | double | lambda | ) |
Scale inplace by a scalar.
All joints in all the state variables are scaled by the same factor.
lambda | The scaling factor |
lambda | The scaling factor |
Definition at line 126 of file JointPositions.cpp.
|
default |
Copy assignment operator that has to be defined to the custom assignment operator.
positions | The state with value to assign |
|
static |
Constructor for the random joint positions.
robot_name | The name of the associated robot |
joint_names | List of joint names |
Definition at line 62 of file JointPositions.cpp.
|
static |
Constructor for random joint positions.
robot_name | The name of the associated robot |
nb_joints | The number of joints for initialization |
Definition at line 58 of file JointPositions.cpp.
|
overridevirtual |
Set the positions data from an Eigen vector.
data | The positions data vector |
Reimplemented from state_representation::JointState.
Definition at line 70 of file JointPositions.cpp.
|
overridevirtual |
Set the positions data from a std vector.
data | The positions data vector |
Reimplemented from state_representation::JointState.
Definition at line 74 of file JointPositions.cpp.
|
static |
Constructor for zero joint positions.
robot_name | The name of the associated robot |
joint_names | List of joint names |
Definition at line 54 of file JointPositions.cpp.
|
static |
Constructor for zero joint positions.
robot_name | The name of the associated robot |
nb_joints | The number of joints for initialization |
Definition at line 50 of file JointPositions.cpp.
|
friend |
Scale joint positions by a matrix.
lambda | The scaling matrix |
accelerations | The joint positions to be scaled |
Definition at line 120 of file JointPositions.cpp.
|
friend |
Scale joint positions by a scalar.
All joints in all the state variables are scaled by the same factor.
lambda | The scaling factor |
lambda | The scaling factor |
positions | The joint positions to be scaled |
Definition at line 114 of file JointPositions.cpp.
|
friend |
Overload the ostream operator for printing.
os | The ostream to append the string representing the state |
positions | The state to print |
Definition at line 186 of file JointPositions.cpp.