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 inTX,
28 double inTY,
29 double inTZ,
30 double inRX,
31 double inRY,
32 double inRZ,
33 double inRW) :
34 src_frame_id(parent),
35 dst_frame_id(child),
36 translation{inTX, inTY, inTZ},
37 rotation(inRX, inRY, inRZ, inRW)
38{
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.tX;
83 t_mat[1][3] = translation.tY;
84 t_mat[2][3] = translation.tZ;
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.tX = mat[0][3];
100 translation.tY = mat[1][3];
101 translation.tZ = 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.tX,
118 translation.tY,
119 translation.tZ,
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.tX,
131 translation.tY,
132 translation.tZ,
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.tX;
144 rotM[1][3] = translation.tY;
145 rotM[2][3] = translation.tZ;
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.tX,
166 translation.tY,
167 translation.tZ,
168 s_rotmr.c_str(),
169 s_rotmd.c_str());
170 }
171 return std::string(buff);
172}
173
175{
176 // auto-convert text mode interaction
177 connection.convertTextMode();
178
179 connection.expectInt32();
180 connection.expectInt32();
181
182 connection.expectInt32();
183 src_frame_id = connection.expectString();
184 connection.expectInt32();
185 dst_frame_id = connection.expectString();
186 connection.expectInt32();
187 timestamp = connection.expectFloat64();
188 connection.expectInt32();
189 isStatic = (connection.expectInt8()==1);
190
191 connection.expectInt32();
192 translation.tX = connection.expectFloat64();
193 connection.expectInt32();
194 translation.tY = connection.expectFloat64();
195 connection.expectInt32();
196 translation.tZ = connection.expectFloat64();
197
198 connection.expectInt32();
199 rotation.x() = connection.expectFloat64();
200 connection.expectInt32();
201 rotation.y() = connection.expectFloat64();
202 connection.expectInt32();
203 rotation.z() = connection.expectFloat64();
204 connection.expectInt32();
205 rotation.w() = connection.expectFloat64();
206
207 return !connection.isError();
208}
209
211{
212 connection.appendInt32(BOTTLE_TAG_LIST);
213 connection.appendInt32(4+3+4);
214
215 connection.appendInt32(BOTTLE_TAG_STRING);
216 connection.appendString(src_frame_id);
217 connection.appendInt32(BOTTLE_TAG_STRING);
218 connection.appendString(dst_frame_id);
220 connection.appendFloat64(timestamp);
221 connection.appendInt32(BOTTLE_TAG_INT8);
222 connection.appendInt8(int8_t(isStatic));
223
225 connection.appendFloat64(translation.tX);
227 connection.appendFloat64(translation.tY);
229 connection.appendFloat64(translation.tZ);
230
232 connection.appendFloat64(rotation.x());
234 connection.appendFloat64(rotation.y());
236 connection.appendFloat64(rotation.z());
238 connection.appendFloat64(rotation.w());
239
240 connection.convertTextMode();
241
242 return !connection.isError();
243}
#define BOTTLE_TAG_INT8
Definition Bottle.h:19
#define BOTTLE_TAG_FLOAT64
Definition Bottle.h:25
#define BOTTLE_TAG_STRING
Definition Bottle.h:26
#define BOTTLE_TAG_LIST
Definition Bottle.h:28
#define M_PI
void transFromVec(double X, double Y, double Z)
yarp::sig::Matrix toMatrix() const
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
yarp::sig::Vector getRPYRot() const
void rotFromRPY(double R, double P, double Y)
bool write(yarp::os::ConnectionWriter &connection) const override
Write data to a connection.
bool fromMatrix(const yarp::sig::Matrix &mat)
std::string toString(display_transform_mode_t format=rotation_as_quaternion) const
struct yarp::math::FrameTransform::Translation_t translation
An interface for reading from a network connection.
virtual std::string expectString()
Read a string from the network connection.
virtual std::int32_t expectInt32()=0
Read a 32-bit integer from the network connection.
virtual bool convertTextMode()=0
Reads in a standard description in text mode, and converts it to a standard description in binary.
virtual bool isError() const =0
virtual std::int8_t expectInt8()=0
Read a 8-bit integer from the network connection.
virtual yarp::conf::float64_t expectFloat64()=0
Read a 64-bit floating point number from the network connection.
An interface for writing to a network connection.
virtual bool isError() const =0
virtual void appendInt8(std::int8_t data)=0
Send a representation of a 8-bit integer to the network connection.
virtual bool convertTextMode()=0
Converts a standard description in binary into a textual description, if the connection is in text-mo...
virtual void appendInt32(std::int32_t data)=0
Send a representation of a 32-bit integer to the network connection.
virtual void appendFloat64(yarp::conf::float64_t data)=0
Send a representation of a 64-bit floating point number to the network connection.
void appendString(const std::string &str)
Send a string to the network connection.
A class for a Matrix.
Definition Matrix.h:39
size_t cols() const
Return number of columns.
Definition Matrix.h:94
size_t rows() const
Return number of rows.
Definition Matrix.h:88
std::string toString(int precision=-1, int width=-1, const char *endRowStr="\n") const
Print matrix to a string.
Definition Matrix.cpp:171
std::string toString(int precision=-1, int width=-1) const
Creates a string object containing a text representation of the object.
Definition Vector.h:358
#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
VectorOf< double > Vector
Definition Vector.h:36
void set(double x, double y, double z)