YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
FrameTransform.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
7
10#include <yarp/math/Math.h>
12#include <yarp/os/LogStream.h>
13#include <cstdio>
14#include <cmath>
15
16namespace {
17YARP_LOG_COMPONENT(FRAMETRANSFORM, "yarp.math.FrameTransform")
18}
19
24
26 const std::string& child,
27 double int_x,
28 double int_y,
29 double int_z,
30 double inRX,
31 double inRY,
32 double inRZ,
33 double inRW)
34{
35 src_frame_id = parent;
36 dst_frame_id = child;
37 translation = Translation (int_x, int_y, int_z);
38 rotation = Quaternion (inRX, inRY, inRZ, inRW);
39}
40
42{
43 //if (isStatic==false && timestamp < 0 ) { return false; }
44 //if (std::isnan(timestamp)) { return false; }
45 if (rotation.isValid()==false) { return false;}
46 return true;
47}
48
50
51void yarp::math::FrameTransform::transFromVec(double X, double Y, double Z)
52{
53 translation.set(X, Y, Z);
54}
55
56void yarp::math::FrameTransform::rotFromRPY(double R, double P, double Y)
57{
58 double rot[3] = { R, P, Y };
59 size_t i = 3;
62 rotV = yarp::sig::Vector(i, rot);
63 rotM = rpy2dcm(rotV);
64 //yCDebug(FRAMETRANSFORM) << rotM.toString();
65 rotation.fromRotationMatrix(rotM);
66}
67
69{
72 rotM = rotation.toRotationMatrix4x4();
73 rotV = dcm2rpy(rotM);
74 return rotV;
75}
76
78{
80 yarp::sig::Matrix t_mat(4,4);
81 t_mat = rotation.toRotationMatrix4x4();
82 t_mat[0][3] = translation.t_x;
83 t_mat[1][3] = translation.t_y;
84 t_mat[2][3] = translation.t_z;
85 return t_mat;
86}
87
89{
90 if (mat.cols() != 4 || mat.rows() != 4)
91 {
92 yCError(FRAMETRANSFORM, "FrameTransform::fromMatrix() failed, matrix should be = 4x4");
93 yCAssert(FRAMETRANSFORM, mat.cols() == 4 && mat.rows() == 4);
94 return false;
95 }
96
98
99 translation.t_x = mat[0][3];
100 translation.t_y = mat[1][3];
101 translation.t_z = mat[2][3];
102 rotation.fromRotationMatrix(mat);
103 return true;
104}
105
106
107
109{
110 char buff[1024];
111
112 if (format == rotation_as_quaternion)
113 {
114 sprintf(buff, "%s -> %s \n tran: %f %f %f \n rot quaternion: %f %f %f %f\n\n",
115 src_frame_id.c_str(),
116 dst_frame_id.c_str(),
117 translation.t_x,
118 translation.t_y,
119 translation.t_z,
120 rotation.x(),
121 rotation.y(),
122 rotation.z(),
123 rotation.w());
124 /*
125 Quaternion normrotation= rotation;
126 normrotation.normalize();
127 sprintf(buff, "%s -> %s \n tran: %f %f %f \n rot norm quaternion: %f %f %f %f\n\n",
128 src_frame_id.c_str(),
129 dst_frame_id.c_str(),
130 translation.t_x,
131 translation.t_y,
132 translation.t_z,
133 normrotation.x(),
134 normrotation.y(),
135 normrotation.z(),
136 normrotation.w());
137 */
138 }
139 else if (format == rotation_as_matrix)
140 {
142 rotM = rotation.toRotationMatrix4x4();
143 rotM[0][3] = translation.t_x;
144 rotM[1][3] = translation.t_y;
145 rotM[2][3] = translation.t_z;
146 std::string s_rotm =rotM.toString();
147 sprintf(buff, "%s -> %s \n transformation matrix:\n %s \n\n",
148 src_frame_id.c_str(),
149 dst_frame_id.c_str(),
150 s_rotm.c_str());
151 }
152 else if (format == rotation_as_rpy)
153 {
154 yarp::sig::Vector rotVrad;
156 rotM = rotation.toRotationMatrix3x3();
157 //yCDebug(FRAMETRANSFORM)<< rotM.toString();
158 rotVrad = dcm2rpy(rotM);
159 yarp::sig::Vector rotVdeg = rotVrad*180/M_PI;
160 std::string s_rotmr = rotVrad.toString();
161 std::string s_rotmd = rotVdeg.toString();
162 sprintf(buff, "%s -> %s \n tran: %f %f %f \n rotation rpy: %s (deg %s)\n\n",
163 src_frame_id.c_str(),
164 dst_frame_id.c_str(),
165 translation.t_x,
166 translation.t_y,
167 translation.t_z,
168 s_rotmr.c_str(),
169 s_rotmd.c_str());
170 }
171 return std::string(buff);
172}
#define M_PI
yarp::math::Translation translation
void transFromVec(double X, double Y, double Z)
yarp::sig::Matrix toMatrix() const
yarp::sig::Vector getRPYRot() const
void rotFromRPY(double R, double P, double Y)
bool fromMatrix(const yarp::sig::Matrix &mat)
void set(double x, double y, double z)
A class for a Matrix.
Definition Matrix.h:40
size_t cols() const
Return number of columns.
Definition Matrix.h:91
size_t rows() const
Return number of rows.
Definition Matrix.h:85
std::string toString(int precision=-1, int width=-1, const char *endRowStr="\n") const
Print matrix to a string.
Definition Matrix.cpp:171
#define yCError(component,...)
#define yCAssert(component, x)
#define YARP_LOG_COMPONENT(name,...)
yarp::sig::Matrix rpy2dcm(const yarp::sig::Vector &rpy)
Converts roll-pitch-yaw angles in the corresponding dcm (direction cosine matrix) rotation matrix (de...
Definition math.cpp:847
yarp::sig::Vector dcm2rpy(const yarp::sig::Matrix &R)
Converts a dcm (direction cosine matrix) rotation matrix to roll-pitch-yaw angles (defined in Math....
Definition math.cpp:808