1#include "state_representation_bindings.hpp"
3#include <state_representation/State.hpp>
4#include <state_representation/space/SpatialState.hpp>
5#include <state_representation/space/cartesian/CartesianState.hpp>
6#include <state_representation/space/cartesian/CartesianPose.hpp>
7#include <state_representation/space/cartesian/CartesianTwist.hpp>
8#include <state_representation/space/cartesian/CartesianAcceleration.hpp>
9#include <state_representation/space/cartesian/CartesianWrench.hpp>
12void spatial_state(py::module_& m) {
13 py::class_<SpatialState, std::shared_ptr<SpatialState>,
State> c(m,
"SpatialState");
15 c.def(py::init(),
"Empty constructor.");
16 c.def(py::init<const std::string&, const std::string&>(),
"Constructor with name and reference frame specification.",
"name"_a,
"reference_frame"_a=std::string(
"world"));
17 c.def(py::init<const SpatialState&>(),
"Copy constructor from another SpatialState.",
"state"_a);
25 c.def(
"__deepcopy__", [](
const SpatialState &state, py::dict) {
29 std::stringstream buffer;
35void cartesian_state_variable(py::module_& m) {
36 py::enum_<CartesianStateVariable>(m,
"CartesianStateVariable")
37 .value(
"POSITION", CartesianStateVariable::POSITION)
38 .value(
"ORIENTATION", CartesianStateVariable::ORIENTATION)
39 .value(
"POSE", CartesianStateVariable::POSE)
40 .value(
"LINEAR_VELOCITY", CartesianStateVariable::LINEAR_VELOCITY)
41 .value(
"ANGULAR_VELOCITY", CartesianStateVariable::ANGULAR_VELOCITY)
42 .value(
"TWIST", CartesianStateVariable::TWIST)
43 .value(
"LINEAR_ACCELERATION", CartesianStateVariable::LINEAR_ACCELERATION)
44 .value(
"ANGULAR_ACCELERATION", CartesianStateVariable::ANGULAR_ACCELERATION)
45 .value(
"ACCELERATION", CartesianStateVariable::ACCELERATION)
46 .value(
"FORCE", CartesianStateVariable::FORCE)
47 .value(
"TORQUE", CartesianStateVariable::TORQUE)
48 .value(
"WRENCH", CartesianStateVariable::WRENCH)
49 .value(
"ALL", CartesianStateVariable::ALL)
53void cartesian_state(py::module_& m) {
54 m.def(
"dist", py::overload_cast<const CartesianState&, const CartesianState&, const CartesianStateVariable&>(&
state_representation::dist),
"Compute the distance between two CartesianStates",
"s1"_a,
"s2"_a,
"state_variable_type"_a=CartesianStateVariable::ALL);
56 py::class_<CartesianState, std::shared_ptr<CartesianState>,
SpatialState> c(m,
"CartesianState");
57 c.def(py::init(),
"Empty constructor");
58 c.def(py::init<const std::string&, const std::string&>(),
"Constructor with name and reference frame provided",
"name"_a,
"reference_frame"_a=std::string(
"world"));
59 c.def(py::init<const CartesianState&>(),
"Copy constructor of a CartesianState",
"state"_a);
61 c.def_static(
"Identity", &
CartesianState::Identity,
"Constructor for the identity CartesianState (identity pose and 0 for the rest)",
"name"_a,
"reference_frame"_a=std::string(
"world"));
62 c.def_static(
"Random", &
CartesianState::Random,
"Constructor for a random state",
"name"_a,
"reference_frame"_a=std::string(
"world"));
65 c.def(
"get_orientation", [](
const CartesianState &state) -> py::object {
66 py::object PyQuaternion = py::module_::import(
"pyquaternion").attr(
"Quaternion");
67 return PyQuaternion(state.get_orientation_coefficients());;
68 },
"Getter of the orientation attribute as pyquaternion object");
70 c.def(
"get_pose", &
CartesianState::get_pose,
"Getter of a 7d pose vector from position and orientation coefficients");
86 c.def(
"set_position", py::overload_cast<
const std::vector<double>&>(&
CartesianState::set_position),
"Setter of the position from a list");
87 c.def(
"set_position", py::overload_cast<const double&, const double&, const double&>(&
CartesianState::set_position),
"Setter of the position from three scalar coordinates",
"x"_a,
"y"_a,
"z"_a);
88 c.def(
"set_orientation", [](
CartesianState &state,
const py::object& quaternion) {
89 py::object PyQuaternion = py::module_::import(
"pyquaternion").attr(
"Quaternion");
90 if (py::isinstance<py::list>(quaternion)) {
91 state.set_orientation(py::cast<std::vector<double>>(quaternion));
92 }
else if (py::isinstance<py::array>(quaternion)) {
93 state.set_orientation(py::cast<Eigen::Vector4d>(quaternion));
94 }
else if (py::isinstance(quaternion, PyQuaternion)) {
95 state.set_orientation(py::cast<Eigen::Vector4d>(quaternion.attr(
"elements")));
97 throw std::invalid_argument(
"Type of input argument quaternion is not supported. "
98 "Supported types are: pyquaternion.Quaternion, numpy.array, list");
100 },
"Setter of the orientation attribute from a pyquaternion.Quaternion, numpy.array(w, x, y, z), or list(w, x, y, z)");
101 c.def(
"set_pose", py::overload_cast<
const Eigen::Matrix<double, 7, 1>&>(&
CartesianState::set_pose),
"Setter of the pose from a 7d vector of position and orientation coefficients (x, y, z, qw, qx, qy, qz)");
102 c.def(
"set_pose", py::overload_cast<
const std::vector<double>&>(&
CartesianState::set_pose),
"Setter of the pose from a 7d list of position and orientation coefficients (x, y, z, qw, qx, qy, qz)");
106 c.def(
"set_linear_velocity", py::overload_cast<const double&, const double&, const double&>(&
CartesianState::set_linear_velocity),
"Setter of the linear velocity from three scalar coordinates",
"x"_a,
"y"_a,
"z"_a);
109 c.def(
"set_angular_velocity", py::overload_cast<const double&, const double&, const double&>(&
CartesianState::set_angular_velocity),
"Setter of the angular velocity from three scalar coordinates",
"x"_a,
"y"_a,
"z"_a);
110 c.def(
"set_twist", py::overload_cast<
const Eigen::Matrix<double, 6, 1>&>(&
CartesianState::set_twist),
"Setter of the linear and angular velocities from a 6d twist vector");
111 c.def(
"set_twist", py::overload_cast<
const std::vector<double>&>(&
CartesianState::set_twist),
"Setter of the linear and angular velocities from a list");
115 c.def(
"set_linear_acceleration", py::overload_cast<const double&, const double&, const double&>(&
CartesianState::set_linear_acceleration),
"Setter of the linear acceleration from three scalar coordinates",
"x"_a,
"y"_a,
"z"_a);
118 c.def(
"set_angular_acceleration", py::overload_cast<const double&, const double&, const double&>(&
CartesianState::set_angular_acceleration),
"Setter of the angular acceleration from three scalar coordinates",
"x"_a,
"y"_a,
"z"_a);
119 c.def(
"set_acceleration", py::overload_cast<
const Eigen::Matrix<double, 6, 1>&>(&
CartesianState::set_acceleration),
"Setter of the linear and angular accelerations from a 6d acceleration vector");
120 c.def(
"set_acceleration", py::overload_cast<
const std::vector<double>&>(&
CartesianState::set_acceleration),
"Setter of the linear and angular accelerations from a list");
122 c.def(
"set_force", py::overload_cast<const Eigen::Vector3d&>(&
CartesianState::set_force),
"Setter of the force attribute");
123 c.def(
"set_force", py::overload_cast<
const std::vector<double>&>(&
CartesianState::set_force),
"Setter of the force from a list");
124 c.def(
"set_force", py::overload_cast<const double&, const double&, const double&>(&
CartesianState::set_force),
"Setter of the force from three scalar coordinates",
"x"_a,
"y"_a,
"z"_a);
125 c.def(
"set_torque", py::overload_cast<const Eigen::Vector3d&>(&
CartesianState::set_torque),
"Setter of the torque attribute");
126 c.def(
"set_torque", py::overload_cast<
const std::vector<double>&>(&
CartesianState::set_torque),
"Setter of the torque from a list");
127 c.def(
"set_torque", py::overload_cast<const double&, const double&, const double&>(&
CartesianState::set_torque),
"Setter of the torque from three scalar coordinates",
"x"_a,
"y"_a,
"z"_a);
128 c.def(
"set_wrench", py::overload_cast<
const Eigen::Matrix<double, 6, 1>&>(&
CartesianState::set_wrench),
"Setter of the force and torque from a 6d wrench vector");
129 c.def(
"set_wrench", py::overload_cast<
const std::vector<double>&>(&
CartesianState::set_wrench),
"Setter of the force and torque velocities from a list");
132 c.def(
"clamp_state_variable", &
CartesianState::clamp_state_variable,
"Clamp inplace the magnitude of the a specific state variable (velocity, acceleration or force)",
"max_value"_a,
"state_variable_type"_a,
"noise_ratio"_a=
double(0));
134 c.def(
"data", &
CartesianState::data,
"Returns the data as the concatenation of all the state variables in a single vector");
136 c.def(
"set_data", py::overload_cast<const Eigen::VectorXd&>(&
CartesianState::set_data),
"Set the data of the state from all the state variables in a single vector",
"data"_a);
137 c.def(
"set_data", py::overload_cast<
const std::vector<double>&>(&
CartesianState::set_data),
"Set the data of the state from all the state variables in a single list",
"data"_a);
139 c.def(py::self *= py::self);
140 c.def(py::self * py::self);
141 c.def(py::self *=
double());
142 c.def(py::self *
double());
143 c.def(
double() * py::self);
144 c.def(py::self * Eigen::Vector3d());
145 c.def(
"__mul__", [](
const CartesianState& self,
const Eigen::MatrixXd& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for *: 'state_representation.CartesianState' and 'np.ndarray'"); });
146 c.def(
"__imul__", [](
const CartesianState& self,
const Eigen::MatrixXd& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for *=: 'state_representation.CartesianState' and 'np.ndarray'"); });
147 c.def(py::self /=
double());
148 c.def(py::self /
double());
149 c.def(
"__truediv__", [](
const CartesianState& self,
const Eigen::MatrixXd& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for /: 'state_representation.CartesianState' and 'np.ndarray'"); });
151 c.def(py::self += py::self);
152 c.def(py::self + py::self);
154 c.def(py::self -= py::self);
155 c.def(py::self - py::self);
158 c.def(
"dist", &
CartesianState::dist,
"Compute the distance to another state as the sum of distances between each features",
"state"_a,
"state_variable_type"_a=CartesianStateVariable::ALL);
159 c.def(
"norms", &
CartesianState::norms,
"Compute the norms of the state variable specified by the input type (default is full state)",
"state_variable_type"_a=CartesianStateVariable::ALL);
160 c.def(
"normalize", &
CartesianState::normalize,
"Normalize inplace the state at the state variable given in argument (default is full state)",
"state_variable_type"_a=CartesianStateVariable::ALL);
161 c.def(
"normalized", &
CartesianState::normalized,
"Compute the normalized state at the state variable given in argument (default is full state)",
"state_variable_type"_a=CartesianStateVariable::ALL);
172 std::stringstream buffer;
178void cartesian_pose(py::module_& m) {
179 py::class_<CartesianPose, std::shared_ptr<CartesianPose>,
CartesianState> c(m,
"CartesianPose");
181 c.def(py::init(),
"Empty constructor");
182 c.def(py::init<const std::string&, const std::string&>(),
"Constructor with name and reference frame provided",
"name"_a,
"reference_frame"_a=std::string(
"world"));
183 c.def(py::init<const CartesianPose&>(),
"Copy constructor of a CartesianPose",
"pose"_a);
184 c.def(py::init<const CartesianState&>(),
"Copy constructor from a CartesianState",
"state"_a);
185 c.def(py::init<const CartesianTwist&>(),
"Copy constructor from a CartesianTwist by considering that it is a displacement over 1 second",
"twist"_a);
187 c.def(py::init<const std::string&, const Eigen::Vector3d&, const std::string&>(),
"Constructor of a CartesianPose from a position given as a vector of coordinates",
"name"_a,
"position"_a,
"reference_frame"_a=std::string(
"world"));
188 c.def(py::init<const std::string&, double, double, double, const std::string&>(),
"Constructor of a CartesianPose from a position given as three scalar coordinates",
"name"_a,
"x"_a,
"y"_a,
"z"_a,
"reference_frame"_a=std::string(
"world"));
189 c.def(py::init([](
const std::string& name,
const Eigen::Vector4d& orientation,
const std::string& reference) {
190 Eigen::Quaterniond q(orientation(0), orientation(1), orientation(2), orientation(3));
192 }),
"Constructor of a CartesianPose from a quaternion",
"name"_a,
"orientation"_a,
"reference_frame"_a=std::string(
"world"));
193 c.def(py::init([](
const std::string& name,
const Eigen::Vector3d& position,
const Eigen::Vector4d& orientation,
const std::string& reference) {
194 Eigen::Quaterniond q(orientation(0), orientation(1), orientation(2), orientation(3));
196 }),
"Constructor of a CartesianPose from a position given as a vector of coordinates and a quaternion",
"name"_a,
"position"_a,
"orientation"_a,
"reference_frame"_a=std::string(
"world"));
198 c.def_static(
"Identity", &
CartesianPose::Identity,
"Constructor for the identity pose",
"name"_a,
"reference_frame"_a=std::string(
"world"));
199 c.def_static(
"Random", &
CartesianPose::Random,
"Constructor for a random pose",
"name"_a,
"reference_frame"_a=std::string(
"world"));
201 std::vector<std::string> deleted_attributes = {
205 "linear_acceleration",
206 "angular_acceleration",
213 for (
const std::string& attr : deleted_attributes) {
214 c.def(std::string(
"get_" + attr).c_str(), [](
const CartesianPose&) ->
void {},
"Deleted method from parent class.");
215 c.def(std::string(
"set_" + attr).c_str(), [](
const CartesianPose& pose) ->
CartesianPose {
return pose; },
"Deleted method from parent class.");
218 c.def(py::self *= py::self);
219 c.def(
"__imul__", [](
const CartesianPose& self,
const CartesianTwist& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for *=: 'state_representation.CartesianPose' and 'state_representation.CartesianTwist'"); });
220 c.def(
"__imul__", [](
const CartesianPose& self,
const CartesianAcceleration& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for *=: 'state_representation.CartesianPose' and 'state_representation.CartesianAcceleration'"); });
221 c.def(
"__imul__", [](
const CartesianPose& self,
const CartesianWrench& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for *=: 'state_representation.CartesianPose' and 'state_representation.CartesianWrench'"); });
223 c.def(py::self * py::self);
228 c.def(py::self *=
double());
229 c.def(py::self *
double());
230 c.def(
double() * py::self);
231 c.def(
"__mul__", [](
const CartesianPose& self,
const Eigen::MatrixXd& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for *: 'state_representation.CartesianPose' and 'np.ndarray'"); });
232 c.def(
"__imul__", [](
const CartesianPose& self,
const Eigen::MatrixXd& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for *=: 'state_representation.CartesianPose' and 'np.ndarray'"); });
233 c.def(py::self /=
double());
234 c.def(py::self /
double());
235 c.def(
"__truediv__", [](
const CartesianPose& self,
const Eigen::MatrixXd& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for /: 'state_representation.CartesianPose' and 'np.ndarray'"); });
237 c.def(py::self / std::chrono::nanoseconds());
239 c.def(py::self += py::self);
240 c.def(
"__iadd__", [](
const CartesianPose& self,
const CartesianTwist& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +=: 'state_representation.CartesianPose' and 'state_representation.CartesianTwist'"); });
241 c.def(
"__iadd__", [](
const CartesianPose& self,
const CartesianAcceleration& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +=: 'state_representation.CartesianPose' and 'state_representation.CartesianAcceleration'"); });
242 c.def(
"__iadd__", [](
const CartesianPose& self,
const CartesianWrench& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +=: 'state_representation.CartesianPose' and 'state_representation.CartesianWrench'"); });
244 c.def(py::self + py::self);
245 c.def(
"__add__", [](
const CartesianPose& self,
const CartesianTwist& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +: 'state_representation.CartesianPose' and 'state_representation.CartesianTwist'"); });
246 c.def(
"__add__", [](
const CartesianPose& self,
const CartesianAcceleration& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +: 'state_representation.CartesianPose' and 'state_representation.CartesianAcceleration'"); });
247 c.def(
"__add__", [](
const CartesianPose& self,
const CartesianWrench& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +: 'state_representation.CartesianPose' and 'state_representation.CartesianWrench'"); });
250 c.def(py::self -= py::self);
251 c.def(
"__isub__", [](
const CartesianPose& self,
const CartesianTwist& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -=: 'state_representation.CartesianPose' and 'state_representation.CartesianTwist'"); });
252 c.def(
"__isub__", [](
const CartesianPose& self,
const CartesianAcceleration& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -=: 'state_representation.CartesianPose' and 'state_representation.CartesianAcceleration'"); });
253 c.def(
"__isub__", [](
const CartesianPose& self,
const CartesianWrench& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -=: 'state_representation.CartesianPose' and 'state_representation.CartesianWrench'"); });
255 c.def(py::self - py::self);
256 c.def(
"__sub__", [](
const CartesianPose& self,
const CartesianTwist& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -: 'state_representation.CartesianPose' and 'state_representation.CartesianTwist'"); });
257 c.def(
"__sub__", [](
const CartesianPose& self,
const CartesianAcceleration& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -: 'state_representation.CartesianPose' and 'state_representation.CartesianAcceleration'"); });
258 c.def(
"__sub__", [](
const CartesianPose& self,
const CartesianWrench& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -: 'state_representation.CartesianPose' and 'state_representation.CartesianWrench'"); });
265 c.def(
"set_data", py::overload_cast<const Eigen::VectorXd&>(&
CartesianPose::set_data),
"Set the pose data from a vector",
"data"_a);
266 c.def(
"set_data", py::overload_cast<
const std::vector<double>&>(&
CartesianPose::set_data),
"Set the pose data from a list",
"data"_a);
267 c.def(
"norms", &
CartesianPose::norms,
"Compute the norms of the state variable specified by the input type (default is full pose)",
"state_variable_type"_a=CartesianStateVariable::POSE);
268 c.def(
"normalized", &
CartesianPose::normalized,
"Compute the normalized pose at the state variable given in argument (default is full pose)",
"state_variable_type"_a=CartesianStateVariable::POSE);
273 c.def(
"__deepcopy__", [](
const CartesianPose &pose, py::dict) {
277 std::stringstream buffer;
283void cartesian_twist(py::module_& m) {
284 py::class_<CartesianTwist, std::shared_ptr<CartesianTwist>,
CartesianState> c(m,
"CartesianTwist");
286 c.def(py::init(),
"Empty constructor");
287 c.def(py::init<const std::string&, const std::string&>(),
"Constructor with name and reference frame provided",
"name"_a,
"reference_frame"_a=std::string(
"world"));
288 c.def(py::init<const CartesianTwist&>(),
"Copy constructor",
"twist"_a);
289 c.def(py::init<const CartesianState&>(),
"Copy constructor from a CartesianState",
"state"_a);
290 c.def(py::init<const CartesianPose&>(),
"Copy constructor from a CartesianPose by considering that it is equivalent to dividing the pose by 1 second",
"pose"_a);
292 c.def(py::init<const std::string&, const Eigen::Vector3d&, const std::string&>(),
"Construct a CartesianTwist from a linear velocity given as a vector.",
"name"_a,
"linear_velocity"_a,
"reference_frame"_a=std::string(
"world"));
293 c.def(py::init<const std::string&, const Eigen::Vector3d&, const Eigen::Vector3d&, const std::string&>(),
"Construct a CartesianTwist from a linear velocity and angular velocity given as vectors.",
"name"_a,
"linear_velocity"_a,
"angular_velocity"_a,
"reference_frame"_a=std::string(
"world"));
294 c.def(py::init<
const std::string&,
const Eigen::Matrix<double, 6, 1>&,
const std::string&>(),
"Construct a CartesianTwist from a single 6d twist vector",
"name"_a,
"twist"_a,
"reference_frame"_a=std::string(
"world"));
296 c.def_static(
"Zero", &
CartesianTwist::Zero,
"Constructor for the zero twist",
"name"_a,
"reference_frame"_a=std::string(
"world"));
297 c.def_static(
"Random", &
CartesianTwist::Random,
"Constructor for a random twist",
"name"_a,
"reference_frame"_a=std::string(
"world"));
299 std::vector<std::string> deleted_attributes = {
303 "linear_acceleration",
304 "angular_acceleration",
311 for (
const std::string& attr : deleted_attributes) {
312 c.def(std::string(
"get_" + attr).c_str(), [](
const CartesianTwist&) ->
void {},
"Deleted method from parent class.");
313 c.def(std::string(
"set_" + attr).c_str(), [](
const CartesianTwist& twist) ->
CartesianTwist {
return twist; },
"Deleted method from parent class.");
315 c.def(std::string(
"get_orientation_coefficients").c_str(), [](
const CartesianTwist&) ->
void {},
"Deleted method from parent class.");
317 c.def(py::self *=
double());
318 c.def(py::self *
double());
319 c.def(
double() * py::self);
320 c.def(Eigen::Matrix<double, 6, 6>() * py::self);
321 c.def(
"__mul__", [](
const CartesianTwist& self,
const Eigen::MatrixXd& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for *: 'state_representation.CartesianTwist' and 'numpy.ndarray'"); });
322 c.def(py::self * std::chrono::nanoseconds());
323 c.def(std::chrono::nanoseconds() * py::self);
324 c.def(py::self /=
double());
325 c.def(py::self /
double());
326 c.def(
"__truediv__", [](
const CartesianTwist& self,
const Eigen::MatrixXd& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for /: 'state_representation.CartesianTwist' and 'np.ndarray'"); });
327 c.def(py::self / std::chrono::nanoseconds());
329 c.def(py::self += py::self);
330 c.def(
"__iadd__", [](
const CartesianTwist& self,
const CartesianPose& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +=: 'state_representation.CartesianTwist' and 'state_representation.CartesianPose'"); });
331 c.def(
"__iadd__", [](
const CartesianTwist& self,
const CartesianAcceleration& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +=: 'state_representation.CartesianTwist' and 'state_representation.CartesianAcceleration'"); });
332 c.def(
"__iadd__", [](
const CartesianTwist& self,
const CartesianWrench& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +=: 'state_representation.CartesianTwist' and 'state_representation.CartesianWrench'"); });
334 c.def(py::self + py::self);
335 c.def(
"__add__", [](
const CartesianTwist& self,
const CartesianPose& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +: 'state_representation.CartesianTwist' and 'state_representation.CartesianPose'"); });
336 c.def(
"__add__", [](
const CartesianTwist& self,
const CartesianAcceleration& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +: 'state_representation.CartesianTwist' and 'state_representation.CartesianAcceleration'"); });
337 c.def(
"__add__", [](
const CartesianTwist& self,
const CartesianWrench& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +: 'state_representation.CartesianTwist' and 'state_representation.CartesianWrench'"); });
340 c.def(py::self -= py::self);
341 c.def(
"__isub__", [](
const CartesianTwist& self,
const CartesianPose& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -=: 'state_representation.CartesianTwist' and 'state_representation.CartesianPose'"); });
342 c.def(
"__isub__", [](
const CartesianTwist& self,
const CartesianAcceleration& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -=: 'state_representation.CartesianTwist' and 'state_representation.CartesianAcceleration'"); });
343 c.def(
"__isub__", [](
const CartesianTwist& self,
const CartesianWrench& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -=: 'state_representation.CartesianTwist' and 'state_representation.CartesianWrench'"); });
345 c.def(py::self - py::self);
346 c.def(
"__sub__", [](
const CartesianTwist& self,
const CartesianPose& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -: 'state_representation.CartesianTwist' and 'state_representation.CartesianPose'"); });
347 c.def(
"__sub__", [](
const CartesianTwist& self,
const CartesianAcceleration& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -: 'state_representation.CartesianTwist' and 'state_representation.CartesianAcceleration'"); });
348 c.def(
"__sub__", [](
const CartesianTwist& self,
const CartesianWrench& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -: 'state_representation.CartesianTwist' and 'state_representation.CartesianWrench'"); });
351 c.def(
"clamp", &
CartesianTwist::clamp,
"Clamp inplace the magnitude of the twist to the values in argument",
"max_linear"_a,
"max_angular"_a,
"linear_noise_ratio"_a=0,
"angular_noise_ratio"_a=0);
352 c.def(
"clamped", &
CartesianTwist::clamped,
"Return the clamped twist",
"max_linear"_a,
"max_angular"_a,
"linear_noise_ratio"_a=0,
"angular_noise_ratio"_a=0);
357 c.def(
"set_data", py::overload_cast<const Eigen::VectorXd&>(&
CartesianTwist::set_data),
"Set the twist data from a vector",
"data"_a);
358 c.def(
"set_data", py::overload_cast<
const std::vector<double>&>(&
CartesianTwist::set_data),
"Set the twist data from a list",
"data"_a);
359 c.def(
"norms", &
CartesianTwist::norms,
"Compute the norms of the state variable specified by the input type (default is full twist)",
"state_variable_type"_a=CartesianStateVariable::TWIST);
360 c.def(
"normalized", &
CartesianTwist::normalized,
"Compute the normalized twist at the state variable given in argument (default is full twist)",
"state_variable_type"_a=CartesianStateVariable::TWIST);
369 std::stringstream buffer;
375void cartesian_acceleration(py::module_& m) {
376 py::class_<CartesianAcceleration, std::shared_ptr<CartesianAcceleration>,
CartesianState> c(m,
"CartesianAcceleration");
378 c.def(py::init(),
"Empty constructor");
379 c.def(py::init<const std::string&, const std::string&>(),
"Constructor with name and reference frame provided",
"name"_a,
"reference_frame"_a=std::string(
"world"));
380 c.def(py::init<const CartesianAcceleration&>(),
"Copy constructor",
"acceleration"_a);
381 c.def(py::init<const CartesianState&>(),
"Copy constructor from a CartesianState",
"state"_a);
382 c.def(py::init<const CartesianTwist&>(),
"Copy constructor from a CartesianTwist by considering that it is equivalent to dividing the twist by 1 second",
"twist"_a);
384 c.def(py::init<const std::string&, const Eigen::Vector3d&, const std::string&>(),
"Construct a CartesianAcceleration from a linear acceleration given as a vector.",
"name"_a,
"linear_acceleration"_a,
"reference_frame"_a=std::string(
"world"));
385 c.def(py::init<const std::string&, const Eigen::Vector3d&, const Eigen::Vector3d&, const std::string&>(),
"Construct a CartesianAcceleration from a linear acceleration and angular acceleration given as vectors.",
"name"_a,
"linear_acceleration"_a,
"angular_acceleration"_a,
"reference_frame"_a=std::string(
"world"));
386 c.def(py::init<
const std::string&,
const Eigen::Matrix<double, 6, 1>&,
const std::string&>(),
"Construct a CartesianAcceleration from a single 6d acceleration vector",
"name"_a,
"acceleration"_a,
"reference_frame"_a=std::string(
"world"));
388 c.def_static(
"Zero", &
CartesianAcceleration::Zero,
"Constructor for the zero acceleration",
"name"_a,
"reference_frame"_a=std::string(
"world"));
389 c.def_static(
"Random", &
CartesianAcceleration::Random,
"Constructor for a random acceleration",
"name"_a,
"reference_frame"_a=std::string(
"world"));
391 std::vector<std::string> deleted_attributes = {
403 for (
const std::string& attr : deleted_attributes) {
404 c.def(std::string(
"get_" + attr).c_str(), [](
const CartesianAcceleration&) ->
void {},
"Deleted method from parent class.");
407 c.def(std::string(
"get_orientation_coefficients").c_str(), [](
const CartesianAcceleration&) ->
void {},
"Deleted method from parent class.");
409 c.def(py::self *=
double());
410 c.def(py::self *
double());
411 c.def(
double() * py::self);
412 c.def(Eigen::Matrix<double, 6, 6>() * py::self);
413 c.def(
"__mul__", [](
const CartesianAcceleration& self,
const Eigen::MatrixXd& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for *: 'state_representation.CartesianAcceleration' and 'numpy.ndarray'"); });
414 c.def(py::self * std::chrono::nanoseconds());
415 c.def(std::chrono::nanoseconds() * py::self);
416 c.def(py::self /=
double());
417 c.def(py::self /
double());
418 c.def(
"__truediv__", [](
const CartesianAcceleration& self,
const Eigen::MatrixXd& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for /: 'state_representation.CartesianAcceleration' and 'np.ndarray'"); });
420 c.def(py::self += py::self);
421 c.def(
"__iadd__", [](
const CartesianAcceleration& self,
const CartesianPose& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +=: 'state_representation.CartesianAcceleration' and 'state_representation.CartesianPose'"); });
422 c.def(
"__iadd__", [](
const CartesianAcceleration& self,
const CartesianTwist& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +=: 'state_representation.CartesianAcceleration' and 'state_representation.CartesianTwist'"); });
423 c.def(
"__iadd__", [](
const CartesianAcceleration& self,
const CartesianWrench& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +=: 'state_representation.CartesianAcceleration' and 'state_representation.CartesianWrench'"); });
425 c.def(py::self + py::self);
426 c.def(
"__add__", [](
const CartesianAcceleration& self,
const CartesianPose& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +: 'state_representation.CartesianAcceleration' and 'state_representation.CartesianPose'"); });
427 c.def(
"__add__", [](
const CartesianAcceleration& self,
const CartesianTwist& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +: 'state_representation.CartesianAcceleration' and 'state_representation.CartesianTwist'"); });
428 c.def(
"__add__", [](
const CartesianAcceleration& self,
const CartesianWrench& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +: 'state_representation.CartesianAcceleration' and 'state_representation.CartesianWrench'"); });
431 c.def(py::self -= py::self);
432 c.def(
"__isub__", [](
const CartesianAcceleration& self,
const CartesianPose& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -=: 'state_representation.CartesianAcceleration' and 'state_representation.CartesianPose'"); });
433 c.def(
"__isub__", [](
const CartesianAcceleration& self,
const CartesianTwist& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -=: 'state_representation.CartesianAcceleration' and 'state_representation.CartesianTwist'"); });
434 c.def(
"__isub__", [](
const CartesianAcceleration& self,
const CartesianWrench& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -=: 'state_representation.CartesianAcceleration' and 'state_representation.CartesianWrench'"); });
436 c.def(py::self - py::self);
437 c.def(
"__sub__", [](
const CartesianAcceleration& self,
const CartesianPose& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -: 'state_representation.CartesianAcceleration' and 'state_representation.CartesianPose'"); });
438 c.def(
"__sub__", [](
const CartesianAcceleration& self,
const CartesianTwist& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -: 'state_representation.CartesianAcceleration' and 'state_representation.CartesianTwist'"); });
439 c.def(
"__sub__", [](
const CartesianAcceleration& self,
const CartesianWrench& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -: 'state_representation.CartesianAcceleration' and 'state_representation.CartesianWrench'"); });
442 c.def(
"clamp", &
CartesianAcceleration::clamp,
"Clamp inplace the magnitude of the acceleration to the values in argument",
"max_linear"_a,
"max_angular"_a,
"linear_noise_ratio"_a=0,
"angular_noise_ratio"_a=0);
443 c.def(
"clamped", &
CartesianAcceleration::clamped,
"Return the clamped acceleration",
"max_linear"_a,
"max_angular"_a,
"linear_noise_ratio"_a=0,
"angular_noise_ratio"_a=0);
450 c.def(
"norms", &
CartesianAcceleration::norms,
"Compute the norms of the state variable specified by the input type (default is full acceleration)",
"state_variable_type"_a=CartesianStateVariable::ACCELERATION);
451 c.def(
"normalized", &
CartesianAcceleration::normalized,
"Compute the normalized acceleration at the state variable given in argument (default is full acceleration)",
"state_variable_type"_a=CartesianStateVariable::ACCELERATION);
460 std::stringstream buffer;
461 buffer << acceleration;
466void cartesian_wrench(py::module_& m) {
467 py::class_<CartesianWrench, std::shared_ptr<CartesianWrench>,
CartesianState> c(m,
"CartesianWrench");
469 c.def(py::init(),
"Empty constructor");
470 c.def(py::init<const std::string&, const std::string&>(),
"Constructor with name and reference frame provided",
"name"_a,
"reference_frame"_a=std::string(
"world"));
471 c.def(py::init<const CartesianWrench&>(),
"Copy constructor",
"twist"_a);
472 c.def(py::init<const CartesianState&>(),
"Copy constructor from a CartesianState",
"state"_a);
474 c.def(py::init<const std::string&, const Eigen::Vector3d&, const std::string&>(),
"Construct a CartesianWrench from a force given as a vector.",
"name"_a,
"force"_a,
"reference_frame"_a=std::string(
"world"));
475 c.def(py::init<const std::string&, const Eigen::Vector3d&, const Eigen::Vector3d&, const std::string&>(),
"Construct a CartesianWrench from a force and torque given as vectors.",
"name"_a,
"force"_a,
"torque"_a,
"reference_frame"_a=std::string(
"world"));
476 c.def(py::init<
const std::string&,
const Eigen::Matrix<double, 6, 1>&,
const std::string&>(),
"Construct a CartesianTwist from a single 6d wrench vector",
"name"_a,
"wrench"_a,
"reference_frame"_a=std::string(
"world"));
478 c.def_static(
"Zero", &
CartesianWrench::Zero,
"Constructor for the zero wrench",
"name"_a,
"reference_frame"_a=std::string(
"world"));
479 c.def_static(
"Random", &
CartesianWrench::Random,
"Constructor for a random wrench",
"name"_a,
"reference_frame"_a=std::string(
"world"));
481 std::vector<std::string> deleted_attributes = {
488 "linear_acceleration",
489 "angular_acceleration",
493 for (
const std::string& attr : deleted_attributes) {
494 c.def(std::string(
"get_" + attr).c_str(), [](
const CartesianWrench&) ->
void {},
"Deleted method from parent class.");
495 c.def(std::string(
"set_" + attr).c_str(), [](
const CartesianWrench& wrench) ->
CartesianWrench {
return wrench; },
"Deleted method from parent class.");
497 c.def(std::string(
"get_orientation_coefficients").c_str(), [](
const CartesianWrench&) ->
void {},
"Deleted method from parent class.");
499 c.def(py::self *=
double());
500 c.def(py::self *
double());
501 c.def(
double() * py::self);
502 c.def(Eigen::Matrix<double, 6, 6>() * py::self);
503 c.def(
"__mul__", [](
const CartesianWrench& self,
const Eigen::MatrixXd& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for *: 'state_representation.CartesianWrench' and 'numpy.ndarray'"); });
504 c.def(py::self /=
double());
505 c.def(py::self /
double());
506 c.def(
"__truediv__", [](
const CartesianWrench& self,
const Eigen::MatrixXd& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for /: 'state_representation.CartesianWrench' and 'np.ndarray'"); });
508 c.def(py::self += py::self);
509 c.def(
"__iadd__", [](
const CartesianWrench& self,
const CartesianPose& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +=: 'state_representation.CartesianWrench' and 'state_representation.CartesianPose'"); });
510 c.def(
"__iadd__", [](
const CartesianWrench& self,
const CartesianTwist& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +=: 'state_representation.CartesianWrench' and 'state_representation.CartesianTwist'"); });
511 c.def(
"__iadd__", [](
const CartesianWrench& self,
const CartesianAcceleration& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +=: 'state_representation.CartesianWrench' and 'state_representation.CartesianAcceleration'"); });
513 c.def(py::self + py::self);
514 c.def(
"__add__", [](
const CartesianWrench& self,
const CartesianPose& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +: 'state_representation.CartesianWrench' and 'state_representation.CartesianPose'"); });
515 c.def(
"__add__", [](
const CartesianWrench& self,
const CartesianTwist& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +: 'state_representation.CartesianWrench' and 'state_representation.CartesianTwist'"); });
516 c.def(
"__add__", [](
const CartesianWrench& self,
const CartesianAcceleration& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for +: 'state_representation.CartesianWrench' and 'state_representation.CartesianAcceleration'"); });
519 c.def(py::self -= py::self);
520 c.def(
"__isub__", [](
const CartesianWrench& self,
const CartesianPose& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -=: 'state_representation.CartesianWrench' and 'state_representation.CartesianPose'"); });
521 c.def(
"__isub__", [](
const CartesianWrench& self,
const CartesianTwist& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -=: 'state_representation.CartesianWrench' and 'state_representation.CartesianTwist'"); });
522 c.def(
"__isub__", [](
const CartesianWrench& self,
const CartesianAcceleration& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -=: 'state_representation.CartesianWrench' and 'state_representation.CartesianAcceleration'"); });
524 c.def(py::self - py::self);
525 c.def(
"__sub__", [](
const CartesianWrench& self,
const CartesianPose& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -: 'state_representation.CartesianWrench' and 'state_representation.CartesianPose'"); });
526 c.def(
"__sub__", [](
const CartesianWrench& self,
const CartesianTwist& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -: 'state_representation.CartesianWrench' and 'state_representation.CartesianTwist'"); });
527 c.def(
"__sub__", [](
const CartesianWrench& self,
const CartesianAcceleration& other) ->
void {
throw py::type_error(
"unsupported operand type(s) for -: 'state_representation.CartesianWrench' and 'state_representation.CartesianAcceleration'"); });
530 c.def(
"clamp", &
CartesianWrench::clamp,
"Clamp inplace the magnitude of the wrench to the values in argument",
"max_force"_a,
"max_torque"_a,
"force_noise_ratio"_a=0,
"torque_noise_ratio"_a=0);
531 c.def(
"clamped", &
CartesianWrench::clamped,
"Return the clamped wrench",
"max_force"_a,
"max_torque"_a,
"force_noise_ratio"_a=0,
"torque_noise_ratio"_a=0);
535 c.def(
"set_data", py::overload_cast<const Eigen::VectorXd&>(&
CartesianWrench::set_data),
"Set the wrench data from a vector",
"data"_a);
536 c.def(
"set_data", py::overload_cast<
const std::vector<double>&>(&
CartesianWrench::set_data),
"Set the wrench data from a list",
"data"_a);
537 c.def(
"norms", &
CartesianWrench::norms,
"Compute the norms of the state variable specified by the input type (default is full wrench)",
"state_variable_type"_a=CartesianStateVariable::WRENCH);
538 c.def(
"normalized", &
CartesianWrench::normalized,
"Compute the normalized twist at the state variable given in argument (default is full wrench)",
"state_variable_type"_a=CartesianStateVariable::WRENCH);
547 std::stringstream buffer;
553void bind_cartesian_space(py::module_& m) {
555 cartesian_state_variable(m);
559 cartesian_acceleration(m);
Class to define acceleration in Cartesian space as 3D linear and angular acceleration vectors.
void clamp(double max_linear, double max_angular, double linear_noise_ratio=0, double angular_noise_ratio=0)
Clamp inplace the magnitude of the acceleration to the values in argument.
CartesianAcceleration copy() const
Return a copy of the Cartesian acceleration.
CartesianAcceleration clamped(double max_linear, double max_angular, double noise_ratio=0, double angular_noise_ratio=0) const
Return the clamped twist.
Eigen::VectorXd data() const override
Returns the acceleration data as an Eigen vector.
static CartesianAcceleration Zero(const std::string &name, const std::string &reference="world")
Constructor for the zero acceleration.
static CartesianAcceleration Random(const std::string &name, const std::string &reference="world")
Constructor for a random acceleration.
std::vector< double > norms(const CartesianStateVariable &state_variable_type=CartesianStateVariable::ACCELERATION) const override
Compute the norms of the state variable specified by the input type (default is full acceleration)
CartesianAcceleration inverse() const
Compute the inverse of the current Cartesian acceleration.
void set_data(const Eigen::VectorXd &data) override
Set the acceleration data from an Eigen vector.
CartesianAcceleration normalized(const CartesianStateVariable &state_variable_type=CartesianStateVariable::ACCELERATION) const
Compute the normalized acceleration at the state variable given in argument (default is full accelera...
Class to define Cartesian pose in Cartesian space as 3D position and quaternion based orientation.
void set_data(const Eigen::VectorXd &data) override
Set the pose data from an Eigen vector.
std::vector< double > norms(const CartesianStateVariable &state_variable_type=CartesianStateVariable::POSE) const override
Compute the norms of the state variable specified by the input type (default is full pose)
static CartesianPose Identity(const std::string &name, const std::string &reference="world")
Constructor for the identity pose.
Eigen::VectorXd data() const override
Returns the pose data as an Eigen vector.
CartesianPose normalized(const CartesianStateVariable &state_variable_type=CartesianStateVariable::POSE) const
Compute the normalized pose at the state variable given in argument (default is full pose)
CartesianPose copy() const
Return a copy of the Cartesian pose.
CartesianPose inverse() const
Compute the inverse of the current Cartesian pose.
static CartesianPose Random(const std::string &name, const std::string &reference="world")
Constructor for a random pose.
Class to represent a state in Cartesian space.
void set_zero()
Set the State to a zero value.
const Eigen::Vector3d & get_force() const
Getter of the force attribute.
void set_acceleration(const Eigen::Matrix< double, 6, 1 > &acceleration)
Setter of the linear and angular acceleration from a 6d acceleration vector.
CartesianState inverse() const
Compute the inverse of the current Cartesian state.
Eigen::ArrayXd array() const
Return the data vector as an Eigen Array.
const Eigen::Vector3d & get_torque() const
Getter of the torque attribute.
static CartesianState Random(const std::string &name, const std::string &reference="world")
Constructor for a random Cartesian state.
const Eigen::Vector3d & get_linear_velocity() const
Getter of the linear velocity attribute.
void set_angular_acceleration(const Eigen::Vector3d &angular_acceleration)
Setter of the angular velocity attribute.
const Eigen::Vector3d & get_position() const
Getter of the position attribute.
void normalize(const CartesianStateVariable &state_variable_type=CartesianStateVariable::ALL)
Normalize inplace the state at the state variable given in argument. Default is full state.
virtual std::vector< double > norms(const CartesianStateVariable &state_variable_type=CartesianStateVariable::ALL) const
Compute the norms of the state variable specified by the input type. Default is full state.
void set_force(const Eigen::Vector3d &force)
Setter of the force attribute.
Eigen::Matrix< double, 6, 1 > get_twist() const
Getter of the 6d twist from linear and angular velocity attributes.
CartesianState normalized(const CartesianStateVariable &state_variable_type=CartesianStateVariable::ALL) const
Compute the normalized state at the state variable given in argument. Default is full state.
friend double dist(const CartesianState &s1, const CartesianState &s2, const CartesianStateVariable &state_variable_type)
Compute the distance between two Cartesian states.
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)
const Eigen::Vector3d & get_linear_acceleration() const
Getter of the linear acceleration attribute.
Eigen::Matrix4d get_transformation_matrix() const
Getter of a pose from position and orientation attributes.
void set_linear_acceleration(const Eigen::Vector3d &linear_acceleration)
Setter of the linear acceleration attribute.
void clamp_state_variable(double max_norm, const CartesianStateVariable &state_variable_type, double noise_ratio=0)
Clamp inplace the norm of the a specific state variable.
Eigen::Matrix< double, 7, 1 > get_pose() const
Getter of the pose from position and orientation attributes.
void set_position(const Eigen::Vector3d &position)
Setter of the position.
void set_wrench(const Eigen::Matrix< double, 6, 1 > &wrench)
Setter of the force and torque from a 6d wrench vector.
Eigen::Matrix< double, 6, 1 > get_wrench() const
Getter of the 6d wrench from force and torque attributes.
void set_twist(const Eigen::Matrix< double, 6, 1 > &twist)
Setter of the linear and angular velocities from a 6d twist vector.
virtual Eigen::VectorXd data() const
Return the data as the concatenation of all the state variables in a single vector.
const Eigen::Vector3d & get_angular_velocity() const
Getter of the angular velocity attribute.
void set_pose(const Eigen::Vector3d &position, const Eigen::Quaterniond &orientation)
Setter of the pose from both position and orientation.
CartesianState copy() const
Return a copy of the Cartesian state.
void set_torque(const Eigen::Vector3d &torque)
Setter of the torque attribute.
virtual void set_data(const Eigen::VectorXd &data) override
Set the data of the state from all the state variables in a single Eigen vector.
Eigen::Vector4d get_orientation_coefficients() const
Getter of the orientation attribute as Vector4d of coefficients.
const Eigen::Vector3d & get_angular_acceleration() const
Getter of the angular acceleration attribute.
std::vector< double > to_std_vector() const
Return the state as a std vector.
void set_linear_velocity(const Eigen::Vector3d &linear_velocity)
Setter of the linear velocity attribute.
void set_angular_velocity(const Eigen::Vector3d &angular_velocity)
Setter of the angular velocity attribute.
Eigen::Matrix< double, 6, 1 > get_acceleration() const
Getter of the 6d acceleration from linear and angular acceleration attributes.
Class to define twist in Cartesian space as 3D linear and angular velocity vectors.
std::vector< double > norms(const CartesianStateVariable &state_variable_type=CartesianStateVariable::TWIST) const override
Compute the norms of the state variable specified by the input type (default is full twist)
Eigen::VectorXd data() const override
Returns the twist data as an Eigen vector.
CartesianTwist clamped(double max_linear, double max_angular, double noise_ratio=0, double angular_noise_ratio=0) const
Return the clamped twist.
static CartesianTwist Random(const std::string &name, const std::string &reference="world")
Constructor for a random twist.
static CartesianTwist Zero(const std::string &name, const std::string &reference="world")
Constructor for the zero twist.
void set_data(const Eigen::VectorXd &data) override
Set the twist data from an Eigen vector.
void clamp(double max_linear, double max_angular, double linear_noise_ratio=0, double angular_noise_ratio=0)
Clamp inplace the magnitude of the twist to the values in argument.
CartesianTwist copy() const
Return a copy of the Cartesian twist.
CartesianTwist normalized(const CartesianStateVariable &state_variable_type=CartesianStateVariable::TWIST) const
Compute the normalized twist at the state variable given in argument (default is full twist)
CartesianTwist inverse() const
Compute the inverse of the current Cartesian twist.
Class to define wrench in Cartesian space as 3D force and torque vectors.
Eigen::VectorXd data() const override
Returns the wrench data as an Eigen vector.
CartesianWrench normalized(const CartesianStateVariable &state_variable_type=CartesianStateVariable::WRENCH) const
Compute the normalized wrench at the state variable given in argument (default is full wrench)
static CartesianWrench Zero(const std::string &name, const std::string &reference="world")
Constructor for the zero wrench.
void set_data(const Eigen::VectorXd &data) override
Set the wrench data from an Eigen vector.
CartesianWrench clamped(double max_force, double max_torque, double force_noise_ratio=0, double torque_noise_ratio=0) const
Return the clamped wrench.
std::vector< double > norms(const CartesianStateVariable &state_variable_type=CartesianStateVariable::WRENCH) const override
Compute the norms of the state variable specified by the input type (default is full wrench)
CartesianWrench copy() const
Return a copy of the Cartesian wrench.
void clamp(double max_force, double max_torque, double force_noise_ratio=0, double torque_noise_ratio=0)
Clamp inplace the magnitude of the wrench to the values in argument.
static CartesianWrench Random(const std::string &name, const std::string &reference="world")
Constructor for a random wrench.
virtual void set_reference_frame(const std::string &reference_frame)
Setter of the reference frame.
const std::string & get_reference_frame() const
Getter of the reference frame as const reference.
Abstract class to represent a state.
double dist(const CartesianState &s1, const CartesianState &s2, const CartesianStateVariable &state_variable_type=CartesianStateVariable::ALL)
Compute the distance between two Cartesian states.