1#ifndef BIORBD_UTILS_ROTO_TRANS_H
2#define BIORBD_UTILS_ROTO_TRANS_H
4#include "biorbdConfig.h"
9#include "Utils/Scalar.h"
10#include "rbdl/rbdl_math.h"
12namespace BIORBD_NAMESPACE {
31class BIORBD_API
RotoTrans :
public RigidBodyDynamics::Math::Matrix4d
40 const RigidBodyDynamics::Math::Matrix4d& matrix =
41 RigidBodyDynamics::Math::Matrix4d::Identity());
105 const String& rotationSequence);
113#ifdef BIORBD_USE_EIGEN3_MATH
119 template <
typename OtherDerived>
120 RotoTrans(
const Eigen::MatrixBase<OtherDerived>& other)
121 : RigidBodyDynamics::Math::Matrix4d(other) {
142 const std::pair<rigidbody::NodeSegment, rigidbody::NodeSegment>&
144 const std::pair<rigidbody::NodeSegment, rigidbody::NodeSegment>&
146 const std::pair<String, String>& axesNames,
147 const String& axisToRecalculate);
216#ifndef BIORBD_USE_CASADI_MATH
226#ifdef BIORBD_USE_EIGEN3_MATH
231 template <
typename OtherDerived>
232 RotoTrans& operator=(
const Eigen::MatrixBase<OtherDerived>& other) {
233 Eigen::Matrix4d::operator=(other);
266std::ostream& operator<<(
A point attached to a segment, generally speaking a skin marker.
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.