Franky 0.12.0
A High-Level Motion API for Franka
All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
twist_acceleration.hpp
Go to the documentation of this file.
1#pragma once
2
3#include <Eigen/Core>
4
5#include "franky/types.hpp"
6#include "franky/util.hpp"
7#include "franky/twist.hpp"
8
9namespace franky {
10
15 public:
17
22 explicit TwistAcceleration(Eigen::Vector3d linear_acceleration = Eigen::Vector3d::Zero(),
23 Eigen::Vector3d angular_acceleration = Eigen::Vector3d::Zero())
24 : linear_acceleration_(std::move(linear_acceleration)), angular_acceleration_(std::move(angular_acceleration)) {}
25
30 return TwistAcceleration{vector_repr.head<3>(), vector_repr.tail<3>()};
31 }
32
40 result << linear_acceleration_, angular_acceleration_;
41 return result;
42 }
43
53
60 template<typename RotationMatrixType>
62 return TwistAcceleration{rotation * linear_acceleration_, rotation * angular_acceleration_};
63 }
64
78 const Eigen::Vector3d &link_translation,
79 const Eigen::Vector3d &base_angular_velocity) const
80 {
81 Eigen::Vector3d propagated_linear_acceleration =
82 linear_acceleration_ +
83 angular_acceleration_.cross(link_translation) +
85
86 return TwistAcceleration{propagated_linear_acceleration, angular_acceleration_};
87 }
88
94 [[nodiscard]] Eigen::Vector3d linear_acceleration() const {
95 return linear_acceleration_;
96 }
97
103 [[nodiscard]] Eigen::Vector3d angular_acceleration() const {
104 return angular_acceleration_;
105 }
106
107 friend std::ostream& operator<<(std::ostream& os, const TwistAcceleration& twist_acceleration);
108
109 private:
110 Eigen::Vector3d linear_acceleration_;
111 Eigen::Vector3d angular_acceleration_;
112};
113
117
118template<typename RotationMatrixType>
122
123inline std::ostream& operator<<(std::ostream& os, const TwistAcceleration& twist_acceleration) {
124 os << "TwistAcceleration(lin=" <<twist_acceleration.linear_acceleration_ << ", ang=" << twist_acceleration.angular_acceleration_ << ")";
125 return os;
126}
127
128} // namespace franky
CartesianState transformWith(const Affine &transform) const
Transform the frame of the state by applying the given affine transform.
Definition cartesian_state.hpp:42
TwistAcceleration acceleration of a frame (2nd derivative of a pose).
Definition twist_acceleration.hpp:14
TwistAcceleration transformWith(const Affine &transformation) const
Transform the frame of the twist acceleration by applying the given affine transform.
Definition twist_acceleration.hpp:50
TwistAcceleration propagateThroughLink(const Eigen::Vector3d &link_translation, const Eigen::Vector3d &base_angular_velocity) const
Propagate the twist acceleration through a link with the given translation. Hence,...
Definition twist_acceleration.hpp:77
TwistAcceleration(Eigen::Vector3d linear_acceleration=Eigen::Vector3d::Zero(), Eigen::Vector3d angular_acceleration=Eigen::Vector3d::Zero())
Definition twist_acceleration.hpp:22
static TwistAcceleration fromVectorRepr(const Vector6d &vector_repr)
Definition twist_acceleration.hpp:29
Eigen::Vector3d angular_acceleration() const
Get the angular acceleration.
Definition twist_acceleration.hpp:103
friend std::ostream & operator<<(std::ostream &os, const TwistAcceleration &twist_acceleration)
Definition twist_acceleration.hpp:123
TwistAcceleration(const TwistAcceleration &twist_acceleration_acceleration)=default
Eigen::Vector3d linear_acceleration() const
Get the linear acceleration.
Definition twist_acceleration.hpp:94
Vector6d vector_repr() const
Get the vector representation of the twist acceleration. It consists of the linear and angular veloci...
Definition twist_acceleration.hpp:38
TwistAcceleration transformWith(const RotationMatrixType &rotation) const
Transform the frame of the twist acceleration by applying the given rotation.
Definition twist_acceleration.hpp:61
Definition dynamics_limit.cpp:8
std::ostream & operator<<(std::ostream &os, const DynamicsLimit< Vector7d > &limit)
Definition dynamics_limit.cpp:47
std::array< double, dims > toStdD(const Eigen::Matrix< double, dims, 1 > &vector)
Definition util.hpp:18
CartesianState operator*(const Affine &transform, const CartesianState &cartesian_state)
Definition cartesian_state.hpp:78
Eigen::Vector< double, 6 > Vector6d
Definition types.hpp:11
Eigen::Affine3d Affine
Definition types.hpp:16