1#define BIORBD_API_EXPORTS
2#include "Utils/RotoTrans.h"
4#include "RigidBody/NodeSegment.h"
5#include "Utils/Error.h"
6#include "Utils/Rotation.h"
7#include "Utils/SpatialTransform.h"
8#include "Utils/String.h"
9#include "Utils/Vector.h"
10#include "Utils/Vector3d.h"
12using namespace BIORBD_NAMESPACE;
15 : RigidBodyDynamics::Math::Matrix4d(matrix) {
20 const utils::Scalar& v00,
21 const utils::Scalar& v01,
22 const utils::Scalar& v02,
23 const utils::Scalar& v03,
24 const utils::Scalar& v10,
25 const utils::Scalar& v11,
26 const utils::Scalar& v12,
27 const utils::Scalar& v13,
28 const utils::Scalar& v20,
29 const utils::Scalar& v21,
30 const utils::Scalar& v22,
31 const utils::Scalar& v23,
32 const utils::Scalar& v30,
33 const utils::Scalar& v31,
34 const utils::Scalar& v32,
35 const utils::Scalar& v33)
36 : RigidBodyDynamics::Math::Matrix4d(
57 : RigidBodyDynamics::Math::Matrix4d(
69 : RigidBodyDynamics::Math::Matrix4d(
77 const std::pair<rigidbody::NodeSegment, rigidbody::NodeSegment>&
79 const std::pair<rigidbody::NodeSegment, rigidbody::NodeSegment>&
81 const std::pair<utils::String, utils::String>& axesNames,
85 axis1markers, axis2markers, axesNames, axisToRecalculate);
86 rt_out.block(0, 3, 3, 1) = origin;
92 return rot().block(0,
static_cast<unsigned int>(idx), 3, 1);
97 tp.block(0, 0, 3, 3) = block(0, 0, 3, 3).
transpose();
98 tp.block(0, 3, 3, 1) = -tp.block(0, 0, 3, 3) * block(0, 3, 3, 1);
99 tp.block(3, 0, 1, 4) =
100 RigidBodyDynamics::Math::Vector4d(0, 0, 0, 1).
transpose();
105 return this->block<3, 1>(0, 3);
109 return this->block<3, 3>(0, 0);
116 out.block(0, 0, 3, 3) =
rot;
117 out.block(0, 3, 3, 1) =
trans;
118 out.block(3, 0, 1, 4) =
119 RigidBodyDynamics::Math::Vector4d(0, 0, 0, 1).
transpose();
135 out.block(0, 0, 3, 3) = rot_mat;
136 out.block(0, 3, 3, 1) =
trans;
146#ifndef BIORBD_USE_CASADI_MATH
149 RigidBodyDynamics::Math::Vector3d v_tp;
151 for (
size_t i = 0; i < mToMean.size(); ++i) {
152 v_tp += mToMean[i].trans();
154 v_tp = v_tp / mToMean.size();
157 std::vector<utils::Rotation> rotations;
158 for (
size_t i = 0; i < mToMean.size(); ++i) {
159 rotations.push_back(mToMean[i].block<3, 3>(0, 0));
168 RigidBodyDynamics::Math::Vector4d v2;
169 v2.block(0, 0, 3, 1) = v1;
175#ifndef BIORBD_USE_CASADI_MATH
179 this->block(3, 0, 1, 4).sum() == 1.,
180 "Last row of the RotoTrans should be (0,0,0,1");
182 (*
this)(3, 3) == 1.,
"Last row of the RotoTrans should be (0,0,0,1");
188 os << a.block(0, 0, 4, 4);
A point attached to a segment, generally speaking a skin marker.
static void check(bool cond, const String &message)
Assert that raises the error message if false.
static utils::Rotation mean(const std::vector< utils::Rotation > &mToMean)
Get the mean of the Rotation matrices.
static Rotation fromEulerAngles(const Vector &rot, const String &seq)
Create a Rotation from Euler angles.
static Vector toEulerAngles(const Rotation &r, const String &seq)
Return extracted angles from the rotation matrix into Euler angles using the provided sequence.
static Rotation fromMarkers(const std::pair< rigidbody::NodeSegment, rigidbody::NodeSegment > &axis1markers, const std::pair< rigidbody::NodeSegment, rigidbody::NodeSegment > &axis2markers, const std::pair< String, String > &axesNames, const String &axisToRecalculate)
fromMarkers Creates a system of axes from two axes defined by markers
Homogenous matrix to describe translations and rotations simultaneously.
Rotation rot() const
Return the rotation matrix.
Vector3d trans() const
Return the translation vector.
static RotoTrans combineRotAndTrans(const Rotation &rot, const Vector3d &trans)
Set the RotoTrans from a rotation and a translation.
RotoTrans(const RigidBodyDynamics::Math::Matrix4d &matrix=RigidBodyDynamics::Math::Matrix4d::Identity())
Construct RotoTrans matrix.
RigidBodyDynamics::Math::Vector4d expand3dTo4d(const Vector3d &v1)
Expand 3D vector to 4D (padding with an extra 1).
static RotoTrans fromSpatialTransform(const utils::SpatialTransform &st)
set the RotoTrans from a spatial transform
static Vector toEulerAngles(const RotoTrans &rt, const String &seq)
Return extracted angles from the rotation matrix into Euler angles using the provided sequence.
static RotoTrans fromEulerAngles(const Vector &rot, const Vector3d &trans, const String &seq)
Create a RotoTrans from Euler angles.
RotoTrans transpose() const
Return the tranposed matrix.
void checkUnitary()
Check if the RotoTrans has a unitary matrix of rotation and the last row is (0, 0,...
static RotoTrans fromMarkers(const rigidbody::NodeSegment &origin, const std::pair< rigidbody::NodeSegment, rigidbody::NodeSegment > &axis1markers, const std::pair< rigidbody::NodeSegment, rigidbody::NodeSegment > &axis2markers, const std::pair< String, String > &axesNames, const String &axisToRecalculate)
fromMarkers Creates a system of axes from two axes and an origin defined by markers
Vector3d axe(size_t idx) const
Get a particular axis of the rotation matrix.
static utils::RotoTrans mean(const std::vector< utils::RotoTrans > &rt)
Get the mean of the 4x4 matrices.
Wrapper around the std::string class with augmented functionality.
Wrapper around Eigen Vector3d and attach it to a parent.
Wrapper of the Eigen VectorXd.