Cartesian state with pose, velocity and acceleration. More...
#include <cartesian_state.h>
Public Member Functions | |
CartesianState () | |
Initializes all quantities to zero and sets the orientation quaternion to identity. More... | |
CartesianState (const cartesian_control_msgs::CartesianTrajectoryPoint &point) | |
Convenience constructor for ROS messages. More... | |
CartesianState | operator- (const CartesianState &other) const |
Difference operator between states. More... | |
Eigen::Vector3d | rot () const |
Get Euler-Rodrigues vector from orientation. More... | |
cartesian_control_msgs::CartesianTrajectoryPoint | toMsg (int time_from_start=0) const |
Convenience method for conversion. More... | |
Public Attributes | |
Eigen::Vector3d | p |
position More... | |
Eigen::Quaterniond | q |
rotation More... | |
Eigen::Vector3d | v |
linear velocity, ![]() | |
Eigen::Vector3d | v_dot |
linear acceleration, ![]() | |
Eigen::Vector3d | w |
angular velocity, ![]() | |
Eigen::Vector3d | w_dot |
angular acceleration, ![]() | |
Friends | |
std::ostream & | operator<< (std::ostream &os, const CartesianState &state) |
Stream operator for testing and debugging. More... | |
Cartesian state with pose, velocity and acceleration.
All quantities are assumed to be given in one common reference frame. This frame is also the reference for the pose defined by p and q.
Definition at line 38 of file cartesian_state.h.
ros_controllers_cartesian::CartesianState::CartesianState | ( | ) |
Initializes all quantities to zero and sets the orientation quaternion to identity.
Definition at line 46 of file cartesian_state.cpp.
ros_controllers_cartesian::CartesianState::CartesianState | ( | const cartesian_control_msgs::CartesianTrajectoryPoint & | point | ) |
Convenience constructor for ROS messages.
Implicitly normalizes the point's orientation quaternion.
point | The desired state |
Definition at line 61 of file cartesian_state.cpp.
CartesianState ros_controllers_cartesian::CartesianState::operator- | ( | const CartesianState & | other | ) | const |
Difference operator between states.
This is the element-wise difference for all vector quantities and the difference of rotation for the quaternion.
other | State to subtract |
Definition at line 81 of file cartesian_state.cpp.
|
inline |
Get Euler-Rodrigues vector from orientation.
Definition at line 80 of file cartesian_state.h.
cartesian_control_msgs::CartesianTrajectoryPoint ros_controllers_cartesian::CartesianState::toMsg | ( | int | time_from_start = 0 | ) | const |
Convenience method for conversion.
time_from_start | Time from start in seconds |
Definition at line 94 of file cartesian_state.cpp.
|
friend |
Stream operator for testing and debugging.
os | The output stream |
state | The CartesianState |
Definition at line 113 of file cartesian_state.cpp.
Eigen::Vector3d ros_controllers_cartesian::CartesianState::p |
position
Definition at line 97 of file cartesian_state.h.
Eigen::Quaterniond ros_controllers_cartesian::CartesianState::q |
rotation
Definition at line 98 of file cartesian_state.h.
Eigen::Vector3d ros_controllers_cartesian::CartesianState::v |
linear velocity,
Definition at line 101 of file cartesian_state.h.
Eigen::Vector3d ros_controllers_cartesian::CartesianState::v_dot |
linear acceleration,
Definition at line 105 of file cartesian_state.h.
Eigen::Vector3d ros_controllers_cartesian::CartesianState::w |
angular velocity,
Definition at line 102 of file cartesian_state.h.
Eigen::Vector3d ros_controllers_cartesian::CartesianState::w_dot |
angular acceleration,
Definition at line 106 of file cartesian_state.h.