CartesianPose.cpp
Go to the documentation of this file.00001 /* ============================================================ 00002 * 00003 * This file is a part of RCI project 00004 * 00005 * Copyright (C) 2011 by Arne Nordmann <anordman at cor-lab dot uni-bielefeld dot de> 00006 * 00007 * This file may be licensed under the terms of the 00008 * GNU Lesser General Public License Version 3 (the ``LGPL''), 00009 * or (at your option) any later version. 00010 * 00011 * Software distributed under the License is distributed 00012 * on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either 00013 * express or implied. See the LGPL for the specific language 00014 * governing rights and limitations. 00015 * 00016 * You should have received a copy of the LGPL along with this 00017 * program. If not, go to http://www.gnu.org/licenses/lgpl.html 00018 * or write to the Free Software Foundation, Inc., 00019 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 00020 * 00021 * The development of this software was supported by: 00022 * CoR-Lab, Research Institute for Cognition and Robotics 00023 * Bielefeld University 00024 * 00025 * ============================================================ */ 00026 00027 #include "CartesianPose.h" 00028 00029 #include <math.h> 00030 00031 #include <boost/math/quaternion.hpp> 00032 00033 using namespace std; 00034 using namespace nemo; 00035 using namespace rsc::math; 00036 00037 namespace rci { 00038 00039 Translation::Translation() : 00040 CartesianTranslational() { 00041 } 00042 00043 Translation::Translation(double valx, double valy, double valz) : 00044 CartesianTranslational(valx, valy, valz) { 00045 } 00046 00047 Translation::Translation(const Translation& transl) : 00048 CartesianTranslational(transl) { 00049 } 00050 00051 Translation::Translation(const TranslationPtr& transl) : 00052 CartesianTranslational(transl) { 00053 } 00054 00055 TranslationPtr Translation::fromMeters(double x, double y, double z) { 00056 return TranslationPtr(new Translation(x, y, z)); 00057 } 00058 00059 Translation::Translation(const RealVector& values) : 00060 CartesianTranslational(values) { 00061 } 00062 00063 std::string Translation::print() const { 00064 ostringstream outstream(ostringstream::out); 00065 outstream.precision(3); // Precision when printing double values 00066 outstream << "Translation<x=" << this->asDouble(0) << ",y=" 00067 << this->asDouble(1) << ",z=" << this->asDouble(2) << ">(m)" 00068 << std::endl; 00069 return outstream.str(); 00070 } 00071 00072 Orientation::Orientation() : 00073 CartesianRotational() { 00074 } 00075 00076 Orientation::Orientation(double vala, double valb, double valc, double vald) : 00077 CartesianRotational(vala, valb, valc, vald) { 00078 } 00079 00080 Orientation::Orientation(const RealVector& values) : 00081 CartesianRotational(values) { 00082 } 00083 00084 Orientation::Orientation(const Orientation& orient) : 00085 CartesianRotational(orient) { 00086 } 00087 00088 Orientation::Orientation(const OrientationPtr& orient) : 00089 CartesianRotational(orient) { 00090 } 00091 00092 Orientation::Orientation(const ::boost::math::quaternion<double>& quaternion) : 00093 CartesianRotational(quaternion) { 00094 } 00095 00096 Orientation::Orientation(double rxx, double rxy, double rxz, double ryx, 00097 double ryy, double ryz, double rzx, double rzy, double rzz) : 00098 CartesianRotational(rxx, rxy, rxz, ryx, ryy, ryz, rzx, rzy, rzz) { 00099 } 00100 00101 OrientationPtr Orientation::rotateBy(const OrientationPtr& rot) const { 00102 return Orientation::fromQuaternion(this->_quaternion * rot->asQuaternion()); 00103 } 00104 00105 std::string Orientation::print() const { 00106 ostringstream outstream(ostringstream::out); 00107 outstream.precision(3); // Precision when printing double values 00108 outstream << "Orientation<q0=" << this->q0() << ",q1=" << this->q1() 00109 << ",q2=" << this->q2() << ",q3=" << this->q3() << ">" << std::endl; 00110 return outstream.str(); 00111 } 00112 00113 OrientationPtr Orientation::fromEulerAngles(double rotX, double rotY, 00114 double rotZ) { 00115 double q0 = cos(rotX / 2) * cos(rotY / 2) * cos(rotZ / 2) 00116 + sin(rotX / 2) * sin(rotY / 2) * sin(rotZ / 2); 00117 double q1 = sin(rotX / 2) * cos(rotY / 2) * cos(rotZ / 2) 00118 - cos(rotX / 2) * sin(rotY / 2) * sin(rotZ / 2); 00119 double q2 = cos(rotX / 2) * sin(rotY / 2) * cos(rotZ / 2) 00120 + sin(rotX / 2) * cos(rotY / 2) * sin(rotZ / 2); 00121 double q3 = cos(rotX / 2) * cos(rotY / 2) * sin(rotZ / 2) 00122 - sin(rotX / 2) * sin(rotY / 2) * cos(rotZ / 2); 00123 00124 return OrientationPtr(new Orientation(q0, q1, q2, q3)); 00125 } 00126 00127 OrientationPtr Orientation::fromAxisAngle(double angle, double vecX, 00128 double vecY, double vecZ) { 00129 00130 if (fabs(angle) < epsilon()) { 00131 // Angle is zero, direction vector has no effect 00132 return OrientationPtr(new Orientation(1.0, 0.0, 0.0, 0.0)); 00133 } 00134 00135 double qx = vecX * sin(angle / 2); 00136 double qy = vecY * sin(angle / 2); 00137 double qz = vecZ * sin(angle / 2); 00138 double qw = cos(angle / 2); 00139 00140 return OrientationPtr(new Orientation(qw, qx, qy, qz)); 00141 } 00142 00143 OrientationPtr Orientation::fromCompactAxisAngle(double aaX, double aaY, 00144 double aaZ) { 00145 double angle = sqrt(aaX * aaX + aaY * aaY + aaZ * aaZ); 00146 return Orientation::fromAxisAngle(angle, aaX / angle, aaY / angle, 00147 aaZ / angle); 00148 } 00149 00150 OrientationPtr Orientation::fromQuaternion(double q0, double q1, double q2, 00151 double q3) { 00152 ::boost::math::quaternion<double> quaternion(q0, q1, q2, q3); 00153 return OrientationPtr(new Orientation(quaternion)); 00154 } 00155 00156 OrientationPtr Orientation::fromQuaternion( 00157 const ::boost::math::quaternion<double>& quaternion) { 00158 return OrientationPtr(new Orientation(quaternion)); 00159 } 00160 00161 OrientationPtr Orientation::fromRotationMatrix(double rxx, double rxy, 00162 double rxz, double ryx, double ryy, double ryz, double rzx, double rzy, 00163 double rzz) { 00164 return OrientationPtr( 00165 new Orientation(rxx, rxy, rxz, ryx, ryy, ryz, rzx, rzy, rzz)); 00166 } 00167 00168 Pose::Pose() : 00169 CartesianValue() { 00170 } 00171 00172 Pose::Pose(double valx, double valy, double valz, double vala, double valb, 00173 double valc, double vald) : 00174 CartesianValue(valx, valy, valz, vala, valb, valc, vald) { 00175 } 00176 00177 Pose::Pose(double valx, double valy, double valz, double vala, double valb, 00178 double valc) : 00179 CartesianValue( 00180 CartesianTranslationalPtr( 00181 new CartesianTranslational(valx, valy, valz)), 00182 CartesianRotationalPtr( 00183 new CartesianRotational(vala, valb, valc))) { 00184 } 00185 00186 Pose::Pose(const Pose& pose) : 00187 CartesianValue(pose) { 00188 } 00189 00190 Pose::Pose(const RealVector& values) : 00191 CartesianValue(values) { 00192 } 00193 00194 Pose::Pose(const TranslationPtr& transl, const OrientationPtr& rot) : 00195 CartesianValue(transl, rot) { 00196 } 00197 00198 TranslationPtr Pose::getTranslation() const { 00199 return Translation::fromMeters(this->x(), this->y(), this->z()); 00200 } 00201 00202 OrientationPtr Pose::getOrientation() const { 00203 return Orientation::fromQuaternion(this->q0(), this->q1(), this->q2(), 00204 this->q3()); 00205 } 00206 00207 double Pose::x() const { 00208 return this->_transl.x(); 00209 } 00210 00211 double Pose::y() const { 00212 return this->_transl.y(); 00213 } 00214 00215 double Pose::z() const { 00216 return this->_transl.z(); 00217 } 00218 00219 double Pose::a() const { 00220 return this->_rot.a(); 00221 } 00222 00223 double Pose::b() const { 00224 return this->_rot.b(); 00225 } 00226 00227 double Pose::c() const { 00228 return this->_rot.c(); 00229 } 00230 00231 double Pose::q0() const { 00232 return this->_rot.q0(); 00233 } 00234 00235 double Pose::q1() const { 00236 return this->_rot.q1(); 00237 } 00238 00239 double Pose::q2() const { 00240 return this->_rot.q2(); 00241 } 00242 00243 double Pose::q3() const { 00244 return this->_rot.q3(); 00245 } 00246 00247 PosePtr Pose::fromMetersAndRadians(double x, double y, double z, double q0, 00248 double q1, double q2, double q3) { 00249 PosePtr pose(new Pose(x, y, z, q0, q1, q2, q3)); 00250 return pose; 00251 } 00252 00253 PosePtr Pose::fromMetersAndRadians(double x, double y, double z, double a, 00254 double b, double c) { 00255 TranslationPtr transl = Translation::fromMeters(x, y, z); 00256 OrientationPtr rot = boost::static_pointer_cast<Orientation>( 00257 Orientation::fromEulerAngles(a, b, c)); 00258 return PosePtr(new Pose(transl, rot)); 00259 } 00260 00261 PosePtr Pose::fromMetersAndRadiansHomogeneous(double rxx, double rxy, 00262 double rxz, double tx, double ryx, double ryy, double ryz, double ty, 00263 double rzx, double rzy, double rzz, double tz) { 00264 TranslationPtr transl = Translation::fromMeters(tx, ty, tz); 00265 OrientationPtr rot = boost::static_pointer_cast<Orientation>( 00266 Orientation::fromRotationMatrix(rxx, rxy, rxz, ryx, ryy, ryz, rzx, 00267 rzy, rzz)); 00268 return PosePtr(new Pose(transl, rot)); 00269 } 00270 00271 PosePtr Pose::fromMetersAndRadiansHomogeneous(const RealMatrix& homogeneous) { 00272 return Pose::fromMetersAndRadiansHomogeneous(homogeneous(0, 0), 00273 homogeneous(0, 1), homogeneous(0, 2), homogeneous(0, 3), 00274 homogeneous(1, 0), homogeneous(1, 1), homogeneous(1, 2), 00275 homogeneous(1, 3), homogeneous(2, 0), homogeneous(2, 1), 00276 homogeneous(2, 2), homogeneous(2, 3)); 00277 } 00278 00279 std::string Pose::print() const { 00280 ostringstream outstream(ostringstream::out); 00281 outstream.precision(3); // Precision when printing double values 00282 outstream << "Pose<" << this->_transl << " , " << this->_rot << ">"; 00283 return outstream.str(); 00284 } 00285 00286 }
Generated on Thu Aug 2 14:02:47 2012 for RCI by 1.6.3